ESP32/main/metrics_ble.c

#include "metrics_ble.h"

#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "esp_ota_ops.h"
#include "esp_err.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/task.h"
#include "host/ble_gap.h"
#include "host/ble_hs.h"
#include "host/ble_uuid.h"
#include "host/util/util.h"
#include "metrics_protocol.h"
#include "nimble/nimble_port.h"
#include "nimble/nimble_port_freertos.h"
#include "nvs_flash.h"
#include "services/gap/ble_svc_gap.h"
#include "services/gatt/ble_svc_gatt.h"
#include "ui_screen.h"

static const char *TAG = "metrics_ble";
static const char *DEVICE_NAME = "RLCD-Metrics";

// Service: 9f4af548-1a14-4fc1-8f5a-4b9f00000001
// Write char: 9f4af548-1a14-4fc1-8f5a-4b9f00000002
// Status notify char: 9f4af548-1a14-4fc1-8f5a-4b9f00000003
// OTA control char: 9f4af548-1a14-4fc1-8f5a-4b9f00000004
// OTA data char: 9f4af548-1a14-4fc1-8f5a-4b9f00000005
static const ble_uuid128_t metrics_svc_uuid =
    BLE_UUID128_INIT(0x01, 0x00, 0x00, 0x00, 0x9f, 0x4b, 0x5a, 0x8f,
                     0xc1, 0x4f, 0x14, 0x1a, 0x48, 0xf5, 0x4a, 0x9f);
static const ble_uuid128_t metrics_chr_uuid =
    BLE_UUID128_INIT(0x02, 0x00, 0x00, 0x00, 0x9f, 0x4b, 0x5a, 0x8f,
                     0xc1, 0x4f, 0x14, 0x1a, 0x48, 0xf5, 0x4a, 0x9f);
static const ble_uuid128_t status_chr_uuid =
    BLE_UUID128_INIT(0x03, 0x00, 0x00, 0x00, 0x9f, 0x4b, 0x5a, 0x8f,
                     0xc1, 0x4f, 0x14, 0x1a, 0x48, 0xf5, 0x4a, 0x9f);
static const ble_uuid128_t ota_control_chr_uuid =
    BLE_UUID128_INIT(0x04, 0x00, 0x00, 0x00, 0x9f, 0x4b, 0x5a, 0x8f,
                     0xc1, 0x4f, 0x14, 0x1a, 0x48, 0xf5, 0x4a, 0x9f);
static const ble_uuid128_t ota_data_chr_uuid =
    BLE_UUID128_INIT(0x05, 0x00, 0x00, 0x00, 0x9f, 0x4b, 0x5a, 0x8f,
                     0xc1, 0x4f, 0x14, 0x1a, 0x48, 0xf5, 0x4a, 0x9f);

static uint8_t own_addr_type;
static uint16_t metrics_chr_val_handle;
static uint16_t status_chr_val_handle;
static uint16_t ota_control_chr_val_handle;
static uint16_t ota_data_chr_val_handle;
static uint16_t s_conn_handle;
static bool s_status_notify_enabled;
static QueueHandle_t s_metrics_queue;
static uint32_t s_ble_write_count;
static bool s_ota_restart_requested;

typedef struct {
    bool active;
    esp_ota_handle_t handle;
    const esp_partition_t *partition;
    uint32_t expected_size;
    uint32_t written;
} ble_ota_state_t;

static ble_ota_state_t s_ota;

void ble_store_config_init(void);

static int metrics_gap_event(struct ble_gap_event *event, void *arg);

static bool metrics_notify_text(const char *line)
{
    if (!s_status_notify_enabled || s_conn_handle == 0 || status_chr_val_handle == 0 || line == NULL) {
        return false;
    }

    struct os_mbuf *om = ble_hs_mbuf_from_flat(line, strlen(line));
    if (om == NULL) {
        return false;
    }

    int rc = ble_gatts_notify_custom(s_conn_handle, status_chr_val_handle, om);
    if (rc != 0) {
        ESP_LOGW(TAG, "status notify failed: %d", rc);
        return false;
    }
    return true;
}

static const char *metrics_phy_name(uint8_t phy)
{
    switch (phy) {
    case BLE_GAP_LE_PHY_1M:
        return "1M";
    case BLE_GAP_LE_PHY_2M:
        return "2M";
    case BLE_GAP_LE_PHY_CODED:
        return "CODED";
    default:
        return "UNKNOWN";
    }
}

static void metrics_request_2m_phy(uint16_t conn_handle)
{
    int rc = ble_gap_set_prefered_le_phy(conn_handle,
                                         BLE_GAP_LE_PHY_2M_MASK,
                                         BLE_GAP_LE_PHY_2M_MASK,
                                         BLE_GAP_LE_PHY_CODED_ANY);
    if (rc == 0) {
        ESP_LOGI(TAG, "requested BLE 5 2M PHY");
    } else {
        ESP_LOGW(TAG, "request BLE 5 2M PHY failed: %d", rc);
    }
}

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

    if (ctxt->op != BLE_GATT_ACCESS_OP_WRITE_CHR) {
        return BLE_ATT_ERR_UNLIKELY;
    }

    if (attr_handle == ota_data_chr_val_handle) {
        if (!s_ota.active || s_ota.handle == 0) {
            metrics_notify_text("OTA,ERR,NO_BEGIN");
            return BLE_ATT_ERR_UNLIKELY;
        }

        uint16_t len = OS_MBUF_PKTLEN(ctxt->om);
        if (len == 0) {
            return 0;
        }

        uint8_t data[256];
        if (len > sizeof(data)) {
            metrics_notify_text("OTA,ERR,CHUNK_TOO_BIG");
            return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
        }

        uint16_t copied = 0;
        int rc = ble_hs_mbuf_to_flat(ctxt->om, data, sizeof(data), &copied);
        if (rc != 0 || copied != len) {
            metrics_notify_text("OTA,ERR,BAD_CHUNK");
            return BLE_ATT_ERR_UNLIKELY;
        }

        esp_err_t ret = esp_ota_write(s_ota.handle, data, copied);
        if (ret != ESP_OK) {
            ESP_LOGE(TAG, "OTA write failed: %s", esp_err_to_name(ret));
            metrics_notify_text("OTA,ERR,WRITE");
            return BLE_ATT_ERR_UNLIKELY;
        }

        s_ota.written += copied;
        if ((s_ota.written % (64 * 1024)) < copied) {
            char status[48];
            snprintf(status, sizeof(status), "OTA,PROG,%lu,%lu",
                     (unsigned long)s_ota.written, (unsigned long)s_ota.expected_size);
            metrics_notify_text(status);
        }
        return 0;
    }

    char line[512] = {0};
    uint16_t len = 0;
    int rc = ble_hs_mbuf_to_flat(ctxt->om, line, sizeof(line) - 1, &len);
    if (rc != 0) {
        return BLE_ATT_ERR_UNLIKELY;
    }
    line[len] = '\0';

    if (attr_handle == ota_control_chr_val_handle) {
        if (strncmp(line, "BEGIN,", 6) == 0) {
            uint32_t expected_size = (uint32_t)strtoul(line + 6, NULL, 10);
            if (expected_size == 0) {
                metrics_notify_text("OTA,ERR,BAD_SIZE");
                return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
            }

            if (s_ota.active && s_ota.handle != 0) {
                esp_ota_abort(s_ota.handle);
            }
            memset(&s_ota, 0, sizeof(s_ota));
            s_ota.partition = esp_ota_get_next_update_partition(NULL);
            if (s_ota.partition == NULL) {
                metrics_notify_text("OTA,ERR,NO_PARTITION");
                return BLE_ATT_ERR_UNLIKELY;
            }

            esp_err_t ret = esp_ota_begin(s_ota.partition, OTA_SIZE_UNKNOWN, &s_ota.handle);
            if (ret != ESP_OK) {
                ESP_LOGE(TAG, "OTA begin failed: %s", esp_err_to_name(ret));
                metrics_notify_text("OTA,ERR,BEGIN");
                return BLE_ATT_ERR_UNLIKELY;
            }

            s_ota.active = true;
            s_ota.expected_size = expected_size;
            s_ota.written = 0;
            metrics_notify_text("OTA,BEGIN");
            return 0;
        }

        if (strcmp(line, "END") == 0) {
            if (!s_ota.active || s_ota.handle == 0) {
                metrics_notify_text("OTA,ERR,NO_BEGIN");
                return BLE_ATT_ERR_UNLIKELY;
            }
            if (s_ota.expected_size > 0 && s_ota.written != s_ota.expected_size) {
                ESP_LOGW(TAG, "OTA size mismatch: written=%lu expected=%lu",
                         (unsigned long)s_ota.written, (unsigned long)s_ota.expected_size);
                esp_ota_abort(s_ota.handle);
                memset(&s_ota, 0, sizeof(s_ota));
                metrics_notify_text("OTA,ERR,SIZE");
                return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
            }

            esp_err_t ret = esp_ota_end(s_ota.handle);
            if (ret != ESP_OK) {
                ESP_LOGE(TAG, "OTA end failed: %s", esp_err_to_name(ret));
                memset(&s_ota, 0, sizeof(s_ota));
                metrics_notify_text("OTA,ERR,END");
                return BLE_ATT_ERR_UNLIKELY;
            }

            ret = esp_ota_set_boot_partition(s_ota.partition);
            if (ret != ESP_OK) {
                ESP_LOGE(TAG, "OTA set boot failed: %s", esp_err_to_name(ret));
                memset(&s_ota, 0, sizeof(s_ota));
                metrics_notify_text("OTA,ERR,BOOT");
                return BLE_ATT_ERR_UNLIKELY;
            }

            char status[48];
            snprintf(status, sizeof(status), "OTA,DONE,%lu", (unsigned long)s_ota.written);
            memset(&s_ota, 0, sizeof(s_ota));
            s_ota_restart_requested = true;
            metrics_notify_text(status);
            return 0;
        }

        if (strcmp(line, "ABORT") == 0) {
            if (s_ota.active && s_ota.handle != 0) {
                esp_ota_abort(s_ota.handle);
            }
            memset(&s_ota, 0, sizeof(s_ota));
            metrics_notify_text("OTA,ABORT");
            return 0;
        }

        metrics_notify_text("OTA,ERR,BAD_CMD");
        return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
    }

    if (attr_handle != metrics_chr_val_handle) {
        return BLE_ATT_ERR_UNLIKELY;
    }

    ui_metrics_t metrics;
    if (!metrics_parse_line(line, &metrics)) {
        ESP_LOGW(TAG, "bad BLE metrics: %s", line);
        ui_screen_set_status("BAD BLE DATA");
        return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
    }

    if (s_metrics_queue != NULL) {
        xQueueOverwrite(s_metrics_queue, &metrics);
    }

    s_ble_write_count++;
    if ((s_ble_write_count % 10) == 0) {
        ESP_LOGI(TAG, "BLE metrics received: %lu", (unsigned long)s_ble_write_count);
    }
    return 0;
}

static const struct ble_gatt_svc_def metrics_gatt_svcs[] = {
    {
        .type = BLE_GATT_SVC_TYPE_PRIMARY,
        .uuid = &metrics_svc_uuid.u,
        .characteristics = (struct ble_gatt_chr_def[]) {
            {
                .uuid = &metrics_chr_uuid.u,
                .access_cb = metrics_gatt_access,
                .flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_WRITE_NO_RSP,
                .val_handle = &metrics_chr_val_handle,
            },
            {
                .uuid = &status_chr_uuid.u,
                .access_cb = metrics_gatt_access,
                .flags = BLE_GATT_CHR_F_NOTIFY,
                .val_handle = &status_chr_val_handle,
            },
            {
                .uuid = &ota_control_chr_uuid.u,
                .access_cb = metrics_gatt_access,
                .flags = BLE_GATT_CHR_F_WRITE,
                .val_handle = &ota_control_chr_val_handle,
            },
            {
                .uuid = &ota_data_chr_uuid.u,
                .access_cb = metrics_gatt_access,
                .flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_WRITE_NO_RSP,
                .val_handle = &ota_data_chr_val_handle,
            },
            {0},
        },
    },
    {0},
};

static void metrics_advertise(void)
{
    struct ble_hs_adv_fields fields;
    memset(&fields, 0, sizeof(fields));

    fields.flags = BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP;
    fields.name = (uint8_t *)DEVICE_NAME;
    fields.name_len = strlen(DEVICE_NAME);
    fields.name_is_complete = 1;

    int rc = ble_gap_adv_set_fields(&fields);
    if (rc != 0) {
        ESP_LOGE(TAG, "set adv fields failed: %d", rc);
        return;
    }

    struct ble_gap_adv_params adv_params;
    memset(&adv_params, 0, sizeof(adv_params));
    adv_params.conn_mode = BLE_GAP_CONN_MODE_UND;
    adv_params.disc_mode = BLE_GAP_DISC_MODE_GEN;

    rc = ble_gap_adv_start(own_addr_type, NULL, BLE_HS_FOREVER,
                           &adv_params, metrics_gap_event, NULL);
    if (rc != 0) {
        ESP_LOGE(TAG, "start advertising failed: %d", rc);
        return;
    }

    ESP_LOGI(TAG, "BLE advertising as %s", DEVICE_NAME);
}

static int metrics_gap_event(struct ble_gap_event *event, void *arg)
{
    (void)arg;

    switch (event->type) {
    case BLE_GAP_EVENT_CONNECT:
        if (event->connect.status == 0) {
            ESP_LOGI(TAG, "BLE connected");
            s_conn_handle = event->connect.conn_handle;
            s_status_notify_enabled = false;
            metrics_request_2m_phy(event->connect.conn_handle);
        } else {
            s_conn_handle = 0;
            s_status_notify_enabled = false;
            metrics_advertise();
        }
        return 0;

    case BLE_GAP_EVENT_PHY_UPDATE_COMPLETE:
        ESP_LOGI(TAG, "BLE PHY updated: status=%d tx=%s rx=%s",
                 event->phy_updated.status,
                 metrics_phy_name(event->phy_updated.tx_phy),
                 metrics_phy_name(event->phy_updated.rx_phy));
        return 0;

    case BLE_GAP_EVENT_DISCONNECT:
        ESP_LOGI(TAG, "BLE disconnected");
        s_conn_handle = 0;
        s_status_notify_enabled = false;
        metrics_advertise();
        return 0;

    case BLE_GAP_EVENT_SUBSCRIBE:
        if (event->subscribe.attr_handle == status_chr_val_handle) {
            s_status_notify_enabled = event->subscribe.cur_notify;
            ESP_LOGI(TAG, "status notify %s",
                     s_status_notify_enabled ? "enabled" : "disabled");
        }
        return 0;

    case BLE_GAP_EVENT_ADV_COMPLETE:
        metrics_advertise();
        return 0;

    default:
        return 0;
    }
}

static void metrics_on_reset(int reason)
{
    ESP_LOGE(TAG, "BLE reset: %d", reason);
}

static void metrics_on_sync(void)
{
    int rc = ble_hs_util_ensure_addr(0);
    assert(rc == 0);

    rc = ble_hs_id_infer_auto(0, &own_addr_type);
    assert(rc == 0);

    rc = ble_gap_set_prefered_default_le_phy(BLE_GAP_LE_PHY_2M_MASK,
                                             BLE_GAP_LE_PHY_2M_MASK);
    if (rc == 0) {
        ESP_LOGI(TAG, "BLE 5 default PHY preference set to 2M");
    } else {
        ESP_LOGW(TAG, "set BLE 5 default PHY preference failed: %d", rc);
    }

    metrics_advertise();
}

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

void metrics_ble_start(void)
{
    esp_err_t nvs_ret = nvs_flash_init();
    if (nvs_ret == ESP_ERR_NVS_NO_FREE_PAGES || nvs_ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        nvs_ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(nvs_ret);

    if (s_metrics_queue == NULL) {
        s_metrics_queue = xQueueCreate(1, sizeof(ui_metrics_t));
        if (s_metrics_queue == NULL) {
            ESP_LOGE(TAG, "failed to create BLE metrics queue");
            return;
        }
    }

    int rc = nimble_port_init();
    if (rc != ESP_OK) {
        ESP_LOGE(TAG, "nimble init failed: %d", rc);
        return;
    }

    ble_hs_cfg.reset_cb = metrics_on_reset;
    ble_hs_cfg.sync_cb = metrics_on_sync;
    ble_hs_cfg.store_status_cb = ble_store_util_status_rr;

    ble_svc_gap_init();
    ble_svc_gatt_init();

    rc = ble_gatts_count_cfg(metrics_gatt_svcs);
    assert(rc == 0);

    rc = ble_gatts_add_svcs(metrics_gatt_svcs);
    assert(rc == 0);

    rc = ble_svc_gap_device_name_set(DEVICE_NAME);
    assert(rc == 0);

    ble_store_config_init();
    nimble_port_freertos_init(metrics_host_task);
}

bool metrics_ble_take_latest(ui_metrics_t *metrics)
{
    if (metrics == NULL || s_metrics_queue == NULL) {
        return false;
    }

    return xQueueReceive(s_metrics_queue, metrics, 0) == pdTRUE;
}

bool metrics_ble_notify_status(int battery_percent, int battery_mv,
                               int temperature_c10, int humidity_percent,
                               uint32_t uptime_seconds, uint32_t internal_free_kb,
                               uint32_t psram_free_kb)
{
    char line[128];
    int len = snprintf(line, sizeof(line), "STA2,1,%d,%d,%d,%d,%lu,%lu,%lu,%lu",
                       battery_percent, battery_mv, temperature_c10, humidity_percent,
                       (unsigned long)s_ble_write_count, (unsigned long)uptime_seconds,
                       (unsigned long)internal_free_kb, (unsigned long)psram_free_kb);
    if (len <= 0 || len >= (int)sizeof(line)) {
        return false;
    }

    return metrics_notify_text(line);
}

bool metrics_ble_take_ota_restart_request(void)
{
    if (!s_ota_restart_requested) {
        return false;
    }
    s_ota_restart_requested = false;
    return true;
}