mirror of
https://github.com/google/pebble.git
synced 2025-11-21 15:02:18 -05:00
762 lines
28 KiB
C
762 lines
28 KiB
C
/*
|
|
* Copyright 2024 Google LLC
|
|
*
|
|
* 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 "ble_ad_parse.h"
|
|
|
|
#include "applib/applib_malloc.auto.h"
|
|
|
|
#include "syscall/syscall.h"
|
|
#include "system/passert.h"
|
|
|
|
#include "util/math.h"
|
|
#include "util/net.h"
|
|
|
|
#include <btutil/bt_uuid.h>
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! Internal parsed advertisement data structures.
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! AD TYPE Values as specified by the Bluetooth 4.0 Spec.
|
|
//! See "Appendix C (Normative): EIR and AD Formats" in Core_v4.0.pdf
|
|
typedef enum {
|
|
BLEAdTypeFlags = 0x01,
|
|
BLEAdTypeService16BitUUIDPartial = 0x02,
|
|
BLEAdTypeService16BitUUIDComplete = 0x03,
|
|
BLEAdTypeService32BitUUIDPartial = 0x04,
|
|
BLEAdTypeService32BitUUIDComplete = 0x05,
|
|
BLEAdTypeService128BitUUIDPartial = 0x06,
|
|
BLEAdTypeService128BitUUIDComplete = 0x07,
|
|
|
|
BLEAdTypeLocalNameShortened = 0x08,
|
|
BLEAdTypeLocalNameComplete = 0x09,
|
|
|
|
BLEAdTypeTxPowerLevel = 0x0a,
|
|
|
|
BLEAdTypeManufacturerSpecific = 0xff,
|
|
} BLEAdType;
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! AD DATA element header
|
|
typedef struct __attribute__((__packed__)) {
|
|
uint8_t length;
|
|
BLEAdType type:8;
|
|
} BLEAdElementHeader;
|
|
|
|
typedef struct __attribute__((__packed__)) {
|
|
BLEAdElementHeader header;
|
|
uint8_t data[];
|
|
} BLEAdElement;
|
|
|
|
// -----------------------------------------------------------------------------
|
|
// Consuming BLEAdData:
|
|
// -----------------------------------------------------------------------------
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! Internal parser callback. Gets called for a parsed Service UUIDs element.
|
|
//! @param uuids The array with Service UUIDs
|
|
//! @param count The number of Service UUIDs the array contains
|
|
//! @param cb_data Pointer to arbitrary client data as passed to the parse call.
|
|
//! @return true to continue parsing or false to stop after returning.
|
|
typedef bool (*BLEAdParseServicesCallback)(const Uuid uuids[], uint8_t count,
|
|
void *cb_data);
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! Internal parser callback. Gets called for a parsed Local Name element.
|
|
//! @param local_name_bytes This is a *NON* zero terminated UTF-8 string.
|
|
//! @param length The length of local_name_bytes
|
|
//! @param cb_data Pointer to arbitrary client data as passed to the parse call.
|
|
//! @return true to continue parsing or false to stop after returning.
|
|
typedef bool (*BLEAdParseLocalNameCallback)(const uint8_t *local_name_bytes,
|
|
uint8_t length, void *cb_data);
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! Internal parser callback. Gets called for a parsed TX Power Level element.
|
|
//! @param tx_power_level The TX Power Level value.
|
|
//! @param cb_data Pointer to arbitrary client data as passed to the parse call.
|
|
//! @return true to continue parsing or false to stop after returning.
|
|
typedef bool (*BLEAdParseTXPowerLevelCallback)(int8_t tx_power_level,
|
|
void *cb_data);
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! Internal parser callback. Gets called for a Manufacturer Specific data elem.
|
|
//! @param company_id The Company ID
|
|
//! @param data The Manufacturer Specific data
|
|
//! @param length The length in bytes of data
|
|
//! @param cb_data Pointer to arbitrary client data as passed to the parse call.
|
|
//! @return true to continue parsing or false to stop after returning.
|
|
typedef bool (*BLEAdParseManufacturerSpecificCallback)(uint16_t company_id,
|
|
const uint8_t *data,
|
|
uint8_t length,
|
|
void *cb_data);
|
|
|
|
typedef struct {
|
|
BLEAdParseServicesCallback services_cb;
|
|
BLEAdParseLocalNameCallback local_name_cb;
|
|
BLEAdParseTXPowerLevelCallback tx_power_level_cb;
|
|
BLEAdParseManufacturerSpecificCallback manufacturer_cb;
|
|
} BLEAdParseCallbacks;
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! Parser for Services List data elements.
|
|
static bool parse_services_list(const BLEAdElement *elem,
|
|
const BLEAdParseCallbacks *callbacks,
|
|
void *cb_data) {
|
|
const uint8_t uuid_type = elem->header.type / 2;
|
|
// Most common is probably 128-bit UUID, then 16-bit, then 32-bit:
|
|
const size_t uuid_width = (uuid_type == 3) ? 16 : ((uuid_type == 1) ? 2 : 4);
|
|
const size_t num_uuids = (elem->header.length - 1 /* Type byte */)
|
|
/ uuid_width;
|
|
|
|
if (!num_uuids) {
|
|
return true; // continue parsing
|
|
}
|
|
|
|
Uuid uuids[num_uuids];
|
|
|
|
// Iterate through the list, expanding each UUID to 128-bit equivalents,
|
|
// then copying them into the uuids[] array:
|
|
const uint8_t *uuid_data = elem->data;
|
|
for (size_t i = 0; i < num_uuids; ++i) {
|
|
switch (uuid_type) {
|
|
case 1: { // 16 bit
|
|
uint16_t u16 = *(uint16_t *) uuid_data;
|
|
uuids[i] = bt_uuid_expand_16bit(u16);
|
|
uuid_data += sizeof(uint16_t);
|
|
break;
|
|
}
|
|
case 2: { // 32 bit
|
|
uint32_t u32 = *(uint32_t *) uuid_data;
|
|
uuids[i] = bt_uuid_expand_32bit(u32);
|
|
uuid_data += sizeof(uint32_t);
|
|
break;
|
|
}
|
|
case 3: // 128-bit
|
|
uuids[i] = UuidMakeFromLEBytes(uuid_data);
|
|
uuid_data += sizeof(Uuid);
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Call back to client with parsed data:
|
|
return callbacks->services_cb(uuids, num_uuids, cb_data);
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! Parser for Local Name data element.
|
|
static bool parse_local_name(const BLEAdElement *elem,
|
|
const BLEAdParseCallbacks *callbacks,
|
|
void *cb_data) {
|
|
// Length of the raw string:
|
|
const uint8_t raw_length = elem->header.length - 1 /* -1 Type byte */;
|
|
if (!raw_length) {
|
|
return true; // continue parsing
|
|
}
|
|
|
|
// Call back to client with parsed data:
|
|
return callbacks->local_name_cb(elem->data, raw_length, cb_data);
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! Parser for TX Power Level data element.
|
|
static bool parse_power_level(const BLEAdElement *elem,
|
|
const BLEAdParseCallbacks *callbacks,
|
|
void *cb_data) {
|
|
if (elem->header.length != 2) {
|
|
// In case the length is not what it should be, do not add data element.
|
|
return true; // continue parsing
|
|
}
|
|
|
|
const int8_t tx_power_level = *(int8_t *)elem->data;
|
|
|
|
// Call back to client with parsed data:
|
|
return callbacks->tx_power_level_cb(tx_power_level, cb_data);
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! Parser for Manufacturer Specific data element.
|
|
|
|
static bool parse_manufact_spec(const BLEAdElement *elem,
|
|
const BLEAdParseCallbacks *callbacks,
|
|
void *cb_data) {
|
|
|
|
if (elem->header.length < 3) {
|
|
// The first 2 octets should be the Company Identifier Code
|
|
// (+1 for Type byte)
|
|
return true;
|
|
}
|
|
|
|
if (callbacks->manufacturer_cb) {
|
|
// Little-endian:
|
|
const uint16_t *company_id = (uint16_t *) elem->data;
|
|
|
|
// Call back to client with parsed data:
|
|
const uint8_t manufacturer_data_size =
|
|
elem->header.length - sizeof(*company_id) - 1 /* -1 Type Byte */;
|
|
return callbacks->manufacturer_cb(ltohs(*company_id),
|
|
elem->data + sizeof(*company_id),
|
|
manufacturer_data_size,
|
|
cb_data);
|
|
}
|
|
|
|
return true; // continue parsing
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! @param ad_data The advertising data and scan response data to parse.
|
|
//! @param callbacks Callbacks for each of the types of data the client wants
|
|
//! to receive parse callbacks for. You can leave a callback NULL if you are not
|
|
//! interested in receiving callbacks for that type of data.
|
|
//! @param cb_data Pointer to client data that is passed into the callback.
|
|
static void ble_ad_parse_ad_data(const BLEAdData *ad_data,
|
|
const BLEAdParseCallbacks *callbacks,
|
|
void *cb_data) {
|
|
const uint8_t *cursor = ad_data->data;
|
|
const uint8_t *end = cursor + ad_data->ad_data_length +
|
|
ad_data->scan_resp_data_length;
|
|
while (cursor < end) {
|
|
|
|
const BLEAdElement *elem = (const BLEAdElement *) cursor;
|
|
if (elem->header.length == 0) {
|
|
// We've hit a padding zero. We should be done, or this packet is corrupt.
|
|
return;
|
|
}
|
|
|
|
if (cursor + elem->header.length + 1 /* +1 length byte */ > end) {
|
|
return; // corrupted
|
|
}
|
|
|
|
bool (*parse_func)(const BLEAdElement *,
|
|
const BLEAdParseCallbacks *, void *) = NULL;
|
|
|
|
switch (elem->header.type) {
|
|
case BLEAdTypeService16BitUUIDPartial ... BLEAdTypeService128BitUUIDComplete:
|
|
if (callbacks->services_cb) {
|
|
parse_func = parse_services_list;
|
|
}
|
|
break;
|
|
|
|
case BLEAdTypeLocalNameShortened ... BLEAdTypeLocalNameComplete:
|
|
if (callbacks->local_name_cb) {
|
|
parse_func = parse_local_name;
|
|
}
|
|
break;
|
|
|
|
case BLEAdTypeTxPowerLevel:
|
|
if (callbacks->tx_power_level_cb) {
|
|
parse_func = parse_power_level;
|
|
}
|
|
break;
|
|
|
|
case BLEAdTypeManufacturerSpecific:
|
|
if (callbacks->manufacturer_cb) {
|
|
parse_func = parse_manufact_spec;
|
|
}
|
|
break;
|
|
|
|
default: // parse_func == NULL
|
|
break;
|
|
|
|
} // switch()
|
|
|
|
if (parse_func) {
|
|
if (!parse_func(elem, callbacks, cb_data)) {
|
|
// The callback indicated we should not continue parsing
|
|
return;
|
|
}
|
|
}
|
|
|
|
// The Length byte itself is not counted, so +1:
|
|
cursor += elem->header.length + 1;
|
|
}
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! ble_ad_includes_service() wrapper and helper function:
|
|
|
|
struct IncludesServiceCtx {
|
|
const Uuid *service_uuid;
|
|
bool included;
|
|
};
|
|
|
|
static bool includes_service_parse_cb(const Uuid uuids[], uint8_t count,
|
|
void *cb_data) {
|
|
struct IncludesServiceCtx *ctx = (struct IncludesServiceCtx *) cb_data;
|
|
for (int i = 0; i < count; ++i) {
|
|
if (uuid_equal(ctx->service_uuid, &uuids[i])) {
|
|
// Found!
|
|
ctx->included = true;
|
|
return false; // stop parsing
|
|
}
|
|
}
|
|
return true; // continue parsing
|
|
}
|
|
|
|
bool ble_ad_includes_service(const BLEAdData *ad, const Uuid *service_uuid) {
|
|
struct IncludesServiceCtx ctx = {
|
|
.service_uuid = service_uuid,
|
|
.included = false,
|
|
};
|
|
const BLEAdParseCallbacks callbacks = (const BLEAdParseCallbacks) {
|
|
.services_cb = includes_service_parse_cb,
|
|
};
|
|
ble_ad_parse_ad_data(ad, &callbacks, &ctx);
|
|
return ctx.included;
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! ble_ad_copy_service_uuids() wrapper and helper function:
|
|
|
|
struct CopyServiceUUIDsCtx {
|
|
Uuid *uuids_out;
|
|
const uint8_t max;
|
|
uint8_t copied;
|
|
uint8_t total;
|
|
};
|
|
|
|
static bool copy_services_parse_cb(const Uuid uuids[], uint8_t count,
|
|
void *cb_data) {
|
|
struct CopyServiceUUIDsCtx *ctx = (struct CopyServiceUUIDsCtx *)cb_data;
|
|
for (int i = 0; i < count; ++i) {
|
|
if (ctx->copied < ctx->max) {
|
|
// Still space left, so copy:
|
|
const Uuid *uuid = &uuids[i];
|
|
memcpy(&ctx->uuids_out[ctx->copied++], uuid, sizeof(Uuid));
|
|
}
|
|
++ctx->total;
|
|
}
|
|
return false; // stop parsing, only one Services UUID element allowed by spec
|
|
}
|
|
|
|
uint8_t ble_ad_copy_service_uuids(const BLEAdData *ad,
|
|
Uuid *uuids_out,
|
|
uint8_t num_uuids) {
|
|
struct CopyServiceUUIDsCtx ctx = {
|
|
.uuids_out = uuids_out,
|
|
.max = num_uuids,
|
|
};
|
|
const BLEAdParseCallbacks callbacks = {
|
|
.services_cb = copy_services_parse_cb,
|
|
};
|
|
ble_ad_parse_ad_data(ad, &callbacks, &ctx);
|
|
return ctx.total;
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! ble_ad_get_tx_power_level() wrapper and helper function:
|
|
|
|
struct TxPowerLevelCtx {
|
|
int8_t *tx_power_level_out;
|
|
bool included;
|
|
};
|
|
|
|
static bool tx_power_level_cb(int8_t tx_power_level,
|
|
void *cb_data) {
|
|
struct TxPowerLevelCtx *ctx = (struct TxPowerLevelCtx *)cb_data;
|
|
*ctx->tx_power_level_out = tx_power_level;
|
|
ctx->included = true;
|
|
return false; // stop parsing
|
|
}
|
|
|
|
bool ble_ad_get_tx_power_level(const BLEAdData *ad,
|
|
int8_t *tx_power_level_out) {
|
|
struct TxPowerLevelCtx ctx = {
|
|
.tx_power_level_out = tx_power_level_out,
|
|
.included = false,
|
|
};
|
|
const BLEAdParseCallbacks callbacks = {
|
|
.tx_power_level_cb = tx_power_level_cb,
|
|
};
|
|
ble_ad_parse_ad_data(ad, &callbacks, &ctx);
|
|
return ctx.included;
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! ble_ad_copy_local_name() wrapper and helper function:
|
|
|
|
struct LocalNameCtx {
|
|
char *buffer;
|
|
const uint8_t size;
|
|
size_t copied_size;
|
|
};
|
|
|
|
static bool copy_local_name_parse_cb(const uint8_t *local_name_bytes,
|
|
uint8_t length, void *cb_data) {
|
|
struct LocalNameCtx *ctx = (struct LocalNameCtx *)cb_data;
|
|
const uint8_t copied_size = MIN(ctx->size, length + 1 /* zero terminator */);
|
|
|
|
memcpy(ctx->buffer, local_name_bytes, copied_size - 1);
|
|
ctx->buffer[copied_size - 1] = 0; // zero terminator
|
|
ctx->copied_size = copied_size;
|
|
|
|
return false; // stop parsing
|
|
}
|
|
|
|
size_t ble_ad_copy_local_name(const BLEAdData *ad, char *buffer, size_t size) {
|
|
struct LocalNameCtx ctx = {
|
|
.buffer = buffer,
|
|
.size = MIN(size, 0xff),
|
|
.copied_size = 0,
|
|
};
|
|
const BLEAdParseCallbacks callbacks = {
|
|
.local_name_cb = copy_local_name_parse_cb,
|
|
};
|
|
ble_ad_parse_ad_data(ad, &callbacks, &ctx);
|
|
return ctx.copied_size;
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! ble_ad_get_raw_data_size() wrapper and helper function:
|
|
|
|
size_t ble_ad_get_raw_data_size(const BLEAdData *ad) {
|
|
return ad->ad_data_length + ad->scan_resp_data_length;
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! ble_ad_copy_raw_data() wrapper and helper function:
|
|
|
|
size_t ble_ad_copy_raw_data(const BLEAdData *ad, uint8_t *buffer, size_t size) {
|
|
const size_t size_to_copy = ble_ad_get_raw_data_size(ad);
|
|
if (size < size_to_copy) {
|
|
return 0;
|
|
}
|
|
memcpy(buffer, ad->data, size_to_copy);
|
|
return size_to_copy;
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! ble_ad_get_manufacturer_specific_data() wrapper and helper function:
|
|
|
|
struct ManufacturerSpecificCtx {
|
|
uint16_t company_id;
|
|
uint8_t *buffer;
|
|
const uint8_t size;
|
|
size_t copied_size;
|
|
};
|
|
|
|
static bool copy_manufacturer_specific_parse_cb(uint16_t company_id,
|
|
const uint8_t *data,
|
|
uint8_t length,
|
|
void *cb_data) {
|
|
|
|
struct ManufacturerSpecificCtx *ctx =
|
|
(struct ManufacturerSpecificCtx *) cb_data;
|
|
const uint8_t copied_size = MIN(ctx->size, length);
|
|
|
|
memcpy(ctx->buffer, data, copied_size);
|
|
ctx->copied_size = copied_size;
|
|
ctx->company_id = company_id;
|
|
|
|
return false; // stop parsing
|
|
}
|
|
|
|
size_t ble_ad_copy_manufacturer_specific_data(const BLEAdData *ad,
|
|
uint16_t *company_id,
|
|
uint8_t *buffer, size_t size) {
|
|
struct ManufacturerSpecificCtx ctx = {
|
|
.size = size,
|
|
.buffer = buffer,
|
|
};
|
|
const BLEAdParseCallbacks callbacks = {
|
|
.manufacturer_cb = copy_manufacturer_specific_parse_cb,
|
|
};
|
|
ble_ad_parse_ad_data(ad, &callbacks, &ctx);
|
|
if (company_id) {
|
|
*company_id = ctx.company_id;
|
|
}
|
|
return ctx.copied_size;
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
// Creating BLEAdData:
|
|
// -----------------------------------------------------------------------------
|
|
|
|
//! Magic high bit used as scan_resp_data_length to indicate that the ad_data
|
|
//! has been finalized and the next write should be counted towards the scan
|
|
//! response payload. The maximum scan_resp_data_length is 31 bytes, so this
|
|
//! value lies outside of the valid range. This is basically a memory savings
|
|
//! optimization, saving another "finalized" bool.
|
|
#define BLE_AD_DATA_FINALIZED ((uint8_t) 0x80)
|
|
|
|
bool prv_ad_is_finalized(const BLEAdData *ad_data) {
|
|
// Scan response data has already been added / started
|
|
return (ad_data->scan_resp_data_length != 0);
|
|
}
|
|
|
|
void ble_ad_start_scan_response(BLEAdData *ad_data) {
|
|
if (prv_ad_is_finalized(ad_data)) {
|
|
// Already finalized
|
|
return;
|
|
}
|
|
ad_data->scan_resp_data_length = BLE_AD_DATA_FINALIZED;
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! Helper to calculate whether a number of bytes will still fit when appended.
|
|
//! @param length Pointer to the length of the part for which to try to fit in
|
|
//! size_to_write number of bytes.
|
|
//! @param size_to_write The number of bytes to that need to be appended.
|
|
//! @return Pointer to length if it could still appends size_to_write bytes or
|
|
//! NULL if not.
|
|
static uint8_t *prv_length_ptr_if_fits_or_null(uint8_t *length,
|
|
size_t size_to_write) {
|
|
// Unset finalized bit:
|
|
const uint8_t used = *length & (~BLE_AD_DATA_FINALIZED);
|
|
const uint8_t left = GAP_LE_AD_REPORT_DATA_MAX_LENGTH - used;
|
|
// Return pointer to the pointer if size_to_write will fit, or NULL otherwise:
|
|
return (left >= size_to_write) ? length : NULL;
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! @return Pointer to the length that is incremented when writing size_to_write
|
|
//! number of bytes, or NULL if there is not enough space left.
|
|
static uint8_t* prv_length_to_increase(BLEAdData *ad_data,
|
|
size_t size_to_write) {
|
|
if (ad_data->scan_resp_data_length) {
|
|
// The scan response part is already being populated:
|
|
return prv_length_ptr_if_fits_or_null(&ad_data->scan_resp_data_length,
|
|
size_to_write);
|
|
} else {
|
|
// The advertisement is still being populated:
|
|
uint8_t *length = prv_length_ptr_if_fits_or_null(&ad_data->ad_data_length,
|
|
size_to_write);
|
|
if (length) {
|
|
// Hurray, the size_to_write fits in the advertisement part:
|
|
return length;
|
|
}
|
|
// Last resort, try fitting into scan response part:
|
|
return prv_length_ptr_if_fits_or_null(&ad_data->scan_resp_data_length,
|
|
size_to_write);
|
|
}
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
bool prv_write_element_to_ad_data(BLEAdData *ad_data,
|
|
const BLEAdElement *element) {
|
|
if (!ad_data || !element) {
|
|
return false;
|
|
}
|
|
const size_t size_to_write = element->header.length + 1 /* Length Byte */;
|
|
uint8_t* length = prv_length_to_increase(ad_data, size_to_write);
|
|
if (!length) {
|
|
// Not enough space...
|
|
return false;
|
|
}
|
|
|
|
// Undo the magic number trick:
|
|
if (*length == BLE_AD_DATA_FINALIZED) {
|
|
*length = 0;
|
|
}
|
|
|
|
// Append the element to the end:
|
|
uint8_t * const end = ad_data->data +
|
|
ad_data->ad_data_length +
|
|
ad_data->scan_resp_data_length;
|
|
memcpy(end, (const uint8_t *) element, size_to_write);
|
|
|
|
// Length book-keeping:
|
|
*length += size_to_write;
|
|
|
|
return true;
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
BLEAdData * ble_ad_create(void) {
|
|
const size_t max_ad_data_size = sizeof(BLEAdData) +
|
|
(GAP_LE_AD_REPORT_DATA_MAX_LENGTH * 2);
|
|
BLEAdData *ad_data = applib_malloc(max_ad_data_size);
|
|
if (ad_data) {
|
|
memset(ad_data, 0, sizeof(BLEAdData));
|
|
}
|
|
return ad_data;
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
void ble_ad_destroy(BLEAdData *ad) {
|
|
applib_free(ad);
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! The smallest UUID width, by reducing the width when a UUID is based on the
|
|
//! Bluetooth base UUID. @see bt_uuid_expand_16bit @see bt_uuid_expand_32bit
|
|
static uint8_t prv_smallest_bt_uuid_width_in_bytes(const Uuid *uuid) {
|
|
const Uuid bt_uuid_base = bt_uuid_expand_16bit(0);
|
|
|
|
// The bytes after the first 4 contain the Bluetooth base.
|
|
// Check if the uuid is based off of the Bluetooth base UUID:
|
|
const bool is_bt_uuid_based = (memcmp(&bt_uuid_base.byte4, &uuid->byte4,
|
|
sizeof(Uuid) - offsetof(Uuid, byte4)) == 0);
|
|
if (!is_bt_uuid_based) {
|
|
// Not based on the Bluetooth base UUID, so use 128-bits:
|
|
return sizeof(Uuid);
|
|
}
|
|
if (uuid->byte0 || uuid->byte1) {
|
|
// If byte0 and byte1 not zero: 32-bit UUID, Bluetooth base UUID based:
|
|
return sizeof(uint32_t);
|
|
}
|
|
// If byte0 and byte1 are zero: 16-bit UUID, Bluetooth base UUID based:
|
|
return sizeof(uint16_t);
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! Finds the largest common UUID width. For UUIDs that are based on the
|
|
//! Bluetooth base UUID, a reduced width will be taken of either 16-bits or
|
|
//! 32-bits.
|
|
static uint8_t prv_largest_common_bt_uuid_width(const Uuid uuids[],
|
|
uint8_t num_uuids) {
|
|
uint8_t max_width_bytes = sizeof(uint16_t);
|
|
for (unsigned int i = 0; i < num_uuids; ++i) {
|
|
const Uuid *uuid = &uuids[i];
|
|
const uint8_t width_bytes = prv_smallest_bt_uuid_width_in_bytes(uuid);
|
|
max_width_bytes = MAX(width_bytes, max_width_bytes);
|
|
}
|
|
return max_width_bytes;
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! Helper to reduces a 128-bit UUID to 16-bits. Note: this function does not
|
|
//! check whether the original UUID is based on the Bluetooth base.
|
|
static uint16_t prv_convert_to_16bit_uuid(const Uuid *uuid) {
|
|
uint16_t uuid_16bits = 0;
|
|
// Use bytes 2-3 of the Uuid:
|
|
for (int i = 2; i < 4; ++i) {
|
|
uuid_16bits <<= 8;
|
|
uuid_16bits += ((const uint8_t *) uuid)[i];
|
|
}
|
|
return uuid_16bits;
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
//! Helper to reduces a 128-bit UUID to 32-bits. Note: this function does not
|
|
//! check whether the original UUID is based on the Bluetooth base.
|
|
static uint32_t prv_convert_to_32bit_uuid(const Uuid *uuid) {
|
|
uint32_t uuid_32bits = 0;
|
|
// Use bytes 0-3 of the Uuid:
|
|
for (int i = 0; i < 4; ++i) {
|
|
uuid_32bits <<= 8;
|
|
uuid_32bits += ((const uint8_t *) uuid)[i];
|
|
}
|
|
return uuid_32bits;
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
bool ble_ad_set_service_uuids(BLEAdData *ad,
|
|
const Uuid uuids[], uint8_t num_uuids) {
|
|
struct __attribute__((__packed__)) BLEAdElementService {
|
|
BLEAdElementHeader header;
|
|
union {
|
|
Uuid uuid_128[0];
|
|
uint32_t uuid_32[0];
|
|
uint16_t uuid_16[0];
|
|
};
|
|
};
|
|
|
|
const uint8_t max_width_bytes = prv_largest_common_bt_uuid_width(uuids,
|
|
num_uuids);
|
|
// Allocate buffer:
|
|
const size_t buffer_size = sizeof(struct BLEAdElementService) +
|
|
(max_width_bytes * num_uuids);
|
|
uint8_t element_buffer[buffer_size];
|
|
struct BLEAdElementService *element = (struct BLEAdElementService *) element_buffer;
|
|
|
|
// Set header fields (assume Complete):
|
|
switch (max_width_bytes) {
|
|
case 16: element->header.type = BLEAdTypeService128BitUUIDComplete; break;
|
|
case 4: element->header.type = BLEAdTypeService32BitUUIDComplete; break;
|
|
case 2: element->header.type = BLEAdTypeService16BitUUIDComplete; break;
|
|
default:
|
|
WTF;
|
|
}
|
|
|
|
element->header.length = buffer_size - 1 /* -1 Length byte */;
|
|
|
|
// Copy UUIDs:
|
|
for (unsigned int i = 0; i < num_uuids; ++i) {
|
|
switch (max_width_bytes) {
|
|
case 16: element->uuid_128[i] = uuids[i]; break;
|
|
case 4: element->uuid_32[i] = prv_convert_to_32bit_uuid(&uuids[i]); break;
|
|
case 2: element->uuid_16[i] = prv_convert_to_16bit_uuid(&uuids[i]); break;
|
|
default:
|
|
WTF;
|
|
}
|
|
}
|
|
|
|
return prv_write_element_to_ad_data(ad, (const BLEAdElement *) element);
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
bool ble_ad_set_local_name(BLEAdData *ad,
|
|
const char *local_name) {
|
|
if (!local_name) {
|
|
return false;
|
|
}
|
|
const size_t length = strlen(local_name);
|
|
uint8_t element_buffer[sizeof(BLEAdElement) + length];
|
|
BLEAdElement *element = (BLEAdElement *) &element_buffer;
|
|
element->header.length = length + 1 /* +1 Type byte */;
|
|
element->header.type = BLEAdTypeLocalNameComplete; /* assume Complete */
|
|
// Note: *not* zero terminated by design
|
|
memcpy(element->data, local_name, length);
|
|
return prv_write_element_to_ad_data(ad, element);
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
bool ble_ad_set_tx_power_level(BLEAdData *ad) {
|
|
uint8_t element_buffer[sizeof(BLEAdElement) + sizeof(int8_t)];
|
|
BLEAdElement *element = (BLEAdElement *) element_buffer;
|
|
element->header.length = sizeof(int8_t) + 1 /* +1 Type byte */;
|
|
element->header.type = BLEAdTypeTxPowerLevel;
|
|
*((int8_t *) element->data) = sys_ble_get_advertising_tx_power();
|
|
|
|
return prv_write_element_to_ad_data(ad, element);
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
bool ble_ad_set_manufacturer_specific_data(BLEAdData *ad, uint16_t company_id,
|
|
const uint8_t *data, size_t size) {
|
|
struct __attribute__((__packed__)) BLEAdElementManufacturerSpecific {
|
|
BLEAdElementHeader header;
|
|
uint16_t company_id;
|
|
uint8_t data[];
|
|
};
|
|
|
|
uint8_t element_buffer[sizeof(struct BLEAdElementManufacturerSpecific) + size];
|
|
struct BLEAdElementManufacturerSpecific *element =
|
|
(struct BLEAdElementManufacturerSpecific *) element_buffer;
|
|
element->header.length = sizeof(struct BLEAdElementManufacturerSpecific)
|
|
- 1 /* -1 Length byte */ + size;
|
|
element->header.type = BLEAdTypeManufacturerSpecific;
|
|
element->company_id = ltohs(company_id);
|
|
memcpy(element->data, data, size);
|
|
return prv_write_element_to_ad_data(ad, (const BLEAdElement *) element);
|
|
}
|
|
|
|
// -----------------------------------------------------------------------------
|
|
bool ble_ad_set_flags(BLEAdData *ad, uint8_t flags) {
|
|
struct __attribute__((__packed__)) BLEAdElementManufacturerSpecific {
|
|
BLEAdElementHeader header;
|
|
uint8_t flags;
|
|
} element = {
|
|
.header = {
|
|
.length = sizeof(struct BLEAdElementManufacturerSpecific)
|
|
- 1 /* -1 Length byte */,
|
|
.type = BLEAdTypeFlags,
|
|
},
|
|
.flags = flags,
|
|
};
|
|
return prv_write_element_to_ad_data(ad, (const BLEAdElement *) &element);
|
|
}
|