UUID handling

features/FEATURE_BLE/ble/blecommon.h

@@ -210,6 +210,11 @@ enum ble_error_t {
      * The platform-specific stack failed.
      */
     BLE_ERROR_INTERNAL_STACK_FAILURE = 12,
+
+    /**
+     * Data not found or there is nothing to return.
+     */
+    BLE_ERROR_NOT_FOUND = 13
 };
 
 /**

features/FEATURE_BLE/ble/gap/AdvertisingDataBuilder.h

@@ -601,7 +601,7 @@ public:
         if (field) {
             return removeField(field);
         } else {
-            return BLE_ERROR_INVALID_PARAM;
+            return BLE_ERROR_NOT_FOUND;
         }
     }
 
@@ -708,6 +708,136 @@ public:
         return addData(adv_data_type_t::MANUFACTURER_SPECIFIC_DATA, data);
     }
 
+    /**
+     * Add manufacturer specific data to the advertising payload.
+     *
+     * @param[in] data New data to be added.
+     * @param[in] complete True if this is a complete list.
+     *
+     * @retval BLE_ERROR_NONE on success.
+     * @retval BLE_ERROR_BUFFER_OVERFLOW if buffer is too small to contain the new data.
+     */
+    ble_error_t setLocalServiceList(
+        mbed::Span<const UUID> data,
+        bool complete = true
+    ) {
+        ble_error_t status = BLE_ERROR_NONE;
+
+        /* first count all the bytes we need to store all the UUIDs */
+        size_t size_incomplete_16 = 0;
+        size_t size_complete_16 = 0;
+        size_t size_incomplete_128 = 0;
+        size_t size_complete_128 = 0;
+        for (size_t i = 0; i < data.size(); ++i) {
+            if (data[i].shortOrLong() == UUID::UUID_TYPE_SHORT) {
+                if (complete) {
+                    size_incomplete_16++;
+                } else {
+                    size_complete_16++;
+                }
+            } else {
+                if (complete) {
+                    size_incomplete_128++;
+                } else {
+                    size_complete_128++;
+                }
+            }
+        }
+
+        /* calculate total size including headers for types */
+        size_t total_size = size_incomplete_16 + (!!size_incomplete_16) * 2 +
+                            size_complete_16 + (!!size_complete_16) * 2 +
+                            size_incomplete_128 + (!!size_incomplete_128) * 2 +
+                            size_complete_128 + (!!size_complete_128) * 2;
+
+        /* count all the bytes of existing data */
+        size_t old_size = 0;
+        mbed::Span<const uint8_t> existing_data;
+        if (getData(existing_data, adv_data_type_t::INCOMPLETE_LIST_16BIT_SERVICE_IDS) == BLE_ERROR_NONE) {
+            old_size += existing_data.size() + 2;
+        }
+        if (getData(existing_data, adv_data_type_t::COMPLETE_LIST_16BIT_SERVICE_IDS) == BLE_ERROR_NONE) {
+            old_size += existing_data.size() + 2;
+        }
+        if (getData(existing_data, adv_data_type_t::INCOMPLETE_LIST_128BIT_SERVICE_IDS) == BLE_ERROR_NONE) {
+            old_size += existing_data.size() + 2;
+        }
+        if (getData(existing_data, adv_data_type_t::COMPLETE_LIST_128BIT_SERVICE_IDS) == BLE_ERROR_NONE) {
+            old_size += existing_data.size() + 2;
+        }
+
+        /* if we can't fit the new data do not proceed */
+        if (total_size - old_size > data.size() - _payload_length) {
+            return BLE_ERROR_BUFFER_OVERFLOW;
+        }
+
+        /* otherwise wipe old data */
+        removeData(adv_data_type_t::INCOMPLETE_LIST_16BIT_SERVICE_IDS);
+        removeData(adv_data_type_t::COMPLETE_LIST_16BIT_SERVICE_IDS);
+        removeData(adv_data_type_t::INCOMPLETE_LIST_128BIT_SERVICE_IDS);
+        removeData(adv_data_type_t::COMPLETE_LIST_128BIT_SERVICE_IDS);
+
+        /* and insert individual UUIDs into appropriate fields */
+        for (size_t i = 0; i < data.size(); ++i) {
+            adv_data_type_t field_type(adv_data_type_t::FLAGS);
+            if (data[i].shortOrLong() == UUID::UUID_TYPE_SHORT) {
+                if (complete) {
+                    field_type = adv_data_type_t::INCOMPLETE_LIST_16BIT_SERVICE_IDS;
+                } else {
+                    field_type = adv_data_type_t::COMPLETE_LIST_16BIT_SERVICE_IDS;
+                }
+            } else {
+                if (complete) {
+                    field_type = adv_data_type_t::INCOMPLETE_LIST_128BIT_SERVICE_IDS;
+                } else {
+                    field_type = adv_data_type_t::COMPLETE_LIST_128BIT_SERVICE_IDS;
+                }
+            }
+
+            mbed::Span<const uint8_t> span(data[i].getBaseUUID(), data[i].getLen());
+
+            uint8_t *field = findField(field_type);
+
+            if (field) {
+                status = appendToField(span, field);
+                if (status != BLE_ERROR_NONE) {
+                    /* we already checked for size so this must not happen */
+                    return BLE_ERROR_INTERNAL_STACK_FAILURE;
+                }
+            } else {
+                status = addField(field_type, span);
+                if (status != BLE_ERROR_NONE) {
+                    /* we already checked for size so this must not happen */
+                    return BLE_ERROR_INTERNAL_STACK_FAILURE;
+                }
+            }
+        }
+
+        return status;
+    }
+
+    /**
+     * Return a span of data containing the the type of data requested.
+     *
+     * @param[out] data Span used to return the requested data.
+     * @param[in] advDataType Data type to return.
+     *
+     * @return BLE_ERROR_NONE if data was found and BLE_ERROR_NOT_FOUND if not.
+     */
+    ble_error_t getData(
+        mbed::Span<const uint8_t> &data,
+        adv_data_type_t advDataType
+    ) {
+        uint8_t *field = findField(advDataType);
+        if (field) {
+            uint8_t data_length = field[0] - 1 /* skip type */;
+            data = mbed::make_Span((const uint8_t*)(field + 2 /* skip type and length */), data_length);
+            return BLE_ERROR_NONE;
+        } else {
+            return BLE_ERROR_NOT_FOUND;
+        }
+    }
+
 private:
     /**
     * Search advertisement data for a specific field.