AdvertisingDataBuilder.cpp

00001 /* mbed Microcontroller Library
00002  * Copyright (c) 2018 ARM Limited
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "ble/gap/AdvertisingDataBuilder.h"
00018 
00019 // Implementation notes
00020 // Advertising data are organized as follow:
00021 //   - byte 0: Size of the rest of the field
00022 //   - byte 1: type of the field
00023 //   - byte 2 to the last byte: field value.
00024 // An advertising data can contain at most a single instance of a field type.
00025 
00026 #define FIELD_TYPE_INDEX 1
00027 #define COMPANY_IDENTIFIER_SIZE 2
00028 
00029 // A field is represented by a type and a value. The size of the field
00030 // must fit in a byte therefore, the size of DATA cannot be larger than
00031 // 0xFE
00032 #define MAX_DATA_FIELD_SIZE 0xFE
00033 
00034 #define FIELD_HEADER_SIZE 2
00035 
00036 namespace ble {
00037 
00038 namespace {
00039 
00040 mbed::Span<const uint8_t>  as_span(const int8_t& v) {
00041     return mbed::Span<const uint8_t> (reinterpret_cast<const uint8_t*>(&v), sizeof(v));
00042 }
00043 
00044 mbed::Span<const uint8_t>  as_span(const uint8_t& v) {
00045     return mbed::Span<const uint8_t> (static_cast<const uint8_t*>(&v), sizeof(v));
00046 }
00047 
00048 template<typename Rep, uint32_t TB, typename Range, typename F>
00049 mbed::Span<const uint8_t>  as_span(const Duration<Rep, TB, Range, F>& d) {
00050     return mbed::Span<const uint8_t> (reinterpret_cast<const uint8_t*>(d.storage()), sizeof(d.value()));
00051 }
00052 
00053 template<typename T, typename Rep>
00054 mbed::Span<const uint8_t>  as_span(const SafeEnum<T, Rep>& v) {
00055     return mbed::Span<const uint8_t> (reinterpret_cast<const uint8_t*>(v.storage()), sizeof(v.value()));
00056 }
00057 
00058 }
00059 
00060 AdvertisingDataBuilder::AdvertisingDataBuilder(mbed::Span<uint8_t>  buffer) :
00061     _buffer(buffer),
00062     _payload_length(0)
00063 {
00064 }
00065 
00066 AdvertisingDataBuilder::AdvertisingDataBuilder(uint8_t *buffer, size_t buffer_size) :
00067     _buffer(buffer, buffer_size),
00068     _payload_length(0)
00069 {
00070 }
00071 
00072 mbed::Span<const uint8_t>  AdvertisingDataBuilder::getAdvertisingData() const
00073 {
00074     return _buffer.first(_payload_length);
00075 }
00076 
00077 ble_error_t AdvertisingDataBuilder::addData(
00078     adv_data_type_t  advDataType,
00079     mbed::Span<const uint8_t>  fieldData
00080 )
00081 {
00082     if (findField(advDataType) != NULL) {
00083         return BLE_ERROR_OPERATION_NOT_PERMITTED;
00084     } else {
00085         return addField(advDataType, fieldData);
00086     }
00087 }
00088 
00089 ble_error_t AdvertisingDataBuilder::replaceData(
00090     adv_data_type_t  advDataType,
00091     mbed::Span<const uint8_t>  fieldData
00092 )
00093 {
00094     uint8_t *field = findField(advDataType);
00095 
00096     if (field == NULL) {
00097         return BLE_ERROR_NOT_FOUND;
00098     }
00099 
00100     return replaceField(advDataType, fieldData, field);
00101 }
00102 
00103 ble_error_t AdvertisingDataBuilder::appendData(
00104     adv_data_type_t  advDataType,
00105     mbed::Span<const uint8_t>  fieldData
00106 )
00107 {
00108     uint8_t *field = findField(advDataType);
00109 
00110     if (field == NULL) {
00111         return BLE_ERROR_NOT_FOUND;
00112     }
00113 
00114     return appendToField(fieldData, field);
00115 }
00116 
00117 ble_error_t AdvertisingDataBuilder::removeData(
00118     adv_data_type_t  advDataType
00119 )
00120 {
00121     uint8_t *field = findField(advDataType);
00122 
00123     if (field == NULL) {
00124         return BLE_ERROR_NOT_FOUND;
00125     }
00126 
00127     return removeField(field);
00128 }
00129 
00130 ble_error_t AdvertisingDataBuilder::addOrReplaceData(
00131     adv_data_type_t  advDataType,
00132     mbed::Span<const uint8_t>  fieldData
00133 )
00134 {
00135     uint8_t *field = findField(advDataType);
00136 
00137     if (field != NULL) {
00138         return replaceField(advDataType, fieldData, field);
00139     } else {
00140         return addField(advDataType, fieldData);
00141     }
00142 }
00143 
00144 ble_error_t AdvertisingDataBuilder::addOrAppendData(
00145     adv_data_type_t  advDataType,
00146     mbed::Span<const uint8_t>  fieldData
00147 )
00148 {
00149     uint8_t *field = findField(advDataType);
00150 
00151     if (field != NULL) {
00152         return appendToField(fieldData, field);
00153     } else {
00154         return addField(advDataType, fieldData);
00155     }
00156 }
00157 
00158 void AdvertisingDataBuilder::clear()
00159 {
00160     memset(_buffer.data(), 0, _buffer.size());
00161     _payload_length = 0;
00162 }
00163 
00164 ble_error_t AdvertisingDataBuilder::setAppearance(
00165     adv_data_appearance_t appearance
00166 )
00167 {
00168     return addOrReplaceData(adv_data_type_t::APPEARANCE, as_span(appearance));
00169 }
00170 
00171 ble_error_t AdvertisingDataBuilder::setFlags(adv_data_flags_t flags)
00172 {
00173     uint8_t flags_byte = flags.value();
00174     return addOrReplaceData(adv_data_type_t::FLAGS, as_span(flags_byte));
00175 }
00176 
00177 ble_error_t AdvertisingDataBuilder::setTxPowerAdvertised(
00178     advertising_power_t txPower
00179 )
00180 {
00181     return addOrReplaceData(adv_data_type_t::TX_POWER_LEVEL, as_span(txPower));
00182 }
00183 
00184 ble_error_t AdvertisingDataBuilder::setName(
00185     const char *name,
00186     bool complete
00187 )
00188 {
00189     mbed::Span<const uint8_t>  name_span((const uint8_t *) name, strlen(name));
00190 
00191     if (complete) {
00192         return addOrReplaceData(adv_data_type_t::COMPLETE_LOCAL_NAME, name_span);
00193     } else {
00194         return addOrReplaceData(adv_data_type_t::SHORTENED_LOCAL_NAME, name_span);
00195     }
00196 }
00197 
00198 ble_error_t AdvertisingDataBuilder::setManufacturerSpecificData(
00199     mbed::Span<const uint8_t>  data
00200 )
00201 {
00202     // manufacturer specific data should at least contain the vendor ID.
00203     if (data.size() < COMPANY_IDENTIFIER_SIZE) {
00204         return BLE_ERROR_INVALID_PARAM;
00205     }
00206 
00207     return addOrReplaceData(adv_data_type_t::MANUFACTURER_SPECIFIC_DATA, data);
00208 }
00209 
00210 ble_error_t AdvertisingDataBuilder::setAdvertisingInterval(
00211     adv_interval_t  interval
00212 )
00213 {
00214     // Note: Advertising interval in advertisement MUST be represented in a 16bit
00215     // value.
00216     if (interval.value() > 0xFFFF) {
00217         return BLE_ERROR_INVALID_PARAM;
00218     }
00219 
00220     return addOrReplaceData(
00221         adv_data_type_t::ADVERTISING_INTERVAL,
00222         as_span(interval)
00223     );
00224 }
00225 
00226 ble_error_t AdvertisingDataBuilder::setConnectionIntervalPreference(
00227     conn_interval_t  min,
00228     conn_interval_t  max
00229 )
00230 {
00231     uint8_t interval[2 * sizeof(conn_interval_t::representation_t)];
00232     memcpy(interval, max.storage(), sizeof(max.value()));
00233     memcpy(interval + sizeof(max.value()), min.storage(), sizeof(min.value()));
00234 
00235     return addOrReplaceData(
00236         adv_data_type_t::SLAVE_CONNECTION_INTERVAL_RANGE,
00237         interval
00238     );
00239 }
00240 
00241 ble_error_t AdvertisingDataBuilder::setServiceData(
00242     UUID service,
00243     mbed::Span<const uint8_t>  data
00244 )
00245 {
00246     if (service.getLen() + data.size() > MAX_DATA_FIELD_SIZE) {
00247         return BLE_ERROR_INVALID_PARAM;
00248     }
00249 
00250     adv_data_type_t  short_type = adv_data_type_t::SERVICE_DATA_16BIT_ID;
00251     adv_data_type_t  long_type = adv_data_type_t::SERVICE_DATA_128BIT_ID;
00252 
00253     size_t total_size = FIELD_HEADER_SIZE + service.getLen() + data.size();
00254     size_t old_size = getFieldSize(
00255         (service.shortOrLong() == UUID::UUID_TYPE_SHORT) ? short_type : long_type
00256     );
00257 
00258     /* if we can't fit the new data do not proceed */
00259     if (total_size > data.size() - (_payload_length - old_size)) {
00260         return BLE_ERROR_BUFFER_OVERFLOW;
00261     }
00262 
00263     /* this will insert only the UUID (and remove old data) */
00264     ble_error_t status = setUUIDData(
00265         mbed::make_Span(&service, 1),
00266         short_type,
00267         long_type
00268     );
00269 
00270     if (status != BLE_ERROR_NONE) {
00271         /* we already checked for size so this must not happen */
00272         return BLE_ERROR_INTERNAL_STACK_FAILURE;
00273     }
00274 
00275     status = appendData(
00276         (service.shortOrLong() == UUID::UUID_TYPE_SHORT) ? short_type : long_type,
00277         data
00278     );
00279 
00280     if (status != BLE_ERROR_NONE) {
00281         return BLE_ERROR_INTERNAL_STACK_FAILURE;
00282     }
00283 
00284     return BLE_ERROR_NONE;
00285 }
00286 
00287 ble_error_t AdvertisingDataBuilder::setLocalServiceList(
00288     mbed::Span<const UUID> data,
00289     bool complete
00290 )
00291 {
00292     adv_data_type_t  short_type = complete ?
00293         adv_data_type_t::COMPLETE_LIST_16BIT_SERVICE_IDS :
00294         adv_data_type_t::INCOMPLETE_LIST_16BIT_SERVICE_IDS;
00295 
00296     adv_data_type_t  long_type = complete ?
00297         adv_data_type_t::COMPLETE_LIST_128BIT_SERVICE_IDS :
00298         adv_data_type_t::INCOMPLETE_LIST_128BIT_SERVICE_IDS;
00299 
00300     return setUUIDData(data, short_type, long_type);
00301 }
00302 
00303 ble_error_t AdvertisingDataBuilder::setRequestedServiceList(
00304     mbed::Span<const UUID> data
00305 )
00306 {
00307     adv_data_type_t  short_type = adv_data_type_t::LIST_16BIT_SOLICITATION_IDS;
00308     adv_data_type_t  long_type = adv_data_type_t::LIST_128BIT_SOLICITATION_IDS;
00309 
00310     return setUUIDData(data, short_type, long_type);
00311 }
00312 
00313 ble_error_t AdvertisingDataBuilder::getData(
00314     mbed::Span<const uint8_t>  &data,
00315     adv_data_type_t  advDataType
00316 )
00317 {
00318     uint8_t *field = findField(advDataType);
00319     if (field) {
00320         uint8_t data_length = field[0] - 1 /* skip type */;
00321         data = mbed::make_Span((const uint8_t *) (field + FIELD_HEADER_SIZE), data_length);
00322         return BLE_ERROR_NONE;
00323     } else {
00324         return BLE_ERROR_NOT_FOUND;
00325     }
00326 }
00327 
00328 uint8_t *AdvertisingDataBuilder::findField(adv_data_type_t  type)
00329 {
00330     /* Scan through advertisement data */
00331     for (uint8_t idx = 0; idx < _payload_length;) {
00332         uint8_t fieldType = _buffer[idx + FIELD_TYPE_INDEX];
00333 
00334         if (fieldType == type.value()) {
00335             return _buffer.data() + idx;
00336         }
00337 
00338         /* Advance to next field */
00339         idx += _buffer[idx] + 1;
00340     }
00341 
00342     return NULL;
00343 }
00344 
00345 uint8_t AdvertisingDataBuilder::getFieldSize(adv_data_type_t type)
00346 {
00347     uint8_t *field = findField(type);
00348     if (field) {
00349         return field[0] + 1 /* field size is not included so we add it */;
00350     } else {
00351         return 0;
00352     }
00353 }
00354 
00355 ble_error_t AdvertisingDataBuilder::addField(
00356     adv_data_type_t advDataType,
00357     mbed::Span<const uint8_t>  fieldData
00358 )
00359 {
00360     if (fieldData.size() > MAX_DATA_FIELD_SIZE) {
00361         return BLE_ERROR_INVALID_PARAM;
00362     }
00363 
00364     /* Make sure we don't exceed the buffer size */
00365     if (_payload_length + fieldData.size() + FIELD_HEADER_SIZE > _buffer.size()) {
00366         return BLE_ERROR_BUFFER_OVERFLOW;
00367     }
00368 
00369     /* Field length (includes field ID byte) */
00370     _buffer[_payload_length] = fieldData.size() + /* type */ 1;
00371     ++_payload_length;
00372 
00373     /* Field ID. */
00374     _buffer[_payload_length] = advDataType.value();
00375     ++_payload_length;
00376 
00377     /* Payload. */
00378     memcpy(&_buffer[_payload_length], fieldData.data(), fieldData.size());
00379     _payload_length += fieldData.size();
00380 
00381     return BLE_ERROR_NONE;
00382 }
00383 
00384 ble_error_t AdvertisingDataBuilder::appendToField(
00385     mbed::Span<const uint8_t>  fieldData,
00386     uint8_t *field
00387 )
00388 {
00389     if (fieldData.size() + field[0] > 0xFF /* field[0] already includes the type byte */) {
00390         return BLE_ERROR_INVALID_PARAM;
00391     }
00392 
00393     /* Check if data fits */
00394     if ((_payload_length + fieldData.size()) <= _buffer.size()) {
00395         uint8_t old_data_length = field[0];
00396 
00397         /* get the size of bytes in the payload after the field */
00398         size_t remainder_size = _payload_length -
00399             (field - _buffer.data()) - /* length of all data before the field */
00400             (old_data_length + 1) /* length of the old field */;
00401 
00402         /* move data after the field to fit new data */
00403         if (remainder_size) {
00404             memmove(
00405                 field + old_data_length + 1 + fieldData.size(),
00406                 field + old_data_length + 1,
00407                 remainder_size
00408             );
00409         }
00410 
00411         /* append new data */
00412         memcpy(field + old_data_length + 1, fieldData.data(), fieldData.size());
00413 
00414         /* Increment lengths */
00415         field[0] += fieldData.size();
00416         _payload_length += fieldData.size();
00417 
00418         return BLE_ERROR_NONE;
00419     } else {
00420         return BLE_ERROR_BUFFER_OVERFLOW;
00421     }
00422 }
00423 
00424 ble_error_t AdvertisingDataBuilder::replaceField(
00425     adv_data_type_t advDataType,
00426     mbed::Span<const uint8_t>  fieldData,
00427     uint8_t *field
00428 )
00429 {
00430     if (fieldData.size() > MAX_DATA_FIELD_SIZE) {
00431         return BLE_ERROR_INVALID_PARAM;
00432     }
00433 
00434     uint8_t old_data_length = field[0] - 1;
00435 
00436     /* New data has same length, do in-order replacement */
00437     if (fieldData.size() == old_data_length) {
00438         memcpy(field + 2, fieldData.data(), old_data_length);
00439 
00440         return BLE_ERROR_NONE;
00441     } else {
00442         /* Check if data fits */
00443         if ((_payload_length - old_data_length + fieldData.size()) <= _buffer.size()) {
00444             removeField(field);
00445 
00446             /* Add new field */
00447             return addField(advDataType, fieldData);
00448         } else {
00449             return BLE_ERROR_BUFFER_OVERFLOW;
00450         }
00451     }
00452 }
00453 
00454 ble_error_t AdvertisingDataBuilder::removeField(uint8_t *field)
00455 {
00456     /* stored length + the byte containing length */
00457     uint8_t old_field_length = field[0] + 1;
00458 
00459     memmove(field, field + old_field_length, old_field_length);
00460 
00461     _payload_length -= old_field_length;
00462 
00463     return BLE_ERROR_NONE;
00464 }
00465 
00466 ble_error_t AdvertisingDataBuilder::setUUIDData(
00467     mbed::Span<const UUID> data,
00468     adv_data_type_t shortType,
00469     adv_data_type_t longType
00470 )
00471 {
00472     ble_error_t status = BLE_ERROR_NONE;
00473 
00474     /* first count all the bytes we need to store all the UUIDs */
00475     size_t size_long = 0;
00476     size_t size_short = 0;
00477 
00478     for (size_t i = 0, end = data.size(); i < end; ++i) {
00479         if (data[i].shortOrLong() == UUID::UUID_TYPE_SHORT) {
00480             size_short += data[i].getLen();
00481         } else {
00482             size_long += data[i].getLen();
00483         }
00484     }
00485 
00486     if ((size_long > MAX_DATA_FIELD_SIZE) || (size_short > MAX_DATA_FIELD_SIZE)) {
00487         return BLE_ERROR_INVALID_PARAM;
00488     }
00489 
00490     /* UUID data consists of a type byte, size byte and the list UUIDs itself, we include
00491      * the header (type and size bytes) size only if the size of the UUIDs is non-zero
00492      * (!!non_zero_variable) == 1 */
00493     size_t long_uuid_data_size = (!!size_long) * FIELD_HEADER_SIZE + size_long;
00494     size_t short_uuid_data_size = (!!size_short) * FIELD_HEADER_SIZE + size_short;
00495     size_t new_size = long_uuid_data_size + short_uuid_data_size;
00496 
00497     /* count all the bytes of existing data */
00498     size_t old_size = getFieldSize(shortType) + getFieldSize(longType);
00499 
00500     /* if we can't fit the new data do not proceed */
00501     if (new_size > _buffer.size() - (_payload_length - old_size)) {
00502         return BLE_ERROR_BUFFER_OVERFLOW;
00503     }
00504 
00505     /* otherwise wipe old data */
00506     removeData(shortType);
00507     removeData(longType);
00508 
00509     /* and insert individual UUIDs into appropriate fields */
00510     for (size_t i = 0, end = data.size(); i < end; ++i) {
00511         adv_data_type_t field_type = (data[i].shortOrLong() == UUID::UUID_TYPE_SHORT) ? shortType : longType;
00512 
00513         mbed::Span<const uint8_t>  span(data[i].getBaseUUID(), data[i].getLen());
00514 
00515         uint8_t *field = findField(field_type);
00516 
00517         if (field) {
00518             status = appendToField(span, field);
00519             if (status != BLE_ERROR_NONE) {
00520                 /* we already checked for size so this must not happen */
00521                 return BLE_ERROR_INTERNAL_STACK_FAILURE;
00522             }
00523         } else {
00524             status = addField(field_type, span);
00525             if (status != BLE_ERROR_NONE) {
00526                 /* we already checked for size so this must not happen */
00527                 return BLE_ERROR_INTERNAL_STACK_FAILURE;
00528             }
00529         }
00530     }
00531 
00532     return status;
00533 }
00534 
00535 } // end of namespace ble