ESP32/main/metrics_ble.c

#include "metrics_ble.h"

#include <assert.h>
#include <stdio.h>
#include <string.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
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 uint8_t own_addr_type;
static uint16_t metrics_chr_val_handle;
static uint16_t status_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;

void ble_store_config_init(void);

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

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 || attr_handle != metrics_chr_val_handle) {
        return BLE_ATT_ERR_UNLIKELY;
    }

    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';

    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,
            },
            {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)
{
    if (!s_status_notify_enabled || s_conn_handle == 0 || status_chr_val_handle == 0) {
        return false;
    }

    char line[96];
    int len = snprintf(line, sizeof(line), "STA,%d,%d,%d,%d,%lu",
                       battery_percent, battery_mv, temperature_c10, humidity_percent,
                       (unsigned long)s_ble_write_count);
    if (len <= 0 || len >= (int)sizeof(line)) {
        return false;
    }

    struct os_mbuf *om = ble_hs_mbuf_from_flat(line, len);
    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;
}