Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: TYBLE16_simple_data_logger TYBLE16_MP3_Air
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
Generated on Tue Jul 12 2022 13:54:00 by
