diff options
Diffstat (limited to 'src/libs/mynewt-nimble/nimble/host/src/ble_hs_adv.c')
| -rw-r--r-- | src/libs/mynewt-nimble/nimble/host/src/ble_hs_adv.c | 803 |
1 files changed, 803 insertions, 0 deletions
diff --git a/src/libs/mynewt-nimble/nimble/host/src/ble_hs_adv.c b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_adv.c new file mode 100644 index 00000000..1d938b95 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/host/src/ble_hs_adv.c @@ -0,0 +1,803 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you 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 <string.h> +#include <errno.h> +#include "nimble/ble.h" +#include "host/ble_hs_adv.h" +#include "ble_hs_priv.h" + +struct find_field_data { + uint8_t type; + const struct ble_hs_adv_field *field; +}; + +static ble_uuid16_t ble_hs_adv_uuids16[BLE_HS_ADV_MAX_FIELD_SZ / 2]; +static ble_uuid32_t ble_hs_adv_uuids32[BLE_HS_ADV_MAX_FIELD_SZ / 4]; +static ble_uuid128_t ble_hs_adv_uuids128[BLE_HS_ADV_MAX_FIELD_SZ / 16]; + +static int +ble_hs_adv_set_hdr(uint8_t type, uint8_t data_len, uint8_t max_len, + uint8_t *dst, uint8_t *dst_len, struct os_mbuf *om) +{ + int rc; + + if (om ) { + data_len++; + rc = os_mbuf_append(om, &data_len, sizeof(data_len)); + if (rc) { + return rc; + } + + return os_mbuf_append(om, &type, sizeof(type)); + } + + + if (*dst_len + 2 + data_len > max_len) { + return BLE_HS_EMSGSIZE; + } + + dst[*dst_len] = data_len + 1; + dst[*dst_len + 1] = type; + + *dst_len += 2; + + return 0; +} + +static int +ble_hs_adv_set_flat_mbuf(uint8_t type, int data_len, const void *data, + uint8_t *dst, uint8_t *dst_len, uint8_t max_len, + struct os_mbuf *om) +{ + int rc; + + BLE_HS_DBG_ASSERT(data_len > 0); + + rc = ble_hs_adv_set_hdr(type, data_len, max_len, dst, dst_len, om); + if (rc != 0) { + return rc; + } + + if (om) { + return os_mbuf_append(om, data, data_len); + } + + memcpy(dst + *dst_len, data, data_len); + *dst_len += data_len; + + return 0; +} + +int +ble_hs_adv_set_flat(uint8_t type, int data_len, const void *data, + uint8_t *dst, uint8_t *dst_len, uint8_t max_len) +{ +#if !NIMBLE_BLE_ADVERTISE + return BLE_HS_ENOTSUP; +#endif + + return ble_hs_adv_set_flat_mbuf(type, data_len, data, dst, dst_len, max_len, + NULL); +} + +static int +ble_hs_adv_set_array_uuid16(uint8_t type, uint8_t num_elems, + const ble_uuid16_t *elems, uint8_t *dst, + uint8_t *dst_len, uint8_t max_len, + struct os_mbuf *om) +{ + int rc; + int i; + + rc = ble_hs_adv_set_hdr(type, num_elems * 2, max_len, dst, + dst_len, om); + if (rc != 0) { + return rc; + } + + for (i = 0; i < num_elems; i++) { + if (om) { + rc = ble_uuid_to_mbuf(&elems[i].u, om); + if (rc) { + return rc; + } + } else { + ble_uuid_flat(&elems[i].u, dst + *dst_len); + *dst_len += 2; + } + } + + return 0; +} + +static int +ble_hs_adv_set_array_uuid32(uint8_t type, uint8_t num_elems, + const ble_uuid32_t *elems, uint8_t *dst, + uint8_t *dst_len, uint8_t max_len, + struct os_mbuf *om) +{ + uint32_t uuid_le; + int rc; + int i; + + rc = ble_hs_adv_set_hdr(type, num_elems * 4, max_len, dst, + dst_len, om); + if (rc != 0) { + return rc; + } + + for (i = 0; i < num_elems; i++) { + /* We cannot use ble_uuid_flat here since it converts 32-bit UUIDs to + * 128-bit as ATT requires. In AD, 32-bit UUID shall be written as an + * actual 32-bit value. + */ + if (om) { + uuid_le = htole32(elems[i].value); + rc = os_mbuf_append(om, &uuid_le, sizeof(uuid_le)); + if (rc) { + return rc; + } + } else { + put_le32(dst + *dst_len, elems[i].value); + *dst_len += 4; + } + } + + return 0; +} + +static int +ble_hs_adv_set_array_uuid128(uint8_t type, uint8_t num_elems, + const ble_uuid128_t *elems, uint8_t *dst, + uint8_t *dst_len, uint8_t max_len, + struct os_mbuf *om) +{ + int rc; + int i; + + rc = ble_hs_adv_set_hdr(type, num_elems * 16, max_len, dst, + dst_len, om); + if (rc != 0) { + return rc; + } + + for (i = 0; i < num_elems; i++) { + if (om) { + rc = ble_uuid_to_mbuf(&elems[i].u, om); + if (rc) { + return rc; + } + } else { + ble_uuid_flat(&elems[i].u, dst + *dst_len); + *dst_len += 16; + } + } + + return 0; +} + +static int +ble_hs_adv_set_array16(uint8_t type, uint8_t num_elems, const uint16_t *elems, + uint8_t *dst, uint8_t *dst_len, uint8_t max_len, + struct os_mbuf *om) +{ + uint16_t tmp; + int rc; + int i; + + rc = ble_hs_adv_set_hdr(type, num_elems * sizeof *elems, max_len, dst, + dst_len, om); + if (rc != 0) { + return rc; + } + + for (i = 0; i < num_elems; i++) { + if (om) { + tmp = htole16(elems[i]); + rc = os_mbuf_append(om, &tmp, sizeof(tmp)); + if (rc) { + return rc; + } + } else { + put_le16(dst + *dst_len, elems[i]); + *dst_len += sizeof elems[i]; + } + } + + return 0; +} + +static int +adv_set_fields(const struct ble_hs_adv_fields *adv_fields, + uint8_t *dst, uint8_t *dst_len, uint8_t max_len, + struct os_mbuf *om) +{ +#if !NIMBLE_BLE_ADVERTISE + return BLE_HS_ENOTSUP; +#endif + + uint8_t type; + int8_t tx_pwr_lvl; + uint8_t dst_len_local; + int rc; + + dst_len_local = 0; + + /*** 0x01 - Flags. */ + /* The application has two options concerning the flags field: + * 1. Don't include it in advertisements (flags == 0). + * 2. Explicitly specify the value (flags != 0). + * + * Note: The CSS prohibits advertising a flags value of 0, so this method + * of specifying option 1 vs. 2 is sound. + */ + if (adv_fields->flags != 0) { + rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_FLAGS, 1, + &adv_fields->flags, dst, &dst_len_local, + max_len, om); + + if (rc != 0) { + return rc; + } + } + + /*** 0x02,0x03 - 16-bit service class UUIDs. */ + if (adv_fields->uuids16 != NULL && adv_fields->num_uuids16) { + if (adv_fields->uuids16_is_complete) { + type = BLE_HS_ADV_TYPE_COMP_UUIDS16; + } else { + type = BLE_HS_ADV_TYPE_INCOMP_UUIDS16; + } + + rc = ble_hs_adv_set_array_uuid16(type, adv_fields->num_uuids16, + adv_fields->uuids16, dst, &dst_len_local, + max_len, om); + if (rc != 0) { + return rc; + } + } + + /*** 0x04,0x05 - 32-bit service class UUIDs. */ + if (adv_fields->uuids32 != NULL && adv_fields->num_uuids32) { + if (adv_fields->uuids32_is_complete) { + type = BLE_HS_ADV_TYPE_COMP_UUIDS32; + } else { + type = BLE_HS_ADV_TYPE_INCOMP_UUIDS32; + } + + rc = ble_hs_adv_set_array_uuid32(type, adv_fields->num_uuids32, + adv_fields->uuids32, dst, &dst_len_local, + max_len, om); + if (rc != 0) { + return rc; + } + } + + /*** 0x06,0x07 - 128-bit service class UUIDs. */ + if (adv_fields->uuids128 != NULL && adv_fields->num_uuids128 > 0) { + if (adv_fields->uuids128_is_complete) { + type = BLE_HS_ADV_TYPE_COMP_UUIDS128; + } else { + type = BLE_HS_ADV_TYPE_INCOMP_UUIDS128; + } + + rc = ble_hs_adv_set_array_uuid128(type, adv_fields->num_uuids128, + adv_fields->uuids128, dst, &dst_len_local, + max_len, om); + if (rc != 0) { + return rc; + } + } + + /*** 0x08,0x09 - Local name. */ + if (adv_fields->name != NULL && adv_fields->name_len > 0) { + if (adv_fields->name_is_complete) { + type = BLE_HS_ADV_TYPE_COMP_NAME; + } else { + type = BLE_HS_ADV_TYPE_INCOMP_NAME; + } + + rc = ble_hs_adv_set_flat_mbuf(type, adv_fields->name_len, + adv_fields->name, dst, &dst_len_local, max_len, + om); + if (rc != 0) { + return rc; + } + } + + /*** 0x0a - Tx power level. */ + if (adv_fields->tx_pwr_lvl_is_present) { + /* Read the power level from the controller if requested; otherwise use + * the explicitly specified value. + */ + if (adv_fields->tx_pwr_lvl == BLE_HS_ADV_TX_PWR_LVL_AUTO) { + rc = ble_hs_hci_util_read_adv_tx_pwr(&tx_pwr_lvl); + if (rc != 0) { + return rc; + } + } else { + tx_pwr_lvl = adv_fields->tx_pwr_lvl; + } + + rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_TX_PWR_LVL, 1, + &tx_pwr_lvl, dst, &dst_len_local, max_len, om); + if (rc != 0) { + return rc; + } + } + + /*** 0x12 - Slave connection interval range. */ + if (adv_fields->slave_itvl_range != NULL) { + rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_SLAVE_ITVL_RANGE, + BLE_HS_ADV_SLAVE_ITVL_RANGE_LEN, + adv_fields->slave_itvl_range, dst, + &dst_len_local, max_len, om); + if (rc != 0) { + return rc; + } + } + + /*** 0x16 - Service data - 16-bit UUID. */ + if (adv_fields->svc_data_uuid16 != NULL && adv_fields->svc_data_uuid16_len) { + rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_SVC_DATA_UUID16, + adv_fields->svc_data_uuid16_len, + adv_fields->svc_data_uuid16, dst, &dst_len_local, + max_len, om); + if (rc != 0) { + return rc; + } + } + + /*** 0x17 - Public target address. */ + if (adv_fields->public_tgt_addr != NULL && + adv_fields->num_public_tgt_addrs != 0) { + + rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_PUBLIC_TGT_ADDR, + BLE_HS_ADV_PUBLIC_TGT_ADDR_ENTRY_LEN * + adv_fields->num_public_tgt_addrs, + adv_fields->public_tgt_addr, dst, &dst_len_local, + max_len, om); + if (rc != 0) { + return rc; + } + } + + /*** 0x19 - Appearance. */ + if (adv_fields->appearance_is_present) { + rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_APPEARANCE, + BLE_HS_ADV_APPEARANCE_LEN, + &adv_fields->appearance, dst, &dst_len_local, + max_len, om); + if (rc != 0) { + return rc; + } + } + + /*** 0x1a - Advertising interval. */ + if (adv_fields->adv_itvl_is_present) { + rc = ble_hs_adv_set_array16(BLE_HS_ADV_TYPE_ADV_ITVL, 1, + &adv_fields->adv_itvl, dst, &dst_len_local, + max_len, om); + if (rc != 0) { + return rc; + } + } + + /*** 0x20 - Service data - 32-bit UUID. */ + if (adv_fields->svc_data_uuid32 != NULL && adv_fields->svc_data_uuid32_len) { + rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_SVC_DATA_UUID32, + adv_fields->svc_data_uuid32_len, + adv_fields->svc_data_uuid32, dst, &dst_len_local, + max_len, om); + if (rc != 0) { + return rc; + } + } + + /*** 0x21 - Service data - 128-bit UUID. */ + if (adv_fields->svc_data_uuid128 != NULL && adv_fields->svc_data_uuid128_len) { + rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_SVC_DATA_UUID128, + adv_fields->svc_data_uuid128_len, + adv_fields->svc_data_uuid128, dst, + &dst_len_local, max_len, om); + if (rc != 0) { + return rc; + } + } + + /*** 0x24 - URI. */ + if (adv_fields->uri != NULL && adv_fields->uri_len) { + rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_URI, adv_fields->uri_len, + adv_fields->uri, dst, &dst_len_local, max_len, + om); + if (rc != 0) { + return rc; + } + } + + /*** 0xff - Manufacturer specific data. */ + if ((adv_fields->mfg_data != NULL) && (adv_fields->mfg_data_len >= 2)) { + rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_MFG_DATA, + adv_fields->mfg_data_len, + adv_fields->mfg_data, + dst, &dst_len_local, max_len, om); + if (rc != 0) { + return rc; + } + } + + if (dst_len) { + *dst_len = dst_len_local; + } + + return 0; +} + +/** + * Converts a high-level set of fields to a byte buffer. + * + * @return 0 on success; nonzero on failure. + */ +int +ble_hs_adv_set_fields(const struct ble_hs_adv_fields *adv_fields, + uint8_t *dst, uint8_t *dst_len, uint8_t max_len) +{ +#if !NIMBLE_BLE_ADVERTISE + return BLE_HS_ENOTSUP; +#endif + + return adv_set_fields(adv_fields, dst, dst_len, max_len, NULL); +} + +int +ble_hs_adv_set_fields_mbuf(const struct ble_hs_adv_fields *adv_fields, + struct os_mbuf *om) +{ +#if !NIMBLE_BLE_ADVERTISE + return BLE_HS_ENOTSUP; +#endif + return adv_set_fields(adv_fields, NULL, NULL, 0, om); +} + +static int +ble_hs_adv_parse_uuids16(struct ble_hs_adv_fields *adv_fields, + const uint8_t *data, uint8_t data_len) +{ + ble_uuid_any_t uuid; + int i; + + if (data_len % 2 != 0) { + return BLE_HS_EBADDATA; + } + + adv_fields->uuids16 = ble_hs_adv_uuids16; + adv_fields->num_uuids16 = data_len / 2; + + for (i = 0; i < adv_fields->num_uuids16; i++) { + ble_uuid_init_from_buf(&uuid, data + i * 2, 2); + ble_hs_adv_uuids16[i] = uuid.u16; + } + + return 0; +} + +static int +ble_hs_adv_parse_uuids32(struct ble_hs_adv_fields *adv_fields, + const uint8_t *data, uint8_t data_len) +{ + ble_uuid_any_t uuid; + int i; + + if (data_len % 4 != 0) { + return BLE_HS_EBADDATA; + } + + adv_fields->uuids32 = ble_hs_adv_uuids32; + adv_fields->num_uuids32 = data_len / 4; + + for (i = 0; i < adv_fields->num_uuids32; i++) { + ble_uuid_init_from_buf(&uuid, data + i * 4, 4); + ble_hs_adv_uuids32[i] = uuid.u32; + } + + return 0; +} + +static int +ble_hs_adv_parse_uuids128(struct ble_hs_adv_fields *adv_fields, + const uint8_t *data, uint8_t data_len) +{ + ble_uuid_any_t uuid; + int i; + + if (data_len % 16 != 0) { + return BLE_HS_EBADDATA; + } + + adv_fields->uuids128 = ble_hs_adv_uuids128; + adv_fields->num_uuids128 = data_len / 16; + + for (i = 0; i < adv_fields->num_uuids128; i++) { + ble_uuid_init_from_buf(&uuid, data + i * 16, 16); + ble_hs_adv_uuids128[i] = uuid.u128; + } + + return 0; +} + +static int +ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields, + uint8_t *total_len, const uint8_t *src, + uint8_t src_len) +{ + uint8_t data_len; + uint8_t type; + const uint8_t *data; + int rc; + + if (src_len < 1) { + return BLE_HS_EMSGSIZE; + } + *total_len = src[0] + 1; + + if (src_len < *total_len) { + return BLE_HS_EMSGSIZE; + } + + type = src[1]; + data = src + 2; + data_len = *total_len - 2; + + if (data_len > BLE_HS_ADV_MAX_FIELD_SZ) { + return BLE_HS_EBADDATA; + } + + switch (type) { + case BLE_HS_ADV_TYPE_FLAGS: + if (data_len != BLE_HS_ADV_FLAGS_LEN) { + return BLE_HS_EBADDATA; + } + adv_fields->flags = *data; + break; + + case BLE_HS_ADV_TYPE_INCOMP_UUIDS16: + rc = ble_hs_adv_parse_uuids16(adv_fields, data, data_len); + if (rc != 0) { + return rc; + } + adv_fields->uuids16_is_complete = 0; + break; + + case BLE_HS_ADV_TYPE_COMP_UUIDS16: + rc = ble_hs_adv_parse_uuids16(adv_fields, data, data_len); + if (rc != 0) { + return rc; + } + adv_fields->uuids16_is_complete = 1; + break; + + case BLE_HS_ADV_TYPE_INCOMP_UUIDS32: + rc = ble_hs_adv_parse_uuids32(adv_fields, data, data_len); + if (rc != 0) { + return rc; + } + adv_fields->uuids32_is_complete = 0; + break; + + case BLE_HS_ADV_TYPE_COMP_UUIDS32: + rc = ble_hs_adv_parse_uuids32(adv_fields, data, data_len); + if (rc != 0) { + return rc; + } + adv_fields->uuids32_is_complete = 1; + break; + + case BLE_HS_ADV_TYPE_INCOMP_UUIDS128: + rc = ble_hs_adv_parse_uuids128(adv_fields, data, data_len); + if (rc != 0) { + return rc; + } + adv_fields->uuids128_is_complete = 0; + break; + + case BLE_HS_ADV_TYPE_COMP_UUIDS128: + rc = ble_hs_adv_parse_uuids128(adv_fields, data, data_len); + if (rc != 0) { + return rc; + } + adv_fields->uuids128_is_complete = 1; + break; + + case BLE_HS_ADV_TYPE_INCOMP_NAME: + adv_fields->name = data; + adv_fields->name_len = data_len; + adv_fields->name_is_complete = 0; + break; + + case BLE_HS_ADV_TYPE_COMP_NAME: + adv_fields->name = data; + adv_fields->name_len = data_len; + adv_fields->name_is_complete = 1; + break; + + case BLE_HS_ADV_TYPE_TX_PWR_LVL: + if (data_len != BLE_HS_ADV_TX_PWR_LVL_LEN) { + return BLE_HS_EBADDATA; + } + adv_fields->tx_pwr_lvl = *data; + adv_fields->tx_pwr_lvl_is_present = 1; + break; + + case BLE_HS_ADV_TYPE_SLAVE_ITVL_RANGE: + if (data_len != BLE_HS_ADV_SLAVE_ITVL_RANGE_LEN) { + return BLE_HS_EBADDATA; + } + adv_fields->slave_itvl_range = data; + break; + + case BLE_HS_ADV_TYPE_SVC_DATA_UUID16: + if (data_len < BLE_HS_ADV_SVC_DATA_UUID16_MIN_LEN) { + return BLE_HS_EBADDATA; + } + adv_fields->svc_data_uuid16 = data; + adv_fields->svc_data_uuid16_len = data_len; + break; + + case BLE_HS_ADV_TYPE_PUBLIC_TGT_ADDR: + if (data_len % BLE_HS_ADV_PUBLIC_TGT_ADDR_ENTRY_LEN != 0) { + return BLE_HS_EBADDATA; + } + adv_fields->public_tgt_addr = data; + adv_fields->num_public_tgt_addrs = + data_len / BLE_HS_ADV_PUBLIC_TGT_ADDR_ENTRY_LEN; + break; + + case BLE_HS_ADV_TYPE_APPEARANCE: + if (data_len != BLE_HS_ADV_APPEARANCE_LEN) { + return BLE_HS_EBADDATA; + } + adv_fields->appearance = get_le16(data); + adv_fields->appearance_is_present = 1; + break; + + case BLE_HS_ADV_TYPE_ADV_ITVL: + if (data_len != BLE_HS_ADV_ADV_ITVL_LEN) { + return BLE_HS_EBADDATA; + } + adv_fields->adv_itvl = get_le16(data); + adv_fields->adv_itvl_is_present = 1; + break; + + case BLE_HS_ADV_TYPE_SVC_DATA_UUID32: + if (data_len < BLE_HS_ADV_SVC_DATA_UUID32_MIN_LEN) { + return BLE_HS_EBADDATA; + } + adv_fields->svc_data_uuid32 = data; + adv_fields->svc_data_uuid32_len = data_len; + break; + + case BLE_HS_ADV_TYPE_SVC_DATA_UUID128: + if (data_len < BLE_HS_ADV_SVC_DATA_UUID128_MIN_LEN) { + return BLE_HS_EBADDATA; + } + adv_fields->svc_data_uuid128 = data; + adv_fields->svc_data_uuid128_len = data_len; + break; + + case BLE_HS_ADV_TYPE_URI: + adv_fields->uri = data; + adv_fields->uri_len = data_len; + break; + + case BLE_HS_ADV_TYPE_MFG_DATA: + adv_fields->mfg_data = data; + adv_fields->mfg_data_len = data_len; + break; + + default: + break; + } + + return 0; +} + +int +ble_hs_adv_parse_fields(struct ble_hs_adv_fields *adv_fields, + const uint8_t *src, uint8_t src_len) +{ + uint8_t field_len; + int rc; + + memset(adv_fields, 0, sizeof *adv_fields); + + while (src_len > 0) { + rc = ble_hs_adv_parse_one_field(adv_fields, &field_len, src, src_len); + if (rc != 0) { + return rc; + } + + src += field_len; + src_len -= field_len; + } + + return 0; +} + +int +ble_hs_adv_parse(const uint8_t *data, uint8_t length, + ble_hs_adv_parse_func_t func, void *user_data) +{ + const struct ble_hs_adv_field *field; + + while (length > 1) { + field = (const void *) data; + + if (field->length >= length) { + return BLE_HS_EBADDATA; + } + + if (func(field, user_data) == 0) { + return 0; + } + + length -= 1 + field->length; + data += 1 + field->length; + } + + return 0; +} + +static int +find_field_func(const struct ble_hs_adv_field *field, void *user_data) +{ + struct find_field_data *ffd = user_data; + + if (field->type != ffd->type) { + return BLE_HS_EAGAIN; + } + + ffd->field = field; + + return 0; +} + +int +ble_hs_adv_find_field(uint8_t type, const uint8_t *data, uint8_t length, + const struct ble_hs_adv_field **out) +{ + int rc; + struct find_field_data ffd = { + .type = type, + .field = NULL, + }; + + rc = ble_hs_adv_parse(data, length, find_field_func, &ffd); + if (rc != 0) { + return rc; + } + + if (!ffd.field) { + return BLE_HS_ENOENT; + } + + *out = ffd.field; + + return 0; +} |