From f7a19780f550e335bad38791dbdf439b0250dfff Mon Sep 17 00:00:00 2001 From: lianggao Date: Wed, 4 Jan 2017 16:18:37 +0800 Subject: [PATCH] Jira #758, Add scan response support for central role, git 370 Feature added: 1. GIT#370 Add scan response support for central role Arduino request that the BLE library, in Peripheral mode, to support Scan Response Data request from a Central. 2. Arduino expects the reply similar to an Apple device. They have provide a policy for the construction of the Scan Response data message. 3. Jira 836 BLE Central scan does not enumerate peripherals that are non connectable. In Central mode, the BLE library should enumerate Peripheral that are non-connectable (eg beacon). Code mods: BLEDevice.cpp: 1. Add the call to get Manufacturer Data info from a Peripheral. Non-connectable device may only broadcast this info. BLEDeviceManager.cpp: 1. Added setAdvertiseData() for the creation of the Scan Response Data message. 2. In advDataInit(), follows the policy (defined by Arduino) to add Adv data to the Scan Response Data message using setAdvertiseData(). 3. Delete the filter about non-connectable advertisement. 4. Added APIs, hasManufacturerData(), setManufacturerData(), hasManufacturerData(), to gain access to adv info of non-connectable Peripherals. BLEDeviceManager.h: BLEDevice.h: Prototyping. --- libraries/CurieBLE/src/BLEDevice.cpp | 11 + libraries/CurieBLE/src/BLEDevice.h | 5 + .../src/internal/BLEDeviceManager.cpp | 721 ++++++++++++------ .../CurieBLE/src/internal/BLEDeviceManager.h | 53 +- 4 files changed, 566 insertions(+), 224 deletions(-) diff --git a/libraries/CurieBLE/src/BLEDevice.cpp b/libraries/CurieBLE/src/BLEDevice.cpp index d96b70c2..2c5a3189 100644 --- a/libraries/CurieBLE/src/BLEDevice.cpp +++ b/libraries/CurieBLE/src/BLEDevice.cpp @@ -133,6 +133,17 @@ void BLEDevice::setManufacturerData(const unsigned char manufacturerData[], BLEDeviceManager::instance()->setManufacturerData(manufacturerData, manufacturerDataLength); } +bool BLEDevice::getManufacturerData (unsigned char* manu_data, + unsigned char& manu_data_len) const +{ + return BLEDeviceManager::instance()->getManufacturerData(this, manu_data, manu_data_len); +} + +bool BLEDevice::hasManufacturerData() const +{ + return BLEDeviceManager::instance()->hasManufacturerData(this); +} + void BLEDevice::setLocalName(const char *localName) { BLEDeviceManager::instance()->setLocalName(localName); diff --git a/libraries/CurieBLE/src/BLEDevice.h b/libraries/CurieBLE/src/BLEDevice.h index 6c1090d6..76e94f14 100644 --- a/libraries/CurieBLE/src/BLEDevice.h +++ b/libraries/CurieBLE/src/BLEDevice.h @@ -191,6 +191,11 @@ class BLEDevice void setManufacturerData(const unsigned char manufacturerData[], unsigned char manufacturerDataLength); + bool getManufacturerData (unsigned char* manu_data, + unsigned char& manu_data_len) const; + + bool hasManufacturerData() const; + /** * Set the local name that the BLE Peripheral Device advertises * diff --git a/libraries/CurieBLE/src/internal/BLEDeviceManager.cpp b/libraries/CurieBLE/src/internal/BLEDeviceManager.cpp index 643e6b97..56e2e960 100644 --- a/libraries/CurieBLE/src/internal/BLEDeviceManager.cpp +++ b/libraries/CurieBLE/src/internal/BLEDeviceManager.cpp @@ -33,9 +33,15 @@ BLEDeviceManager* BLEDeviceManager::_instance; BLEDeviceManager::BLEDeviceManager(): _min_conn_interval(0), _max_conn_interval(0), + _peer_temp_dev_index(0), _adv_critical_local_name(""), + _wait_for_connect_peripheral_adv_data_len(0), + _wait_for_connect_peripheral_scan_rsp_data_len(0), _wait_for_connect_peripheral_adv_rssi(0), + _available_for_connect_peripheral_adv_data_len(0), + _available_for_connect_peripheral_scan_rsp_data_len(0), _available_for_connect_peripheral_adv_rssi(0), + _available_for_connect_peripheral_connectable(false), _connecting(false), _has_service_uuid(false), _has_service_solicit_uuid(false), @@ -44,6 +50,7 @@ BLEDeviceManager::BLEDeviceManager(): _service_data_length(0), _adv_type(0), _adv_data_idx(0), + _scan_rsp_data_idx(0), _local_name(""), _state(BLE_PERIPH_STATE_NOT_READY), _local_ble(NULL), @@ -53,14 +60,7 @@ BLEDeviceManager::BLEDeviceManager(): _adv_duplicate_filter_enabled(false) { memset(&_local_bda, 0, sizeof(_local_bda)); - memset(&_wait_for_connect_peripheral, 0, sizeof(_wait_for_connect_peripheral)); - memset(&_service_uuid, 0, sizeof(_service_uuid)); - memset(&_service_solicit_uuid, 0, sizeof(_service_solicit_uuid)); - memset(&_service_data_uuid, 0, sizeof(_service_data_uuid)); - memset(_service_data, 0, sizeof(_service_data)); - memset(_service_data_buf, 0, sizeof(_service_data_buf)); - memset(_adv_data, 0, sizeof(_adv_data)); memset(&_peer_central, 0, sizeof (bt_addr_le_t)); @@ -78,20 +78,51 @@ BLEDeviceManager::BLEDeviceManager(): memset(_peer_adv_buffer, 0, sizeof(_peer_adv_buffer)); memset(_peer_adv_mill, 0, sizeof(_peer_adv_mill)); + memset(_peer_adv_data, 0, sizeof(_peer_adv_data)); + memset(_peer_adv_data_len, 0, sizeof(_peer_adv_data_len)); + memset(_peer_scan_rsp_data, 0, sizeof(_peer_scan_rsp_data)); + memset(_peer_scan_rsp_data_len, 0, sizeof(_peer_scan_rsp_data_len)); + memset(_peer_adv_rssi, 0, sizeof(_peer_adv_rssi)); + + memset(_peer_adv_connectable, 0, sizeof(_peer_adv_connectable)); + + memset(_peer_temp_adv_buffer, 0, sizeof(_peer_temp_adv_buffer)); + memset(_peer_temp_adv_data, 0, sizeof(_peer_temp_adv_data)); + memset(_peer_temp_adv_data_len, 0, sizeof(_peer_temp_adv_data_len)); + memset(_peer_temp_adv_connectable, 0, sizeof(_peer_adv_connectable)); + memset(&_adv_accept_critical, 0, sizeof(_adv_accept_critical)); memset(&_adv_critical_service_uuid, 0, sizeof(_adv_critical_service_uuid)); memset(&_adv_accept_device, 0, sizeof(_adv_accept_device)); + memset(&_wait_for_connect_peripheral, 0, sizeof(_wait_for_connect_peripheral)); + memset(&_wait_for_connect_peripheral_adv_data, 0, sizeof(_wait_for_connect_peripheral_adv_data)); + memset(&_wait_for_connect_peripheral_scan_rsp_data, 0, sizeof(_wait_for_connect_peripheral_scan_rsp_data)); + + memset(&_available_for_connect_peripheral_adv_data, 0, sizeof(_available_for_connect_peripheral_adv_data)); + memset(&_available_for_connect_peripheral_scan_rsp_data, 0, sizeof(_available_for_connect_peripheral_scan_rsp_data)); + + memset(&_wait_for_connect_peripheral, 0, sizeof(_wait_for_connect_peripheral)); + + memset(&_service_uuid, 0, sizeof(_service_uuid)); + memset(&_service_solicit_uuid, 0, sizeof(_service_solicit_uuid)); + memset(_manufacturer_data, 0, sizeof(_manufacturer_data)); + + memset(&_service_data_uuid, 0, sizeof(_service_data_uuid)); + memset(_service_data, 0, sizeof(_service_data)); + memset(_service_data_buf, 0, sizeof(_service_data_buf)); + + memset(_adv_data, 0, sizeof(_adv_data)); + memset(_scan_rsp_data, 0, sizeof(_scan_rsp_data)); + memset(_peer_peripheral, 0, sizeof(_peer_peripheral)); memset(_peer_peripheral_adv_data, 0, sizeof(_peer_peripheral_adv_data)); memset(_peer_peripheral_adv_data_len, 0, sizeof(_peer_peripheral_adv_data_len)); + memset(_peer_peripheral_scan_rsp_data, 0, sizeof(_peer_peripheral_scan_rsp_data)); + memset(_peer_peripheral_scan_rsp_data_len, 0, sizeof(_peer_peripheral_scan_rsp_data_len)); memset(_peer_peripheral_adv_rssi, 0, sizeof(_peer_peripheral_adv_rssi)); memset(_device_events, 0, sizeof(_device_events)); - memset(_manufacturer_data, 0, sizeof(_manufacturer_data)); - memset(_peer_adv_data, 0, sizeof(_peer_adv_data)); - memset(_peer_adv_data_len, 0, sizeof(_peer_adv_data_len)); - memset(_peer_adv_rssi, 0, sizeof(_peer_adv_rssi)); } BLEDeviceManager::~BLEDeviceManager() @@ -311,109 +342,132 @@ void BLEDeviceManager::setAppearance(unsigned short appearance) _appearance = appearance; } +BLE_STATUS_T +BLEDeviceManager::setAdvertiseData(uint8_t type, const uint8_t* data, uint8_t length) +{ + uint8_t lengthOfAdv = 0; // Flags data length + uint8_t lengthOfScanRsp = 0; // Flags data length + bt_data_t *fill_area = NULL; + + // Get the length of the Advertisement + for (uint8_t i = 0; i < _adv_data_idx; i++) + { + lengthOfAdv += _adv_data[i].data_len + 2; + } + + for (uint8_t i = 0; i < _scan_rsp_data_idx; i++) + { + lengthOfAdv += _scan_rsp_data[i].data_len + 2; + } + + + if (((length + lengthOfAdv) < BLE_MAX_ADV_SIZE) && + (_adv_data_idx < ARRAY_SIZE(_adv_data))) + { + fill_area = &_adv_data[_adv_data_idx]; + _adv_data_idx++; + } + else if ((length + lengthOfScanRsp) < BLE_MAX_ADV_SIZE && + (_scan_rsp_data_idx < ARRAY_SIZE(_scan_rsp_data))) + { + fill_area = &_scan_rsp_data[_scan_rsp_data_idx]; + _scan_rsp_data_idx++; + } + else + { + // Service data block is too large. + return BLE_STATUS_ERROR_PARAMETER; + } + + if (fill_area) + { + fill_area->type = type; + fill_area->data = data; + fill_area->data_len = length; + + pr_info(LOG_MODULE_BLE, "ADV type %d Len - %d",type, length); + } + return BLE_STATUS_SUCCESS; +} + BLE_STATUS_T BLEDeviceManager::_advDataInit(void) { - uint8_t lengthTotal = 2; // Flags data length + BLE_STATUS_T ret = BLE_STATUS_SUCCESS; + // Clear the indexs _adv_data_idx = 0; + _scan_rsp_data_idx = 0; /* Add flags */ _adv_type = (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR); - _adv_data[_adv_data_idx].type = BT_DATA_FLAGS; - _adv_data[_adv_data_idx].data = &_adv_type; - _adv_data[_adv_data_idx].data_len = 1; - _adv_data_idx++; + ret = setAdvertiseData (BT_DATA_FLAGS, &_adv_type, sizeof(_adv_type)); - if (_has_service_uuid) + if (_has_service_solicit_uuid && + (BLE_STATUS_SUCCESS == ret)) { uint8_t type; uint8_t length; uint8_t *data = NULL; - pr_info(LOG_MODULE_BLE, "ADV Type-%d", _service_uuid.uuid.type); - if (BT_UUID_TYPE_16 == _service_uuid.uuid.type) + pr_info(LOG_MODULE_BLE, "ADV Type-%d", _service_solicit_uuid.uuid.type); + if (BT_UUID_TYPE_16 == _service_solicit_uuid.uuid.type) { - //UINT16_TO_LESTREAM(adv_tmp, uuid.uuid16); - data = (uint8_t *)&(((bt_uuid_16_t *)&_service_uuid)->val); + data = (uint8_t *)&(((bt_uuid_16_t *)&_service_solicit_uuid)->val); length = UUID_SIZE_16; - type = BT_DATA_UUID16_ALL; + type = BT_DATA_SOLICIT16; } - else // Sid. KW, default is BT_UUID_TYPE_128 + else // Sid. KW, default is BT_UUID_TYPE_128 { - data = _service_uuid.val; + data = _service_solicit_uuid.val; length = UUID_SIZE_128; - type = BT_DATA_UUID128_ALL; - } - - // if (data) // Sid. KW, data is always initialized - { - _adv_data[_adv_data_idx].type = type; - _adv_data[_adv_data_idx].data = data; - _adv_data[_adv_data_idx].data_len = length; - _adv_data_idx++; - lengthTotal += length; - - pr_info(LOG_MODULE_BLE, "Service UUID Len -%d", length); + type = BT_DATA_SOLICIT128; } + + ret = setAdvertiseData(type, data, length); } - if (_has_service_solicit_uuid) + if (_has_service_uuid && + (BLE_STATUS_SUCCESS == ret)) { uint8_t type; uint8_t length; uint8_t *data = NULL; - pr_info(LOG_MODULE_BLE, "ADV Type-%d", _service_solicit_uuid.uuid.type); - if (BT_UUID_TYPE_16 == _service_solicit_uuid.uuid.type) + pr_info(LOG_MODULE_BLE, "ADV Type-%d", _service_uuid.uuid.type); + if (BT_UUID_TYPE_16 == _service_uuid.uuid.type) { - //UINT16_TO_LESTREAM(adv_tmp, uuid.uuid16); - data = (uint8_t *)&(((bt_uuid_16_t *)&_service_solicit_uuid)->val); + data = (uint8_t *)&(((bt_uuid_16_t *)&_service_uuid)->val); length = UUID_SIZE_16; - type = BT_DATA_SOLICIT16; + type = BT_DATA_UUID16_ALL; } - else // Sid. KW, default is BT_UUID_TYPE_128 + else // Sid. KW, default is BT_UUID_TYPE_128 { - data = _service_solicit_uuid.val; + data = _service_uuid.val; length = UUID_SIZE_128; - type = BT_DATA_SOLICIT128; - } - // Sid. KW, data is always initialized. if (data) - { - _adv_data[_adv_data_idx].type = type; - _adv_data[_adv_data_idx].data = data; - _adv_data[_adv_data_idx].data_len = length; - _adv_data_idx++; - lengthTotal += length; - - pr_info(LOG_MODULE_BLE, "Service UUID Len -%d", length); + type = BT_DATA_UUID128_ALL; } + ret = setAdvertiseData(type, data, length); } - - if (_local_name.length() > 0) + + if (_manufacturer_data_length > 0 && + (BLE_STATUS_SUCCESS == ret)) { - /* Add device name (truncated if too long) */ - _adv_data[_adv_data_idx].type = BT_DATA_NAME_COMPLETE; - _adv_data[_adv_data_idx].data = (const uint8_t*)_local_name.c_str(); - _adv_data[_adv_data_idx].data_len = _local_name.length(); - _adv_data_idx++; - - lengthTotal += _local_name.length(); - pr_info(LOG_MODULE_BLE, "Local Name -%s", _local_name.c_str()); - pr_info(LOG_MODULE_BLE, "Local Name Len -%d", _local_name.length()); + ret = setAdvertiseData (BT_DATA_MANUFACTURER_DATA, + _manufacturer_data, + _manufacturer_data_length); } - - if (_manufacturer_data_length > 0) + + if (_local_name.length() > 0 && + (BLE_STATUS_SUCCESS == ret)) { - // Add manufacturer data - _adv_data[_adv_data_idx].type = BT_DATA_MANUFACTURER_DATA; - _adv_data[_adv_data_idx].data = _manufacturer_data; - _adv_data[_adv_data_idx].data_len = _manufacturer_data_length; - _adv_data_idx++; - - lengthTotal += _manufacturer_data_length; + uint8_t length = _local_name.length(); + ret = setAdvertiseData (BT_DATA_NAME_COMPLETE, + (const uint8_t*)_local_name.c_str(), + length); } - if (_service_data_length > 0) + if (_service_data_length > 0 && + (BLE_STATUS_SUCCESS == ret)) { /* Add Service Data (if it will fit) */ @@ -431,29 +485,18 @@ BLEDeviceManager::_advDataInit(void) return BLE_STATUS_ERROR_PARAMETER; } - _adv_data[_adv_data_idx].type = BT_DATA_SVC_DATA16; - _adv_data[_adv_data_idx].data = _service_data_buf; - _adv_data[_adv_data_idx].data_len = block_len; - _adv_data_idx++; + ret = setAdvertiseData (BT_DATA_SVC_DATA16, + _service_data_buf, + block_len); uint8_t *adv_tmp = _service_data_buf; - //UINT16_TO_LESTREAM(adv_tmp, (((bt_uuid_16_t *)&_service_data_uuid)->val)); memcpy(adv_tmp, &((bt_uuid_16_t*)&_service_data_uuid)->val, sizeof(uint16_t)); adv_tmp += 2; memcpy(adv_tmp, _service_data, _service_data_length); - - lengthTotal += block_len; - pr_info(LOG_MODULE_BLE, "SVC Len -%d", block_len); } - - if (lengthTotal > BLE_MAX_ADV_SIZE) - { - pr_error(LOG_MODULE_BLE, "ADV Total length-%d", lengthTotal); - // Service data block is too large. - return BLE_STATUS_ERROR_PARAMETER; - } - return BLE_STATUS_SUCCESS; + + return ret; } BLE_STATUS_T BLEDeviceManager::startAdvertising() @@ -470,7 +513,9 @@ BLE_STATUS_T BLEDeviceManager::startAdvertising() if (_state != BLE_PERIPH_STATE_READY) return BLE_STATUS_WRONG_STATE; - ret = bt_le_adv_start(&_adv_param, _adv_data, _adv_data_idx, NULL, 0); + ret = bt_le_adv_start(&_adv_param, + _adv_data, _adv_data_idx, + _scan_rsp_data, _scan_rsp_data_idx); if (0 != ret) { pr_error(LOG_MODULE_APP, "[ADV] Start failed. Error: %d", ret); @@ -617,48 +662,119 @@ void BLEDeviceManager::setAdvertiseCritical(const char* macaddress) BLEUtils::macAddressString2BT(macaddress, _adv_accept_device); } -bool BLEDeviceManager::hasLocalName(const BLEDevice* device) const +bool BLEDeviceManager::getDataFromAdvertiseByType(const BLEDevice* device, + const uint8_t eir_type, + const uint8_t* &data, + uint8_t &data_len) const { - if (BLEUtils::isLocalBLE(*device) == true) - { - return (_local_name.length() != 0); - } - const uint8_t* adv_data = NULL; uint8_t adv_data_len = 0; + bool retval = false; + bool scan_response_proced = false; + getDeviceAdvertiseBuffer(device->bt_le_address(), adv_data, adv_data_len); - if (NULL == adv_data) - { - return false; - } - - while (adv_data_len > 1) - { - uint8_t len = adv_data[0]; - uint8_t type = adv_data[1]; - /* Check for early termination */ - if (len == 0) + while (NULL != adv_data) + { + while (adv_data_len > 1) { - return false; - } + uint8_t len = adv_data[0]; + uint8_t type = adv_data[1]; - if ((len + 1) > adv_data_len) { // Sid. KW, can't be (adv_data_len < 2) - pr_info(LOG_MODULE_BLE, "AD malformed\n"); - return false; - } + /* Check for early termination */ + if ((len == 0) || ((len + 1) > adv_data_len)) { + break; + } + + if (type == eir_type) + { + if (len >= BLE_MAX_ADV_SIZE) + { + len = BLE_MAX_ADV_SIZE-1; + } + data = &adv_data[2]; + data_len = len - 1; + retval = true; + break; + } - if (type == BT_DATA_NAME_COMPLETE) + adv_data_len -= len + 1; + adv_data += len + 1; + } + if (retval == true || scan_response_proced == true) { - return true; + break; } + getDeviceScanResponseBuffer(device->bt_le_address(), + adv_data, + adv_data_len); + scan_response_proced = true; + } + return retval; +} - adv_data_len -= len + 1; - adv_data += len + 1; + +bool BLEDeviceManager::hasLocalName(const BLEDevice* device) const +{ + if (BLEUtils::isLocalBLE(*device) == true) + { + return (_local_name.length() != 0); } - return false; + + const uint8_t* local_name = NULL; + uint8_t local_name_len = 0; + bool retval = getDataFromAdvertiseByType(device, + BT_DATA_NAME_COMPLETE, + local_name, + local_name_len); + if (false == retval) + { + retval = getDataFromAdvertiseByType(device, + BT_DATA_NAME_SHORTENED, + local_name, + local_name_len); + } + return retval; +} + +bool BLEDeviceManager::hasManufacturerData(const BLEDevice* device) const +{ + if (BLEUtils::isLocalBLE(*device) == true) + { + return (_manufacturer_data_length != 0); + } + + const uint8_t* manufactgurer_data = NULL; + uint8_t manufactgurer_data_len = 0; + return getDataFromAdvertiseByType(device, + BT_DATA_MANUFACTURER_DATA, + manufactgurer_data, + manufactgurer_data_len); +} + +bool BLEDeviceManager::getManufacturerData (const BLEDevice* device, + uint8_t* manu_data, + uint8_t&manu_data_len) const +{ + if (BLEUtils::isLocalBLE(*device) == true) + { + return (_manufacturer_data_length != 0); + } + + const uint8_t* manufactgurer_data = NULL; + uint8_t manufactgurer_data_len = 0; + bool retval = getDataFromAdvertiseByType(device, + BT_DATA_MANUFACTURER_DATA, + manufactgurer_data, + manufactgurer_data_len); + if (retval) + { + memcpy (manu_data, manufactgurer_data, manufactgurer_data_len); + manu_data_len = manufactgurer_data_len; + } + return retval; } bool BLEDeviceManager::hasAdvertisedServiceUuid(const BLEDevice* device) const @@ -713,6 +829,41 @@ void BLEDeviceManager::getDeviceAdvertiseBuffer(const bt_addr_le_t* addr, return; } +void BLEDeviceManager::getDeviceScanResponseBuffer(const bt_addr_le_t* addr, + const uint8_t* &adv_data, + uint8_t &adv_len) const +{ + const bt_addr_le_t* temp = NULL; + // Connected device + for (int i = 0; i < BLE_MAX_CONN_CFG; i++) + { + temp = &_peer_peripheral[i]; + if (bt_addr_le_cmp(temp, addr) == 0) + { + adv_data = _peer_peripheral_scan_rsp_data[i]; + adv_len = _peer_peripheral_scan_rsp_data_len[i]; + return; + } + } + + // Connecting device + if (bt_addr_le_cmp(&_wait_for_connect_peripheral, addr) == 0) + { + adv_data = _wait_for_connect_peripheral_scan_rsp_data; + adv_len = _wait_for_connect_peripheral_scan_rsp_data_len; + return; + } + + // Available device + if (bt_addr_le_cmp(&_available_for_connect_peripheral, addr) == 0) + { + adv_data = _available_for_connect_peripheral_scan_rsp_data; + adv_len = _available_for_connect_peripheral_scan_rsp_data_len; + return; + } + return; +} + int BLEDeviceManager::advertisedServiceUuidCount(const BLEDevice* device) const { const uint8_t* adv_data = NULL; @@ -740,7 +891,7 @@ int BLEDeviceManager::advertisedServiceUuidCount(const BLEDevice* device) const uint8_t type = adv_data[1]; /* Check for early termination */ - if (len == 0) + if (len == 0 || ((len + 1) > adv_data_len)) { return service_cnt; } @@ -773,57 +924,34 @@ String BLEDeviceManager::localName(const BLEDevice* device) const { return _local_name; } - const uint8_t* adv_data = NULL; - uint8_t adv_data_len = 0; - char localname_string[BLE_MAX_ADV_SIZE]; - memset(localname_string, 0, sizeof(localname_string)); - - getDeviceAdvertiseBuffer(device->bt_le_address(), - adv_data, - adv_data_len); - if (NULL == adv_data) { - String temp(localname_string); - return temp; + const uint8_t* local_name = NULL; + uint8_t local_name_len = 0; + String temp(""); + char local_name_buff[BLE_MAX_ADV_SIZE]; + bool retval = getDataFromAdvertiseByType(device, + BT_DATA_NAME_COMPLETE, + local_name, + local_name_len); + if (false == retval) + { + retval = getDataFromAdvertiseByType(device, + BT_DATA_NAME_SHORTENED, + local_name, + local_name_len); } - while (adv_data_len > 1) + if (true == retval) { - uint8_t len = adv_data[0]; - uint8_t type = adv_data[1]; - - /* Check for early termination */ - if (len == 0) { - String temp(localname_string); - return temp; - } - - if ((len + 1) > adv_data_len) { // Sid. KW, cannot be (adv_data_len < 2) - pr_info(LOG_MODULE_BLE, "AD malformed\n"); - String temp(localname_string); - return temp; - } - - /* Sid, 2/15/2017. Support both forms of data name. - */ - if (type == BT_DATA_NAME_COMPLETE || - type == BT_DATA_NAME_SHORTENED) + if (local_name_len >= BLE_MAX_ADV_SIZE) { - if (len >= BLE_MAX_ADV_SIZE) - { - len = BLE_MAX_ADV_SIZE-1; - } - uint8_t copy_len = len - 1; - memcpy(localname_string, &adv_data[2], copy_len); - localname_string[copy_len] = '\0'; - break; + local_name_len = BLE_MAX_ADV_SIZE - 1; } - - adv_data_len -= len + 1; - adv_data += len + 1; + memcpy(local_name_buff, local_name, local_name_len); + local_name_buff[local_name_len] = '\0'; + temp = local_name_buff; } - - String temp(localname_string); + return temp; } @@ -867,14 +995,9 @@ String BLEDeviceManager::advertisedServiceUuid(const BLEDevice* device, int inde uint8_t type = adv_data[1]; /* Check for early termination */ - if (len == 0) + if (len == 0 || ((len + 1) > adv_data_len)) { - return String(uuid_string); - } - - if ((len + 1) > adv_data_len) { // Sid. KW, cannot be adv_data_len < 2 - pr_info(LOG_MODULE_BLE, "AD malformed\n"); - return String(uuid_string); + break; } if (type == BT_DATA_UUID16_ALL || @@ -947,7 +1070,9 @@ bool BLEDeviceManager::connect(BLEDevice &device) bt_addr_le_copy(&_wait_for_connect_peripheral, device.bt_le_address()); // Buffer the ADV data memcpy(_wait_for_connect_peripheral_adv_data, _available_for_connect_peripheral_adv_data, BLE_MAX_ADV_SIZE); + memcpy(_wait_for_connect_peripheral_scan_rsp_data, _available_for_connect_peripheral_scan_rsp_data, BLE_MAX_ADV_SIZE); _wait_for_connect_peripheral_adv_data_len = _available_for_connect_peripheral_adv_data_len; + _wait_for_connect_peripheral_scan_rsp_data_len = _available_for_connect_peripheral_scan_rsp_data_len; _wait_for_connect_peripheral_adv_rssi = _available_for_connect_peripheral_adv_rssi; startScanning(); @@ -1016,6 +1141,10 @@ bool BLEDeviceManager::connectToDevice(BLEDevice &device) _wait_for_connect_peripheral_adv_data, BLE_MAX_ADV_SIZE); _peer_peripheral_adv_data_len[i] = _wait_for_connect_peripheral_adv_data_len; + memcpy(_peer_peripheral_scan_rsp_data[i], + _wait_for_connect_peripheral_scan_rsp_data, + BLE_MAX_ADV_SIZE); + _peer_peripheral_scan_rsp_data_len[i] = _wait_for_connect_peripheral_scan_rsp_data_len; _peer_peripheral_adv_rssi[i] = _wait_for_connect_peripheral_adv_rssi; } } @@ -1240,10 +1369,12 @@ BLEDevice BLEDeviceManager::available() tempdevice.setAddress(*temp); bt_addr_le_copy(&_available_for_connect_peripheral, temp); memcpy(_available_for_connect_peripheral_adv_data, _peer_adv_data[index], BLE_MAX_ADV_SIZE); + memcpy(_available_for_connect_peripheral_scan_rsp_data, _peer_scan_rsp_data[index], BLE_MAX_ADV_SIZE); + _available_for_connect_peripheral_scan_rsp_data_len = _peer_scan_rsp_data_len[index]; _available_for_connect_peripheral_adv_data_len = _peer_adv_data_len[index]; _available_for_connect_peripheral_adv_rssi = _peer_adv_rssi[index]; - - pr_debug(LOG_MODULE_BLE, "%s-%d:Con addr-%s", __FUNCTION__, __LINE__, BLEUtils::macAddressBT2String(*temp).c_str()); + _available_for_connect_peripheral_connectable = _peer_adv_connectable[index]; + //pr_debug(LOG_MODULE_BLE, "%s-%d:Con addr-%s", __FUNCTION__, __LINE__, BLEUtils::macAddressBT2String(*temp).c_str()); _peer_adv_mill[index] -= 2000; // Set it as expired if (_adv_duplicate_filter_enabled) { @@ -1257,7 +1388,8 @@ BLEDevice BLEDeviceManager::available() bool BLEDeviceManager::setAdvertiseBuffer(const bt_addr_le_t* bt_addr, const uint8_t *ad, uint8_t data_len, - int8_t rssi) + int8_t rssi, + bool connectable) { bt_addr_le_t* temp = NULL; uint64_t timestamp = millis(); @@ -1303,12 +1435,163 @@ bool BLEDeviceManager::setAdvertiseBuffer(const bt_addr_le_t* bt_addr, _peer_adv_rssi[index] = rssi; // Update the timestamp _peer_adv_mill[index] = timestamp; + _peer_adv_connectable[index] = connectable; + retval = true; + } + + return retval; +} + +bool BLEDeviceManager::setScanRespBuffer(const bt_addr_le_t* bt_addr, + const uint8_t *ad, + uint8_t data_len, + int8_t rssi) +{ + bt_addr_le_t* temp = NULL; + uint64_t timestamp = millis(); + uint8_t index = BLE_MAX_ADV_BUFFER_CFG; + uint8_t i = 0; + bool retval = false; + //pr_debug(LOG_MODULE_BLE, "%s-%d-1", __FUNCTION__, __LINE__); + for (i = 0; i < BLE_MAX_ADV_BUFFER_CFG; i++) + { + temp = &_peer_adv_buffer[i]; + + if (bt_addr_le_cmp(temp, bt_addr) == 0) + { + // The device alread in the buffer + index = i; + break; + } + } + + //pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__); + if (index < BLE_MAX_ADV_BUFFER_CFG) + { + if (data_len > BLE_MAX_ADV_SIZE) + { + data_len = BLE_MAX_ADV_SIZE; + } + memcpy(_peer_scan_rsp_data[index], ad, data_len); + _peer_scan_rsp_data_len[index] = data_len; + //_peer_adv_rssi[index] = rssi; + // Update the timestamp + _peer_adv_mill[index] = timestamp; retval = true; } return retval; } + +uint8_t BLEDeviceManager::getTempAdvertiseIndexFromBuffer(const bt_addr_le_t* bt_addr) +{ + bt_addr_le_t* temp = NULL; + uint8_t i = 0; + + for (i = 0; i < BLE_MAX_ADV_BUFFER_CFG; i++) + { + temp = &_peer_temp_adv_buffer[i]; + + if (bt_addr_le_cmp(temp, bt_addr) == 0) + { + // The device alread in the buffer + break; + } + } + + return i; +} + +void BLEDeviceManager::setTempAdvertiseBuffer(const bt_addr_le_t* bt_addr, + int8_t rssi, + const uint8_t *ad, + uint8_t data_len, + bool connectable) +{ + bt_addr_le_t* temp = NULL; + uint8_t i = getTempAdvertiseIndexFromBuffer(bt_addr); + if (i >= BLE_MAX_ADV_BUFFER_CFG) + { + _peer_temp_dev_index = (_peer_temp_dev_index + 1) % BLE_MAX_ADV_BUFFER_CFG; + i = _peer_temp_dev_index; + } + + temp = &_peer_temp_adv_buffer[i]; + memcpy(temp, bt_addr, sizeof (bt_addr_le_t)); + if (data_len > BLE_MAX_ADV_SIZE) + { + data_len = BLE_MAX_ADV_SIZE; + } + + memcpy(_peer_temp_adv_data[i], ad, data_len); + _peer_temp_adv_data_len[i] = data_len; + _peer_temp_adv_connectable[i] = connectable; + + return; +} + +void BLEDeviceManager::advertiseAcceptHandler(const bt_addr_le_t *addr, + int8_t rssi, + uint8_t type, + const uint8_t *ad, + uint8_t data_len) +{ + if (true == BLEUtils::macAddressValid(_wait_for_connect_peripheral)) + { + // Not add to the buffer when try to establish the connection + if (true == BLEUtils::macAddressSame(*addr, _wait_for_connect_peripheral)) + { + BLEDevice testdev(addr); + stopScanning(); + connectToDevice(testdev); + } + } + else + { + const uint8_t *adv_data = ad; + uint8_t adv_data_len = data_len; + bool connectable = (BT_LE_ADV_NONCONN_IND != type); + bool update_advertise_data = true; + // The critical is accepted + // Find the oldest and expired buffer + if (BT_LE_ADV_SCAN_RSP == type) + { + update_advertise_data = false; + pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__); + if (false == setScanRespBuffer(addr, ad, data_len, rssi)) + { + // Find the device in the ADV temp buffer + uint8_t tempIndex = getTempAdvertiseIndexFromBuffer(addr); + if (tempIndex < BLE_MAX_ADV_BUFFER_CFG) + { + adv_data = _peer_temp_adv_data[tempIndex]; + adv_data_len = _peer_temp_adv_data_len[tempIndex]; + connectable = _peer_temp_adv_connectable[tempIndex]; + update_advertise_data = true; + } + } + } + pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__); + + if (true == update_advertise_data) + { + if (false == setAdvertiseBuffer(addr, + adv_data, + adv_data_len, + rssi, + connectable)) + { + pr_info(LOG_MODULE_BLE, "No buffer to store the ADV\n"); + } + else if (BT_LE_ADV_SCAN_RSP == type) + { + setScanRespBuffer(addr, ad, data_len, rssi); + } + } + } +} + void BLEDeviceManager::handleDeviceFound(const bt_addr_le_t *addr, int8_t rssi, uint8_t type, @@ -1319,60 +1602,52 @@ void BLEDeviceManager::handleDeviceFound(const bt_addr_le_t *addr, uint8_t real_adv_len = data_len; /* We're only interested in connectable events */ - if (type == BT_LE_ADV_IND || type == BT_LE_ADV_DIRECT_IND) + //pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__); + // Filter address + if (BLEUtils::macAddressValid(_adv_accept_device) == true && + (memcmp(addr->val, _adv_accept_device.val, sizeof (addr->val)) != 0)) + { + pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__); + return; + } + + while (data_len > 1) { - //pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__); - // Filter address - if (BLEUtils::macAddressValid(_adv_accept_device) == true && - (memcmp(addr->val, _adv_accept_device.val, sizeof (addr->val)) != 0)) + uint8_t len = data[0]; + + /* Check for early termination */ + if (len == 0) { return; } - - while (data_len > 1) - { - uint8_t len = data[0]; - /* Check for early termination */ - if (len == 0) - { - return; - } - - if ((len + 1) > data_len) { // Sid. KW, cannot be (data_len < 2) - pr_info(LOG_MODULE_BLE, "AD malformed\n"); - return; - } - - if (true == advertiseDataProc(data[1], &data[2], len - 1)) - { - if (true == BLEUtils::macAddressValid(_wait_for_connect_peripheral)) - { - // Not add to the buffer when try to establish the connection - if (true == BLEUtils::macAddressSame(*addr, _wait_for_connect_peripheral)) - { - BLEDevice testdev(addr); - stopScanning(); - connectToDevice(testdev); - } - } - else - { - // The critical is accepted - // Find the oldest and expired buffer - if(false == setAdvertiseBuffer(addr, ad, real_adv_len, rssi)) - { - pr_info(LOG_MODULE_BLE, "No buffer to store the ADV\n"); - } - } - pr_debug(LOG_MODULE_BLE, "%s-%d: Done", __FUNCTION__, __LINE__); - return; - } + if ((len + 1) > data_len) { // Sid. KW, cannot be (data_len < 2) + pr_info(LOG_MODULE_BLE, "AD malformed\n"); + return; + } - data_len -= len + 1; - data += len + 1; + if (true == advertiseDataProc(data[1], &data[2], len - 1)) + { + advertiseAcceptHandler(addr, rssi, type, ad, real_adv_len); + //pr_debug(LOG_MODULE_BLE, "%s-%d: Done", __FUNCTION__, __LINE__); + return; } - //pr_debug(LOG_MODULE_BLE, "%s: done", __FUNCTION__); + + data_len -= len + 1; + data += len + 1; + } + //pr_debug(LOG_MODULE_BLE, "%s: done", __FUNCTION__); + // Doesn't accept the ADV/scan data + // Check it in the buffer + if (BT_LE_ADV_SCAN_RSP == type) + { + // Find the ADV and set response + setScanRespBuffer(addr, ad, real_adv_len, rssi); + } + else + { + // Add advertise into buffer + setTempAdvertiseBuffer(addr, rssi, ad, real_adv_len, BT_LE_ADV_NONCONN_IND != type); } } diff --git a/libraries/CurieBLE/src/internal/BLEDeviceManager.h b/libraries/CurieBLE/src/internal/BLEDeviceManager.h index e09bfe6c..118c4c11 100644 --- a/libraries/CurieBLE/src/internal/BLEDeviceManager.h +++ b/libraries/CurieBLE/src/internal/BLEDeviceManager.h @@ -140,6 +140,10 @@ class BLEDeviceManager */ void setManufacturerData(const unsigned char manufacturerData[], unsigned char manufacturerDataLength); + bool getManufacturerData (const BLEDevice* device, + uint8_t* manu_data, + uint8_t&manu_data_len) const; + bool hasManufacturerData(const BLEDevice* device) const; /** * Set the local name that the BLE Peripheral Device advertises @@ -350,6 +354,9 @@ class BLEDeviceManager protected: private: + BLE_STATUS_T setAdvertiseData (uint8_t type, + const uint8_t* data, + uint8_t length); BLE_STATUS_T _advDataInit(void); bool advertiseDataProc(uint8_t type, const uint8_t *dataPtr, @@ -357,13 +364,36 @@ class BLEDeviceManager bool setAdvertiseBuffer(const bt_addr_le_t* bt_addr, const uint8_t *ad, uint8_t data_len, - int8_t rssi); + int8_t rssi, + bool connectable); void getDeviceAdvertiseBuffer(const bt_addr_le_t* addr, const uint8_t* &adv_data, uint8_t &adv_len) const; + bool setScanRespBuffer(const bt_addr_le_t* bt_addr, + const uint8_t *ad, + uint8_t data_len, + int8_t rssi); + void getDeviceScanResponseBuffer(const bt_addr_le_t* addr, + const uint8_t* &adv_data, + uint8_t &adv_len) const; + bool getDataFromAdvertiseByType(const BLEDevice* device, + const uint8_t eir_type, + const uint8_t* &data, + uint8_t &data_len) const; bool disconnectSingle(const bt_addr_le_t *peer); void updateDuplicateFilter(const bt_addr_le_t* addr); bool deviceInDuplicateFilterBuffer(const bt_addr_le_t* addr); + void advertiseAcceptHandler(const bt_addr_le_t *addr, + int8_t rssi, + uint8_t type, + const uint8_t *ad, + uint8_t data_len); + void setTempAdvertiseBuffer(const bt_addr_le_t* bt_addr, + int8_t rssi, + const uint8_t *ad, + uint8_t data_len, + bool connectable); + uint8_t getTempAdvertiseIndexFromBuffer(const bt_addr_le_t* bt_addr); private: uint16_t _min_conn_interval; @@ -377,7 +407,19 @@ class BLEDeviceManager uint64_t _peer_adv_mill[BLE_MAX_ADV_BUFFER_CFG]; // The ADV found time stamp uint8_t _peer_adv_data[BLE_MAX_ADV_BUFFER_CFG][BLE_MAX_ADV_SIZE]; uint8_t _peer_adv_data_len[BLE_MAX_ADV_BUFFER_CFG]; + uint8_t _peer_scan_rsp_data[BLE_MAX_ADV_BUFFER_CFG][BLE_MAX_ADV_SIZE]; + uint8_t _peer_scan_rsp_data_len[BLE_MAX_ADV_BUFFER_CFG]; int8_t _peer_adv_rssi[BLE_MAX_ADV_BUFFER_CFG]; + bool _peer_adv_connectable[BLE_MAX_ADV_BUFFER_CFG]; + + // The accept critical may include in scan response + bt_addr_le_t _peer_temp_adv_buffer[BLE_MAX_ADV_BUFFER_CFG]; + uint8_t _peer_temp_dev_index; + uint8_t _peer_temp_adv_data[BLE_MAX_ADV_BUFFER_CFG][BLE_MAX_ADV_SIZE]; + uint8_t _peer_temp_adv_data_len[BLE_MAX_ADV_BUFFER_CFG]; + bool _peer_temp_adv_connectable[BLE_MAX_ADV_BUFFER_CFG]; + + // The critical for central scan bt_data_t _adv_accept_critical; // The filters for central device String _adv_critical_local_name; bt_uuid_128_t _adv_critical_service_uuid; @@ -386,12 +428,17 @@ class BLEDeviceManager bt_addr_le_t _wait_for_connect_peripheral; uint8_t _wait_for_connect_peripheral_adv_data[BLE_MAX_ADV_SIZE]; uint8_t _wait_for_connect_peripheral_adv_data_len; + uint8_t _wait_for_connect_peripheral_scan_rsp_data[BLE_MAX_ADV_SIZE]; + uint8_t _wait_for_connect_peripheral_scan_rsp_data_len; int8_t _wait_for_connect_peripheral_adv_rssi; bt_addr_le_t _available_for_connect_peripheral; uint8_t _available_for_connect_peripheral_adv_data[BLE_MAX_ADV_SIZE]; uint8_t _available_for_connect_peripheral_adv_data_len; + uint8_t _available_for_connect_peripheral_scan_rsp_data[BLE_MAX_ADV_SIZE]; + uint8_t _available_for_connect_peripheral_scan_rsp_data_len; int8_t _available_for_connect_peripheral_adv_rssi; + bool _available_for_connect_peripheral_connectable; volatile bool _connecting; // For peripheral @@ -412,6 +459,8 @@ class BLEDeviceManager uint8_t _adv_type; bt_data_t _adv_data[6]; // KW: fount _advDataInit() can use 6 slots. size_t _adv_data_idx; + bt_data_t _scan_rsp_data[6]; + size_t _scan_rsp_data_idx; String _local_name; // Peripheral states @@ -433,6 +482,8 @@ class BLEDeviceManager uint8_t _peer_peripheral_index; uint8_t _peer_peripheral_adv_data[BLE_MAX_CONN_CFG][BLE_MAX_ADV_SIZE]; uint8_t _peer_peripheral_adv_data_len[BLE_MAX_CONN_CFG]; + uint8_t _peer_peripheral_scan_rsp_data[BLE_MAX_CONN_CFG][BLE_MAX_ADV_SIZE]; + uint8_t _peer_peripheral_scan_rsp_data_len[BLE_MAX_CONN_CFG]; uint8_t _peer_peripheral_adv_rssi[BLE_MAX_CONN_CFG]; bt_addr_le_t _peer_duplicate_address_buffer[BLE_MAX_ADV_FILTER_SIZE_CFG]; uint8_t _duplicate_filter_header;