gateway/components/gateway_ble/src/gateway_ble.cpp

#include "gateway_ble.hpp"

#include "dali_domain.hpp"
#include "gateway_controller.hpp"
#include "gateway_runtime.hpp"

#include "esp_log.h"
#include "esp_timer.h"
#include "host/ble_gap.h"
#include "host/ble_gatt.h"
#include "host/ble_hs.h"
#include "host/ble_store.h"
#include "host/util/util.h"
#include "nimble/ble.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 <algorithm>

namespace {

constexpr const char* kTag = "gateway_ble";
constexpr uint16_t kServiceUuid = 0xFFF6;
constexpr uint16_t kChannel1Uuid = 0xFFF1;
constexpr uint16_t kChannel2Uuid = 0xFFF2;
constexpr uint16_t kGatewayUuid = 0xFFF3;
constexpr int64_t kGenericDedupeWindowUs = 120000;
constexpr size_t kGatewayCharacteristicIndex = 2;

gateway::GatewayBleBridge* s_active_bridge = nullptr;
uint16_t s_value_handles[3] = {0, 0, 0};

int GapEvent(struct ble_gap_event* event, void* arg);
int AccessCharacteristic(uint16_t conn_handle, uint16_t attr_handle,
                         struct ble_gatt_access_ctxt* ctxt, void* arg);

const ble_uuid16_t kServiceUuidDef = BLE_UUID16_INIT(kServiceUuid);
const ble_uuid16_t kChannel1UuidDef = BLE_UUID16_INIT(kChannel1Uuid);
const ble_uuid16_t kChannel2UuidDef = BLE_UUID16_INIT(kChannel2Uuid);
const ble_uuid16_t kGatewayUuidDef = BLE_UUID16_INIT(kGatewayUuid);

const ble_gatt_chr_def kGattCharacteristics[] = {
  {
    .uuid = &kChannel1UuidDef.u,
    .access_cb = AccessCharacteristic,
    .arg = nullptr,
    .descriptors = nullptr,
    .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_WRITE_NO_RSP |
         BLE_GATT_CHR_F_NOTIFY,
    .min_key_size = 0,
    .val_handle = &s_value_handles[0],
    .cpfd = nullptr,
  },
  {
    .uuid = &kChannel2UuidDef.u,
    .access_cb = AccessCharacteristic,
    .arg = nullptr,
    .descriptors = nullptr,
    .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_WRITE_NO_RSP |
         BLE_GATT_CHR_F_NOTIFY,
    .min_key_size = 0,
    .val_handle = &s_value_handles[1],
    .cpfd = nullptr,
  },
  {
    .uuid = &kGatewayUuidDef.u,
    .access_cb = AccessCharacteristic,
    .arg = nullptr,
    .descriptors = nullptr,
    .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_WRITE_NO_RSP |
         BLE_GATT_CHR_F_NOTIFY,
    .min_key_size = 0,
    .val_handle = &s_value_handles[2],
    .cpfd = nullptr,
  },
  {
    .uuid = nullptr,
    .access_cb = nullptr,
    .arg = nullptr,
    .descriptors = nullptr,
    .flags = 0,
    .min_key_size = 0,
    .val_handle = nullptr,
    .cpfd = nullptr,
  },
};

extern "C" void ble_store_config_init(void);

int GapEvent(struct ble_gap_event* event, void* arg) {
  auto* bridge = static_cast<gateway::GatewayBleBridge*>(arg != nullptr ? arg : s_active_bridge);
  return bridge != nullptr ? bridge->handleGapEvent(event) : 0;
}

int AccessCharacteristic(uint16_t conn_handle, uint16_t attr_handle,
                         struct ble_gatt_access_ctxt* ctxt, void* arg) {
  auto* bridge = static_cast<gateway::GatewayBleBridge*>(arg != nullptr ? arg : s_active_bridge);
  return bridge != nullptr ? bridge->handleAccess(conn_handle, attr_handle, ctxt)
                           : BLE_ATT_ERR_UNLIKELY;
}

void OnReset(int reason) {
  ESP_LOGW(kTag, "nimble reset reason=%d", reason);
}

void OnSync(void) {
  if (s_active_bridge == nullptr) {
    return;
  }
  s_active_bridge->handleSync();
}

void HostTask(void* param) {
  (void)param;
  nimble_port_run();
  nimble_port_freertos_deinit();
}

void RegisterGatt(struct ble_gatt_register_ctxt* ctxt, void* arg) {
  (void)arg;
  char buffer[BLE_UUID_STR_LEN] = {0};
  switch (ctxt->op) {
    case BLE_GATT_REGISTER_OP_SVC:
      ESP_LOGD(kTag, "registered service %s handle=%u",
               ble_uuid_to_str(ctxt->svc.svc_def->uuid, buffer), ctxt->svc.handle);
      break;
    case BLE_GATT_REGISTER_OP_CHR:
      ESP_LOGD(kTag, "registered characteristic %s def=%u val=%u",
               ble_uuid_to_str(ctxt->chr.chr_def->uuid, buffer), ctxt->chr.def_handle,
               ctxt->chr.val_handle);
      break;
    case BLE_GATT_REGISTER_OP_DSC:
      ESP_LOGD(kTag, "registered descriptor %s handle=%u",
               ble_uuid_to_str(ctxt->dsc.dsc_def->uuid, buffer), ctxt->dsc.handle);
      break;
    default:
      break;
  }
}

std::vector<uint8_t> LegacyRawPayload(const std::vector<uint8_t>& data) {
  if (data.size() == 1) {
    return {0xBE, data[0]};
  }
  return data;
}

const struct ble_gatt_svc_def kGattServices[] = {
    {
        .type = BLE_GATT_SVC_TYPE_PRIMARY,
    .uuid = &kServiceUuidDef.u,
    .includes = nullptr,
    .characteristics = kGattCharacteristics,
    },
    {
    .type = 0,
    .uuid = nullptr,
    .includes = nullptr,
    .characteristics = nullptr,
    },
};

}  // namespace

namespace gateway {

GatewayBleBridge::GatewayBleBridge(GatewayController& controller, GatewayRuntime& runtime,
                                   DaliDomainService& dali_domain)
    : controller_(controller), runtime_(runtime), dali_domain_(dali_domain) {}

esp_err_t GatewayBleBridge::start() {
  if (started_) {
    return ESP_OK;
  }

  s_active_bridge = this;
  enabled_ = runtime_.bleEnabled();
  refreshDeviceName();

  controller_.addNotificationSink(
      [this](const std::vector<uint8_t>& frame) { handleGatewayNotification(frame); });
  controller_.addBleStateSink([this](bool enabled) { setEnabled(enabled); });
  controller_.addGatewayNameSink([this](uint8_t) { refreshDeviceName(); });
  dali_domain_.addRawFrameSink([this](const DaliRawFrame& frame) { handleDaliRawFrame(frame); });

  const esp_err_t err = initNimble();
  if (err != ESP_OK) {
    s_active_bridge = nullptr;
    return err;
  }

  started_ = true;
  ESP_LOGI(kTag, "BLE bridge started enabled=%d name=%s", enabled_, device_name_.c_str());
  return ESP_OK;
}

esp_err_t GatewayBleBridge::initNimble() {
  const esp_err_t err = nimble_port_init();
  if (err != ESP_OK) {
    ESP_LOGE(kTag, "failed to init NimBLE err=%s", esp_err_to_name(err));
    return err;
  }

  ble_hs_cfg.reset_cb = OnReset;
  ble_hs_cfg.sync_cb = OnSync;
  ble_hs_cfg.gatts_register_cb = RegisterGatt;
  ble_hs_cfg.store_status_cb = ble_store_util_status_rr;

  ble_svc_gap_init();
  ble_svc_gatt_init();

  int rc = ble_gatts_count_cfg(kGattServices);
  if (rc != 0) {
    ESP_LOGE(kTag, "failed to count GATT services rc=%d", rc);
    return ESP_FAIL;
  }

  rc = ble_gatts_add_svcs(kGattServices);
  if (rc != 0) {
    ESP_LOGE(kTag, "failed to add GATT services rc=%d", rc);
    return ESP_FAIL;
  }

#if CONFIG_BT_NIMBLE_GAP_SERVICE
  rc = ble_svc_gap_device_name_set(device_name_.c_str());
  if (rc != 0) {
    ESP_LOGE(kTag, "failed to set BLE device name rc=%d", rc);
    return ESP_FAIL;
  }
#endif

  ble_store_config_init();
  nimble_port_freertos_init(HostTask);
  return ESP_OK;
}

void GatewayBleBridge::handleSync() {
  int rc = ble_hs_util_ensure_addr(0);
  if (rc != 0) {
    ESP_LOGE(kTag, "failed to ensure BLE address rc=%d", rc);
    return;
  }

  rc = ble_hs_id_infer_auto(0, &own_addr_type_);
  if (rc != 0) {
    ESP_LOGE(kTag, "failed to infer BLE address type rc=%d", rc);
    return;
  }

  synced_ = true;
  if (enabled_) {
    startAdvertising();
  }
}

void GatewayBleBridge::refreshDeviceName() {
  device_name_ = resolvedDeviceName();

  if (!started_) {
    return;
  }

#if CONFIG_BT_NIMBLE_GAP_SERVICE
  const int rc = ble_svc_gap_device_name_set(device_name_.c_str());
  if (rc != 0) {
    ESP_LOGW(kTag, "failed to refresh BLE name rc=%d", rc);
  }
#endif

  if (synced_ && enabled_ && conn_handle_ == kInvalidConnectionHandle) {
    stopAdvertising();
    startAdvertising();
  }
}

void GatewayBleBridge::setEnabled(bool enabled) {
  enabled_ = enabled;
  if (!synced_) {
    return;
  }

  if (enabled_) {
    refreshDeviceName();
    if (conn_handle_ == kInvalidConnectionHandle) {
      startAdvertising();
    }
    return;
  }

  stopAdvertising();
  if (conn_handle_ != kInvalidConnectionHandle) {
    const int rc = ble_gap_terminate(conn_handle_, BLE_ERR_REM_USER_CONN_TERM);
    if (rc != 0) {
      ESP_LOGW(kTag, "failed to terminate BLE connection rc=%d", rc);
    }
  }
}

void GatewayBleBridge::startAdvertising() {
  if (!enabled_ || ble_gap_adv_active()) {
    return;
  }

  ble_uuid16_t advertised_service = BLE_UUID16_INIT(kServiceUuid);
  struct ble_hs_adv_fields fields = {};
  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;
  fields.name = reinterpret_cast<uint8_t*>(device_name_.data());
  fields.name_len = device_name_.size();
  fields.name_is_complete = 1;
  fields.uuids16 = &advertised_service;
  fields.num_uuids16 = 1;
  fields.uuids16_is_complete = 1;

  int rc = ble_gap_adv_set_fields(&fields);
  if (rc != 0) {
    ESP_LOGE(kTag, "failed to set BLE advertising fields rc=%d", rc);
    return;
  }

  struct ble_gap_adv_params params = {};
  params.conn_mode = BLE_GAP_CONN_MODE_UND;
  params.disc_mode = BLE_GAP_DISC_MODE_GEN;
  rc = ble_gap_adv_start(own_addr_type_, nullptr, BLE_HS_FOREVER, &params, GapEvent, this);
  if (rc != 0) {
    ESP_LOGE(kTag, "failed to start BLE advertising rc=%d", rc);
  }
}

void GatewayBleBridge::stopAdvertising() {
  if (!ble_gap_adv_active()) {
    return;
  }
  const int rc = ble_gap_adv_stop();
  if (rc != 0) {
    ESP_LOGW(kTag, "failed to stop BLE advertising rc=%d", rc);
  }
}

void GatewayBleBridge::notifyCharacteristic(size_t index, const std::vector<uint8_t>& payload) {
  if (index >= std::size(s_value_handles) || conn_handle_ == kInvalidConnectionHandle ||
      !notify_enabled_[index]) {
    return;
  }

  characteristic_values_[index] = payload;
  struct os_mbuf* buffer = ble_hs_mbuf_from_flat(payload.data(), payload.size());
  if (buffer == nullptr) {
    ESP_LOGW(kTag, "failed to allocate notify mbuf idx=%u", static_cast<unsigned>(index));
    return;
  }
  const int rc = ble_gatts_notify_custom(conn_handle_, s_value_handles[index], buffer);
  if (rc != 0) {
    ESP_LOGW(kTag, "failed to send BLE notify idx=%u rc=%d", static_cast<unsigned>(index), rc);
  }
}

void GatewayBleBridge::handleGatewayNotification(const std::vector<uint8_t>& frame) {
  if (!enabled_ || conn_handle_ == kInvalidConnectionHandle || !notify_enabled_[kGatewayCharacteristicIndex] ||
      frame.empty()) {
    return;
  }

  const int64_t now = esp_timer_get_time();
  int64_t dedupe_window = kGenericDedupeWindowUs;
  if (frame.size() > 1 && (frame[1] == 0x03 || frame[1] == 0x04)) {
    dedupe_window = 0;
  }
  if (dedupe_window > 0 && frame == last_notify_payload_ &&
      (now - last_notify_at_us_) <= dedupe_window) {
    return;
  }

  notifyCharacteristic(kGatewayCharacteristicIndex, frame);
  last_notify_payload_ = frame;
  last_notify_at_us_ = now;
}

void GatewayBleBridge::handleDaliRawFrame(const DaliRawFrame& frame) {
  if (!enabled_ || conn_handle_ == kInvalidConnectionHandle || frame.data.empty()) {
    return;
  }
  notifyCharacteristic(frame.channel_index, LegacyRawPayload(frame.data));
}

void GatewayBleBridge::handleRawWrite(size_t channel_index, const std::vector<uint8_t>& payload) {
  const auto channels = dali_domain_.channelInfo();
  const auto channel_it = std::find_if(channels.begin(), channels.end(),
                                       [channel_index](const DaliChannelInfo& channel) {
                                         return channel.channel_index == channel_index;
                                       });
  if (channel_it == channels.end()) {
    ESP_LOGW(kTag, "raw BLE write for unavailable DALI channel=%u",
             static_cast<unsigned>(channel_index));
    return;
  }

  if (!payload.empty() && payload[0] == 0x12) {
    const auto response =
        dali_domain_.transactBridgeFrame(channel_it->gateway_id, payload.data(), payload.size());
    if (!response.empty()) {
      notifyCharacteristic(channel_index, response);
    }
    return;
  }

  if (!dali_domain_.writeBridgeFrame(channel_it->gateway_id, payload.data(), payload.size())) {
    ESP_LOGW(kTag, "failed to forward raw BLE payload channel=%u len=%u",
             static_cast<unsigned>(channel_index), static_cast<unsigned>(payload.size()));
  }
}

void GatewayBleBridge::handleGatewayWrite(const std::vector<uint8_t>& payload) {
  if (payload.empty()) {
    return;
  }
  if (runtime_.hasPendingQueryCommand(payload)) {
    return;
  }
  controller_.enqueueCommandFrame(payload);
}

std::string GatewayBleBridge::resolvedDeviceName() const {
  const auto channels = dali_domain_.channelInfo();
  if (channels.empty()) {
    return runtime_.defaultBleGatewayName();
  }

  const auto channel_it = std::min_element(channels.begin(), channels.end(),
                                           [](const DaliChannelInfo& lhs,
                                              const DaliChannelInfo& rhs) {
                                             return lhs.channel_index < rhs.channel_index;
                                           });
  return runtime_.bleGatewayName(channel_it->gateway_id, channel_it->name);
}

int GatewayBleBridge::characteristicIndex(uint16_t attr_handle) const {
  for (size_t index = 0; index < 3; ++index) {
    if (s_value_handles[index] == attr_handle) {
      return static_cast<int>(index);
    }
  }
  return -1;
}

int GatewayBleBridge::handleGapEvent(struct ble_gap_event* event) {
  switch (event->type) {
    case BLE_GAP_EVENT_CONNECT:
      if (event->connect.status == 0) {
        conn_handle_ = event->connect.conn_handle;
        notify_enabled_.fill(false);
        last_notify_payload_.clear();
        last_notify_at_us_ = 0;
        struct ble_gap_upd_params params = {
            .itvl_min = 15,
            .itvl_max = 15,
            .latency = 3,
            .supervision_timeout = 1000,
            .min_ce_len = 0,
            .max_ce_len = 0,
        };
        int rc = ble_gap_update_params(event->connect.conn_handle, &params);
        if (rc != 0) {
          ESP_LOGW(kTag, "ble_gap_update_params rc=%d", rc);
        }
        ESP_LOGI(kTag, "BLE client connected handle=%u", conn_handle_);
      } else if (enabled_) {
        startAdvertising();
      }
      return 0;
    case BLE_GAP_EVENT_DISCONNECT:
      conn_handle_ = kInvalidConnectionHandle;
      notify_enabled_.fill(false);
      last_notify_payload_.clear();
      last_notify_at_us_ = 0;
      if (enabled_) {
        startAdvertising();
      }
      ESP_LOGI(kTag, "BLE client disconnected reason=%d", event->disconnect.reason);
      return 0;
    case BLE_GAP_EVENT_ADV_COMPLETE:
      if (enabled_ && conn_handle_ == kInvalidConnectionHandle) {
        startAdvertising();
      }
      return 0;
    case BLE_GAP_EVENT_SUBSCRIBE: {
      const int index = characteristicIndex(event->subscribe.attr_handle);
      if (index >= 0 && event->subscribe.conn_handle == conn_handle_) {
        notify_enabled_[index] = event->subscribe.cur_notify != 0;
      }
      return 0;
    }
    default:
      return 0;
  }
}

int GatewayBleBridge::handleAccess(uint16_t, uint16_t attr_handle,
                                   struct ble_gatt_access_ctxt* ctxt) {
  const int index = characteristicIndex(attr_handle);
  if (index < 0) {
    return BLE_ATT_ERR_UNLIKELY;
  }

  switch (ctxt->op) {
    case BLE_GATT_ACCESS_OP_READ_CHR:
      if (!characteristic_values_[index].empty()) {
        const int rc = os_mbuf_append(ctxt->om, characteristic_values_[index].data(),
                                      characteristic_values_[index].size());
        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
      }
      return 0;

    case BLE_GATT_ACCESS_OP_WRITE_CHR: {
      const uint16_t len = OS_MBUF_PKTLEN(ctxt->om);
      std::vector<uint8_t> payload(len, 0);
      if (len > 0) {
        uint16_t copied = 0;
        const int rc = ble_hs_mbuf_to_flat(ctxt->om, payload.data(), len, &copied);
        if (rc != 0 || copied != len) {
          return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
        }
      }

      characteristic_values_[index] = payload;
      if (!enabled_) {
        return 0;
      }

      if (index == static_cast<int>(kGatewayCharacteristicIndex)) {
        handleGatewayWrite(payload);
      } else {
        handleRawWrite(static_cast<size_t>(index), payload);
      }
      return 0;
    }

    default:
      return BLE_ATT_ERR_UNLIKELY;
  }
}

}  // namespace gateway