fix(nimble): Advertising fixes

Author: lucasssvaz

libraries/BLE/examples/Beacon_Scanner/Beacon_Scanner.ino

@@ -36,25 +36,33 @@ class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks {
     if (advertisedDevice.haveManufacturerData() == true) {
       String strManufacturerData = advertisedDevice.getManufacturerData();
 
-      uint8_t cManufacturerData[100];
-      memcpy(cManufacturerData, strManufacturerData.c_str(), strManufacturerData.length());
+      // Buffer to store manufacturer data (BLE max is 255 bytes)
+      uint8_t cManufacturerData[255];
+      size_t dataLength = strManufacturerData.length();
 
-      if (strManufacturerData.length() == 25 && cManufacturerData[0] == 0x4C && cManufacturerData[1] == 0x00) {
-        Serial.println("Found an iBeacon!");
-        BLEBeacon oBeacon = BLEBeacon();
-        oBeacon.setData(strManufacturerData);
-        Serial.printf("iBeacon Frame\n");
-        Serial.printf(
-          "ID: %04X Major: %d Minor: %d UUID: %s Power: %d\n", oBeacon.getManufacturerId(), ENDIAN_CHANGE_U16(oBeacon.getMajor()),
-          ENDIAN_CHANGE_U16(oBeacon.getMinor()), oBeacon.getProximityUUID().toString().c_str(), oBeacon.getSignalPower()
-        );
-      } else {
-        Serial.println("Found another manufacturers beacon!");
-        Serial.printf("strManufacturerData: %d ", strManufacturerData.length());
-        for (int i = 0; i < strManufacturerData.length(); i++) {
-          Serial.printf("[%X]", cManufacturerData[i]);
+      // Bounds checking to prevent buffer overflow
+      if (dataLength <= sizeof(cManufacturerData)) {
+        memcpy(cManufacturerData, strManufacturerData.c_str(), dataLength);
+
+        if (dataLength == 25 && cManufacturerData[0] == 0x4C && cManufacturerData[1] == 0x00) {
+          Serial.println("Found an iBeacon!");
+          BLEBeacon oBeacon = BLEBeacon();
+          oBeacon.setData(strManufacturerData);
+          Serial.printf("iBeacon Frame\n");
+          Serial.printf(
+            "ID: %04X Major: %d Minor: %d UUID: %s Power: %d\n", oBeacon.getManufacturerId(), ENDIAN_CHANGE_U16(oBeacon.getMajor()),
+            ENDIAN_CHANGE_U16(oBeacon.getMinor()), oBeacon.getProximityUUID().toString().c_str(), oBeacon.getSignalPower()
+          );
+        } else {
+          Serial.println("Found another manufacturers beacon!");
+          Serial.printf("strManufacturerData: %d ", dataLength);
+          for (int i = 0; i < dataLength; i++) {
+            Serial.printf("[%X]", cManufacturerData[i]);
+          }
+          Serial.printf("\n");
         }
-        Serial.printf("\n");
+      } else {
+        Serial.printf("Manufacturer data too large (%d bytes), skipping\n", dataLength);
       }
     }
 

libraries/BLE/src/BLEAdvertisedDevice.cpp

@@ -46,19 +46,47 @@ BLEAdvertisedDevice::BLEAdvertisedDevice() {
   m_serviceDataUUIDs = {};
   m_txPower = 0;
   m_pScan = nullptr;
+  m_advType = 0;
+
 #if defined(CONFIG_NIMBLE_ENABLED)
   m_callbackSent = false;
-  m_advType = 0;
 #endif
 
   m_haveAppearance = false;
   m_haveManufacturerData = false;
   m_haveName = false;
   m_haveRSSI = false;
   m_haveTXPower = false;
-
+  m_isLegacyAdv = true;
 }  // BLEAdvertisedDevice
 
+bool BLEAdvertisedDevice::isLegacyAdvertisement() {
+  return m_isLegacyAdv;
+}
+
+bool BLEAdvertisedDevice::isScannable() {
+#if defined(CONFIG_BLUEDROID_ENABLED)
+  return isLegacyAdvertisement() && (m_advType == ESP_BLE_EVT_CONN_ADV || m_advType == ESP_BLE_EVT_DISC_ADV);
+#endif
+
+#if defined(CONFIG_NIMBLE_ENABLED)
+  return isLegacyAdvertisement() && (m_advType == BLE_HCI_ADV_TYPE_ADV_IND || m_advType == BLE_HCI_ADV_TYPE_ADV_SCAN_IND);
+#endif
+}
+
+bool BLEAdvertisedDevice::isConnectable() {
+#if defined(CONFIG_BLUEDROID_ENABLED)
+  return m_advType == ESP_BLE_EVT_CONN_ADV || m_advType == ESP_BLE_EVT_CONN_DIR_ADV;
+#endif
+
+#if defined(CONFIG_NIMBLE_ENABLED)
+  if (m_isLegacyAdv) {
+    return m_advType == BLE_HCI_ADV_RPT_EVTYPE_ADV_IND || m_advType == BLE_HCI_ADV_RPT_EVTYPE_DIR_IND;
+  }
+  return (m_advType & BLE_HCI_ADV_CONN_MASK) || (m_advType & BLE_HCI_ADV_DIRECT_MASK);
+#endif
+}
+
 /**
  * @brief Get the address.
  *
@@ -623,19 +651,13 @@ size_t BLEAdvertisedDevice::getPayloadLength() {
   return m_payloadLength;
 }
 
-/***************************************************************************
- *                           NimBLE functions                              *
- ***************************************************************************/
-
-#if defined(CONFIG_NIMBLE_ENABLED)
 void BLEAdvertisedDevice::setAdvType(uint8_t type) {
   m_advType = type;
 }
 
 uint8_t BLEAdvertisedDevice::getAdvType() {
   return m_advType;
 }
-#endif /* CONFIG_NIMBLE_ENABLED */
 
 #endif /* CONFIG_BLUEDROID_ENABLED || CONFIG_NIMBLE_ENABLED */
 #endif /* SOC_BLE_SUPPORTED */

libraries/BLE/src/BLEAdvertisedDevice.h

@@ -95,7 +95,12 @@ class BLEAdvertisedDevice {
   uint8_t getAddressType();
   ble_frame_type_t getFrameType();
   void setAddressType(uint8_t type);
+  void setAdvType(uint8_t type);
+  uint8_t getAdvType();
 
+  bool isLegacyAdvertisement();
+  bool isScannable();
+  bool isConnectable();
   bool isAdvertisingService(BLEUUID uuid);
   bool haveAppearance();
   bool haveManufacturerData();
@@ -107,15 +112,6 @@ class BLEAdvertisedDevice {
 
   String toString();
 
-  /***************************************************************************
-   *                       NimBLE public declarations                        *
-   ***************************************************************************/
-
-#if defined(CONFIG_NIMBLE_ENABLED)
-  void setAdvType(uint8_t type);
-  uint8_t getAdvType();
-#endif
-
 private:
   friend class BLEScan;
 
@@ -143,14 +139,15 @@ class BLEAdvertisedDevice {
   uint8_t *m_payload;
   size_t m_payloadLength = 0;
   uint8_t m_addressType;
+  uint8_t m_advType;
+  bool m_isLegacyAdv;
 
   /***************************************************************************
    *                       NimBLE private properties                         *
    ***************************************************************************/
 
 #if defined(CONFIG_NIMBLE_ENABLED)
   bool m_callbackSent;
-  uint8_t m_advType;
 #endif
 
   /***************************************************************************

libraries/BLE/src/BLEAdvertising.cpp

@@ -1154,7 +1154,7 @@ int BLEAdvertising::handleGAPEvent(struct ble_gap_event *event, void *arg) {
       case BLE_HS_EOS:
       case BLE_HS_ECONTROLLER:
       case BLE_HS_ENOTSYNCED:
-        log_v("host reset, rc=%d", event->adv_complete.reason);
+        log_e("host reset, rc=%d", event->adv_complete.reason);
         BLEDevice::onReset(event->adv_complete.reason);
         return 0;
       default: break;
@@ -1179,6 +1179,12 @@ bool BLEAdvertising::start(uint32_t duration, void (*advCompleteCB)(BLEAdvertisi
     return false;
   }
 
+  // If already advertising just return
+  if (ble_gap_adv_active()) {
+    log_w("Advertising already active");
+    return true;
+  }
+
   BLEServer *pServer = BLEDevice::getServer();
   if (pServer != nullptr) {
     if (!pServer->m_gattsStarted) {
@@ -1189,12 +1195,6 @@ bool BLEAdvertising::start(uint32_t duration, void (*advCompleteCB)(BLEAdvertisi
     }
   }
 
-  // If already advertising just return
-  if (ble_gap_adv_active()) {
-    log_w("Advertising already active");
-    return false;
-  }
-
   // Save the duration in case of host reset so we can restart with the same parameters
   m_duration = duration;
 

libraries/BLE/src/BLEDevice.cpp

@@ -626,6 +626,15 @@ BLEClient *BLEDevice::getClientByGattIf(uint16_t conn_id) {
   return (BLEClient *)m_connectedClientsMap.find(conn_id)->second.peer_device;
 }
 
+BLEClient *BLEDevice::getClientByAddress(BLEAddress address) {
+  for (auto &it : m_connectedClientsMap) {
+    if (((BLEClient *)it.second.peer_device)->getPeerAddress() == address) {
+      return (BLEClient *)it.second.peer_device;
+    }
+  }
+  return nullptr;
+}
+
 void BLEDevice::updatePeerDevice(void *peer, bool _client, uint16_t conn_id) {
   log_d("update conn_id: %d, GATT role: %s", conn_id, _client ? "client" : "server");
   std::map<uint16_t, conn_status_t>::iterator it = m_connectedClientsMap.find(ESP_GATT_IF_NONE);
@@ -928,7 +937,8 @@ bool BLEDevice::onWhiteList(BLEAddress &address) {
 }
 
 void BLEDevice::host_task(void *param) {
-  nimble_port_run();
+  log_i("NimBLE host task started");
+  nimble_port_run();  // This function will return only when nimble_port_stop() is executed
   nimble_port_freertos_deinit();
 }
 
@@ -972,8 +982,9 @@ void BLEDevice::onSync() {
     m_ownAddrType = BLE_OWN_ADDR_RANDOM;
   }
 
+  // Yield for housekeeping tasks before returning to operations.
   // Occasionally triggers exception without.
-  vTaskDelay(1 / portTICK_PERIOD_MS);
+  ble_npl_time_delay(1);
 
   m_synced = true;
 

libraries/BLE/src/BLEDevice.h

@@ -175,6 +175,7 @@ class BLEDevice {
   static void updatePeerDevice(void *peer, bool _client, uint16_t conn_id);
   static void removePeerDevice(uint16_t conn_id, bool client);
   static BLEClient *getClientByID(uint16_t conn_id);
+  static BLEClient *getClientByAddress(BLEAddress address);
   static BLEClient *getClientByGattIf(uint16_t conn_id);
   static void setCustomGapHandler(gap_event_handler handler);
   static void deinit(bool release_memory = false);

libraries/BLE/src/BLEScan.cpp

@@ -58,7 +58,7 @@ BLEScan::BLEScan() {
   m_scan_params.filter_policy = BLE_HCI_SCAN_FILT_NO_WL;
   m_scan_params.passive = 1;            // If set, don’t send scan requests to advertisers (i.e., don’t request additional advertising data).
   m_scan_params.limited = 0;            // If set, only discover devices in limited discoverable mode.
-  m_scan_params.filter_duplicates = 0;  // If set, the controller ignores all but the first advertisement from each device.
+  m_scan_params.filter_duplicates = 1;  // If set, the controller ignores all but the first advertisement from each device.
   m_pAdvertisedDeviceCallbacks = nullptr;
   m_ignoreResults = false;
   m_pTaskData = nullptr;
@@ -363,6 +363,7 @@ void BLEScan::handleGAPEvent(esp_gap_ble_cb_event_t event, esp_ble_gap_cb_param_
           advertisedDevice->setAddress(advertisedAddress);
           advertisedDevice->setRSSI(param->scan_rst.rssi);
           advertisedDevice->setAdFlag(param->scan_rst.flag);
+          advertisedDevice->setAdvType(param->scan_rst.ble_evt_type);
           if (m_shouldParse) {
             advertisedDevice->parseAdvertisement((uint8_t *)param->scan_rst.ble_adv, param->scan_rst.adv_data_len + param->scan_rst.scan_rsp_len);
           } else {
@@ -610,13 +611,16 @@ int BLEScan::handleGAPEvent(ble_gap_event *event, void *arg) {
         return 0;
       }
 
-      BLEAddress advertisedAddress(event->disc.addr);
+      const auto &disc = event->disc;
+      const auto event_type = disc.event_type;
+      const bool isLegacyAdv = true;
+      BLEAddress advertisedAddress(disc.addr);
 
-      /* // Examine our list of ignored addresses and stop processing if we don't want to see it or are already connected
-      if (BLEDevice::isIgnored(advertisedAddress)) {
-        log_i("Ignoring device: address: %s", advertisedAddress.toString().c_str());
+      BLEClient *client = BLEDevice::getClientByAddress(advertisedAddress);
+      if (client != nullptr && client->isConnected()) {
+        log_i("Client %s connected - ignoring event", advertisedAddress.toString().c_str());
         return 0;
-      } */
+      }
 
       BLEAdvertisedDevice *advertisedDevice = nullptr;
 
@@ -630,16 +634,21 @@ int BLEScan::handleGAPEvent(ble_gap_event *event, void *arg) {
 
       // If we haven't seen this device before; create a new instance and insert it in the vector.
       // Otherwise just update the relevant parameters of the already known device.
-      if (advertisedDevice == nullptr && event->disc.event_type != BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) {
+      if (advertisedDevice == nullptr) {
         // Check if we have reach the scan results limit, ignore this one if so.
         // We still need to store each device when maxResults is 0 to be able to append the scan results
         if (pScan->m_maxResults > 0 && pScan->m_maxResults < 0xFF && (pScan->m_scanResults.m_vectorAdvertisedDevices.size() >= pScan->m_maxResults)) {
           return 0;
         }
+
+        if (isLegacyAdv && event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) {
+          log_i("Scan response without advertisement: %s", advertisedAddress.toString().c_str());
+        }
+
         advertisedDevice = new BLEAdvertisedDevice();
         advertisedDevice->setAddress(advertisedAddress);
         advertisedDevice->setAddressType(event->disc.addr.type);
-        advertisedDevice->setAdvType(event->disc.event_type);
+        advertisedDevice->setAdvType(event_type);
         pScan->m_scanResults.m_vectorAdvertisedDevices.insert(std::pair<std::string, BLEAdvertisedDevice *>(advertisedAddress.toString().c_str(), advertisedDevice));
         log_i("New advertiser: %s", advertisedAddress.toString().c_str());
       } else if (advertisedDevice != nullptr) {
@@ -654,28 +663,28 @@ int BLEScan::handleGAPEvent(ble_gap_event *event, void *arg) {
       if (pScan->m_shouldParse) {
         advertisedDevice->parseAdvertisement((uint8_t *)event->disc.data, event->disc.length_data);
       } else {
-        advertisedDevice->setPayload((uint8_t *)event->disc.data, event->disc.length_data, event->disc.event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP);
+        advertisedDevice->setPayload((uint8_t *)event->disc.data, event->disc.length_data, event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP);
       }
 
       advertisedDevice->setScan(pScan);
 
       if (pScan->m_pAdvertisedDeviceCallbacks) {
         // If not active scanning or scan response is not available
         // report the result to the callback now.
-        if (pScan->m_scan_params.passive
-            || (advertisedDevice->getAdvType() != BLE_HCI_ADV_TYPE_ADV_IND && advertisedDevice->getAdvType() != BLE_HCI_ADV_TYPE_ADV_SCAN_IND)) {
+        if (pScan->m_scan_params.passive || !isLegacyAdv || !advertisedDevice->isScannable()) {
           advertisedDevice->m_callbackSent = true;
           pScan->m_pAdvertisedDeviceCallbacks->onResult(*advertisedDevice);
 
           // Otherwise, wait for the scan response so we can report the complete data.
-        } else if (event->disc.event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) {
+        } else if (isLegacyAdv && event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) {
           advertisedDevice->m_callbackSent = true;
           pScan->m_pAdvertisedDeviceCallbacks->onResult(*advertisedDevice);
         }
-        // If not storing results and we have invoked the callback, delete the device.
-        if (pScan->m_maxResults == 0 && advertisedDevice->m_callbackSent) {
-          pScan->erase(advertisedAddress);
-        }
+      }
+
+      // If not storing results and we have invoked the callback, delete the device.
+      if (pScan->m_maxResults == 0 && advertisedDevice->m_callbackSent) {
+        pScan->erase(advertisedAddress);
       }
 
       return 0;
@@ -724,6 +733,10 @@ int BLEScan::handleGAPEvent(ble_gap_event *event, void *arg) {
 bool BLEScan::start(uint32_t duration, void (*scanCompleteCB)(BLEScanResults), bool is_continue) {
   log_d(">> start(duration=%d)", duration);
 
+  if (!is_continue) {
+    clearResults();
+  }
+
   // Save the callback to be invoked when the scan completes.
   m_scanCompleteCB = scanCompleteCB;
   // Save the duration in the case that the host is reset so we can reuse it.
@@ -746,11 +759,6 @@ bool BLEScan::start(uint32_t duration, void (*scanCompleteCB)(BLEScanResults), b
 
   switch (rc) {
     case 0:
-      if (!is_continue) {
-        clearResults();
-      }
-      break;
-
     case BLE_HS_EALREADY:
       break;