Jira 824, BLE Peripheral to create Scan Response Data

Feature added:
  - Arduino request that the BLE library, in Peripheral mode,
    to support Scan Response Data request from a Central.
  - Arduino expects the reply similar to an Apple device.
    They have provide a policy for the construction of the
   Scan Response data message.

Code mods:
1. BLEDeviceManager.cpp:
   - Added setAdvertiseData() for the creation of the Scan
     Response Data message.
   - In advDataInit(), follows the policy (defined by
     Arduino) to add Adv data to the Scan Response
     Data message using setAdvertiseData().
2. BLEDeviceManager.h:
   - prototypeing.

Author: sgbihu

libraries/CurieBLE/src/internal/BLEDeviceManager.cpp

@@ -338,110 +338,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) */
 
@@ -459,29 +481,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()
@@ -498,7 +509,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, _scan_rsp_data, _scan_rsp_data_idx);
+    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);

libraries/CurieBLE/src/internal/BLEDeviceManager.h

@@ -350,6 +350,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,