Skip to content

Commit 8e9183c

Browse files
lucasssvazlucasssvaz
committed
fix(nimble): Advertising fixes
1 parent 574350c commit 8e9183c

File tree

7 files changed

+112
-65
lines changed

7 files changed

+112
-65
lines changed

‎libraries/BLE/examples/Beacon_Scanner/Beacon_Scanner.ino

Lines changed: 25 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -36,25 +36,33 @@ class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks {
3636
if (advertisedDevice.haveManufacturerData() == true) {
3737
String strManufacturerData = advertisedDevice.getManufacturerData();
3838

39-
uint8_t cManufacturerData[100];
40-
memcpy(cManufacturerData, strManufacturerData.c_str(), strManufacturerData.length());
39+
// Buffer to store manufacturer data (BLE max is 255 bytes)
40+
uint8_t cManufacturerData[255];
41+
size_t dataLength = strManufacturerData.length();
4142

42-
if (strManufacturerData.length() == 25 && cManufacturerData[0] == 0x4C && cManufacturerData[1] == 0x00) {
43-
Serial.println("Found an iBeacon!");
44-
BLEBeacon oBeacon = BLEBeacon();
45-
oBeacon.setData(strManufacturerData);
46-
Serial.printf("iBeacon Frame\n");
47-
Serial.printf(
48-
"ID: %04X Major: %d Minor: %d UUID: %s Power: %d\n", oBeacon.getManufacturerId(), ENDIAN_CHANGE_U16(oBeacon.getMajor()),
49-
ENDIAN_CHANGE_U16(oBeacon.getMinor()), oBeacon.getProximityUUID().toString().c_str(), oBeacon.getSignalPower()
50-
);
51-
} else {
52-
Serial.println("Found another manufacturers beacon!");
53-
Serial.printf("strManufacturerData: %d ", strManufacturerData.length());
54-
for (int i = 0; i < strManufacturerData.length(); i++) {
55-
Serial.printf("[%X]", cManufacturerData[i]);
43+
// Bounds checking to prevent buffer overflow
44+
if (dataLength <= sizeof(cManufacturerData)) {
45+
memcpy(cManufacturerData, strManufacturerData.c_str(), dataLength);
46+
47+
if (dataLength == 25 && cManufacturerData[0] == 0x4C && cManufacturerData[1] == 0x00) {
48+
Serial.println("Found an iBeacon!");
49+
BLEBeacon oBeacon = BLEBeacon();
50+
oBeacon.setData(strManufacturerData);
51+
Serial.printf("iBeacon Frame\n");
52+
Serial.printf(
53+
"ID: %04X Major: %d Minor: %d UUID: %s Power: %d\n", oBeacon.getManufacturerId(), ENDIAN_CHANGE_U16(oBeacon.getMajor()),
54+
ENDIAN_CHANGE_U16(oBeacon.getMinor()), oBeacon.getProximityUUID().toString().c_str(), oBeacon.getSignalPower()
55+
);
56+
} else {
57+
Serial.println("Found another manufacturers beacon!");
58+
Serial.printf("strManufacturerData: %d ", dataLength);
59+
for (int i = 0; i < dataLength; i++) {
60+
Serial.printf("[%X]", cManufacturerData[i]);
61+
}
62+
Serial.printf("\n");
5663
}
57-
Serial.printf("\n");
64+
} else {
65+
Serial.printf("Manufacturer data too large (%d bytes), skipping\n", dataLength);
5866
}
5967
}
6068

‎libraries/BLE/src/BLEAdvertisedDevice.cpp

Lines changed: 30 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -46,19 +46,47 @@ BLEAdvertisedDevice::BLEAdvertisedDevice() {
4646
m_serviceDataUUIDs = {};
4747
m_txPower = 0;
4848
m_pScan = nullptr;
49+
m_advType = 0;
50+
4951
#if defined(CONFIG_NIMBLE_ENABLED)
5052
m_callbackSent = false;
51-
m_advType = 0;
5253
#endif
5354

5455
m_haveAppearance = false;
5556
m_haveManufacturerData = false;
5657
m_haveName = false;
5758
m_haveRSSI = false;
5859
m_haveTXPower = false;
59-
60+
m_isLegacyAdv = true;
6061
} // BLEAdvertisedDevice
6162

63+
bool BLEAdvertisedDevice::isLegacyAdvertisement() {
64+
return m_isLegacyAdv;
65+
}
66+
67+
bool BLEAdvertisedDevice::isScannable() {
68+
#if defined(CONFIG_BLUEDROID_ENABLED)
69+
return isLegacyAdvertisement() && (m_advType == ESP_BLE_EVT_CONN_ADV || m_advType == ESP_BLE_EVT_DISC_ADV);
70+
#endif
71+
72+
#if defined(CONFIG_NIMBLE_ENABLED)
73+
return isLegacyAdvertisement() && (m_advType == BLE_HCI_ADV_TYPE_ADV_IND || m_advType == BLE_HCI_ADV_TYPE_ADV_SCAN_IND);
74+
#endif
75+
}
76+
77+
bool BLEAdvertisedDevice::isConnectable() {
78+
#if defined(CONFIG_BLUEDROID_ENABLED)
79+
return m_advType == ESP_BLE_EVT_CONN_ADV || m_advType == ESP_BLE_EVT_CONN_DIR_ADV;
80+
#endif
81+
82+
#if defined(CONFIG_NIMBLE_ENABLED)
83+
if (m_isLegacyAdv) {
84+
return m_advType == BLE_HCI_ADV_RPT_EVTYPE_ADV_IND || m_advType == BLE_HCI_ADV_RPT_EVTYPE_DIR_IND;
85+
}
86+
return (m_advType & BLE_HCI_ADV_CONN_MASK) || (m_advType & BLE_HCI_ADV_DIRECT_MASK);
87+
#endif
88+
}
89+
6290
/**
6391
* @brief Get the address.
6492
*
@@ -623,19 +651,13 @@ size_t BLEAdvertisedDevice::getPayloadLength() {
623651
return m_payloadLength;
624652
}
625653

626-
/***************************************************************************
627-
* NimBLE functions *
628-
***************************************************************************/
629-
630-
#if defined(CONFIG_NIMBLE_ENABLED)
631654
void BLEAdvertisedDevice::setAdvType(uint8_t type) {
632655
m_advType = type;
633656
}
634657

635658
uint8_t BLEAdvertisedDevice::getAdvType() {
636659
return m_advType;
637660
}
638-
#endif /* CONFIG_NIMBLE_ENABLED */
639661

640662
#endif /* CONFIG_BLUEDROID_ENABLED || CONFIG_NIMBLE_ENABLED */
641663
#endif /* SOC_BLE_SUPPORTED */

‎libraries/BLE/src/BLEAdvertisedDevice.h

Lines changed: 7 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -95,7 +95,12 @@ class BLEAdvertisedDevice {
9595
uint8_t getAddressType();
9696
ble_frame_type_t getFrameType();
9797
void setAddressType(uint8_t type);
98+
void setAdvType(uint8_t type);
99+
uint8_t getAdvType();
98100

101+
bool isLegacyAdvertisement();
102+
bool isScannable();
103+
bool isConnectable();
99104
bool isAdvertisingService(BLEUUID uuid);
100105
bool haveAppearance();
101106
bool haveManufacturerData();
@@ -107,15 +112,6 @@ class BLEAdvertisedDevice {
107112

108113
String toString();
109114

110-
/***************************************************************************
111-
* NimBLE public declarations *
112-
***************************************************************************/
113-
114-
#if defined(CONFIG_NIMBLE_ENABLED)
115-
void setAdvType(uint8_t type);
116-
uint8_t getAdvType();
117-
#endif
118-
119115
private:
120116
friend class BLEScan;
121117

@@ -143,14 +139,15 @@ class BLEAdvertisedDevice {
143139
uint8_t *m_payload;
144140
size_t m_payloadLength = 0;
145141
uint8_t m_addressType;
142+
uint8_t m_advType;
143+
bool m_isLegacyAdv;
146144

147145
/***************************************************************************
148146
* NimBLE private properties *
149147
***************************************************************************/
150148

151149
#if defined(CONFIG_NIMBLE_ENABLED)
152150
bool m_callbackSent;
153-
uint8_t m_advType;
154151
#endif
155152

156153
/***************************************************************************

‎libraries/BLE/src/BLEAdvertising.cpp

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1154,7 +1154,7 @@ int BLEAdvertising::handleGAPEvent(struct ble_gap_event *event, void *arg) {
11541154
case BLE_HS_EOS:
11551155
case BLE_HS_ECONTROLLER:
11561156
case BLE_HS_ENOTSYNCED:
1157-
log_v("host reset, rc=%d", event->adv_complete.reason);
1157+
log_e("host reset, rc=%d", event->adv_complete.reason);
11581158
BLEDevice::onReset(event->adv_complete.reason);
11591159
return 0;
11601160
default: break;
@@ -1179,6 +1179,12 @@ bool BLEAdvertising::start(uint32_t duration, void (*advCompleteCB)(BLEAdvertisi
11791179
return false;
11801180
}
11811181

1182+
// If already advertising just return
1183+
if (ble_gap_adv_active()) {
1184+
log_w("Advertising already active");
1185+
return true;
1186+
}
1187+
11821188
BLEServer *pServer = BLEDevice::getServer();
11831189
if (pServer != nullptr) {
11841190
if (!pServer->m_gattsStarted) {
@@ -1189,12 +1195,6 @@ bool BLEAdvertising::start(uint32_t duration, void (*advCompleteCB)(BLEAdvertisi
11891195
}
11901196
}
11911197

1192-
// If already advertising just return
1193-
if (ble_gap_adv_active()) {
1194-
log_w("Advertising already active");
1195-
return false;
1196-
}
1197-
11981198
// Save the duration in case of host reset so we can restart with the same parameters
11991199
m_duration = duration;
12001200

‎libraries/BLE/src/BLEDevice.cpp

Lines changed: 13 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -626,6 +626,15 @@ BLEClient *BLEDevice::getClientByGattIf(uint16_t conn_id) {
626626
return (BLEClient *)m_connectedClientsMap.find(conn_id)->second.peer_device;
627627
}
628628

629+
BLEClient *BLEDevice::getClientByAddress(BLEAddress address) {
630+
for (auto &it : m_connectedClientsMap) {
631+
if (((BLEClient *)it.second.peer_device)->getPeerAddress() == address) {
632+
return (BLEClient *)it.second.peer_device;
633+
}
634+
}
635+
return nullptr;
636+
}
637+
629638
void BLEDevice::updatePeerDevice(void *peer, bool _client, uint16_t conn_id) {
630639
log_d("update conn_id: %d, GATT role: %s", conn_id, _client ? "client" : "server");
631640
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) {
928937
}
929938

930939
void BLEDevice::host_task(void *param) {
931-
nimble_port_run();
940+
log_i("NimBLE host task started");
941+
nimble_port_run(); // This function will return only when nimble_port_stop() is executed
932942
nimble_port_freertos_deinit();
933943
}
934944

@@ -972,8 +982,9 @@ void BLEDevice::onSync() {
972982
m_ownAddrType = BLE_OWN_ADDR_RANDOM;
973983
}
974984

985+
// Yield for housekeeping tasks before returning to operations.
975986
// Occasionally triggers exception without.
976-
vTaskDelay(1 / portTICK_PERIOD_MS);
987+
ble_npl_time_delay(1);
977988

978989
m_synced = true;
979990

‎libraries/BLE/src/BLEDevice.h

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -175,6 +175,7 @@ class BLEDevice {
175175
static void updatePeerDevice(void *peer, bool _client, uint16_t conn_id);
176176
static void removePeerDevice(uint16_t conn_id, bool client);
177177
static BLEClient *getClientByID(uint16_t conn_id);
178+
static BLEClient *getClientByAddress(BLEAddress address);
178179
static BLEClient *getClientByGattIf(uint16_t conn_id);
179180
static void setCustomGapHandler(gap_event_handler handler);
180181
static void deinit(bool release_memory = false);

‎libraries/BLE/src/BLEScan.cpp

Lines changed: 29 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -58,7 +58,7 @@ BLEScan::BLEScan() {
5858
m_scan_params.filter_policy = BLE_HCI_SCAN_FILT_NO_WL;
5959
m_scan_params.passive = 1; // If set, don’t send scan requests to advertisers (i.e., don’t request additional advertising data).
6060
m_scan_params.limited = 0; // If set, only discover devices in limited discoverable mode.
61-
m_scan_params.filter_duplicates = 0; // If set, the controller ignores all but the first advertisement from each device.
61+
m_scan_params.filter_duplicates = 1; // If set, the controller ignores all but the first advertisement from each device.
6262
m_pAdvertisedDeviceCallbacks = nullptr;
6363
m_ignoreResults = false;
6464
m_pTaskData = nullptr;
@@ -363,6 +363,7 @@ void BLEScan::handleGAPEvent(esp_gap_ble_cb_event_t event, esp_ble_gap_cb_param_
363363
advertisedDevice->setAddress(advertisedAddress);
364364
advertisedDevice->setRSSI(param->scan_rst.rssi);
365365
advertisedDevice->setAdFlag(param->scan_rst.flag);
366+
advertisedDevice->setAdvType(param->scan_rst.ble_evt_type);
366367
if (m_shouldParse) {
367368
advertisedDevice->parseAdvertisement((uint8_t *)param->scan_rst.ble_adv, param->scan_rst.adv_data_len + param->scan_rst.scan_rsp_len);
368369
} else {
@@ -610,13 +611,16 @@ int BLEScan::handleGAPEvent(ble_gap_event *event, void *arg) {
610611
return 0;
611612
}
612613

613-
BLEAddress advertisedAddress(event->disc.addr);
614+
const auto &disc = event->disc;
615+
const auto event_type = disc.event_type;
616+
const bool isLegacyAdv = true;
617+
BLEAddress advertisedAddress(disc.addr);
614618

615-
/* // Examine our list of ignored addresses and stop processing if we don't want to see it or are already connected
616-
if (BLEDevice::isIgnored(advertisedAddress)) {
617-
log_i("Ignoring device: address: %s", advertisedAddress.toString().c_str());
619+
BLEClient *client = BLEDevice::getClientByAddress(advertisedAddress);
620+
if (client != nullptr && client->isConnected()) {
621+
log_i("Client %s connected - ignoring event", advertisedAddress.toString().c_str());
618622
return 0;
619-
} */
623+
}
620624

621625
BLEAdvertisedDevice *advertisedDevice = nullptr;
622626

@@ -630,16 +634,21 @@ int BLEScan::handleGAPEvent(ble_gap_event *event, void *arg) {
630634

631635
// If we haven't seen this device before; create a new instance and insert it in the vector.
632636
// Otherwise just update the relevant parameters of the already known device.
633-
if (advertisedDevice == nullptr && event->disc.event_type != BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) {
637+
if (advertisedDevice == nullptr) {
634638
// Check if we have reach the scan results limit, ignore this one if so.
635639
// We still need to store each device when maxResults is 0 to be able to append the scan results
636640
if (pScan->m_maxResults > 0 && pScan->m_maxResults < 0xFF && (pScan->m_scanResults.m_vectorAdvertisedDevices.size() >= pScan->m_maxResults)) {
637641
return 0;
638642
}
643+
644+
if (isLegacyAdv && event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) {
645+
log_i("Scan response without advertisement: %s", advertisedAddress.toString().c_str());
646+
}
647+
639648
advertisedDevice = new BLEAdvertisedDevice();
640649
advertisedDevice->setAddress(advertisedAddress);
641650
advertisedDevice->setAddressType(event->disc.addr.type);
642-
advertisedDevice->setAdvType(event->disc.event_type);
651+
advertisedDevice->setAdvType(event_type);
643652
pScan->m_scanResults.m_vectorAdvertisedDevices.insert(std::pair<std::string, BLEAdvertisedDevice *>(advertisedAddress.toString().c_str(), advertisedDevice));
644653
log_i("New advertiser: %s", advertisedAddress.toString().c_str());
645654
} else if (advertisedDevice != nullptr) {
@@ -654,28 +663,28 @@ int BLEScan::handleGAPEvent(ble_gap_event *event, void *arg) {
654663
if (pScan->m_shouldParse) {
655664
advertisedDevice->parseAdvertisement((uint8_t *)event->disc.data, event->disc.length_data);
656665
} else {
657-
advertisedDevice->setPayload((uint8_t *)event->disc.data, event->disc.length_data, event->disc.event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP);
666+
advertisedDevice->setPayload((uint8_t *)event->disc.data, event->disc.length_data, event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP);
658667
}
659668

660669
advertisedDevice->setScan(pScan);
661670

662671
if (pScan->m_pAdvertisedDeviceCallbacks) {
663672
// If not active scanning or scan response is not available
664673
// report the result to the callback now.
665-
if (pScan->m_scan_params.passive
666-
|| (advertisedDevice->getAdvType() != BLE_HCI_ADV_TYPE_ADV_IND && advertisedDevice->getAdvType() != BLE_HCI_ADV_TYPE_ADV_SCAN_IND)) {
674+
if (pScan->m_scan_params.passive || !isLegacyAdv || !advertisedDevice->isScannable()) {
667675
advertisedDevice->m_callbackSent = true;
668676
pScan->m_pAdvertisedDeviceCallbacks->onResult(*advertisedDevice);
669677

670678
// Otherwise, wait for the scan response so we can report the complete data.
671-
} else if (event->disc.event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) {
679+
} else if (isLegacyAdv && event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) {
672680
advertisedDevice->m_callbackSent = true;
673681
pScan->m_pAdvertisedDeviceCallbacks->onResult(*advertisedDevice);
674682
}
675-
// If not storing results and we have invoked the callback, delete the device.
676-
if (pScan->m_maxResults == 0 && advertisedDevice->m_callbackSent) {
677-
pScan->erase(advertisedAddress);
678-
}
683+
}
684+
685+
// If not storing results and we have invoked the callback, delete the device.
686+
if (pScan->m_maxResults == 0 && advertisedDevice->m_callbackSent) {
687+
pScan->erase(advertisedAddress);
679688
}
680689

681690
return 0;
@@ -724,6 +733,10 @@ int BLEScan::handleGAPEvent(ble_gap_event *event, void *arg) {
724733
bool BLEScan::start(uint32_t duration, void (*scanCompleteCB)(BLEScanResults), bool is_continue) {
725734
log_d(">> start(duration=%d)", duration);
726735

736+
if (!is_continue) {
737+
clearResults();
738+
}
739+
727740
// Save the callback to be invoked when the scan completes.
728741
m_scanCompleteCB = scanCompleteCB;
729742
// 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
746759

747760
switch (rc) {
748761
case 0:
749-
if (!is_continue) {
750-
clearResults();
751-
}
752-
break;
753-
754762
case BLE_HS_EALREADY:
755763
break;
756764

0 commit comments

Comments
 (0)