diff --git a/nimble/host/src/ble_audio_broadcast_sink.c b/nimble/host/src/ble_audio_broadcast_sink.c new file mode 100644 index 0000000000..1c55017271 --- /dev/null +++ b/nimble/host/src/ble_audio_broadcast_sink.c @@ -0,0 +1,274 @@ +/* + * 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 "ble_hs_priv.h" +#include "host/ble_gap.h" +#include "host/ble_uuid.h" +#include "host/ble_audio_broadcast_sink.h" +#include "syscfg/syscfg.h" + +#if MYNEWT_VAL(BLE_ISO_BROADCAST_SINK) + +#ifndef min +#define min(a, b) ((a) < (b) ? (a) : (b)) +#endif + +struct uuid_found_status { + ble_uuid_any_t uuid_to_find; + const uint8_t *svc_data; + uint8_t svc_data_len; +}; + +static struct { + uint8_t filter_policy; + ble_uuid_any_t uuid; + uint32_t broadcast_id; + + ble_audio_broadcast_fn *cb; + void *cb_arg; +} broadcast_sink_event_handler; + +static int +find_uuid_func(const struct ble_hs_adv_field *field, void *user_data) +{ + struct uuid_found_status *uuid_find_status = user_data; + uint8_t svc_uuid_type_to_find; + ble_uuid_any_t svc_uuid; + int match_result; + + switch (uuid_find_status->uuid_to_find.u.type) { + case BLE_UUID_TYPE_16: + if (field->type != BLE_HS_ADV_TYPE_SVC_DATA_UUID16) { + return BLE_HS_ENOENT; + } + + match_result = (uuid_find_status->uuid_to_find.u16.value == + get_le16(field->value)); + break; + case BLE_UUID_TYPE_32: + if (field->type != BLE_HS_ADV_TYPE_SVC_DATA_UUID32) { + return BLE_HS_ENOENT; + } + + match_result = (uuid_find_status->uuid_to_find.u32.value == + get_le32(field->value)); + break; + case BLE_UUID_TYPE_128: + if (field->type != BLE_HS_ADV_TYPE_SVC_DATA_UUID128) { + return BLE_HS_ENOENT; + } + match_result = !memcmp(uuid_find_status->uuid_to_find.u128.value, + field->value, 16); + break; + default: + return BLE_HS_EINVAL; + } + + if (match_result) { + uuid_find_status->svc_data = field->value; + uuid_find_status->svc_data_len = field->length; + return 0; + } + + return BLE_HS_ENOENT; +} + +static int +ble_audio_broadcast_sink_gap_event(struct ble_gap_event *event, void *arg) +{ + struct ble_broadcast_event ev; + const struct ble_hs_adv_field *parsed_ad_field; + struct uuid_found_status uuid_find_status = { + .uuid_to_find = broadcast_sink_event_handler.uuid, + .svc_data = NULL, + .svc_data_len = 0 + }; + uint8_t data_offset = 0; + uint8_t subgroup_num_bis; + uint8_t bis_cnt; + uint8_t i, j; + + switch (event->type) { + case BLE_GAP_EVENT_EXT_DISC: + if ((broadcast_sink_event_handler.filter_policy & + BLE_BROADCAST_FILT_USE_UUID)) { + ble_hs_adv_parse(event->ext_disc.data, event->ext_disc.length_data, + find_uuid_func, &uuid_find_status); + + if (!uuid_find_status.svc_data) { + return 0; + } + + if (uuid_find_status.uuid_to_find.u16.value == + BLE_BROADCAST_PUBLIC_BROADCAST_ANNOUNCEMENT_SVC_UUID) { + ev.announcement_found.desc.pbas_features = + uuid_find_status.svc_data[2]; + ev.announcement_found.desc.pbas_metadata_len = + uuid_find_status.svc_data[3]; + ev.announcement_found.desc.pbas_metadata = + uuid_find_status.svc_data + 4; + } + } + uuid_find_status.uuid_to_find.u16.u.type = + BLE_HS_ADV_TYPE_SVC_DATA_UUID16; + uuid_find_status.uuid_to_find.u16.value = + BLE_BROADCAST_AUDIO_ANNOUNCEMENT_SVC_UUID; + uuid_find_status.svc_data = NULL; + uuid_find_status.svc_data_len = 0; + + ble_hs_adv_parse(event->ext_disc.data, + event->ext_disc.length_data, + find_uuid_func, &uuid_find_status); + ev.type = BLE_BROADCAST_EVENT_BAA_FOUND; + ev.announcement_found.desc.broadcast_id = get_le24 + (uuid_find_status.svc_data + 2); + + if ((broadcast_sink_event_handler.filter_policy & + BLE_BROADCAST_FILT_USE_BROADCAST_ID) && + broadcast_sink_event_handler.broadcast_id != + ev.announcement_found.desc.broadcast_id) { + return 0; + } + + ev.announcement_found.desc.sync_info.addr = event->ext_disc.addr; + ev.announcement_found.desc.sync_info.adv_sid = event->ext_disc.sid; + break; + case BLE_GAP_EVENT_DISC_COMPLETE: + ev.type = BLE_BROADCAST_EVENT_DISC_COMPLETE; + ev.disc_complete.reason = event->disc_complete.reason; + break; + case BLE_GAP_EVENT_PERIODIC_SYNC: + ev.type = BLE_BROADCAST_EVENT_SYNC_ESTABLISHED; + ev.sync_established.sync_info.addr = event->periodic_sync.adv_addr; + ev.sync_established.sync_info.adv_sid = event->periodic_sync.sid; + case BLE_GAP_EVENT_PERIODIC_SYNC_LOST: + ev.type = BLE_BROADCAST_EVENT_SYNC_LOST; + ev.sync_lost.sync_handle = event->periodic_sync_lost.sync_handle; + ev.sync_lost.reason = event->periodic_sync_lost.reason; + case BLE_GAP_EVENT_PERIODIC_REPORT: + uuid_find_status.uuid_to_find.u16.u.type = + BLE_HS_ADV_TYPE_SVC_DATA_UUID16; + uuid_find_status.uuid_to_find.u16.value = + BLE_BROADCAST_AUDIO_ANNOUNCEMENT_SVC_UUID; + ble_hs_adv_parse(event->periodic_report.data, + event->periodic_report.data_length, + find_uuid_func, &uuid_find_status); + + ev.type = BLE_BROADCAST_EVENT_BASE_READ; + data_offset += 2; + ev.base_read.base.presentation_delay = + get_le24(uuid_find_status.svc_data + data_offset); + data_offset += 3; + ev.base_read.base.num_subgroups = uuid_find_status.svc_data[data_offset]; + data_offset++; + for (i = 0; i < ev.base_read.base.num_subgroups; i++) { + subgroup_num_bis = uuid_find_status.svc_data[data_offset]; + data_offset++; + ev.base_read.base.subgroup[i].num_bis = subgroup_num_bis; + bis_cnt += subgroup_num_bis; + + if (bis_cnt > MYNEWT_VAL(BLE_MAX_BIS)) { + return BLE_HS_ENOMEM; + } + + memcpy(&ev.base_read.base.subgroup[i].codec_id, + &uuid_find_status.svc_data[data_offset], 5); + data_offset += 5; + ev.base_read.base.subgroup[i].codec_specific_conf_len = + uuid_find_status.svc_data[data_offset]; + data_offset++; + + if (ev.base_read.base.subgroup[i].codec_specific_conf_len > + BLE_AUDIO_BROADCAST_CODEC_SPEC_CONF_MAX_SZ) { + return BLE_HS_EINVAL; + } + + memcpy(ev.base_read.base.subgroup[i].codec_specific_conf, + &uuid_find_status.svc_data[data_offset], + ev.base_read.base.subgroup[i].codec_specific_conf_len); + data_offset += + ev.base_read.base.subgroup[i].codec_specific_conf_len; + + ev.base_read.base.subgroup[i].metadata_len = + uuid_find_status.svc_data[data_offset]; + data_offset++; + + if (ev.base_read.base.subgroup[i].metadata_len > + MYNEWT_VAL(BLE_AUDIO_BROADCAST_PBA_METADATA_MAX_SZ)) { + return BLE_HS_EINVAL; + } + + memcpy(ev.base_read.base.subgroup[i].metadata, + &uuid_find_status.svc_data[data_offset], + ev.base_read.base.subgroup[i].metadata_len); + data_offset += ev.base_read.base.subgroup[i].metadata_len; + + for (j = 0; j < subgroup_num_bis; j++) { + ev.base_read.base.subgroup[i].bis[j].subgroup = i; + ev.base_read.base.subgroup[i].bis[j].bis_idx = + uuid_find_status.svc_data[data_offset]; + data_offset++; + ev.base_read.base.subgroup[i].bis[j].codec_specific_conf_len = + min(uuid_find_status.svc_data[data_offset], + BLE_AUDIO_BROADCAST_CODEC_SPEC_CONF_MAX_SZ); + data_offset++; + memcpy(&ev.base_read.base.subgroup[i].bis[j].codec_specific_conf, + &uuid_find_status.svc_data[data_offset], + min(ev.base_read.base.subgroup[i].bis[j].codec_specific_conf_len, + BLE_AUDIO_BROADCAST_CODEC_SPEC_CONF_MAX_SZ)); + data_offset += 5; + } + } + default: + return 0; + } + broadcast_sink_event_handler.cb(&ev, broadcast_sink_event_handler.cb_arg); + return 0; +} + +int +ble_audio_broadcast_baa_disc(uint8_t own_addr_type, uint16_t duration, + uint8_t filter_policy, + const struct ble_gap_ext_disc_params *uncoded_params, + const struct ble_gap_ext_disc_params *coded_params, + const ble_uuid_t *uuid, + uint32_t broadcast_id, + ble_audio_broadcast_fn *cb, void *cb_arg) +{ + if (filter_policy == BLE_BROADCAST_FILT_USE_UUID && !uuid) { + return BLE_HS_EINVAL; + } + + broadcast_sink_event_handler.cb = cb; + broadcast_sink_event_handler.cb_arg = cb_arg; + broadcast_sink_event_handler.filter_policy = filter_policy; + + if (uuid) { + ble_uuid_copy(&broadcast_sink_event_handler.uuid, uuid); + } + + broadcast_sink_event_handler.broadcast_id = broadcast_id; + + return ble_gap_ext_disc(own_addr_type, duration, 0, 1, + BLE_HCI_CONN_FILT_NO_WL, 0, uncoded_params, + coded_params, ble_audio_broadcast_sink_gap_event, + NULL); +} + +#endif diff --git a/nimble/host/src/ble_iso.c b/nimble/host/src/ble_iso.c index e1115a1d5a..63a17d79e8 100644 --- a/nimble/host/src/ble_iso.c +++ b/nimble/host/src/ble_iso.c @@ -30,6 +30,11 @@ #include "nimble/hci_common.h" #include "sys/queue.h" #include "ble_hs_hci_priv.h" +#include "ble_hs_priv.h" + +#define BLE_ISO_SET_FLAG(flags, flag) (flags |= (1 << flag)) +#define BLE_ISO_GROUP_IDS_IDX (MYNEWT_VAL(BLE_MAX_ISO_GROUPS) / 32 + 1) +#define BLE_ISO_INVALID_CIG_ID (0xFF) struct ble_iso_big { SLIST_ENTRY(ble_iso_big) next; @@ -42,11 +47,65 @@ struct ble_iso_big { void *cb_arg; }; +struct ble_iso_conn { + union { + SLIST_ENTRY(ble_iso_conn) next; + STAILQ_ENTRY(ble_iso_conn) free_conn; + STAILQ_ENTRY(ble_iso_conn) pend_conn; + }; + + /* Common for bis_handles and cis */ + uint8_t id; + uint16_t iso_handle; + uint8_t flags; + ble_iso_event_fn *cb; + void *cb_arg; + + /* CIS related only */ + uint8_t cig_id; + uint16_t acl_handle; + + /*params*/ + uint16_t max_pdu_output; + uint16_t max_pdu_input; + uint16_t seq_num; + uint32_t last_timestamp; +}; + +struct ble_iso_group { + uint8_t id; + uint8_t iso_num; + uint8_t is_broadcast; + ble_iso_event_fn *cb; + void *cb_arg; + SLIST_HEAD(, ble_iso_conn) iso_head; + + union { + SLIST_ENTRY(ble_iso_group) active_group; + STAILQ_ENTRY(ble_iso_group) free_group; + }; + + struct ble_iso_active_group *list; +}; + static SLIST_HEAD(, ble_iso_big) ble_iso_bigs; static struct os_mempool ble_iso_big_pool; static os_membuf_t ble_iso_big_mem[ OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_MAX_BIG), sizeof (struct ble_iso_big))]; +static struct os_mempool ble_iso_conn_pool; +static os_membuf_t ble_iso_conn_elem_mem[ + OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_MAX_ISO_CONNECTIONS), + sizeof (struct ble_iso_conn)) +]; + +SLIST_HEAD(ble_iso_active_group, ble_iso_group); +STAILQ_HEAD(ble_iso_free_groups, ble_iso_group); +static struct ble_iso_free_groups g_ble_iso_free_group_list; +static struct ble_iso_active_group g_ble_iso_group_list; + +static uint32_t ble_iso_group_ids[BLE_ISO_GROUP_IDS_IDX]; + static struct ble_iso_big * ble_iso_big_alloc(uint8_t adv_handle) { @@ -176,12 +235,183 @@ ble_iso_terminate_big(uint8_t big_id) return rc; } +static int +ble_iso_num_of_available_conns(void) +{ + return ble_iso_conn_pool.mp_num_free; +} + +static struct ble_iso_group * +ble_iso_get_new_group_and_put_to_active(struct ble_iso_active_group + *active_list) +{ + struct ble_iso_group *group; + + group = STAILQ_FIRST(&g_ble_iso_free_group_list); + if (group) { + STAILQ_REMOVE_HEAD(&g_ble_iso_free_group_list, free_group); + SLIST_INSERT_HEAD(active_list, group, active_group); + group->list = active_list; + } + + return group; +} + +static int +ble_iso_pick_and_set_group_id(struct ble_iso_group *cig) +{ + int i; + int bit; + + for (i = 0; i < BLE_ISO_GROUP_IDS_IDX; i++) { + bit = __builtin_ffs(~(unsigned int)(ble_iso_group_ids[i])); + if (bit < 32) { + break; + } + } + + if (i == BLE_ISO_GROUP_IDS_IDX) { + return -1; + } + + BLE_ISO_SET_FLAG(ble_iso_group_ids[i], (bit - 1)); + cig->id = (i * 32 + bit -1); + + return cig->id; +} + +static void +ble_iso_release_group(struct ble_iso_group * group) +{ + struct ble_iso_conn *iso_conn; + int idx; + + group->id = BLE_ISO_INVALID_CIG_ID; + + if (group->is_broadcast) { + while ((iso_conn = SLIST_FIRST(&group->iso_head))) { + SLIST_REMOVE_HEAD(&group->iso_head, next); + os_memblock_put(&ble_iso_conn_pool, iso_conn); + } + } + + assert(SLIST_FIRST(&group->iso_head) == NULL); + + SLIST_REMOVE(group->list, group, ble_iso_group, active_group); + STAILQ_INSERT_TAIL(&g_ble_iso_free_group_list, group, free_group); +} + +int +ble_iso_create_big_sync(uint8_t *out_big_handle, uint16_t sync_handle, + bool encrypted, uint8_t *broadcast_code, + uint8_t mse, uint32_t sync_timeout_ms, + uint8_t bis_cnt, uint8_t *bis, + ble_iso_event_fn *cb, void *cb_arg) +{ + struct ble_hci_le_big_create_sync_cp *cmd; + uint8_t cmd_len = sizeof(*cmd) + bis_cnt * sizeof(uint8_t); + uint8_t cmd_buf[cmd_len] ; + struct ble_iso_group *big; + int rc; + + if (bis_cnt < 1) { + BLE_HS_LOG_ERROR("No bis_handles? \n"); + return BLE_HS_EINVAL; + } + + if (encrypted && !broadcast_code) { + BLE_HS_LOG_ERROR("Requested encryption but no broadcast code\n"); + return BLE_HS_EINVAL; + } + + if (ble_iso_num_of_available_conns() < bis_cnt) { + BLE_HS_LOG_ERROR("Requested number of iso conn not available (%d != %d)\n", + bis_cnt, ble_iso_num_of_available_conns()); + return BLE_HS_ENOMEM; + } + big = ble_iso_get_new_group_and_put_to_active(&g_ble_iso_group_list); + if (!big) { + return BLE_HS_ENOMEM; + } + ble_iso_pick_and_set_group_id(big); + big->is_broadcast = true; + big->cb_arg = cb_arg; + big->cb = cb; + + cmd = (struct ble_hci_le_big_create_sync_cp *)cmd_buf; + memset(cmd, 0, cmd_len); + cmd->big_handle = big->id; + cmd->sync_handle = htole16(sync_handle/10); + cmd->encryption = encrypted; + + if (encrypted) { + memcpy(cmd->broadcast_code, broadcast_code, 16); + } + + cmd->sync_timeout = sync_timeout_ms; + cmd->num_bis = bis_cnt; + memcpy(cmd->bis, bis, bis_cnt); + + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_BIG_CREATE_SYNC), + cmd, cmd_len,NULL, 0); + if (rc) { + ble_iso_release_group(big); + return rc; + } + *out_big_handle = big->id; + return 0; +} + +static struct ble_iso_group * +ble_iso_find_group(uint8_t group_id, struct ble_iso_active_group + *active_group_list) +{ + struct ble_iso_group *group; + + ble_hs_lock(); + SLIST_FOREACH(group, active_group_list, active_group) { + if (group_id == group->id) { + ble_hs_unlock(); + return group; + } + } + ble_hs_unlock(); + return NULL; +} + +int +ble_iso_terminate_big_sync(uint8_t big_handle) +{ + struct ble_hci_le_big_terminate_sync_cp cp; + struct ble_iso_group *big; + int rc; + + big = ble_iso_find_group(big_handle, &g_ble_iso_group_list); + if (!big) { + BLE_HS_LOG_ERROR("Could not find big 0x%02x\n", big_handle); + return 0; + } + + cp.big_handle = big_handle; + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_BIG_TERMINATE_SYNC), + &cp, sizeof(cp),NULL, 0); + if (rc == 0) { + ble_iso_release_group(big); + } + + return rc; +} + int ble_iso_init(void) { int rc; SLIST_INIT(&ble_iso_bigs); + SLIST_INIT(&g_ble_iso_group_list); + STAILQ_INIT(&g_ble_iso_free_group_list); rc = os_mempool_init(&ble_iso_big_pool, MYNEWT_VAL(BLE_MAX_BIG),