/*
|
* 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.
|
*/
|
#pragma diag_suppress 111
|
#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;
|
}
|
