EIS-BLE-S3/main/ble_prov.c

#include "ble_prov.h"
#include "wifi_cfg.h"
#include "protocol.h"
#include <string.h>
#include <stdio.h>
#include "nimble/nimble_port.h"
#include "nimble/nimble_port_freertos.h"
#include "host/ble_hs.h"
#include "host/ble_uuid.h"
#include "host/util/util.h"
#include "services/gap/ble_svc_gap.h"
#include "services/gatt/ble_svc_gatt.h"
#include "esp_mac.h"

/* wifi_transport provides these */
extern esp_err_t wifi_transport_reconnect_sta(const char *ssid, const char *pass);
extern void      wifi_transport_get_sta_state(uint8_t *state, uint32_t *ip);

#define DEVICE_NAME    "EIS4"
#define FW_VERSION     "4.0"
#define RSP_BUF_MAX    128

/* EIS4 provisioning service UUID: 4e455334-a001-4801-b947-001122334455 */
static const ble_uuid128_t svc_uuid =
    BLE_UUID128_INIT(0x55, 0x44, 0x33, 0x22, 0x11, 0x00, 0x47, 0xb9,
                     0x01, 0x48, 0x01, 0xa0, 0x34, 0x53, 0x45, 0x4e);

/* EIS4 provisioning characteristic UUID: 4e455334-a002-4801-b947-001122334455 */
static const ble_uuid128_t chr_uuid =
    BLE_UUID128_INIT(0x55, 0x44, 0x33, 0x22, 0x11, 0x00, 0x47, 0xb9,
                     0x01, 0x48, 0x02, 0xa0, 0x34, 0x53, 0x45, 0x4e);

static bool     s_ble_active;
static uint16_t s_chr_handle;
static uint8_t  s_rsp_buf[RSP_BUF_MAX];
static uint16_t s_rsp_len;
static uint8_t  s_own_addr_type;

static void start_advertise(void);

/* -- response builders -- */

static void build_wifi_cfg_rsp(void)
{
    char ssid[WIFI_CFG_SSID_MAX], pass[WIFI_CFG_PASS_MAX];
    uint8_t *p = s_rsp_buf;

    *p++ = 0xF0;
    *p++ = 0x7D;
    *p++ = RSP_WIFI_CFG;

    /* STA SSID */
    if (wifi_cfg_get_sta(ssid, sizeof(ssid), pass, sizeof(pass)) == ESP_OK) {
        uint8_t slen = (uint8_t)strlen(ssid);
        *p++ = slen;
        memcpy(p, ssid, slen); p += slen;
        /* STA status */
        uint8_t state; uint32_t ip;
        wifi_transport_get_sta_state(&state, &ip);
        *p++ = state;
    } else {
        *p++ = 0;
        *p++ = WIFI_STATE_DISCONNECTED;
    }

    /* AP SSID */
    wifi_cfg_get_ap(ssid, sizeof(ssid), pass, sizeof(pass));
    {
        uint8_t slen = (uint8_t)strlen(ssid);
        *p++ = slen;
        memcpy(p, ssid, slen); p += slen;
    }

    *p++ = 0xF7;
    s_rsp_len = (uint16_t)(p - s_rsp_buf);
}

static void build_set_result(uint8_t ok)
{
    uint8_t *p = s_rsp_buf;
    *p++ = 0xF0;
    *p++ = 0x7D;
    *p++ = RSP_SET_WIFI_RESULT;
    *p++ = ok ? 0 : 1;
    *p++ = 0xF7;
    s_rsp_len = (uint16_t)(p - s_rsp_buf);
}

static void build_device_info_rsp(void)
{
    uint8_t mac[6];
    esp_read_mac(mac, ESP_MAC_WIFI_STA);

    uint8_t *p = s_rsp_buf;
    *p++ = 0xF0;
    *p++ = 0x7D;
    *p++ = RSP_DEVICE_INFO;

    /* version */
    uint8_t vlen = (uint8_t)strlen(FW_VERSION);
    *p++ = vlen;
    memcpy(p, FW_VERSION, vlen); p += vlen;

    /* name */
    uint8_t nlen = (uint8_t)strlen(DEVICE_NAME);
    *p++ = nlen;
    memcpy(p, DEVICE_NAME, nlen); p += nlen;

    /* MAC (6 bytes, each masked to 7-bit halves) */
    for (int i = 0; i < 6; i++) {
        *p++ = mac[i] & 0x7F;
        *p++ = (mac[i] >> 7) & 0x01;
    }

    *p++ = 0xF7;
    s_rsp_len = (uint16_t)(p - s_rsp_buf);
}

static void build_wifi_status_rsp(void)
{
    uint8_t state;
    uint32_t ip;
    wifi_transport_get_sta_state(&state, &ip);

    uint8_t *p = s_rsp_buf;
    *p++ = 0xF0;
    *p++ = 0x7D;
    *p++ = RSP_WIFI_STATUS;
    *p++ = state;
    if (state == WIFI_STATE_CONNECTED) {
        *p++ = (ip >>  0) & 0x7F;
        *p++ = (ip >>  7) & 0x7F;
        *p++ = (ip >> 14) & 0x7F;
        *p++ = (ip >> 21) & 0x7F;
        *p++ = (ip >> 28) & 0x0F;
    }
    *p++ = 0xF7;
    s_rsp_len = (uint16_t)(p - s_rsp_buf);
}

/* -- command processing -- */

static void process_command(const uint8_t *data, uint16_t len)
{
    if (len < 4 || data[0] != 0xF0 || data[1] != 0x7D)
        return;

    uint16_t end = 0;
    for (uint16_t i = 2; i < len; i++) {
        if (data[i] == 0xF7) { end = i; break; }
    }
    if (!end) return;

    uint8_t cmd = data[2];

    switch (cmd) {
    case CMD_GET_WIFI_CFG:
        build_wifi_cfg_rsp();
        break;

    case CMD_SET_WIFI_STA: {
        if (end < 6) { build_set_result(0); break; }
        uint8_t ssid_len = data[3];
        if (3 + 1 + ssid_len + 1 >= end) { build_set_result(0); break; }

        char ssid[WIFI_CFG_SSID_MAX] = {0};
        memcpy(ssid, &data[4], ssid_len);

        uint8_t pass_len = data[4 + ssid_len];
        char pass[WIFI_CFG_PASS_MAX] = {0};
        if (pass_len > 0 && (4 + ssid_len + 1 + pass_len) <= end)
            memcpy(pass, &data[5 + ssid_len], pass_len);

        esp_err_t err = wifi_cfg_set_sta(ssid, pass);
        if (err == ESP_OK)
            err = wifi_transport_reconnect_sta(ssid, pass);
        build_set_result(err == ESP_OK);
        break;
    }

    case CMD_SET_WIFI_AP: {
        if (end < 6) { build_set_result(0); break; }
        uint8_t ssid_len = data[3];
        if (3 + 1 + ssid_len + 1 >= end) { build_set_result(0); break; }

        char ssid[WIFI_CFG_SSID_MAX] = {0};
        memcpy(ssid, &data[4], ssid_len);

        uint8_t pass_len = data[4 + ssid_len];
        char pass[WIFI_CFG_PASS_MAX] = {0};
        if (pass_len > 0 && (4 + ssid_len + 1 + pass_len) <= end)
            memcpy(pass, &data[5 + ssid_len], pass_len);

        esp_err_t err = wifi_cfg_set_ap(ssid, pass);
        build_set_result(err == ESP_OK);
        break;
    }

    case CMD_GET_DEVICE_INFO:
        build_device_info_rsp();
        break;

    case CMD_WIFI_STATUS:
        build_wifi_status_rsp();
        break;

    default:
        return;
    }
}

/* -- GATT -- */

static int chr_access_cb(uint16_t conn_handle, uint16_t attr_handle,
                         struct ble_gatt_access_ctxt *ctxt, void *arg)
{
    (void)conn_handle;
    (void)attr_handle;
    (void)arg;

    switch (ctxt->op) {
    case BLE_GATT_ACCESS_OP_READ_CHR:
        if (s_rsp_len > 0)
            os_mbuf_append(ctxt->om, s_rsp_buf, s_rsp_len);
        return 0;

    case BLE_GATT_ACCESS_OP_WRITE_CHR: {
        uint8_t buf[RSP_BUF_MAX];
        uint16_t out_len = 0;
        uint16_t om_len = OS_MBUF_PKTLEN(ctxt->om);
        if (om_len > sizeof(buf)) om_len = sizeof(buf);
        ble_hs_mbuf_to_flat(ctxt->om, buf, sizeof(buf), &out_len);
        process_command(buf, out_len);
        return 0;
    }

    default:
        return BLE_ATT_ERR_UNLIKELY;
    }
}

static const struct ble_gatt_svc_def gatt_svr_svcs[] = {
    {
        .type = BLE_GATT_SVC_TYPE_PRIMARY,
        .uuid = &svc_uuid.u,
        .characteristics = (struct ble_gatt_chr_def[]) {
            {
                .uuid = &chr_uuid.u,
                .access_cb = chr_access_cb,
                .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE,
                .val_handle = &s_chr_handle,
            },
            { 0 },
        },
    },
    { 0 },
};

static int gatt_svr_init(void)
{
    ble_svc_gap_init();
    ble_svc_gatt_init();

    int rc = ble_gatts_count_cfg(gatt_svr_svcs);
    if (rc != 0) return rc;

    return ble_gatts_add_svcs(gatt_svr_svcs);
}

/* -- GAP -- */

static int gap_event_cb(struct ble_gap_event *event, void *arg)
{
    (void)arg;
    switch (event->type) {
    case BLE_GAP_EVENT_CONNECT:
        if (event->connect.status != 0)
            start_advertise();
        return 0;

    case BLE_GAP_EVENT_DISCONNECT:
        if (s_ble_active)
            start_advertise();
        return 0;

    case BLE_GAP_EVENT_ADV_COMPLETE:
        if (s_ble_active)
            start_advertise();
        return 0;

    default:
        return 0;
    }
}

static void start_advertise(void)
{
    struct ble_hs_adv_fields fields = {0};
    fields.flags = BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP;
    fields.tx_pwr_lvl_is_present = 1;
    fields.tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO;

    const char *name = ble_svc_gap_device_name();
    fields.name = (uint8_t *)name;
    fields.name_len = strlen(name);
    fields.name_is_complete = 1;

    ble_gap_adv_set_fields(&fields);

    struct ble_gap_adv_params params = {0};
    params.conn_mode = BLE_GAP_CONN_MODE_UND;
    params.disc_mode = BLE_GAP_DISC_MODE_GEN;

    ble_gap_adv_start(s_own_addr_type, NULL, BLE_HS_FOREVER,
                      &params, gap_event_cb, NULL);
}

static void on_sync(void)
{
    ble_hs_util_ensure_addr(0);
    ble_hs_id_infer_auto(0, &s_own_addr_type);
    start_advertise();
    printf("BLE: advertising\n");
}

static void on_reset(int reason)
{
    printf("BLE: host reset, reason=%d\n", reason);
}

static void ble_host_task(void *param)
{
    (void)param;
    nimble_port_run();
    nimble_port_freertos_deinit();
}

/* -- public API -- */

esp_err_t ble_prov_start(void)
{
    if (s_ble_active)
        return ESP_OK;

    esp_err_t err = nimble_port_init();
    if (err != ESP_OK) {
        printf("BLE: nimble_port_init failed: %d\n", (int)err);
        return err;
    }

    ble_hs_cfg.reset_cb = on_reset;
    ble_hs_cfg.sync_cb = on_sync;
    ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_NO_IO;

    int rc = gatt_svr_init();
    if (rc != 0) {
        printf("BLE: GATT init failed: %d\n", rc);
        nimble_port_deinit();
        return ESP_FAIL;
    }

    ble_svc_gap_device_name_set(DEVICE_NAME);
    nimble_port_freertos_init(ble_host_task);

    s_ble_active = true;
    s_rsp_len = 0;
    printf("BLE: provisioning started\n");
    return ESP_OK;
}

esp_err_t ble_prov_stop(void)
{
    if (!s_ble_active)
        return ESP_OK;

    s_ble_active = false;

    if (ble_gap_adv_active())
        ble_gap_adv_stop();

    nimble_port_stop();
    nimble_port_deinit();

    printf("BLE: provisioning stopped\n");
    return ESP_OK;
}

bool ble_prov_is_active(void)
{
    return s_ble_active;
}