HeavenlyHeroStar/main/services/ble_manager.c

#include "ble_manager.h"

#include "board_config.h"
#include "esp_log.h"
#include "sdkconfig.h"

#if BOARD_BLE_ENABLED

#include "driver/gpio.h"
#include "host/ble_hs.h"
#include "host/ble_uuid.h"
#include "host/util/util.h"
#include "nimble/nimble_port.h"
#include "nimble/nimble_port_freertos.h"
#include "services/gap/ble_svc_gap.h"
#include "services/gatt/ble_svc_gatt.h"
#include "store/config/ble_store_config.h"

#include <string.h>

#define BLE_DEVICE_NAME CONFIG_ESP_BLE_DEVICE_NAME

#define HHS_SVC_UUID16 0xFFF0
#define HHS_CHR_UUID16 0xFFF1

static const char *TAG = "ble_manager";

static ble_status_cb_t s_status_cb;
static bool s_connected;
static uint8_t s_own_addr_type;
static uint16_t s_conn_handle = BLE_HS_CONN_HANDLE_NONE;

static uint8_t s_chr_val[20] = "HeavenlyHeroStar";
static uint16_t s_chr_val_handle;
static const ble_uuid16_t s_hhs_svc_uuid = BLE_UUID16_INIT(HHS_SVC_UUID16);
static const ble_uuid16_t s_hhs_chr_uuid = BLE_UUID16_INIT(HHS_CHR_UUID16);

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

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

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

    switch (ctxt->op) {
    case BLE_GATT_ACCESS_OP_READ_CHR:
        return os_mbuf_append(ctxt->om, s_chr_val, sizeof(s_chr_val)) == 0
                   ? 0
                   : BLE_ATT_ERR_INSUFFICIENT_RES;

    case BLE_GATT_ACCESS_OP_WRITE_CHR: {
        uint16_t len = OS_MBUF_PKTLEN(ctxt->om);
        if (len > sizeof(s_chr_val)) {
            return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
        }
        memset(s_chr_val, 0, sizeof(s_chr_val));
        int rc = ble_hs_mbuf_to_flat(ctxt->om, s_chr_val, sizeof(s_chr_val), NULL);
        if (rc != 0) {
            return BLE_ATT_ERR_UNLIKELY;
        }
        ESP_LOGI(TAG, "characteristic write from conn=%d, len=%u", conn_handle, len);
        return 0;
    }

    default:
        return BLE_ATT_ERR_UNLIKELY;
    }
}

static const struct ble_gatt_svc_def s_gatt_svcs[] = {
    {
        .type = BLE_GATT_SVC_TYPE_PRIMARY,
        .uuid = &s_hhs_svc_uuid.u,
        .characteristics =
            (struct ble_gatt_chr_def[]){
                {
                    .uuid = &s_hhs_chr_uuid.u,
                    .access_cb = chr_access,
                    .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE,
                    .val_handle = &s_chr_val_handle,
                },
                {0},
            },
    },
    {0},
};

static void notify_status(bool connected)
{
    s_connected = connected;
    gpio_set_level(BOARD_PIN_LED, connected ? 1 : 0);
    if (s_status_cb != NULL) {
        s_status_cb(connected);
    }
}

static void start_advertising(void)
{
    struct ble_hs_adv_fields fields = {0};
    struct ble_gap_adv_params adv_params = {0};
    const char *name = ble_svc_gap_device_name();
    int rc;

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

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

    adv_params.conn_mode = BLE_GAP_CONN_MODE_UND;
    adv_params.disc_mode = BLE_GAP_DISC_MODE_GEN;

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

    ESP_LOGI(TAG, "advertising as \"%s\"", name);
}

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

    switch (event->type) {
    case BLE_GAP_EVENT_CONNECT:
        if (event->connect.status == 0) {
            s_conn_handle = event->connect.conn_handle;
            ESP_LOGI(TAG, "connected, handle=%d", s_conn_handle);
            notify_status(true);
        } else {
            ESP_LOGW(TAG, "connection failed, status=%d", event->connect.status);
            start_advertising();
        }
        return 0;

    case BLE_GAP_EVENT_DISCONNECT:
        ESP_LOGI(TAG, "disconnected, reason=%d", event->disconnect.reason);
        s_conn_handle = BLE_HS_CONN_HANDLE_NONE;
        notify_status(false);
        start_advertising();
        return 0;

    case BLE_GAP_EVENT_ADV_COMPLETE:
        start_advertising();
        return 0;

    default:
        return 0;
    }
}

static void on_stack_sync(void)
{
    int rc = ble_hs_util_ensure_addr(0);
    if (rc != 0) {
        ESP_LOGE(TAG, "no available BT address");
        return;
    }

    rc = ble_hs_id_infer_auto(0, &s_own_addr_type);
    if (rc != 0) {
        ESP_LOGE(TAG, "failed to infer address type: %d", rc);
        return;
    }

    start_advertising();
}

static void on_stack_reset(int reason)
{
    ESP_LOGW(TAG, "nimble stack reset, reason=%d", reason);
}

static void gatt_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg)
{
    (void)ctxt;
    (void)arg;
}

static int gatt_svc_init(void)
{
    int rc;

    ble_svc_gatt_init();
    rc = ble_gatts_count_cfg(s_gatt_svcs);
    if (rc != 0) {
        return rc;
    }

    return ble_gatts_add_svcs(s_gatt_svcs);
}

void ble_store_config_init(void);

static void nimble_host_task(void *param)
{
    (void)param;
    nimble_port_run();
    vTaskDelete(NULL);
}

#endif /* BOARD_BLE_ENABLED */

void ble_manager_set_status_cb(ble_status_cb_t cb)
{
#if BOARD_BLE_ENABLED
    s_status_cb = cb;
#else
    (void)cb;
#endif
}

bool ble_manager_is_connected(void)
{
#if BOARD_BLE_ENABLED
    return s_connected;
#else
    return false;
#endif
}

void ble_manager_init(void)
{
#if BOARD_BLE_ENABLED
    int rc;

    gpio_reset_pin(BOARD_PIN_LED);
    gpio_set_direction(BOARD_PIN_LED, GPIO_MODE_OUTPUT);
    gpio_set_level(BOARD_PIN_LED, 0);

    ESP_ERROR_CHECK(nimble_port_init());

    ble_svc_gap_init();
    rc = ble_svc_gap_device_name_set(BLE_DEVICE_NAME);
    if (rc != 0) {
        ESP_LOGE(TAG, "failed to set device name");
        return;
    }

    rc = gatt_svc_init();
    if (rc != 0) {
        ESP_LOGE(TAG, "failed to init GATT service: %d", rc);
        return;
    }

    ble_hs_cfg.reset_cb = on_stack_reset;
    ble_hs_cfg.sync_cb = on_stack_sync;
    ble_hs_cfg.gatts_register_cb = gatt_register_cb;
    ble_hs_cfg.store_status_cb = ble_store_util_status_rr;
    ble_store_config_init();

    nimble_port_freertos_init(nimble_host_task);

    ESP_LOGI(TAG, "BLE peripheral started");
#else
    ESP_LOGI("ble_manager", "BLE disabled by board config");
#endif
}