Skip to content

Commit ae045aa

Browse files
chegewarachegewara
committed
Fix issues in last commit
1 parent 5736444 commit ae045aa

File tree

9 files changed

+164
-50
lines changed

9 files changed

+164
-50
lines changed

‎cpp_utils/BLEAdvertising.cpp

Lines changed: 147 additions & 39 deletions
Original file line numberDiff line numberDiff line change
@@ -23,22 +23,29 @@
2323
#include <esp_err.h>
2424
#include "BLEUtils.h"
2525
#include "GeneralUtils.h"
26+
#include "string.h"
27+
#include "mbedtls/md.h"
28+
#include "mbedtls/entropy.h"
29+
#include "mbedtls/ctr_drbg.h"
30+
31+
32+
#define BLOCK_SIZE 64
33+
#define HMAC MBEDTLS_MD_SHA256
2634

2735
#ifdef ARDUINO_ARCH_ESP32
2836
#include "esp32-hal-log.h"
2937
#endif
30-
38+
static bool is_advertising = false;
3139
static const char* LOG_TAG = "BLEAdvertising";
3240

33-
3441
/**
3542
* @brief Construct a default advertising object.
3643
*
3744
*/
3845
BLEAdvertising::BLEAdvertising() {
3946
m_advData.set_scan_rsp = false;
4047
m_advData.include_name = true;
41-
m_advData.include_txpower = true;
48+
m_advData.include_txpower = false;
4249
m_advData.min_interval = 0x20;
4350
m_advData.max_interval = 0x40;
4451
m_advData.appearance = 0x00;
@@ -61,6 +68,65 @@ BLEAdvertising::BLEAdvertising() {
6168
m_customScanResponseData = false; // No custom scan response data
6269
} // BLEAdvertising
6370

71+
void BLEAdvertising::setPrivateAddress(esp_ble_addr_type_t type) {
72+
esp_bd_addr_t addr;
73+
m_advParams.own_addr_type = type;
74+
75+
if(type == BLE_ADDR_TYPE_RPA_PUBLIC) {
76+
esp_ble_gap_config_local_privacy(true);
77+
return;
78+
}
79+
else{
80+
mbedtls_ctr_drbg_context ctr_drbg;
81+
mbedtls_entropy_context entropy;
82+
mbedtls_md_context_t ctx;
83+
84+
char pers[] = "aes generate key";
85+
int ret;
86+
unsigned char key[BLOCK_SIZE] = {0};
87+
const char inp[] = "random static address";
88+
unsigned char outp[BLOCK_SIZE/2];
89+
90+
mbedtls_entropy_init( &entropy );
91+
mbedtls_ctr_drbg_init( &ctr_drbg );
92+
93+
if( ( ret = mbedtls_ctr_drbg_seed( &ctr_drbg, mbedtls_entropy_func, &entropy,
94+
(unsigned char *) pers, strlen( pers ) ) ) != 0 )
95+
{
96+
printf( " failed\n ! mbedtls_ctr_drbg_init returned -0x%04x\n", -ret );
97+
goto exit;
98+
}
99+
100+
if( ( ret = mbedtls_ctr_drbg_random( &ctr_drbg, key, BLOCK_SIZE ) ) != 0 )
101+
{
102+
printf( " failed\n ! mbedtls_ctr_drbg_random returned -0x%04x\n", -ret );
103+
goto exit;
104+
}
105+
106+
mbedtls_md_init(&ctx);
107+
mbedtls_md_setup(&ctx, mbedtls_md_info_from_type(HMAC), true);
108+
mbedtls_md_hmac_starts(&ctx, (const unsigned char *)key, BLOCK_SIZE);
109+
mbedtls_md_hmac_update(&ctx, (const unsigned char *)inp, strlen(inp));
110+
mbedtls_md_hmac_finish(&ctx, outp);
111+
mbedtls_md_free(&ctx);
112+
ESP_LOG_BUFFER_HEX("random key", key, BLOCK_SIZE);
113+
ESP_LOG_BUFFER_HEX("HASH step 2", outp, BLOCK_SIZE/2);
114+
115+
memcpy(addr, outp, 6);
116+
}
117+
if(type == BLE_ADDR_TYPE_RANDOM) {
118+
addr[0] &= 0x3F; // <--- Format of non-resolvable private address 00xx xxxx
119+
}
120+
else{
121+
addr[0] |= 0xC0; // <--- Format of static address 11xx xxxx
122+
}
123+
124+
esp_ble_gap_set_rand_addr(addr);
125+
ESP_LOG_BUFFER_HEX("random address", addr, 6);
126+
exit:
127+
return;
128+
}
129+
64130

65131
/**
66132
* @brief Add a service uuid to exposed list of services.
@@ -107,7 +173,7 @@ void BLEAdvertising::setMaxInterval(uint16_t maxinterval) {
107173
* @param [in] scanRequestWhitelistOnly If true, only allow scan requests from those on the white list.
108174
* @param [in] connectWhitelistOnly If true, only allow connections from those on the white list.
109175
*/
110-
void BLEAdvertising::setScanFilter(bool scanRequestWhitelistOnly, bool connectWhitelistOnly) {
176+
void BLEAdvertising::setScanFilter(bool scanRequestWhitelistOnly, bool connectWhitelistOnly) {
111177
ESP_LOGD(LOG_TAG, ">> setScanFilter: scanRequestWhitelistOnly: %d, connectWhitelistOnly: %d", scanRequestWhitelistOnly, connectWhitelistOnly);
112178
if (!scanRequestWhitelistOnly && !connectWhitelistOnly) {
113179
m_advParams.adv_filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_ANY;
@@ -173,20 +239,21 @@ void BLEAdvertising::setScanResponseData(BLEAdvertisementData& advertisementData
173239
void BLEAdvertising::start() {
174240
ESP_LOGD(LOG_TAG, ">> start: customAdvData: %d, customScanResponseData: %d", m_customAdvData, m_customScanResponseData);
175241

242+
176243
// We have a vector of service UUIDs that we wish to advertise. In order to use the
177244
// ESP-IDF framework, these must be supplied in a contiguous array of their 128bit (16 byte)
178245
// representations. If we have 1 or more services to advertise then we allocate enough
179246
// storage to host them and then copy them in one at a time into the contiguous storage.
180247
int numServices = m_serviceUUIDs.size();
181248
if (numServices > 0) {
182-
m_advData.service_uuid_len = 16 * numServices;
183-
m_advData.p_service_uuid = new uint8_t[m_advData.service_uuid_len];
249+
m_advData.service_uuid_len = 16*numServices;
250+
m_advData.p_service_uuid = new uint8_t[m_advData.service_uuid_len];
184251
uint8_t* p = m_advData.p_service_uuid;
185-
for (int i = 0; i < numServices; i++) {
252+
for (int i=0; i<numServices; i++) {
186253
ESP_LOGD(LOG_TAG, "- advertising service: %s", m_serviceUUIDs[i].toString().c_str());
187254
BLEUUID serviceUUID128 = m_serviceUUIDs[i].to128();
188255
memcpy(p, serviceUUID128.getNative()->uuid.uuid128, 16);
189-
p += 16;
256+
p+=16;
190257
}
191258
} else {
192259
m_advData.service_uuid_len = 0;
@@ -195,36 +262,45 @@ void BLEAdvertising::start() {
195262

196263
esp_err_t errRc;
197264

198-
if (!m_customAdvData) {
265+
if(!is_advertising){
266+
267+
// m_semaphoreSetAdv.take("config_adv");
268+
if (m_customAdvData == false) {
199269
// Set the configuration for advertising.
200-
m_advData.set_scan_rsp = false;
201-
errRc = ::esp_ble_gap_config_adv_data(&m_advData);
202-
if (errRc != ESP_OK) {
203-
ESP_LOGE(LOG_TAG, "<< esp_ble_gap_config_adv_data: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
204-
return;
270+
m_advData.set_scan_rsp = false;
271+
errRc = ::esp_ble_gap_config_adv_data(&m_advData);
272+
if (errRc != ESP_OK) {
273+
ESP_LOGE(LOG_TAG, "<< esp_ble_gap_config_adv_data: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
274+
// m_semaphoreSetAdv.give();
275+
return;
276+
}
205277
}
206-
}
207278

208-
if (!m_customScanResponseData) {
209-
m_advData.set_scan_rsp = true;
210-
errRc = ::esp_ble_gap_config_adv_data(&m_advData);
211-
if (errRc != ESP_OK) {
212-
ESP_LOGE(LOG_TAG, "<< esp_ble_gap_config_adv_data (Scan response): rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
213-
return;
279+
// m_semaphoreSetAdv.take("config_rsp");
280+
if (m_customScanResponseData == false) {
281+
m_advData.set_scan_rsp = true;
282+
errRc = ::esp_ble_gap_config_adv_data(&m_advData);
283+
if (errRc != ESP_OK) {
284+
ESP_LOGE(LOG_TAG, "<< esp_ble_gap_config_adv_data (Scan response): rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
285+
// m_semaphoreSetAdv.give();
286+
return;
287+
}
214288
}
215289
}
216-
217290
// If we had services to advertise then we previously allocated some storage for them.
218291
// Here we release that storage.
219292
if (m_advData.service_uuid_len > 0) {
220293
delete[] m_advData.p_service_uuid;
221294
m_advData.p_service_uuid = nullptr;
222295
}
296+
is_advertising = true;
223297

298+
// m_semaphoreSetAdv.take("start");
224299
// Start advertising.
225300
errRc = ::esp_ble_gap_start_advertising(&m_advParams);
226301
if (errRc != ESP_OK) {
227302
ESP_LOGE(LOG_TAG, "<< esp_ble_gap_start_advertising: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
303+
// m_semaphoreSetAdv.give();
228304
return;
229305
}
230306
ESP_LOGD(LOG_TAG, "<< start");
@@ -238,6 +314,7 @@ void BLEAdvertising::start() {
238314
*/
239315
void BLEAdvertising::stop() {
240316
ESP_LOGD(LOG_TAG, ">> stop");
317+
// m_semaphoreSetAdv.take("stop");
241318
esp_err_t errRc = ::esp_ble_gap_stop_advertising();
242319
if (errRc != ESP_OK) {
243320
ESP_LOGE(LOG_TAG, "esp_ble_gap_stop_advertising: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
@@ -269,7 +346,8 @@ void BLEAdvertisementData::setAppearance(uint16_t appearance) {
269346
char cdata[2];
270347
cdata[0] = 3;
271348
cdata[1] = ESP_BLE_AD_TYPE_APPEARANCE; // 0x19
272-
addData(std::string(cdata, 2) + std::string((char*) &appearance, 2));
349+
addData(std::string(cdata, 2) + std::string((char *)&appearance,2));
350+
esp_ble_gap_config_local_icon(appearance);
273351
} // setAppearance
274352

275353

@@ -279,28 +357,28 @@ void BLEAdvertisementData::setAppearance(uint16_t appearance) {
279357
*/
280358
void BLEAdvertisementData::setCompleteServices(BLEUUID uuid) {
281359
char cdata[2];
282-
switch (uuid.bitSize()) {
360+
switch(uuid.bitSize()) {
283361
case 16: {
284362
// [Len] [0x02] [LL] [HH]
285363
cdata[0] = 3;
286364
cdata[1] = ESP_BLE_AD_TYPE_16SRV_CMPL; // 0x03
287-
addData(std::string(cdata, 2) + std::string((char*) &uuid.getNative()->uuid.uuid16, 2));
365+
addData(std::string(cdata, 2) + std::string((char *)&uuid.getNative()->uuid.uuid16,2));
288366
break;
289367
}
290368

291369
case 32: {
292370
// [Len] [0x04] [LL] [LL] [HH] [HH]
293371
cdata[0] = 5;
294372
cdata[1] = ESP_BLE_AD_TYPE_32SRV_CMPL; // 0x05
295-
addData(std::string(cdata, 2) + std::string((char*) &uuid.getNative()->uuid.uuid32, 4));
373+
addData(std::string(cdata, 2) + std::string((char *)&uuid.getNative()->uuid.uuid32,4));
296374
break;
297375
}
298376

299377
case 128: {
300378
// [Len] [0x04] [0] [1] ... [15]
301379
cdata[0] = 17;
302380
cdata[1] = ESP_BLE_AD_TYPE_128SRV_CMPL; // 0x07
303-
addData(std::string(cdata, 2) + std::string((char*) uuid.getNative()->uuid.uuid128, 16));
381+
addData(std::string(cdata, 2) + std::string((char *)uuid.getNative()->uuid.uuid128,16));
304382
break;
305383
}
306384

@@ -340,7 +418,7 @@ void BLEAdvertisementData::setManufacturerData(std::string data) {
340418
char cdata[2];
341419
cdata[0] = data.length() + 1;
342420
cdata[1] = ESP_BLE_AD_MANUFACTURER_SPECIFIC_TYPE; // 0xff
343-
addData(std::string(cdata, 2) + data);
421+
addData(std::string(cdata, 2) + data);
344422
ESP_LOGD("BLEAdvertisementData", "<< setManufacturerData");
345423
} // setManufacturerData
346424

@@ -354,7 +432,7 @@ void BLEAdvertisementData::setName(std::string name) {
354432
char cdata[2];
355433
cdata[0] = name.length() + 1;
356434
cdata[1] = ESP_BLE_AD_TYPE_NAME_CMPL; // 0x09
357-
addData(std::string(cdata, 2) + name);
435+
addData(std::string(cdata, 2) + name);
358436
ESP_LOGD("BLEAdvertisementData", "<< setName");
359437
} // setName
360438

@@ -365,28 +443,28 @@ void BLEAdvertisementData::setName(std::string name) {
365443
*/
366444
void BLEAdvertisementData::setPartialServices(BLEUUID uuid) {
367445
char cdata[2];
368-
switch (uuid.bitSize()) {
446+
switch(uuid.bitSize()) {
369447
case 16: {
370448
// [Len] [0x02] [LL] [HH]
371449
cdata[0] = 3;
372450
cdata[1] = ESP_BLE_AD_TYPE_16SRV_PART; // 0x02
373-
addData(std::string(cdata, 2) + std::string((char *) &uuid.getNative()->uuid.uuid16, 2));
451+
addData(std::string(cdata, 2) + std::string((char *)&uuid.getNative()->uuid.uuid16,2));
374452
break;
375453
}
376454

377455
case 32: {
378456
// [Len] [0x04] [LL] [LL] [HH] [HH]
379457
cdata[0] = 5;
380458
cdata[1] = ESP_BLE_AD_TYPE_32SRV_PART; // 0x04
381-
addData(std::string(cdata, 2) + std::string((char *) &uuid.getNative()->uuid.uuid32, 4));
459+
addData(std::string(cdata, 2) + std::string((char *)&uuid.getNative()->uuid.uuid32,4));
382460
break;
383461
}
384462

385463
case 128: {
386464
// [Len] [0x04] [0] [1] ... [15]
387465
cdata[0] = 17;
388466
cdata[1] = ESP_BLE_AD_TYPE_128SRV_PART; // 0x06
389-
addData(std::string(cdata, 2) + std::string((char *) &uuid.getNative()->uuid.uuid128, 16));
467+
addData(std::string(cdata, 2) + std::string((char *)uuid.getNative()->uuid.uuid128,16));
390468
break;
391469
}
392470

@@ -403,28 +481,28 @@ void BLEAdvertisementData::setPartialServices(BLEUUID uuid) {
403481
*/
404482
void BLEAdvertisementData::setServiceData(BLEUUID uuid, std::string data) {
405483
char cdata[2];
406-
switch (uuid.bitSize()) {
484+
switch(uuid.bitSize()) {
407485
case 16: {
408486
// [Len] [0x16] [UUID16] data
409487
cdata[0] = data.length() + 3;
410488
cdata[1] = ESP_BLE_AD_TYPE_SERVICE_DATA; // 0x16
411-
addData(std::string(cdata, 2) + std::string((char*) &uuid.getNative()->uuid.uuid16, 2) + data);
489+
addData(std::string(cdata, 2) + std::string((char *)&uuid.getNative()->uuid.uuid16,2) + data);
412490
break;
413491
}
414492

415493
case 32: {
416494
// [Len] [0x20] [UUID32] data
417495
cdata[0] = data.length() + 5;
418496
cdata[1] = ESP_BLE_AD_TYPE_32SERVICE_DATA; // 0x20
419-
addData(std::string(cdata, 2) + std::string((char*) &uuid.getNative()->uuid.uuid32, 4) + data);
497+
addData(std::string(cdata, 2) + std::string((char *)&uuid.getNative()->uuid.uuid32,4) + data);
420498
break;
421499
}
422500

423501
case 128: {
424502
// [Len] [0x21] [UUID128] data
425503
cdata[0] = data.length() + 17;
426504
cdata[1] = ESP_BLE_AD_TYPE_128SERVICE_DATA; // 0x21
427-
addData(std::string(cdata, 2) + std::string((char*) &uuid.getNative()->uuid.uuid128, 16) + data);
505+
addData(std::string(cdata, 2) + std::string((char *)uuid.getNative()->uuid.uuid128,16) + data);
428506
break;
429507
}
430508

@@ -443,11 +521,12 @@ void BLEAdvertisementData::setShortName(std::string name) {
443521
char cdata[2];
444522
cdata[0] = name.length() + 1;
445523
cdata[1] = ESP_BLE_AD_TYPE_NAME_SHORT; // 0x08
446-
addData(std::string(cdata, 2) + name);
524+
addData(std::string(cdata, 2) + name);
447525
ESP_LOGD("BLEAdvertisementData", "<< setShortName");
448526
} // setShortName
449527

450528

529+
451530
/**
452531
* @brief Retrieve the payload that is to be advertised.
453532
* @return The payload that is to be advertised.
@@ -456,5 +535,34 @@ std::string BLEAdvertisementData::getPayload() {
456535
return m_payload;
457536
} // getPayload
458537

538+
void BLEAdvertising::gapEventHandler(
539+
esp_gap_ble_cb_event_t event,
540+
esp_ble_gap_cb_param_t* param) {
541+
542+
ESP_LOGD(LOG_TAG, "gapEventHandler [event no: %d]", (int)event);
543+
544+
switch(event) {
545+
case ESP_GAP_BLE_ADV_DATA_SET_COMPLETE_EVT: {
546+
// m_semaphoreSetAdv.give();
547+
break;
548+
}
549+
case ESP_GAP_BLE_SCAN_RSP_DATA_SET_COMPLETE_EVT: {
550+
// m_semaphoreSetAdv.give();
551+
break;
552+
}
553+
case ESP_GAP_BLE_ADV_START_COMPLETE_EVT: {
554+
// m_semaphoreSetAdv.give();
555+
break;
556+
}
557+
case ESP_GAP_BLE_ADV_STOP_COMPLETE_EVT: {
558+
ESP_LOGI(LOG_TAG, "STOP advertising");
559+
start();
560+
break;
561+
}
562+
default:
563+
break;
564+
}
565+
}
566+
459567

460-
#endif /* CONFIG_BT_ENABLED */
568+
#endif /* CONFIG_BT_ENABLED */

0 commit comments

Comments
 (0)