diff --git a/samples/bluetooth/hap_ha/CMakeLists.txt b/samples/bluetooth/hap_ha/CMakeLists.txt index 3e594ca5ba3c9..a824c7251b9ea 100644 --- a/samples/bluetooth/hap_ha/CMakeLists.txt +++ b/samples/bluetooth/hap_ha/CMakeLists.txt @@ -9,6 +9,8 @@ target_sources(app PRIVATE src/has_server.c src/main.c src/vcp_vol_renderer.c + src/bap_broadcast_snk.c + src/stream_rx.c ) target_sources_ifdef(CONFIG_HAP_HA_HEARING_AID_BINAURAL app PRIVATE diff --git a/samples/bluetooth/hap_ha/Kconfig b/samples/bluetooth/hap_ha/Kconfig index 358380b96419f..23c4dd081ba58 100644 --- a/samples/bluetooth/hap_ha/Kconfig +++ b/samples/bluetooth/hap_ha/Kconfig @@ -49,4 +49,4 @@ config HAP_HA_HEARING_AID_RIGHT endchoice # HAP_HA_HEARING_AID_LOCATION -endmenu +endmenu diff --git a/samples/bluetooth/hap_ha/prj.conf b/samples/bluetooth/hap_ha/prj.conf index 964280e8d9413..cb4a781ba0488 100644 --- a/samples/bluetooth/hap_ha/prj.conf +++ b/samples/bluetooth/hap_ha/prj.conf @@ -73,4 +73,18 @@ CONFIG_BT_BAS_BLS_IDENTIFIER_PRESENT=y CONFIG_BT_BAS_BLS_BATTERY_LEVEL_PRESENT=y CONFIG_BT_BAS_BLS_ADDITIONAL_STATUS_PRESENT=y + +# Broadcast Sink +CONFIG_BT_OBSERVER=y +CONFIG_BT_PER_ADV_SYNC=y +CONFIG_BT_BAP_BROADCAST_SINK=y +CONFIG_BT_BAP_SCAN_DELEGATOR=y +CONFIG_BT_BAP_BROADCAST_SNK_SUBGROUP_COUNT=2 +CONFIG_BT_BAP_BROADCAST_SNK_STREAM_COUNT=2 +CONFIG_BT_BAP_BASS_MAX_SUBGROUPS=4 +CONFIG_BT_ISO_RX_BUF_COUNT=4 +CONFIG_BT_ISO_SYNC_RECEIVER=y + CONFIG_LOG=y +CONFIG_BT_BUF_ACL_RX_SIZE=255 +CONFIG_BT_BUF_ACL_TX_SIZE=251 diff --git a/samples/bluetooth/hap_ha/src/bap_broadcast_snk.c b/samples/bluetooth/hap_ha/src/bap_broadcast_snk.c new file mode 100644 index 0000000000000..a37e50416f6b0 --- /dev/null +++ b/samples/bluetooth/hap_ha/src/bap_broadcast_snk.c @@ -0,0 +1,1024 @@ +/** @file + * @brief Bluetooth Basic Audio Profile (BAP) broadcast sink. + * + * Copyright (c) 2022-2024 Nordic Semiconductor ASA + * Copyright (c) 2024-2025 Demant A/S + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "bap_internal.h" +#include "stream_rx.h" + +#define SEM_TIMEOUT K_SECONDS(60) +#define BROADCAST_ASSISTANT_TIMEOUT K_SECONDS(120) +#define ADV_TIMEOUT K_FOREVER +#define PA_SYNC_INTERVAL_TO_TIMEOUT_RATIO 10 /* Set the timeout relative to interval */ +#define PA_SYNC_SKIP 10 +#define BAP_BRAODCAST_SNK_THREAD_STACK_SIZE 2048 +#define THREAD_PRIORITY 5 + +K_THREAD_STACK_DEFINE(snk_thread_stack, BAP_BRAODCAST_SNK_THREAD_STACK_SIZE) +struct k_thread snk_tid; + +/*Broadcast Sink Semaphores. */ +static K_SEM_DEFINE(sem_broadcast_sink_stopped, 0U, 1U); +static K_SEM_DEFINE(sem_connected, 0U, 1U); +static K_SEM_DEFINE(sem_disconnected, 0U, 1U); +static K_SEM_DEFINE(sem_broadcaster_found, 0U, 1U); +static K_SEM_DEFINE(sem_pa_synced, 0U, 1U); +static K_SEM_DEFINE(sem_base_received, 0U, 1U); +static K_SEM_DEFINE(sem_syncable, 0U, 1U); +static K_SEM_DEFINE(sem_pa_sync_lost, 0U, 1U); +static K_SEM_DEFINE(sem_broadcast_code_received, 0U, 1U); +static K_SEM_DEFINE(sem_pa_request, 0U, 1U); +static K_SEM_DEFINE(sem_past_request, 0U, 1U); +static K_SEM_DEFINE(sem_bis_sync_requested, 0U, 1U); +static K_SEM_DEFINE(sem_stream_connected, 0U, CONFIG_BT_BAP_BROADCAST_SNK_STREAM_COUNT); +static K_SEM_DEFINE(sem_stream_started, 0U, CONFIG_BT_BAP_BROADCAST_SNK_STREAM_COUNT); +static K_SEM_DEFINE(sem_big_synced, 0U, 1U); + + +/* Sample assumes that we only have a single Scan Delegator receive state */ +static const struct bt_bap_scan_delegator_recv_state *req_recv_state; +static struct bt_bap_broadcast_sink *broadcast_sink; +static struct bt_le_scan_recv_info broadcaster_info; +static bt_addr_le_t broadcaster_addr; +static struct bt_le_per_adv_sync *pa_sync; +static uint32_t broadcaster_broadcast_id; +static volatile bool big_synced; +static volatile bool base_received; +static struct bt_bap_stream *bap_streams_p[CONFIG_BT_BAP_BROADCAST_SNK_STREAM_COUNT]; + + + + +/** + * The base_recv_cb() function will populate struct bis_audio_allocation with channel allocation + * information for a BIS. + * + * The valid value is false if no valid allocation exists. + */ +struct bis_audio_allocation { + bool valid; + enum bt_audio_location value; +}; + +/** + * The base_recv_cb() function will populate struct base_subgroup_data with the BIS index and + * channel allocation information for each BIS in the subgroup. + * + * The bis_index_bitfield is a bitfield where each bit represents a BIS index. The + * first bit (bit 0) represents BIS index 1, the second bit (bit 1) represents BIS index 2, + * and so on. + * + * The audio_allocation array holds the channel allocation information for each BIS in the + * subgroup. The first element (index 0) is not used (BIS index 0 does not exist), the second + * element (index 1) corresponds to BIS index 1, and so on. + */ +struct base_subgroup_data { + uint32_t bis_index_bitfield; + struct bis_audio_allocation + audio_allocation[BT_ISO_BIS_INDEX_MAX + 1]; /* First BIS index is 1 */ +}; + +/** + * The base_recv_cb() function will populate struct base_data with BIS data + * for each subgroup. + * + * The subgroup_cnt is the number of subgroups in the BASE. + */ +struct base_data { + struct base_subgroup_data subgroup_bis[CONFIG_BT_BAP_BASS_MAX_SUBGROUPS]; + uint8_t subgroup_cnt; +}; + +static struct base_data base_recv_data; /* holds data from base_recv_cb */ +static uint32_t + requested_bis_sync[CONFIG_BT_BAP_BASS_MAX_SUBGROUPS]; /* holds data from bis_sync_req_cb */ +static uint8_t sink_broadcast_code[BT_ISO_BROADCAST_CODE_SIZE]; + +static uint8_t get_stream_count(uint32_t bitfield); + + +static void stream_connected_cb(struct bt_bap_stream *bap_stream) +{ + printk("Stream %p connected\n", bap_stream); + + k_sem_give(&sem_stream_connected); +} + +static void stream_disconnected_cb(struct bt_bap_stream *bap_stream, uint8_t reason) +{ + int err; + + printk("Stream %p disconnected with reason 0x%02X\n", bap_stream, reason); + + err = k_sem_take(&sem_stream_connected, K_NO_WAIT); + if (err != 0) { + printk("Failed to take sem_stream_connected: %d\n", err); + } +} + +static void stream_started_cb(struct bt_bap_stream *bap_stream) +{ + printk("Stream Started\n"); + k_sem_give(&sem_stream_started); +} + +static void stream_stopped_cb(struct bt_bap_stream *bap_stream, uint8_t reason) +{ + int err; + + printk("Stream %p stopped\n", bap_stream); + + err = k_sem_take(&sem_stream_started, K_NO_WAIT); + if (err != 0) { + printk("Failed to take sem_stream_started: %d\n", err); + } +} + +static void stream_recv_cb(struct bt_bap_stream *bap_stream, const struct bt_iso_recv_info *info, + struct net_buf *buf) +{ + printk("%s\n", __func__); +} + +static struct bt_bap_stream_ops stream_ops = { + .connected = stream_connected_cb, + .disconnected = stream_disconnected_cb, + .started = stream_started_cb, + .stopped = stream_stopped_cb, + .recv = stream_recv_cb, +}; + +/** + * This is called for each BIS in a subgroup + * + * Gets BIS channel allocation (if exists). + * Always returns `true` to continue to next BIS + */ +static bool bis_get_channel_allocation_cb(const struct bt_bap_base_subgroup_bis *bis, + void *user_data) +{ + struct base_subgroup_data *base_subgroup_bis = user_data; + struct bt_audio_codec_cfg codec_cfg; + int err; + + printk("%s: for each BIS in a subgroup\n", __func__); + + err = bt_bap_base_subgroup_bis_codec_to_codec_cfg(bis, &codec_cfg); + if (err != 0) { + printk("Could not get codec configuration for BIS: %d\n", err); + + return true; /* continue to next BIS */ + } + + err = bt_audio_codec_cfg_get_chan_allocation( + &codec_cfg, &base_subgroup_bis->audio_allocation[bis->index].value, true); + if (err != 0) { + printk("Could not find channel allocation for BIS: %d\n", err); + + return true; /* continue to next BIS */ + } + + /* Channel allocation data available for this bis */ + base_subgroup_bis->audio_allocation[bis->index].valid = true; + + return true; /* continue to next BIS */ +} + +/** + * Called for each subgroup in the BASE. Will populate the struct base_subgroup_data variable with + * BIS index and channel allocation information. + * + * The channel allocation may + * - Not exist at all, implicitly meaning BT_AUDIO_LOCATION_MONO_AUDIO + * - Exist only in the subgroup codec configuration + * - Exist only in the BIS codec configuration + * - Exist in both the subgroup and BIS codec configuration, in which case, the BIS codec + * configuration overwrites the subgroup values + */ +static bool subgroup_get_valid_bis_indexes_cb(const struct bt_bap_base_subgroup *subgroup, + void *user_data) +{ + enum bt_audio_location subgroup_chan_allocation; + bool subgroup_chan_allocation_available = false; + struct base_data *data = user_data; + struct base_subgroup_data *base_subgroup_bis = &data->subgroup_bis[data->subgroup_cnt]; + struct bt_audio_codec_cfg codec_cfg; + int err; + + printk("\n%s\n", __func__); + + err = bt_bap_base_subgroup_codec_to_codec_cfg(subgroup, &codec_cfg); + if (err != 0) { + printk("Could not get codec configuration: %d\n", err); + goto next_subgroup; + } + + if (codec_cfg.id != BT_HCI_CODING_FORMAT_LC3) { + printk("Only LC3 codec supported (%u)\n", codec_cfg.id); + goto next_subgroup; + } + + /* Get all BIS indexes for subgroup */ + err = bt_bap_base_subgroup_get_bis_indexes(subgroup, + &base_subgroup_bis->bis_index_bitfield); + if (err != 0) { + printk("Failed to parse all BIS in subgroup: %d\n", err); + goto next_subgroup; + } + + /* Get channel allocation at subgroup level */ + err = bt_audio_codec_cfg_get_chan_allocation(&codec_cfg, &subgroup_chan_allocation, true); + if (err == 0) { + printk("Channel allocation (subgroup level) 0x%08x\n", subgroup_chan_allocation); + subgroup_chan_allocation_available = true; + } else { + printk("Subgroup error chan allocation error: %d\n", err); + goto next_subgroup; + } + + /* Get channel allocation at BIS level */ + err = bt_bap_base_subgroup_foreach_bis(subgroup, bis_get_channel_allocation_cb, + base_subgroup_bis); + if (err != 0) { + printk("Get channel allocation error (BIS level) %d\n", err); + goto next_subgroup; + } + + /* If no BIS channel allocation available use subgroup channel allocation instead if + * exists (otherwise mono assumed) + */ + for (uint8_t idx = 1U; idx <= BT_ISO_BIS_INDEX_MAX; idx++) { + if (base_subgroup_bis->bis_index_bitfield & BT_ISO_BIS_INDEX_BIT(idx)) { + if (!base_subgroup_bis->audio_allocation[idx].valid) { + base_subgroup_bis->audio_allocation[idx].value = + subgroup_chan_allocation_available + ? subgroup_chan_allocation + : BT_AUDIO_LOCATION_MONO_AUDIO; + base_subgroup_bis->audio_allocation[idx].valid = true; + } + printk("BIS index 0x%08x allocation = 0x%08x\n", idx, + base_subgroup_bis->audio_allocation[idx].value); + } + } + +next_subgroup: + data->subgroup_cnt++; + + return true; /* continue to next subgroup */ +} + +static void base_recv_cb(struct bt_bap_broadcast_sink *sink, const struct bt_bap_base *base, + size_t base_size) +{ + int err; + + if (base_received) { + return; + } + + printk("Received BASE with %d subgroups from broadcast sink %p\n", + bt_bap_base_get_subgroup_count(base), sink); + + (void)memset(&base_recv_data, 0, sizeof(base_recv_data)); + + /* Get BIS index data for each subgroup */ + err = bt_bap_base_foreach_subgroup(base, subgroup_get_valid_bis_indexes_cb, + &base_recv_data); + if (err != 0) { + printk("Failed to get valid BIS indexes: %d\n", err); + + return; + } + + if (!bap_unicast_sr_has_connection()) { + /* No broadcast assistant requesting anything */ + for (int i = 0; i < CONFIG_BT_BAP_BASS_MAX_SUBGROUPS; i++) { + requested_bis_sync[i] = BT_BAP_BIS_SYNC_NO_PREF; + } + k_sem_give(&sem_bis_sync_requested); + } + + base_received = true; + k_sem_give(&sem_base_received); +} + +static void syncable_cb(struct bt_bap_broadcast_sink *sink, const struct bt_iso_biginfo *biginfo) +{ + printk("Broadcast sink (%p) is syncable, BIG %s\n", (void *)sink, + biginfo->encryption ? "encrypted" : "not encrypted"); + + k_sem_give(&sem_syncable); + + if (!biginfo->encryption) { + k_sem_give(&sem_broadcast_code_received); + } +} + +static void broadcast_sink_started_cb(struct bt_bap_broadcast_sink *sink) +{ + printk("Broadcast sink %p started\n", sink); + + big_synced = true; + k_sem_give(&sem_big_synced); +} + +static void broadcast_sink_stopped_cb(struct bt_bap_broadcast_sink *sink, uint8_t reason) +{ + printk("Broadcast sink %p stopped with reason 0x%02X\n", sink, reason); + + big_synced = false; + k_sem_give(&sem_broadcast_sink_stopped); +} + +static struct bt_bap_broadcast_sink_cb broadcast_sink_cbs = { + .base_recv = base_recv_cb, + .syncable = syncable_cb, + .started = broadcast_sink_started_cb, + .stopped = broadcast_sink_stopped_cb, +}; + +/* SCAN DELEGATOR */ +static void pa_timer_handler(struct k_work *work) +{ + if (req_recv_state != NULL) { + enum bt_bap_pa_state pa_state; + + if (req_recv_state->pa_sync_state == BT_BAP_PA_STATE_INFO_REQ) { + pa_state = BT_BAP_PA_STATE_NO_PAST; + } else { + pa_state = BT_BAP_PA_STATE_FAILED; + } + + bt_bap_scan_delegator_set_pa_state(req_recv_state->src_id, + pa_state); + } + + printk("PA timeout\n"); +} + + +static K_WORK_DELAYABLE_DEFINE(pa_timer, pa_timer_handler); + +static uint16_t interval_to_sync_timeout(uint16_t pa_interval) +{ + uint16_t pa_timeout; + + if (pa_interval == BT_BAP_PA_INTERVAL_UNKNOWN) { + /* Use maximum value to maximize chance of success */ + pa_timeout = BT_GAP_PER_ADV_MAX_TIMEOUT; + } else { + uint32_t interval_us; + uint32_t timeout; + + /* Add retries and convert to unit in 10's of ms */ + interval_us = BT_GAP_PER_ADV_INTERVAL_TO_US(pa_interval); + timeout = BT_GAP_US_TO_PER_ADV_SYNC_TIMEOUT(interval_us) * + PA_SYNC_INTERVAL_TO_TIMEOUT_RATIO; + + /* Enforce restraints */ + pa_timeout = CLAMP(timeout, BT_GAP_PER_ADV_MIN_TIMEOUT, BT_GAP_PER_ADV_MAX_TIMEOUT); + } + + return pa_timeout; +} + +static int pa_sync_past(struct bt_conn *conn, uint16_t pa_interval) +{ + struct bt_le_per_adv_sync_transfer_param param = { 0 }; + int err; + + param.skip = PA_SYNC_SKIP; + param.timeout = interval_to_sync_timeout(pa_interval); + + err = bt_le_per_adv_sync_transfer_subscribe(conn, ¶m); + if (err != 0) { + printk("Could not do PAST subscribe: %d\n", err); + } else { + printk("Syncing with PAST\n"); + (void)k_work_reschedule(&pa_timer, K_MSEC(param.timeout * 10)); + } + + return err; +} + +static void recv_state_updated_cb(struct bt_conn *conn, + const struct bt_bap_scan_delegator_recv_state *recv_state) +{ + printk("Receive state updated, pa sync state: %u, encrypt_state %u\n", + recv_state->pa_sync_state, recv_state->encrypt_state); + + for (uint8_t i = 0; i < recv_state->num_subgroups; i++) { + printk("subgroup %d bis_sync: 0x%08x\n", i, recv_state->subgroups[i].bis_sync); + } + + req_recv_state = recv_state; +} + +static int pa_sync_req_cb(struct bt_conn *conn, + const struct bt_bap_scan_delegator_recv_state *recv_state, + bool past_avail, uint16_t pa_interval) +{ + + struct bt_conn_info info; + + bt_conn_get_info(conn, &info); + printk("PA sync from conn id: %d\n", info.id); + + printk("Received request to sync to PA (PAST %savailble): %u\n", past_avail ? "" : "not ", + recv_state->pa_sync_state); + + req_recv_state = recv_state; + + if (recv_state->pa_sync_state == BT_BAP_PA_STATE_SYNCED || + recv_state->pa_sync_state == BT_BAP_PA_STATE_INFO_REQ) { + /* Already syncing */ + /* TODO: Terminate existing sync and then sync to new?*/ + return -1; + } + + if (IS_ENABLED(CONFIG_BT_PER_ADV_SYNC_TRANSFER_RECEIVER) && past_avail) { + int err; + + err = pa_sync_past(conn, pa_interval); + if (err != 0) { + printk("Failed to subscribe to PAST: %d\n", err); + + return err; + } + + k_sem_give(&sem_past_request); + + err = bt_bap_scan_delegator_set_pa_state(recv_state->src_id, + BT_BAP_PA_STATE_INFO_REQ); + if (err != 0) { + printk("Failed to set PA state to BT_BAP_PA_STATE_INFO_REQ: %d\n", err); + + return err; + } + } + + k_sem_give(&sem_pa_request); + + return 0; +} + +static int pa_sync_term_req_cb(struct bt_conn *conn, + const struct bt_bap_scan_delegator_recv_state *recv_state) +{ + int err; + + printk("PA sync termination req, pa sync state: %u\n", recv_state->pa_sync_state); + + for (uint8_t i = 0; i < recv_state->num_subgroups; i++) { + printk("subgroup %d bis_sync: 0x%08x\n", i, recv_state->subgroups[i].bis_sync); + } + + req_recv_state = recv_state; + + printk("Delete periodic advertising sync\n"); + err = bt_le_per_adv_sync_delete(pa_sync); + if (err != 0) { + printk("Could not delete per adv sync: %d\n", err); + + return err; + } + + return 0; +} + +static void broadcast_code_cb(struct bt_conn *conn, + const struct bt_bap_scan_delegator_recv_state *recv_state, + const uint8_t broadcast_code[BT_ISO_BROADCAST_CODE_SIZE]) +{ + printk("Broadcast code received for %p\n", recv_state); + + req_recv_state = recv_state; + + (void)memcpy(sink_broadcast_code, broadcast_code, BT_ISO_BROADCAST_CODE_SIZE); + + k_sem_give(&sem_broadcast_code_received); +} + +static int bis_sync_req_cb(struct bt_conn *conn, + const struct bt_bap_scan_delegator_recv_state *recv_state, + const uint32_t bis_sync_req[CONFIG_BT_BAP_BASS_MAX_SUBGROUPS]) +{ + bool sync_req = false; + bool bis_sync_req_no_pref = true; + uint8_t subgroup_sync_req_cnt = 0; + uint32_t bis_sync_req_bitfield = 0; + + (void)memset(requested_bis_sync, 0, sizeof(requested_bis_sync)); + + for (uint8_t subgroup = 0; subgroup < recv_state->num_subgroups; subgroup++) { + + printk("bis_sync_req[%u] = 0x%0x\n", subgroup, bis_sync_req[subgroup]); + if (bis_sync_req[subgroup] != 0) { + requested_bis_sync[subgroup] = bis_sync_req[subgroup]; + if (bis_sync_req[subgroup] != BT_BAP_BIS_SYNC_NO_PREF) { + bis_sync_req_no_pref = false; + } + bis_sync_req_bitfield |= bis_sync_req[subgroup]; + subgroup_sync_req_cnt++; + sync_req = true; + } + } + + if (!bis_sync_req_no_pref) { + uint8_t stream_count = get_stream_count(bis_sync_req_bitfield); + + /* We only want to sync to a single subgroup. If no preference is given, we will + * later set the first possible subgroup as the one to sync to. + */ + if (subgroup_sync_req_cnt > 1U) { + printk("Only request sync to 1 subgroup!\n"); + + return -EINVAL; + } + + if (stream_count > CONFIG_BT_BAP_BROADCAST_SNK_STREAM_COUNT) { + printk("Too many BIS requested for sync: %u > %d\n", stream_count, + CONFIG_BT_BAP_BROADCAST_SNK_STREAM_COUNT); + + return -EINVAL; + } + } + + printk("BIS sync req for %p, broadcast id: 0x%06x, (%s)\n", recv_state, + recv_state->broadcast_id, big_synced ? "BIG synced" : "BIG not synced"); + + if (big_synced) { + int err; + + if (sync_req) { + printk("Already synced!\n"); + + return -EINVAL; + } + + /* The stream stopped callback will be called as part of this, + * and we do not need to wait for any events from the + * controller. Thus, when this returns, the `big_synced` + * is back to false. + */ + err = bt_bap_broadcast_sink_stop(broadcast_sink); + if (err != 0) { + printk("Failed to stop Broadcast Sink: %d\n", err); + + return err; + } + } + + broadcaster_broadcast_id = recv_state->broadcast_id; + if (sync_req) { + k_sem_give(&sem_bis_sync_requested); + } + + return 0; +} + +static struct bt_bap_scan_delegator_cb scan_delegator_cbs = { + .recv_state_updated = recv_state_updated_cb, + .pa_sync_req = pa_sync_req_cb, + .pa_sync_term_req = pa_sync_term_req_cb, + .broadcast_code = broadcast_code_cb, + .bis_sync_req = bis_sync_req_cb, +}; + +static bool scan_check_and_sync_broadcast(struct bt_data *data, void *user_data) +{ + const struct bt_le_scan_recv_info *info = user_data; + char le_addr[BT_ADDR_LE_STR_LEN]; + struct bt_uuid_16 adv_uuid; + uint32_t broadcast_id; + + if (data->type != BT_DATA_SVC_DATA16) { + return true; + } + + if (data->data_len < BT_UUID_SIZE_16 + BT_AUDIO_BROADCAST_ID_SIZE) { + return true; + } + + if (!bt_uuid_create(&adv_uuid.uuid, data->data, BT_UUID_SIZE_16)) { + return true; + } + + if (bt_uuid_cmp(&adv_uuid.uuid, BT_UUID_BROADCAST_AUDIO)) { + return true; + } + + broadcast_id = sys_get_le24(data->data + BT_UUID_SIZE_16); + + bt_addr_le_to_str(info->addr, le_addr, sizeof(le_addr)); + + printk("Found broadcaster with ID 0x%06X and addr %s and sid 0x%02X\n", broadcast_id, + le_addr, info->sid); + + if (!bap_unicast_sr_has_connection()/* Not requested by Broadcast Assistant */ || + (req_recv_state != NULL && bt_addr_le_eq(info->addr, &req_recv_state->addr) && + info->sid == req_recv_state->adv_sid && + broadcast_id == req_recv_state->broadcast_id)) { + + /* Store info for PA sync parameters */ + memcpy(&broadcaster_info, info, sizeof(broadcaster_info)); + bt_addr_le_copy(&broadcaster_addr, info->addr); + broadcaster_broadcast_id = broadcast_id; + printk("broadcaster_broadcast_id = 0x%06X\n", broadcaster_broadcast_id); + k_sem_give(&sem_broadcaster_found); + } + + /* Stop parsing */ + return false; +} + + +static void bap_pa_sync_synced_cb(struct bt_le_per_adv_sync *sync, + struct bt_le_per_adv_sync_synced_info *info) +{ + if (sync == pa_sync || + (req_recv_state != NULL && bt_addr_le_eq(info->addr, &req_recv_state->addr) && + info->sid == req_recv_state->adv_sid)) { + printk("PA sync %p synced for broadcast sink with broadcast ID 0x%06X\n", sync, + broadcaster_broadcast_id); + + if (pa_sync == NULL) { + pa_sync = sync; + } + + k_work_cancel_delayable(&pa_timer); + k_sem_give(&sem_pa_synced); + } +} + +static void bap_pa_sync_terminated_cb(struct bt_le_per_adv_sync *sync, + const struct bt_le_per_adv_sync_term_info *info) +{ + if (sync == pa_sync) { + printk("PA sync %p lost with reason 0x%02X\n", sync, info->reason); + pa_sync = NULL; + + k_sem_give(&sem_pa_sync_lost); + + if (info->reason != BT_HCI_ERR_LOCALHOST_TERM_CONN && req_recv_state != NULL) { + int err; + + if (big_synced) { + err = bt_bap_broadcast_sink_stop(broadcast_sink); + if (err != 0) { + printk("Failed to stop Broadcast Sink: %d\n", err); + + return; + } + } + + err = bt_bap_scan_delegator_rem_src(req_recv_state->src_id); + if (err != 0) { + printk("Failed to remove source: %d\n", err); + + return; + } + } + } +} + +static struct bt_le_per_adv_sync_cb bap_pa_sync_cb = { + .synced = bap_pa_sync_synced_cb, + .term = bap_pa_sync_terminated_cb, +}; + +static uint8_t get_stream_count(uint32_t bitfield) +{ + uint8_t count = 0U; + + for (uint8_t i = 0U; i < BT_ISO_MAX_GROUP_ISO_COUNT; i++) { + if ((bitfield & BIT(i)) != 0) { + count++; + } + } + + return count; +} + +static uint32_t keep_n_least_significant_ones(uint32_t bitfield, uint8_t n) +{ + uint32_t result = 0U; + + for (uint8_t i = 0; i < n && bitfield != 0; i++) { + uint32_t lsb = bitfield & -bitfield; /* extract lsb */ + + result |= lsb; + bitfield &= ~lsb; /* clear the extracted bit */ + } + + return result; +} + +static uint32_t select_bis_sync_bitfield(struct base_data *base_sg_data, + uint32_t bis_sync_req[CONFIG_BT_BAP_BASS_MAX_SUBGROUPS]) + +{ + uint32_t result = 0U; + + bool bis_sync_req_no_pref = false; + + for (uint8_t i = 0; i < CONFIG_BT_BAP_BASS_MAX_SUBGROUPS; i++) { + if (bis_sync_req[i] != 0) { + if (bis_sync_req[i] == BT_BAP_BIS_SYNC_NO_PREF) { + bis_sync_req_no_pref = true; + } + result |= + bis_sync_req[i] & base_sg_data->subgroup_bis[i].bis_index_bitfield; + } + } + + if (bis_sync_req_no_pref) { + /** Keep the CONFIG_BT_BAP_BROADCAST_SNK_STREAM_COUNT least significant bits + * of bitfield, as that is the maximum number of BISes we can sync to + */ + result = keep_n_least_significant_ones(result, + CONFIG_BT_BAP_BROADCAST_SNK_STREAM_COUNT); + } + + return result; +} + +void bap_broadcast_snk_signal_connected(void) +{ + k_sem_give(&sem_connected); +} + +static int bap_broadcast_sink_reset(void) + +{ + int err; + req_recv_state = NULL; + big_synced = false; + base_received = false; + + (void)memset(&base_recv_data, 0, sizeof(base_recv_data)); + (void)memset(&requested_bis_sync, 0, sizeof(requested_bis_sync)); + (void)memset(sink_broadcast_code, 0, sizeof(sink_broadcast_code)); + (void)memset(&broadcaster_info, 0, sizeof(broadcaster_info)); + (void)memset(&broadcaster_addr, 0, sizeof(broadcaster_addr)); + + broadcaster_broadcast_id = BT_BAP_INVALID_BROADCAST_ID; + + if (broadcast_sink != NULL) { + err = bt_bap_broadcast_sink_delete(broadcast_sink); + if (err) { + printk("Deleting broadcast sink failed (err %d)\n", err); + return err; + } + broadcast_sink = NULL; + } + + if (pa_sync != NULL) { + bt_le_per_adv_sync_delete(pa_sync); + if (err) { + printk("Deleting PA sync failed (err %d)\n", err); + return err; + } + pa_sync = NULL; + } + + k_sem_reset(&sem_broadcaster_found); + k_sem_reset(&sem_pa_synced); + k_sem_reset(&sem_base_received); + k_sem_reset(&sem_syncable); + k_sem_reset(&sem_pa_sync_lost); + k_sem_reset(&sem_broadcast_code_received); + k_sem_reset(&sem_bis_sync_requested); + k_sem_reset(&sem_stream_connected); + k_sem_reset(&sem_stream_started); + k_sem_reset(&sem_broadcast_sink_stopped); + + return 0; + +} + + +void bap_broadcast_snk_thread(void *p1, void *p2, void *p3) +{ + + enum { + BROADCAST_SNK_STATE_RESET, + BROADCAST_SNK_STATE_WAIT_BA, + BROADCAST_SNK_STATE_PA_SYNC, + BROADCAST_SNK_STATE_CREATE_SINK, + BROADCAST_SNK_STATE_WAIT_BASE, + BROADCAST_SNK_STATE_WAIT_SYNCABLE, + BROADCAST_SNK_STATE_WAIT_CODE, + BROADCAST_SNK_STATE_WAIT_BIS_REQ, + BROADCAST_SNK_STATE_SYNC_BIS, + BROADCAST_SNK_STATE_WAIT_STREAM, + BROADCAST_SNK_STATE_WAIT_DISCONNECT, + BROADCAST_SNK_STATE_WAIT_STOP + } state = BROADCAST_SNK_STATE_RESET; + + int err; + uint8_t stream_count; + uint32_t sync_bitfield; + + printk("Broadcast sink thread started\n"); + + while (true) { + switch (state) { + case BROADCAST_SNK_STATE_RESET: + + err = bap_broadcast_sink_reset(); + if (err) { + printk("BAP Sink RESET STATE: Reset failed (%d)\n", err); + k_sleep(K_SECONDS(1)); + continue; + } + state = BROADCAST_SNK_STATE_WAIT_BA; + break; + + + case BROADCAST_SNK_STATE_WAIT_BA: + + if (!bap_unicast_sr_has_connection()) { + if (k_sem_take(&sem_connected, ADV_TIMEOUT) != 0) { + break; + } + } + + if (bap_unicast_sr_has_connection()) { + k_sem_reset(&sem_pa_request); + k_sem_reset(&sem_past_request); + k_sem_reset(&sem_disconnected); + + /* Wait for the PA request to determine if we + * should start scanning, or wait for PAST + */ + printk("Waiting for PA sync\n"); + err = k_sem_take(&sem_pa_request, + BROADCAST_ASSISTANT_TIMEOUT); + if (err != 0) { + printk("sem_pa_request timed out, resetting\n"); + state = BROADCAST_SNK_STATE_RESET; + break; + } + //TODO: invistigate here + if (k_sem_take(&sem_past_request, K_NO_WAIT) == 0) { + state = BROADCAST_SNK_STATE_PA_SYNC; + break; + } + } + + break; + + case BROADCAST_SNK_STATE_PA_SYNC: + err = k_sem_take(&sem_pa_synced, SEM_TIMEOUT); + if (err != 0) { + printk("sem_pa_synced timed out, resetting\n"); + state = BROADCAST_SNK_STATE_RESET; + break; + } + state = BROADCAST_SNK_STATE_CREATE_SINK; + break; + + case BROADCAST_SNK_STATE_CREATE_SINK: + err = bt_bap_broadcast_sink_create(pa_sync, broadcaster_broadcast_id, &broadcast_sink); + if (err != 0) { + printk("Failed to create broadcast sink: %d\n", err); + state = BROADCAST_SNK_STATE_RESET; + break; + } + printk("Broadcast Sink created, waiting for BASE\n"); + state = BROADCAST_SNK_STATE_WAIT_BASE; + break; + + case BROADCAST_SNK_STATE_WAIT_BASE: + err = k_sem_take(&sem_base_received, SEM_TIMEOUT); + if (err != 0) { + printk("sem_base_received timed out, resetting\n"); + state = BROADCAST_SNK_STATE_RESET; + break; + } + printk("BASE received, waiting for syncable\n"); + state = BROADCAST_SNK_STATE_WAIT_SYNCABLE; + break; + + case BROADCAST_SNK_STATE_WAIT_SYNCABLE: + err = k_sem_take(&sem_syncable, SEM_TIMEOUT); + if (err != 0) { + printk("sem_syncable timed out, resetting\n"); + state = BROADCAST_SNK_STATE_RESET; + break; + } + state = BROADCAST_SNK_STATE_WAIT_CODE; + break; + + case BROADCAST_SNK_STATE_WAIT_CODE: + err = k_sem_take(&sem_broadcast_code_received, SEM_TIMEOUT); + if (err != 0) { + printk("sem_broadcast_code_received timed out, resetting\n"); + state = BROADCAST_SNK_STATE_RESET; + break; + } + state = BROADCAST_SNK_STATE_WAIT_BIS_REQ; + break; + + case BROADCAST_SNK_STATE_WAIT_BIS_REQ: + err = k_sem_take(&sem_bis_sync_requested, SEM_TIMEOUT); + if (err != 0) { + printk("sem_bis_sync_requested timed out, resetting\n"); + state = BROADCAST_SNK_STATE_RESET; + break; + } + sync_bitfield = select_bis_sync_bitfield(&base_recv_data, requested_bis_sync); + if (sync_bitfield == 0U) { + printk("No valid BIS sync found, resetting\n"); + state = BROADCAST_SNK_STATE_RESET; + break; + } + stream_count = get_stream_count(sync_bitfield); + printk("Syncing to broadcast with bitfield: 0x%08x, stream_count = %u\n", sync_bitfield, stream_count); + state = BROADCAST_SNK_STATE_SYNC_BIS; + break; + + case BROADCAST_SNK_STATE_SYNC_BIS: + err = bt_bap_broadcast_sink_sync(broadcast_sink, sync_bitfield, bap_streams_p, sink_broadcast_code); + if (err != 0) { + printk("Unable to sync to broadcast source: %d\n", err); + state = BROADCAST_SNK_STATE_RESET; + break; + } + printk("Waiting for stream(s) started\n"); + state = BROADCAST_SNK_STATE_WAIT_STREAM; + break; + + case BROADCAST_SNK_STATE_WAIT_STREAM: + err = k_sem_take(&sem_big_synced, SEM_TIMEOUT); + if (err != 0) { + printk("sem_big_synced timed out, resetting\n"); + state = BROADCAST_SNK_STATE_RESET; + break; + } + state = BROADCAST_SNK_STATE_WAIT_DISCONNECT; + break; + + case BROADCAST_SNK_STATE_WAIT_DISCONNECT: + k_sem_take(&sem_pa_sync_lost, K_FOREVER); + state = BROADCAST_SNK_STATE_WAIT_STOP; + break; + + case BROADCAST_SNK_STATE_WAIT_STOP: + err = k_sem_take(&sem_broadcast_sink_stopped, SEM_TIMEOUT); + if (err != 0) { + printk("sem_broadcast_sink_stopped timed out, resetting\n"); + state = BROADCAST_SNK_STATE_RESET; + break; + } + printk("BAP Sink: Broadcast session ended\n"); + state = BROADCAST_SNK_STATE_RESET; + break; + } + + k_sleep(K_MSEC(10)); + } +} + +void start_broadcast_snk_thread(void) +{ + k_thread_create(&snk_tid, snk_thread_stack, K_THREAD_STACK_SIZEOF(snk_thread_stack), + bap_broadcast_snk_thread, NULL, NULL, NULL, + 5, 0, K_NO_WAIT); +} + + +int init_bap_broadcast_sink(void) +{ + printk("%s: initialise Scan delegator and callbacks for BAP Sink\n", __func__); + int err; + + err = bt_bap_scan_delegator_register(&scan_delegator_cbs); + if (err) { + printk("Scan delegator register failed (err %d)\n", err); + return err; + } + + bt_bap_broadcast_sink_register_cb(&broadcast_sink_cbs); + bt_le_per_adv_sync_cb_register(&bap_pa_sync_cb); + + stream_rx_get_streams(bap_streams_p); + for (size_t i = 0U; i < CONFIG_BT_BAP_BROADCAST_SNK_STREAM_COUNT; i++) { + bt_bap_stream_cb_register(bap_streams_p[i], &stream_ops); + } + + return 0; +} \ No newline at end of file diff --git a/samples/bluetooth/hap_ha/src/bap_internal.h b/samples/bluetooth/hap_ha/src/bap_internal.h new file mode 100644 index 0000000000000..c4e340ecdb050 --- /dev/null +++ b/samples/bluetooth/hap_ha/src/bap_internal.h @@ -0,0 +1,25 @@ +/* + * Copyright (c) 2025 Demant A/S + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef BAP_INTERNAL_H +#define BAP_INTERNAL_H + +#include + +/** + * @brief Check if a default unicast connection exists + * + * @return true if default_conn is not NULL, false otherwise. + */ +bool bap_unicast_sr_has_connection(void); + +/** + * @brief Give the semaphore sem_conencted + * + */ +void bap_broadcast_snk_signal_connected(void); + +#endif /* BAP_INTERNAL_H */ diff --git a/samples/bluetooth/hap_ha/src/bap_unicast_sr.c b/samples/bluetooth/hap_ha/src/bap_unicast_sr.c index b6f9658fc61f0..2139f0b433d08 100644 --- a/samples/bluetooth/hap_ha/src/bap_unicast_sr.c +++ b/samples/bluetooth/hap_ha/src/bap_unicast_sr.c @@ -29,6 +29,8 @@ #include #include +#include "bap_internal.h" + NET_BUF_POOL_FIXED_DEFINE(tx_pool, CONFIG_BT_ASCS_MAX_ASE_SRC_COUNT, BT_ISO_SDU_BUF_SIZE(CONFIG_BT_ISO_TX_MTU), CONFIG_BT_CONN_TX_USER_DATA_SIZE, NULL); @@ -130,6 +132,17 @@ static void print_qos(const struct bt_bap_qos_cfg *qos) qos->rtn, qos->latency, qos->pd); } +/** + * @brief check for connection + * + * + * @param bool return true if there is a connection. + */ +bool bap_unicast_sr_has_connection(void) +{ + return default_conn != NULL; +} + /** * @brief Send audio data on timeout * @@ -375,6 +388,8 @@ static void connected(struct bt_conn *conn, uint8_t err) printk("Connected: %s\n", addr); default_conn = bt_conn_ref(conn); + + bap_broadcast_snk_signal_connected(); } static void disconnected(struct bt_conn *conn, uint8_t reason) diff --git a/samples/bluetooth/hap_ha/src/hap_ha.h b/samples/bluetooth/hap_ha/src/hap_ha.h index 5e1915c970a0d..6f35a1106f90a 100644 --- a/samples/bluetooth/hap_ha/src/hap_ha.h +++ b/samples/bluetooth/hap_ha/src/hap_ha.h @@ -1,11 +1,22 @@ /* * Copyright (c) 2022 Codecoup + * Copyright (c) 2025 Demant A/S * * SPDX-License-Identifier: Apache-2.0 */ #include +/** + * @brief Initialize the BAP broadcast Sink + * + * This will register hearing aid sample presets. + * + * @return 0 if success, errno on failure. + */ +int has_server_init(void); + + /** * @brief Initialize the BAP Unicast Server role * @@ -51,10 +62,8 @@ int micp_mic_dev_init(void); int ccp_call_ctrl_init(void); /** - * @brief Initialize the HAS Server - * - * This will register hearing aid sample presets. + * @brief Initialize the Broadcast Sink role * * @return 0 if success, errno on failure. */ -int has_server_init(void); +int init_bap_sink(void); diff --git a/samples/bluetooth/hap_ha/src/lc3.c b/samples/bluetooth/hap_ha/src/lc3.c new file mode 100644 index 0000000000000..0104b8bffdcdb --- /dev/null +++ b/samples/bluetooth/hap_ha/src/lc3.c @@ -0,0 +1,487 @@ +/** + * @file + * @brief Bluetooth BAP Broadcast Sink LC3 extension + * + * This files handles all the LC3 related functionality for the sample + * + * Copyright (c) 2025 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "lc3.h" +#include "stream_rx.h" +#include "usb.h" + +LOG_MODULE_REGISTER(lc3, CONFIG_LOG_DEFAULT_LEVEL); + +#define LC3_ENCODER_STACK_SIZE 4096 +#define LC3_ENCODER_PRIORITY 5 + +struct lc3_data { + void *fifo_reserved; /* 1st word reserved for use by FIFO */ + struct net_buf *buf; + struct stream_rx *stream; + uint32_t ts; + bool do_plc; +}; + +K_MEM_SLAB_DEFINE_STATIC(lc3_data_slab, sizeof(struct lc3_data), CONFIG_BT_ISO_RX_BUF_COUNT, + __alignof__(struct lc3_data)); + +static int16_t lc3_rx_buf[LC3_MAX_NUM_SAMPLES_MONO]; +static K_FIFO_DEFINE(lc3_in_fifo); + +/* We only want to send USB to left/right from a single stream. If we have 2 left streams, the + * outgoing audio is going to be terrible. + * Since a stream can contain stereo data, both of these may be the same stream. + */ +static struct stream_rx *usb_left_stream; +static struct stream_rx *usb_right_stream; + +static int init_lc3_decoder(struct stream_rx *stream, uint32_t lc3_frame_duration_us, + uint32_t lc3_freq_hz) +{ + if (stream == NULL) { + LOG_ERR("NULL stream to init LC3 decoder"); + return -EINVAL; + } + + if (stream->lc3_decoder != NULL) { + LOG_ERR("Already initialized"); + return -EALREADY; + } + + if (lc3_freq_hz == 0U || lc3_frame_duration_us == 0U) { + LOG_ERR("Invalid freq (%u) or frame duration (%u)", lc3_freq_hz, + lc3_frame_duration_us); + + return -EINVAL; + } + + LOG_INF("Initializing the LC3 decoder with %u us duration and %u Hz frequency", + lc3_frame_duration_us, lc3_freq_hz); + /* Create the decoder instance. This shall complete before stream_started() is called. */ + stream->lc3_decoder = + lc3_setup_decoder(lc3_frame_duration_us, lc3_freq_hz, + IS_ENABLED(CONFIG_USE_USB_AUDIO_OUTPUT) ? USB_SAMPLE_RATE_HZ : 0, + &stream->lc3_decoder_mem); + if (stream->lc3_decoder == NULL) { + LOG_ERR("Failed to setup LC3 decoder - wrong parameters?\n"); + return -EINVAL; + } + + LOG_INF("Initialized LC3 decoder for %p", stream); + + return 0; +} + +static bool decode_frame(struct lc3_data *data, size_t frame_cnt) +{ + const struct stream_rx *stream = data->stream; + const size_t total_frames = stream->lc3_chan_cnt * stream->lc3_frame_blocks_per_sdu; + const uint16_t octets_per_frame = stream->lc3_octets_per_frame; + struct net_buf *buf = data->buf; + void *iso_data; + int err; + + if (data->do_plc) { + iso_data = NULL; /* perform PLC */ + +#if CONFIG_INFO_REPORTING_INTERVAL > 0 + if ((stream->reporting_info.lc3_decoded_cnt % CONFIG_INFO_REPORTING_INTERVAL) == + 0) { + LOG_DBG("[%zu]: Performing PLC", stream->reporting_info.lc3_decoded_cnt); + } +#endif /* CONFIG_INFO_REPORTING_INTERVAL > 0 */ + + data->do_plc = false; /* clear flag */ + } else { + iso_data = net_buf_pull_mem(data->buf, octets_per_frame); + +#if CONFIG_INFO_REPORTING_INTERVAL > 0 + if ((stream->reporting_info.lc3_decoded_cnt % CONFIG_INFO_REPORTING_INTERVAL) == + 0) { + LOG_DBG("[%zu]: Decoding frame of size %u (%u/%u)", + stream->reporting_info.lc3_decoded_cnt, octets_per_frame, + frame_cnt + 1, total_frames); + } +#endif /* CONFIG_INFO_REPORTING_INTERVAL > 0 */ + } + + err = lc3_decode(stream->lc3_decoder, iso_data, octets_per_frame, LC3_PCM_FORMAT_S16, + lc3_rx_buf, 1); + if (err < 0) { + LOG_ERR("Failed to decode LC3 data (%u/%u - %u/%u)", frame_cnt + 1, total_frames, + octets_per_frame * frame_cnt, buf->len); + return false; + } + + return true; +} + +static int get_lc3_chan_alloc_from_index(const struct stream_rx *stream, uint8_t index, + enum bt_audio_location *chan_alloc) +{ +#if defined(CONFIG_USE_USB_AUDIO_OUTPUT) + const bool has_left = (stream->lc3_chan_allocation & BT_AUDIO_LOCATION_FRONT_LEFT) != 0; + const bool has_right = (stream->lc3_chan_allocation & BT_AUDIO_LOCATION_FRONT_RIGHT) != 0; + const bool is_mono = stream->lc3_chan_allocation == BT_AUDIO_LOCATION_MONO_AUDIO; + const bool is_left = index == 0 && has_left; + const bool is_right = has_right && (index == 0U || (index == 1U && has_left)); + + /* LC3 is always Left before Right, so we can use the index and the stream channel + * allocation to determine if index 0 is left or right. + */ + if (is_left) { + *chan_alloc = BT_AUDIO_LOCATION_FRONT_LEFT; + } else if (is_right) { + *chan_alloc = BT_AUDIO_LOCATION_FRONT_RIGHT; + } else if (is_mono) { + *chan_alloc = BT_AUDIO_LOCATION_MONO_AUDIO; + } else { + /* Not suitable for USB */ + return -EINVAL; + } + + return 0; +#else /* !CONFIG_USE_USB_AUDIO_OUTPUT */ + return -EINVAL; +#endif /* CONFIG_USE_USB_AUDIO_OUTPUT */ +} + +static size_t decode_frame_block(struct lc3_data *data, size_t frame_cnt) +{ + const struct stream_rx *stream = data->stream; + const uint8_t chan_cnt = stream->lc3_chan_cnt; + size_t decoded_frames = 0U; + + for (uint8_t i = 0U; i < chan_cnt; i++) { + /* We provide the total number of decoded frames to `decode_frame` for logging + * purposes + */ + if (decode_frame(data, frame_cnt + decoded_frames)) { + decoded_frames++; + + if (IS_ENABLED(CONFIG_USE_USB_AUDIO_OUTPUT)) { + enum bt_audio_location chan_alloc; + int err; + + err = get_lc3_chan_alloc_from_index(stream, i, &chan_alloc); + if (err != 0) { + /* Not suitable for USB */ + continue; + } + + /* We only want to left or right from one stream to USB */ + if ((chan_alloc == BT_AUDIO_LOCATION_FRONT_LEFT && + stream != usb_left_stream) || + (chan_alloc == BT_AUDIO_LOCATION_FRONT_RIGHT && + stream != usb_right_stream)) { + continue; + } + + /* TODO: Add support for properly support the presentation delay. + * For now we just send audio to USB as soon as we get it + */ + err = usb_add_frame_to_usb(chan_alloc, lc3_rx_buf, + sizeof(lc3_rx_buf), data->ts); + if (err == -EINVAL) { + continue; + } + } + } else { + /* If decoding failed, we clear the data to USB as it would contain + * invalid data + */ + if (IS_ENABLED(CONFIG_USE_USB_AUDIO_OUTPUT)) { + usb_clear_frames_to_usb(); + } + + break; + } + } + + return decoded_frames; +} + +static void do_lc3_decode(struct lc3_data *data) +{ + struct stream_rx *stream = data->stream; + + if (stream->lc3_decoder != NULL) { + const uint8_t frame_blocks_per_sdu = stream->lc3_frame_blocks_per_sdu; + size_t frame_cnt; + + frame_cnt = 0; + for (uint8_t i = 0U; i < frame_blocks_per_sdu; i++) { + const size_t decoded_frames = decode_frame_block(data, frame_cnt); + + if (decoded_frames == 0) { + break; + } + + frame_cnt += decoded_frames; + } + +#if CONFIG_INFO_REPORTING_INTERVAL > 0 + stream->reporting_info.lc3_decoded_cnt++; +#endif /* CONFIG_INFO_REPORTING_INTERVAL > 0 */ + } + + net_buf_unref(data->buf); +} + +static void lc3_decoder_thread_func(void *arg1, void *arg2, void *arg3) +{ + while (true) { + struct lc3_data *data = k_fifo_get(&lc3_in_fifo, K_FOREVER); + struct stream_rx *stream = data->stream; + + if (stream->lc3_decoder == NULL) { + LOG_WRN("Decoder is NULL, discarding data from FIFO"); + k_mem_slab_free(&lc3_data_slab, (void *)data); + continue; /* Wait for new data */ + } + + do_lc3_decode(data); + + k_mem_slab_free(&lc3_data_slab, (void *)data); + } +} + +int lc3_enable(struct stream_rx *stream) +{ + const struct bt_audio_codec_cfg *codec_cfg = stream->stream.codec_cfg; + uint32_t lc3_frame_duration_us; + uint32_t lc3_freq_hz; + int ret; + + if (codec_cfg->id != BT_HCI_CODING_FORMAT_LC3) { + return -EINVAL; + } + + ret = bt_audio_codec_cfg_get_freq(codec_cfg); + if (ret >= 0) { + ret = bt_audio_codec_cfg_freq_to_freq_hz(ret); + + if (ret > 0) { + if (ret == 8000 || ret == 16000 || ret == 24000 || ret == 32000 || + ret == 48000) { + lc3_freq_hz = (uint32_t)ret; + } else { + LOG_ERR("Unsupported frequency for LC3: %d", ret); + lc3_freq_hz = 0U; + } + } else { + LOG_ERR("Invalid frequency: %d", ret); + lc3_freq_hz = 0U; + } + } else { + LOG_ERR("Could not get frequency: %d", ret); + lc3_freq_hz = 0U; + } + + if (lc3_freq_hz == 0U) { + return -EINVAL; + } + + ret = bt_audio_codec_cfg_get_frame_dur(codec_cfg); + if (ret >= 0) { + ret = bt_audio_codec_cfg_frame_dur_to_frame_dur_us(ret); + if (ret > 0) { + lc3_frame_duration_us = (uint32_t)ret; + } else { + LOG_ERR("Invalid frame duration: %d", ret); + lc3_frame_duration_us = 0U; + } + } else { + LOG_ERR("Could not get frame duration: %d", ret); + lc3_frame_duration_us = 0U; + } + + if (lc3_frame_duration_us == 0U) { + return -EINVAL; + } + + ret = bt_audio_codec_cfg_get_chan_allocation(codec_cfg, &stream->lc3_chan_allocation, true); + if (ret == 0) { + stream->lc3_chan_cnt = bt_audio_get_chan_count(stream->lc3_chan_allocation); + } else { + LOG_DBG("Could not get channel allocation: %d", ret); + stream->lc3_chan_cnt = 0U; + } + + if (stream->lc3_chan_cnt == 0U) { + return -EINVAL; + } + + ret = bt_audio_codec_cfg_get_frame_blocks_per_sdu(codec_cfg, true); + if (ret >= 0) { + stream->lc3_frame_blocks_per_sdu = (uint8_t)ret; + } else { + LOG_ERR("Could not get frame blocks per SDU: %d", ret); + stream->lc3_frame_blocks_per_sdu = 0U; + } + + if (stream->lc3_frame_blocks_per_sdu == 0U) { + return -EINVAL; + } + + ret = bt_audio_codec_cfg_get_octets_per_frame(codec_cfg); + if (ret >= 0) { + stream->lc3_octets_per_frame = (uint16_t)ret; + } else { + LOG_ERR("Could not get octets per frame: %d", ret); + stream->lc3_octets_per_frame = 0U; + } + + if (stream->lc3_octets_per_frame == 0U) { + return -EINVAL; + } + + if (stream->lc3_decoder == NULL) { + const int err = init_lc3_decoder(stream, lc3_frame_duration_us, lc3_freq_hz); + + if (err != 0) { + LOG_ERR("Failed to init LC3 decoder: %d", err); + + return err; + } + } + + if (IS_ENABLED(CONFIG_USE_USB_AUDIO_OUTPUT)) { + if ((stream->lc3_chan_allocation & BT_AUDIO_LOCATION_FRONT_LEFT) != 0) { + if (usb_left_stream == NULL) { + LOG_INF("Setting USB left stream to %p", stream); + usb_left_stream = stream; + } else { + LOG_WRN("Multiple left streams started"); + } + } + + if ((stream->lc3_chan_allocation & BT_AUDIO_LOCATION_FRONT_RIGHT) != 0) { + if (usb_right_stream == NULL) { + LOG_INF("Setting USB right stream to %p", stream); + usb_right_stream = stream; + } else { + LOG_WRN("Multiple right streams started"); + } + } + } + + return 0; +} + +int lc3_disable(struct stream_rx *stream) +{ + if (stream->lc3_decoder == NULL) { + return -EINVAL; + } + + stream->lc3_decoder = NULL; + + if (IS_ENABLED(CONFIG_USE_USB_AUDIO_OUTPUT)) { + if (usb_left_stream == stream) { + usb_left_stream = NULL; + } + if (usb_right_stream == stream) { + usb_right_stream = NULL; + } + } + + return 0; +} + +void lc3_enqueue_for_decoding(struct stream_rx *stream, const struct bt_iso_recv_info *info, + struct net_buf *buf) +{ + const uint8_t frame_blocks_per_sdu = stream->lc3_frame_blocks_per_sdu; + const uint16_t octets_per_frame = stream->lc3_octets_per_frame; + const uint8_t chan_cnt = stream->lc3_chan_cnt; + struct lc3_data *data; + + if (stream->lc3_decoder == NULL) { + return; + } + + /* Allocate a context that holds both the buffer and the stream so that we can + * send both of these values to the LC3 decoder thread as a single struct + * in a FIFO + */ + if (k_mem_slab_alloc(&lc3_data_slab, (void **)&data, K_NO_WAIT)) { + LOG_WRN("Could not allocate LC3 data item"); + return; + } + + if ((info->flags & BT_ISO_FLAGS_VALID) == 0) { + data->do_plc = true; + } else if (buf->len != (octets_per_frame * chan_cnt * frame_blocks_per_sdu)) { + if (buf->len != 0U) { + LOG_WRN("Expected %u frame blocks with %u channels of size %u, but " + "length is %u", + frame_blocks_per_sdu, chan_cnt, octets_per_frame, buf->len); + } + + data->do_plc = true; + } + + data->buf = net_buf_ref(buf); + data->stream = stream; + if (info->flags & BT_ISO_FLAGS_TS) { + data->ts = info->ts; + } else { + data->ts = 0U; + } + + k_fifo_put(&lc3_in_fifo, data); +} + +int lc3_init(void) +{ + static K_KERNEL_STACK_DEFINE(lc3_decoder_thread_stack, 4096); + const int lc3_decoder_thread_prio = K_PRIO_PREEMPT(5); + static struct k_thread lc3_decoder_thread; + static bool initialized; + + if (initialized) { + return -EALREADY; + } + + k_thread_create(&lc3_decoder_thread, lc3_decoder_thread_stack, + K_KERNEL_STACK_SIZEOF(lc3_decoder_thread_stack), lc3_decoder_thread_func, + NULL, NULL, NULL, lc3_decoder_thread_prio, 0, K_NO_WAIT); + k_thread_name_set(&lc3_decoder_thread, "LC3 Decoder"); + + LOG_INF("LC3 initialized"); + initialized = true; + + return 0; +} diff --git a/samples/bluetooth/hap_ha/src/lc3.h b/samples/bluetooth/hap_ha/src/lc3.h new file mode 100644 index 0000000000000..a7baf8862f84b --- /dev/null +++ b/samples/bluetooth/hap_ha/src/lc3.h @@ -0,0 +1,85 @@ +/** + * @file + * @brief Bluetooth BAP Broadcast Sink LC3 header + * + * This files handles all the LC3 related functionality for the sample + * + * Copyright (c) 2025 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ +#ifndef SAMPLE_HAP_HA_LC3_H +#define SAMPLE_HAP_HA_LC3_H + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "stream_rx.h" + +#define LC3_MAX_SAMPLE_RATE_HZ 48000U +#define LC3_MAX_FRAME_DURATION_US 10000U +#define LC3_MAX_NUM_SAMPLES_MONO \ + ((LC3_MAX_FRAME_DURATION_US * LC3_MAX_SAMPLE_RATE_HZ) / USEC_PER_SEC) +#define LC3_MAX_NUM_SAMPLES_STEREO (LC3_MAX_NUM_SAMPLES_MONO * 2U) + +/** + * @brief Enables LC3 for a stream + * + * This will initialize the LC3 decoder given the @p stream codec configuration + * + * @param stream The stream to enable LC3 for + + * @retval 0 Success + * @retval -EINVAL The stream is not LC3 codec configured or the codec configuration is invalid + */ +int lc3_enable(struct stream_rx *stream); + +/** + * @brief Disabled LC3 for a stream + * + * @param stream The stream to disable LC3 for + + * @retval 0 Success + * @retval -EINVAL The stream is LC3 initialized + */ +int lc3_disable(struct stream_rx *stream); + +/** + * @brief Enqueue an SDU for decoding + * + * @param stream The stream that received the SDU + * @param info Information about the SDU + * @param buf The buffer of the SDU + */ +void lc3_enqueue_for_decoding(struct stream_rx *stream, const struct bt_iso_recv_info *info, + struct net_buf *buf); + +/** + * @brief Initialize the LC3 module + * + * This will start the thread if not already initialized + * + * @retval 0 Success + * @retval -EALREADY Already initialized + */ +int lc3_init(void); +#endif /* SAMPLE_HAP_HA_LC3_H */ diff --git a/samples/bluetooth/hap_ha/src/main.c b/samples/bluetooth/hap_ha/src/main.c index 0822ef95abd50..f6b64110c918d 100644 --- a/samples/bluetooth/hap_ha/src/main.c +++ b/samples/bluetooth/hap_ha/src/main.c @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -23,6 +24,7 @@ #include #include +#include "stream_rx.h" #include "hap_ha.h" #define MANDATORY_SINK_CONTEXT (BT_AUDIO_CONTEXT_TYPE_UNSPECIFIED | \ @@ -46,7 +48,9 @@ static uint8_t csis_rsi_addata[BT_CSIP_RSI_SIZE]; /* TODO: Expand with BAP data */ static const struct bt_data ad[] = { BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)), - BT_DATA_BYTES(BT_DATA_UUID16_ALL, BT_UUID_16_ENCODE(BT_UUID_ASCS_VAL)), + BT_DATA_BYTES(BT_DATA_UUID16_ALL, + BT_UUID_16_ENCODE(BT_UUID_BASS_VAL), + BT_UUID_16_ENCODE(BT_UUID_ASCS_VAL)), #if defined(CONFIG_BT_CSIP_SET_MEMBER) BT_DATA(BT_DATA_CSIS_RSI, csis_rsi_addata, ARRAY_SIZE(csis_rsi_addata)), #endif /* CONFIG_BT_CSIP_SET_MEMBER */ @@ -175,6 +179,11 @@ int main(void) return 0; } + err = init_bap_broadcast_sink(); + if (err != 0) { + printk("BAP broadcast sink init failed (err %d)\n", err); + } + if (IS_ENABLED(CONFIG_HAP_HA_HEARING_AID_BINAURAL)) { err = csip_set_member_init(); if (err != 0) { @@ -237,5 +246,8 @@ int main(void) k_work_init_delayable(&adv_work, adv_work_handler); k_work_schedule(&adv_work, K_NO_WAIT); + + start_broadcast_snk_thread(); + return 0; } diff --git a/samples/bluetooth/hap_ha/src/stream_rx.c b/samples/bluetooth/hap_ha/src/stream_rx.c new file mode 100644 index 0000000000000..d441b820f5a98 --- /dev/null +++ b/samples/bluetooth/hap_ha/src/stream_rx.c @@ -0,0 +1,168 @@ +/* + * Copyright (c) 2025 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if defined(CONFIG_LIBLC3) +#include +#endif /* defined(CONFIG_LIBLC3) */ + +#include "stream_rx.h" +#include "lc3.h" + +struct stream_rx rx_streams[CONFIG_BT_BAP_BROADCAST_SNK_STREAM_COUNT]; +uint64_t total_rx_iso_packet_count; /* This value is exposed to test code */ + +LOG_MODULE_REGISTER(stream_rx, CONFIG_LOG_DEFAULT_LEVEL); + +#if CONFIG_INFO_REPORTING_INTERVAL > 0 +static void log_stream_rx(const struct stream_rx *stream, const struct bt_iso_recv_info *info, + const struct net_buf *buf) +{ + LOG_INF("[%zu]: Incoming audio on stream %p len %u, flags 0x%02X, seq_num %u and ts %u: " + "Valid %zu | Error %zu | Loss %zu | Dup TS %zu | Dup PSN %zu", + stream->reporting_info.recv_cnt, &stream->stream, buf->len, info->flags, + info->seq_num, info->ts, stream->reporting_info.valid_cnt, + stream->reporting_info.error_cnt, stream->reporting_info.loss_cnt, + stream->reporting_info.dup_ts_cnt, stream->reporting_info.dup_psn_cnt); +} +#endif /* CONFIG_INFO_REPORTING_INTERVAL > 0 */ + +void stream_rx_recv(struct bt_bap_stream *bap_stream, const struct bt_iso_recv_info *info, + struct net_buf *buf) +{ + struct stream_rx *stream = CONTAINER_OF(bap_stream, struct stream_rx, stream); + +#if CONFIG_INFO_REPORTING_INTERVAL > 0 + bool do_log = false; + + stream->reporting_info.recv_cnt++; + if (stream->reporting_info.recv_cnt == 1U) { + /* Log first reception */ + do_log = true; + + } else if ((stream->reporting_info.recv_cnt % CONFIG_INFO_REPORTING_INTERVAL) == 0U) { + /* Log once for every CONFIG_INFO_REPORTING_INTERVAL reception */ + do_log = true; + } + + if (stream->reporting_info.recv_cnt > 1U && info->ts == stream->reporting_info.last_ts) { + stream->reporting_info.dup_ts_cnt++; + do_log = true; + LOG_WRN("Duplicated timestamp received: %u", stream->reporting_info.last_ts); + } + + if (stream->reporting_info.recv_cnt > 1U && + info->seq_num == stream->reporting_info.last_seq_num) { + stream->reporting_info.dup_psn_cnt++; + do_log = true; + LOG_WRN("Duplicated PSN received: %u", stream->reporting_info.last_seq_num); + } + + if (info->flags & BT_ISO_FLAGS_ERROR) { + stream->reporting_info.error_cnt++; + do_log = true; + LOG_DBG("ISO receive error"); + } + + if (info->flags & BT_ISO_FLAGS_LOST) { + stream->reporting_info.loss_cnt++; + do_log = true; + LOG_DBG("ISO receive lost"); + } + + if (info->flags & BT_ISO_FLAGS_VALID) { + if (buf->len == 0U) { + stream->reporting_info.empty_sdu_cnt++; + } else { + stream->reporting_info.valid_cnt++; + } + } + + if (do_log) { + log_stream_rx(stream, info, buf); + } + + stream->reporting_info.last_seq_num = info->seq_num; + stream->reporting_info.last_ts = info->ts; +#endif /* CONFIG_INFO_REPORTING_INTERVAL > 0 */ + + total_rx_iso_packet_count++; + + if (IS_ENABLED(CONFIG_LIBLC3)) { + /* Invalid SDUs will trigger PLC */ + lc3_enqueue_for_decoding(stream, info, buf); + } +} + +int stream_rx_started(struct bt_bap_stream *bap_stream) +{ + struct stream_rx *stream = CONTAINER_OF(bap_stream, struct stream_rx, stream); + + if (stream == NULL) { + return -EINVAL; + } + +#if CONFIG_INFO_REPORTING_INTERVAL > 0 + memset(&stream->reporting_info, 0, sizeof((stream->reporting_info))); +#endif /* CONFIG_INFO_REPORTING_INTERVAL > 0 */ + + if (IS_ENABLED(CONFIG_LIBLC3) && bap_stream->codec_cfg != NULL && + bap_stream->codec_cfg->id == BT_HCI_CODING_FORMAT_LC3) { + int err; + + err = lc3_enable(stream); + if (err < 0) { + LOG_ERR("Error: cannot enable LC3 codec: %d", err); + return err; + } + } + + return 0; +} + +int stream_rx_stopped(struct bt_bap_stream *bap_stream) +{ + struct stream_rx *stream = CONTAINER_OF(bap_stream, struct stream_rx, stream); + + if (bap_stream == NULL) { + return -EINVAL; + } + + if (IS_ENABLED(CONFIG_LIBLC3) && bap_stream->codec_cfg != NULL && + bap_stream->codec_cfg->id == BT_HCI_CODING_FORMAT_LC3) { + int err; + + err = lc3_disable(stream); + if (err < 0) { + LOG_ERR("Error: cannot disable LC3 codec: %d", err); + return err; + } + } + + return 0; +} + +void stream_rx_get_streams( + struct bt_bap_stream *bap_streams[CONFIG_BT_BAP_BROADCAST_SNK_STREAM_COUNT]) +{ + for (size_t i = 0U; i < ARRAY_SIZE(rx_streams); i++) { + bap_streams[i] = &rx_streams[i].stream; + } +} diff --git a/samples/bluetooth/hap_ha/src/stream_rx.h b/samples/bluetooth/hap_ha/src/stream_rx.h new file mode 100644 index 0000000000000..494269f230ce5 --- /dev/null +++ b/samples/bluetooth/hap_ha/src/stream_rx.h @@ -0,0 +1,91 @@ +/* + * Copyright (c) 2025 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ +#ifndef SAMPLE_HAP_HA_STREAM_RX_H +#define SAMPLE_HAP_HA_STREAM_RX_H + +#include +#include + +#include +#include +#include +#include +#include +#include + +#if defined(CONFIG_LIBLC3) +#include +#endif /* defined(CONFIG_LIBLC3) */ + +struct stream_rx { + /* A BAP stream object */ + struct bt_bap_stream stream; +#if CONFIG_INFO_REPORTING_INTERVAL > 0 + /** Struct containing information useful for logging purposes */ + struct { + /** Total Number of SDUs received */ + size_t recv_cnt; + /** Number of lost SDUs */ + size_t loss_cnt; + /** Number of SDUs containing errors */ + size_t error_cnt; + /** Number of valid SDUs received */ + size_t valid_cnt; + /** Number of empty SDUs received */ + size_t empty_sdu_cnt; + /** Number of SDUs with duplicated packet sequence number received */ + size_t dup_psn_cnt; + /** Number of SDUs with duplicated timestamps received */ + size_t dup_ts_cnt; + /** The last received timestamp to track dup_ts_cnt */ + uint32_t last_ts; + /** The last received sequence number to track dup_psn_cnt */ + uint16_t last_seq_num; +#if CONFIG_LIBLC3 > 0 + /** Number of SDUs decoded */ + size_t lc3_decoded_cnt; +#endif /* CONFIG_LIBLC3 > 0 */ + } reporting_info; +#endif /* CONFIG_INFO_REPORTING_INTERVAL > 0 */ + +#if defined(CONFIG_LIBLC3) + /** Octets per frame - Used to validate that the incoming data is of correct size */ + uint16_t lc3_octets_per_frame; + /** Frame blocks per SDU - Used to split the SDU into frame blocks when decoding */ + uint8_t lc3_frame_blocks_per_sdu; + + /** Number of channels - Used to split the SDU into frame blocks when decoding */ + uint8_t lc3_chan_cnt; + + /** + * @brief The configured channels of the stream + * + * Used to determine whether to send data to USB and count number of channels + */ + enum bt_audio_location lc3_chan_allocation; + + /** Memory use for the LC3 decoder - Supports any configuration */ + lc3_decoder_mem_48k_t lc3_decoder_mem; + /** Reference to the LC3 decoder */ + lc3_decoder_t lc3_decoder; +#endif /* defined(CONFIG_LIBLC3) */ +}; + +/** + * @brief Function to call for each SDU received + * + * Will decode with LC3 and send to USB if enabled + */ +void stream_rx_recv(struct bt_bap_stream *bap_stream, const struct bt_iso_recv_info *info, + struct net_buf *buf); + +size_t stream_rx_get_streaming_cnt(void); +int stream_rx_started(struct bt_bap_stream *bap_stream); +int stream_rx_stopped(struct bt_bap_stream *bap_stream); +void stream_rx_get_streams( + struct bt_bap_stream *bap_streams[CONFIG_BT_BAP_BROADCAST_SNK_STREAM_COUNT]); + +#endif /* SAMPLE_HAP_HA_STREAM_RX_H */