Import Mbed OS hard-float snapshot

connectivity/FEATURE_BLE/source/gap/AdvertisingDataBuilder.cpp

@@ -0,0 +1,539 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2018 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "gap/AdvertisingDataBuilder.h"
+
+// Implementation notes
+// Advertising data are organized as follow:
+//   - byte 0: Size of the rest of the field
+//   - byte 1: type of the field
+//   - byte 2 to the last byte: field value.
+// An advertising data can contain at most a single instance of a field type.
+
+#define FIELD_TYPE_INDEX 1
+#define COMPANY_IDENTIFIER_SIZE 2
+
+// A field is represented by a type and a value. The size of the field
+// must fit in a byte therefore, the size of DATA cannot be larger than
+// 0xFE
+#define MAX_DATA_FIELD_SIZE 0xFE
+
+#define FIELD_HEADER_SIZE 2
+
+namespace ble {
+
+namespace {
+
+mbed::Span<const uint8_t> as_span(const int8_t& v)
+{
+    return mbed::Span<const uint8_t>(reinterpret_cast<const uint8_t*>(&v), sizeof(v));
+}
+
+mbed::Span<const uint8_t> as_span(const uint8_t& v)
+{
+    return mbed::Span<const uint8_t>(static_cast<const uint8_t*>(&v), sizeof(v));
+}
+
+template<typename Rep, uint32_t TB, typename Range, typename F>
+mbed::Span<const uint8_t> as_span(const Duration<Rep, TB, Range, F>& d)
+{
+    return mbed::Span<const uint8_t>(reinterpret_cast<const uint8_t*>(d.storage()), sizeof(d.value()));
+}
+
+template<typename T, typename Rep>
+mbed::Span<const uint8_t> as_span(const SafeEnum<T, Rep>& v)
+{
+    return mbed::Span<const uint8_t>(reinterpret_cast<const uint8_t*>(v.storage()), sizeof(v.value()));
+}
+
+}
+
+AdvertisingDataBuilder::AdvertisingDataBuilder(mbed::Span<uint8_t> buffer) :
+    _buffer(buffer),
+    _payload_length(0)
+{
+}
+
+AdvertisingDataBuilder::AdvertisingDataBuilder(uint8_t *buffer, size_t buffer_size) :
+    _buffer(buffer, buffer_size),
+    _payload_length(0)
+{
+}
+
+mbed::Span<const uint8_t> AdvertisingDataBuilder::getAdvertisingData() const
+{
+    return _buffer.first(_payload_length);
+}
+
+ble_error_t AdvertisingDataBuilder::addData(
+    adv_data_type_t advDataType,
+    mbed::Span<const uint8_t> fieldData
+)
+{
+    if (findField(advDataType) != nullptr) {
+        return BLE_ERROR_OPERATION_NOT_PERMITTED;
+    } else {
+        return addField(advDataType, fieldData);
+    }
+}
+
+ble_error_t AdvertisingDataBuilder::replaceData(
+    adv_data_type_t advDataType,
+    mbed::Span<const uint8_t> fieldData
+)
+{
+    uint8_t *field = findField(advDataType);
+
+    if (field == nullptr) {
+        return BLE_ERROR_NOT_FOUND;
+    }
+
+    return replaceField(advDataType, fieldData, field);
+}
+
+ble_error_t AdvertisingDataBuilder::appendData(
+    adv_data_type_t advDataType,
+    mbed::Span<const uint8_t> fieldData
+)
+{
+    uint8_t *field = findField(advDataType);
+
+    if (field == nullptr) {
+        return BLE_ERROR_NOT_FOUND;
+    }
+
+    return appendToField(fieldData, field);
+}
+
+ble_error_t AdvertisingDataBuilder::removeData(
+    adv_data_type_t advDataType
+)
+{
+    uint8_t *field = findField(advDataType);
+
+    if (field == nullptr) {
+        return BLE_ERROR_NOT_FOUND;
+    }
+
+    return removeField(field);
+}
+
+ble_error_t AdvertisingDataBuilder::addOrReplaceData(
+    adv_data_type_t advDataType,
+    mbed::Span<const uint8_t> fieldData
+)
+{
+    uint8_t *field = findField(advDataType);
+
+    if (field != nullptr) {
+        return replaceField(advDataType, fieldData, field);
+    } else {
+        return addField(advDataType, fieldData);
+    }
+}
+
+ble_error_t AdvertisingDataBuilder::addOrAppendData(
+    adv_data_type_t advDataType,
+    mbed::Span<const uint8_t> fieldData
+)
+{
+    uint8_t *field = findField(advDataType);
+
+    if (field != nullptr) {
+        return appendToField(fieldData, field);
+    } else {
+        return addField(advDataType, fieldData);
+    }
+}
+
+void AdvertisingDataBuilder::clear()
+{
+    memset(_buffer.data(), 0, _buffer.size());
+    _payload_length = 0;
+}
+
+ble_error_t AdvertisingDataBuilder::setAppearance(
+    adv_data_appearance_t appearance
+)
+{
+    return addOrReplaceData(adv_data_type_t::APPEARANCE, as_span(appearance));
+}
+
+ble_error_t AdvertisingDataBuilder::setFlags(adv_data_flags_t flags)
+{
+    uint8_t flags_byte = flags.value();
+    return addOrReplaceData(adv_data_type_t::FLAGS, as_span(flags_byte));
+}
+
+ble_error_t AdvertisingDataBuilder::setTxPowerAdvertised(
+    advertising_power_t txPower
+)
+{
+    return addOrReplaceData(adv_data_type_t::TX_POWER_LEVEL, as_span(txPower));
+}
+
+ble_error_t AdvertisingDataBuilder::setName(
+    const char *name,
+    bool complete
+)
+{
+    mbed::Span<const uint8_t> name_span((const uint8_t *) name, strlen(name));
+
+    if (complete) {
+        return addOrReplaceData(adv_data_type_t::COMPLETE_LOCAL_NAME, name_span);
+    } else {
+        return addOrReplaceData(adv_data_type_t::SHORTENED_LOCAL_NAME, name_span);
+    }
+}
+
+ble_error_t AdvertisingDataBuilder::setManufacturerSpecificData(
+    mbed::Span<const uint8_t> data
+)
+{
+    // manufacturer specific data should at least contain the vendor ID.
+    if (data.size() < COMPANY_IDENTIFIER_SIZE) {
+        return BLE_ERROR_INVALID_PARAM;
+    }
+
+    return addOrReplaceData(adv_data_type_t::MANUFACTURER_SPECIFIC_DATA, data);
+}
+
+ble_error_t AdvertisingDataBuilder::setAdvertisingInterval(
+    adv_interval_t interval
+)
+{
+    // Note: Advertising interval in advertisement MUST be represented in a 16bit
+    // value.
+    if (interval.value() > 0xFFFF) {
+        return BLE_ERROR_INVALID_PARAM;
+    }
+
+    return addOrReplaceData(
+        adv_data_type_t::ADVERTISING_INTERVAL,
+        as_span(interval)
+    );
+}
+
+ble_error_t AdvertisingDataBuilder::setConnectionIntervalPreference(
+    conn_interval_t min,
+    conn_interval_t max
+)
+{
+    uint8_t interval[2 * sizeof(conn_interval_t::representation_t)];
+    memcpy(interval, max.storage(), sizeof(max.value()));
+    memcpy(interval + sizeof(max.value()), min.storage(), sizeof(min.value()));
+
+    return addOrReplaceData(
+        adv_data_type_t::SLAVE_CONNECTION_INTERVAL_RANGE,
+        interval
+    );
+}
+
+ble_error_t AdvertisingDataBuilder::setServiceData(
+    UUID service,
+    mbed::Span<const uint8_t> data
+)
+{
+    if (service.getLen() + data.size() > MAX_DATA_FIELD_SIZE) {
+        return BLE_ERROR_INVALID_PARAM;
+    }
+
+    adv_data_type_t short_type = adv_data_type_t::SERVICE_DATA_16BIT_ID;
+    adv_data_type_t long_type = adv_data_type_t::SERVICE_DATA_128BIT_ID;
+
+    size_t total_size = FIELD_HEADER_SIZE + service.getLen() + data.size();
+    size_t old_size = getFieldSize(
+        (service.shortOrLong() == UUID::UUID_TYPE_SHORT) ? short_type : long_type
+    );
+
+    /* if we can't fit the new data do not proceed */
+    if (total_size > (_buffer.size() - (_payload_length - old_size))) {
+        return BLE_ERROR_BUFFER_OVERFLOW;
+    }
+
+    /* this will insert only the UUID (and remove old data) */
+    ble_error_t status = setUUIDData(
+        mbed::make_Span(&service, 1),
+        short_type,
+        long_type
+    );
+
+    if (status != BLE_ERROR_NONE) {
+        /* we already checked for size so this must not happen */
+        return BLE_ERROR_INTERNAL_STACK_FAILURE;
+    }
+
+    status = appendData(
+        (service.shortOrLong() == UUID::UUID_TYPE_SHORT) ? short_type : long_type,
+        data
+    );
+
+    if (status != BLE_ERROR_NONE) {
+        return BLE_ERROR_INTERNAL_STACK_FAILURE;
+    }
+
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t AdvertisingDataBuilder::setLocalServiceList(
+    mbed::Span<const UUID> data,
+    bool complete
+)
+{
+    adv_data_type_t short_type = complete ?
+        adv_data_type_t::COMPLETE_LIST_16BIT_SERVICE_IDS :
+        adv_data_type_t::INCOMPLETE_LIST_16BIT_SERVICE_IDS;
+
+    adv_data_type_t long_type = complete ?
+        adv_data_type_t::COMPLETE_LIST_128BIT_SERVICE_IDS :
+        adv_data_type_t::INCOMPLETE_LIST_128BIT_SERVICE_IDS;
+
+    return setUUIDData(data, short_type, long_type);
+}
+
+ble_error_t AdvertisingDataBuilder::setRequestedServiceList(
+    mbed::Span<const UUID> data
+)
+{
+    adv_data_type_t short_type = adv_data_type_t::LIST_16BIT_SOLICITATION_IDS;
+    adv_data_type_t long_type = adv_data_type_t::LIST_128BIT_SOLICITATION_IDS;
+
+    return setUUIDData(data, short_type, long_type);
+}
+
+ble_error_t AdvertisingDataBuilder::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 + FIELD_HEADER_SIZE), data_length);
+        return BLE_ERROR_NONE;
+    } else {
+        return BLE_ERROR_NOT_FOUND;
+    }
+}
+
+uint8_t *AdvertisingDataBuilder::findField(adv_data_type_t type)
+{
+    /* Scan through advertisement data */
+    for (uint8_t idx = 0; idx < _payload_length;) {
+        uint8_t fieldType = _buffer[idx + FIELD_TYPE_INDEX];
+
+        if (fieldType == type.value()) {
+            return _buffer.data() + idx;
+        }
+
+        /* Advance to next field */
+        idx += _buffer[idx] + 1;
+    }
+
+    return nullptr;
+}
+
+uint8_t AdvertisingDataBuilder::getFieldSize(adv_data_type_t type)
+{
+    uint8_t *field = findField(type);
+    if (field) {
+        return field[0] + 1 /* field size is not included so we add it */;
+    } else {
+        return 0;
+    }
+}
+
+ble_error_t AdvertisingDataBuilder::addField(
+    adv_data_type_t advDataType,
+    mbed::Span<const uint8_t> fieldData
+)
+{
+    if (fieldData.size() > MAX_DATA_FIELD_SIZE) {
+        return BLE_ERROR_INVALID_PARAM;
+    }
+
+    /* Make sure we don't exceed the buffer size */
+    if (_payload_length + fieldData.size() + FIELD_HEADER_SIZE > _buffer.size()) {
+        return BLE_ERROR_BUFFER_OVERFLOW;
+    }
+
+    /* Field length (includes field ID byte) */
+    _buffer[_payload_length] = fieldData.size() + /* type */ 1;
+    ++_payload_length;
+
+    /* Field ID. */
+    _buffer[_payload_length] = advDataType.value();
+    ++_payload_length;
+
+    /* Payload. */
+    memcpy(&_buffer[_payload_length], fieldData.data(), fieldData.size());
+    _payload_length += fieldData.size();
+
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t AdvertisingDataBuilder::appendToField(
+    mbed::Span<const uint8_t> fieldData,
+    uint8_t *field
+)
+{
+    if (fieldData.size() + field[0] > 0xFF /* field[0] already includes the type byte */) {
+        return BLE_ERROR_INVALID_PARAM;
+    }
+
+    /* Check if data fits */
+    if ((_payload_length + fieldData.size()) <= _buffer.size()) {
+        uint8_t old_data_length = field[0];
+
+        /* get the size of bytes in the payload after the field */
+        size_t remainder_size = _payload_length -
+            (field - _buffer.data()) - /* length of all data before the field */
+            (old_data_length + 1) /* length of the old field */;
+
+        /* move data after the field to fit new data */
+        if (remainder_size) {
+            memmove(
+                field + old_data_length + 1 + fieldData.size(),
+                field + old_data_length + 1,
+                remainder_size
+            );
+        }
+
+        /* append new data */
+        memcpy(field + old_data_length + 1, fieldData.data(), fieldData.size());
+
+        /* Increment lengths */
+        field[0] += fieldData.size();
+        _payload_length += fieldData.size();
+
+        return BLE_ERROR_NONE;
+    } else {
+        return BLE_ERROR_BUFFER_OVERFLOW;
+    }
+}
+
+ble_error_t AdvertisingDataBuilder::replaceField(
+    adv_data_type_t advDataType,
+    mbed::Span<const uint8_t> fieldData,
+    uint8_t *field
+)
+{
+    if (fieldData.size() > MAX_DATA_FIELD_SIZE) {
+        return BLE_ERROR_INVALID_PARAM;
+    }
+
+    uint8_t old_data_length = field[0] - 1;
+
+    /* New data has same length, do in-order replacement */
+    if (fieldData.size() == old_data_length) {
+        memcpy(field + 2, fieldData.data(), old_data_length);
+
+        return BLE_ERROR_NONE;
+    } else {
+        /* Check if data fits */
+        if ((_payload_length - old_data_length + fieldData.size()) <= _buffer.size()) {
+            removeField(field);
+
+            /* Add new field */
+            return addField(advDataType, fieldData);
+        } else {
+            return BLE_ERROR_BUFFER_OVERFLOW;
+        }
+    }
+}
+
+ble_error_t AdvertisingDataBuilder::removeField(uint8_t *field)
+{
+    /* stored length + the byte containing length */
+    uint8_t old_field_length = field[0] + 1;
+
+    memmove(field, field + old_field_length, old_field_length);
+
+    _payload_length -= old_field_length;
+
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t AdvertisingDataBuilder::setUUIDData(
+    mbed::Span<const UUID> data,
+    adv_data_type_t shortType,
+    adv_data_type_t longType
+)
+{
+    ble_error_t status = BLE_ERROR_NONE;
+
+    /* first count all the bytes we need to store all the UUIDs */
+    size_t size_long = 0;
+    size_t size_short = 0;
+
+    for (size_t i = 0, end = data.size(); i < end; ++i) {
+        if (data[i].shortOrLong() == UUID::UUID_TYPE_SHORT) {
+            size_short += data[i].getLen();
+        } else {
+            size_long += data[i].getLen();
+        }
+    }
+
+    if ((size_long > MAX_DATA_FIELD_SIZE) || (size_short > MAX_DATA_FIELD_SIZE)) {
+        return BLE_ERROR_INVALID_PARAM;
+    }
+
+    /* UUID data consists of a type byte, size byte and the list UUIDs itself, we include
+     * the header (type and size bytes) size only if the size of the UUIDs is non-zero
+     * (!!non_zero_variable) == 1 */
+    size_t long_uuid_data_size = (!!size_long) * FIELD_HEADER_SIZE + size_long;
+    size_t short_uuid_data_size = (!!size_short) * FIELD_HEADER_SIZE + size_short;
+    size_t new_size = long_uuid_data_size + short_uuid_data_size;
+
+    /* count all the bytes of existing data */
+    size_t old_size = getFieldSize(shortType) + getFieldSize(longType);
+
+    /* if we can't fit the new data do not proceed */
+    if (new_size > _buffer.size() - (_payload_length - old_size)) {
+        return BLE_ERROR_BUFFER_OVERFLOW;
+    }
+
+    /* otherwise wipe old data */
+    removeData(shortType);
+    removeData(longType);
+
+    /* and insert individual UUIDs into appropriate fields */
+    for (size_t i = 0, end = data.size(); i < end; ++i) {
+        adv_data_type_t field_type = (data[i].shortOrLong() == UUID::UUID_TYPE_SHORT) ? shortType : longType;
+
+        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;
+}
+
+} // end of namespace ble