This is an automated email from the ASF dual-hosted git repository. rymek pushed a commit to branch master in repository https://gitbox.apache.org/repos/asf/mynewt-nimble.git
commit 8c06f256374ae9be9ff43d9270bcee3e9481a01f Author: Mariusz Skamra <[email protected]> AuthorDate: Mon Jan 22 20:00:49 2024 +0100 nimble/iso: Add ISO Rx datapath and extend ISO Broadcast API This adds ISO Rx datapath and along with Broadcast Sink ISO API and implementation. --- nimble/host/include/host/ble_iso.h | 230 +++++++++- nimble/host/src/ble_hs.c | 5 + nimble/host/src/ble_hs_hci_evt.c | 43 ++ nimble/host/src/ble_hs_startup.c | 11 + nimble/host/src/ble_iso.c | 616 +++++++++++++++++++++++++-- nimble/host/src/ble_iso_priv.h | 9 + nimble/include/nimble/hci_common.h | 28 +- nimble/transport/socket/src/ble_hci_socket.c | 38 ++ 8 files changed, 937 insertions(+), 43 deletions(-) diff --git a/nimble/host/include/host/ble_iso.h b/nimble/host/include/host/ble_iso.h index 786d2eb21..8b0182fa6 100644 --- a/nimble/host/include/host/ble_iso.h +++ b/nimble/host/include/host/ble_iso.h @@ -19,22 +19,31 @@ #ifndef H_BLE_ISO_ #define H_BLE_ISO_ +#include <inttypes.h> + +#include "nimble/hci_common.h" #include "syscfg/syscfg.h" /** ISO event: BIG Create Completed */ -#define BLE_ISO_EVENT_BIG_CREATE_COMPLETE 0 +#define BLE_ISO_EVENT_BIG_CREATE_COMPLETE 0 /** ISO event: BIG Terminate Completed */ -#define BLE_ISO_EVENT_BIG_TERMINATE_COMPLETE 1 +#define BLE_ISO_EVENT_BIG_TERMINATE_COMPLETE 1 -#include <inttypes.h> +/** ISO event: BIG Sync Established */ +#define BLE_ISO_EVENT_BIG_SYNC_ESTABLISHED 2 -struct ble_iso_big_desc -{ +/** ISO event: BIG Sync Terminated */ +#define BLE_ISO_EVENT_BIG_SYNC_TERMINATED 3 + +/** ISO event: ISO Data received */ +#define BLE_ISO_EVENT_ISO_RX 4 + +/** @brief Broadcast Isochronous Group (BIG) description */ +struct ble_iso_big_desc { uint8_t big_handle; uint32_t big_sync_delay; uint32_t transport_latency_big; - uint8_t phy; uint8_t nse; uint8_t bn; uint8_t pto; @@ -45,6 +54,36 @@ struct ble_iso_big_desc uint16_t conn_handle[MYNEWT_VAL(BLE_MAX_BIS)]; }; +/** @brief Received ISO Data status possible values */ +enum ble_iso_rx_data_status { + /** The complete SDU was received correctly. */ + BLE_ISO_DATA_STATUS_VALID = BLE_HCI_ISO_PKT_STATUS_VALID, + + /** May contain errors or part of the SDU may be missing. */ + BLE_ISO_DATA_STATUS_ERROR = BLE_HCI_ISO_PKT_STATUS_INVALID, + + /** Part(s) of the SDU were not received correctly */ + BLE_ISO_DATA_STATUS_LOST = BLE_HCI_ISO_PKT_STATUS_LOST, +}; + +/** @brief Received ISO data info structure */ +struct ble_iso_rx_data_info { + /** ISO Data timestamp. Valid if @ref ble_iso_data_info.ts_valid is set */ + uint32_t ts; + + /** Packet sequence number */ + uint16_t seq_num; + + /** SDU length */ + uint16_t sdu_len : 12; + + /** ISO Data status. See @ref ble_iso_data_status */ + uint16_t status : 2; + + /** Timestamp is valid */ + uint16_t ts_valid : 1; +}; + /** * Represents a ISO-related event. When such an event occurs, the host * notifies the application by passing an instance of this structure to an @@ -69,17 +108,41 @@ struct ble_iso_event { */ struct { struct ble_iso_big_desc desc; + uint8_t status; + uint8_t phy; } big_created; /** * Represents a completion of BIG termination. Valid for the following * event types: * o BLE_ISO_EVENT_BIG_TERMINATE_COMPLETE + * o BLE_ISO_EVENT_BIG_SYNC_TERMINATED */ struct { uint16_t big_handle; uint8_t reason; } big_terminated; + + /** + * Represents a completion of BIG synchronization. Valid for the following + * event types: + * o BLE_ISO_EVENT_BIG_SYNC_ESTABLISHED + */ + struct { + struct ble_iso_big_desc desc; + uint8_t status; + } big_sync_established; + + /** + * Represents a reception of ISO Data. Valid for the following + * event types: + * o BLE_ISO_EVENT_ISO_RX + */ + struct { + uint16_t conn_handle; + const struct ble_iso_rx_data_info *info; + struct os_mbuf *om; + } iso_rx; }; }; @@ -109,8 +172,161 @@ int ble_iso_create_big(const struct ble_iso_create_big_params *create_params, int ble_iso_terminate_big(uint8_t big_handle); +/** @brief BIS parameters for @ref ble_iso_big_sync_create */ +struct ble_iso_bis_params { + /** BIS index */ + uint8_t bis_index; + + /** The callback to associate with the BIS. + * Received ISO data is reported through this callback. + */ + ble_iso_event_fn *cb; + + /** The optional argument to pass to the callback function */ + void *cb_arg; +}; + +/** @brief BIG Sync parameters for @ref ble_iso_big_sync_create */ +struct ble_iso_big_sync_create_params { + /** Periodic advertising train sync handle */ + uint16_t sync_handle; + + /** Null-terminated broadcast code for encrypted BIG or + * NULL if the BIG is unencrypted + */ + const char *broadcast_code; + + /** Maximum Subevents to be used to receive data payloads in each BIS event */ + uint8_t mse; + + /** The maximum permitted time between successful receptions of BIS PDUs */ + uint16_t sync_timeout; + + /** The callback to associate with this sync procedure. + * Sync establishment and termination are reported through this callback. + */ + ble_iso_event_fn *cb; + + /** The optional argument to pass to the callback function */ + void *cb_arg; + + /** Number of a BISes */ + uint8_t bis_cnt; + + /** BIS parameters */ + struct ble_iso_bis_params *bis_params; +}; + +/** + * @brief Synchronize to Broadcast Isochronous Group (BIG) + * + * This function is used to synchronize to a BIG described in the periodic + * advertising train specified by the @p param->pa_sync_handle parameter. + * + * @param[in] params BIG synchronization parameters + * @param[out] big_handle BIG instance handle + * + * @return 0 on success; + * A non-zero value on failure. + */ +int ble_iso_big_sync_create(const struct ble_iso_big_sync_create_params *params, + uint8_t *big_handle); + +/** + * @brief Terminate Broadcast Isochronous Group (BIG) sync + * + * This function is used to stop synchronizing or cancel the process of + * synchronizing to the BIG identified by the @p big_handle parameter. + * The command also terminates the reception of BISes in the BIG. + * + * @param[in] big_handle BIG handle + * + * @return 0 on success; + * A non-zero value on failure. + */ +int ble_iso_big_sync_terminate(uint8_t big_handle); + +/** @brief ISO Data direction */ +enum ble_iso_data_dir { + BLE_ISO_DATA_DIR_TX, + BLE_ISO_DATA_DIR_RX, +}; + +/** @brief ISO Codec ID */ +struct ble_iso_codec_id { + /** Coding Format */ + uint8_t format; + + /** Company ID */ + uint16_t company_id; + + /** Vendor Specific Codec ID */ + uint16_t vendor_specific; +}; + +/** @brief Setup ISO Data Path parameters */ +struct ble_iso_data_path_setup_params { + /** Connection handle of the CIS or BIS */ + uint16_t conn_handle; + + /** Data path direction */ + enum ble_iso_data_dir data_path_dir; + + /** Data path ID. 0x00 for HCI */ + uint8_t data_path_id; + + /** Controller delay */ + uint32_t ctrl_delay; + + /** Codec ID */ + struct ble_iso_codec_id codec_id; + + /** Codec Configuration Length */ + uint8_t codec_config_len; + + /** Codec Configuration */ + const uint8_t *codec_config; +}; + +/** + * @brief Setup ISO Data Path + * + * This function is used to identify and create the isochronous data path + * between the Host and the Controller for a CIS, CIS configuration, or BIS + * identified by the @p param->conn_handle parameter. + * + * @param[in] params BIG synchronization parameters + * + * @return 0 on success; + * A non-zero value on failure. + */ +int ble_iso_data_path_setup(const struct ble_iso_data_path_setup_params *param); + +/** @brief @brief Remove ISO Data Path parameters */ +struct ble_iso_data_path_remove_params { + /** Connection handle of the CIS or BIS */ + uint16_t conn_handle; + + /** Data path direction */ + enum ble_iso_data_dir data_path_dir; +}; + +/** + * @brief Remove ISO Data Path + * + * This function is used to remove the input and/or output data path(s) + * associated with a CIS, CIS configuration, or BIS identified by the + * @p param->conn_handle parameter. + * + * @param[in] params BIG synchronization parameters + * + * @return 0 on success; + * A non-zero value on failure. + */ +int ble_iso_data_path_remove(const struct ble_iso_data_path_remove_params *param); + int ble_iso_tx(uint16_t conn_handle, void *data, uint16_t data_len); int ble_iso_init(void); -#endif +#endif /* H_BLE_ISO_ */ diff --git a/nimble/host/src/ble_hs.c b/nimble/host/src/ble_hs.c index 801722177..d084c041a 100644 --- a/nimble/host/src/ble_hs.c +++ b/nimble/host/src/ble_hs.c @@ -26,6 +26,7 @@ #include "host/ble_hs.h" #include "host/ble_audio_broadcast_source.h" #include "ble_hs_priv.h" +#include "ble_iso_priv.h" #include "nimble/nimble_npl.h" #ifndef MYNEWT #include "nimble/nimble_port.h" @@ -815,9 +816,13 @@ ble_transport_to_hs_acl_impl(struct os_mbuf *om) int ble_transport_to_hs_iso_impl(struct os_mbuf *om) { +#if MYNEWT_VAL(BLE_ISO) + return ble_iso_rx_data(om, NULL); +#else os_mbuf_free_chain(om); return 0; +#endif } void diff --git a/nimble/host/src/ble_hs_hci_evt.c b/nimble/host/src/ble_hs_hci_evt.c index e4fdad471..cc88bfd83 100644 --- a/nimble/host/src/ble_hs_hci_evt.c +++ b/nimble/host/src/ble_hs_hci_evt.c @@ -69,6 +69,10 @@ static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_periodic_adv_sync_transfer; static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_create_big_complete; static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_terminate_big_complete; #endif +#if MYNEWT_VAL(BLE_ISO_BROADCAST_SINK) +static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_big_sync_established; +static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_big_sync_lost; +#endif #if MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_BIGINFO_REPORTS) static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_biginfo_adv_report; #endif @@ -143,6 +147,12 @@ static ble_hs_hci_evt_le_fn * const ble_hs_hci_evt_le_dispatch[] = { [BLE_HCI_LE_SUBEV_TERMINATE_BIG_COMPLETE] = ble_hs_hci_evt_le_terminate_big_complete, #endif +#if MYNEWT_VAL(BLE_ISO_BROADCAST_SINK) + [BLE_HCI_LE_SUBEV_BIG_SYNC_ESTABLISHED] = + ble_hs_hci_evt_le_big_sync_established, + [BLE_HCI_LE_SUBEV_BIG_SYNC_LOST] = + ble_hs_hci_evt_le_big_sync_lost, +#endif #if MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_BIGINFO_REPORTS) [BLE_HCI_LE_SUBEV_BIGINFO_ADV_REPORT] = ble_hs_hci_evt_le_biginfo_adv_report, #endif @@ -779,6 +789,39 @@ ble_hs_hci_evt_le_terminate_big_complete(uint8_t subevent, const void *data, } #endif +#if MYNEWT_VAL(BLE_ISO_BROADCAST_SINK) +static int +ble_hs_hci_evt_le_big_sync_established(uint8_t subevent, const void *data, + unsigned int len) +{ + const struct ble_hci_ev_le_subev_big_sync_established *ev = data; + + if (len < sizeof(*ev) || + len != (sizeof(*ev) + ev->num_bis * sizeof(ev->conn_handle[0]))) { + return BLE_HS_EBADDATA; + } + + ble_iso_rx_big_sync_established(ev); + + return 0; +} + +static int +ble_hs_hci_evt_le_big_sync_lost(uint8_t subevent, const void *data, + unsigned int len) +{ + const struct ble_hci_ev_le_subev_big_sync_lost *ev = data; + + if (len != sizeof(*ev)) { + return BLE_HS_EBADDATA; + } + + ble_iso_rx_big_sync_lost(ev); + + return 0; +} +#endif + #if MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_BIGINFO_REPORTS) static int ble_hs_hci_evt_le_biginfo_adv_report(uint8_t subevent, const void *data, diff --git a/nimble/host/src/ble_hs_startup.c b/nimble/host/src/ble_hs_startup.c index d9907f376..827c00d2a 100644 --- a/nimble/host/src/ble_hs_startup.c +++ b/nimble/host/src/ble_hs_startup.c @@ -303,6 +303,17 @@ ble_hs_startup_le_set_evmask_tx(void) } #endif +#if MYNEWT_VAL(BLE_ISO_BROADCAST_SINK) + if (version >= BLE_HCI_VER_BCS_5_2) { + /** + * Enable the following LE events: + * 0x0000000010000000 LE BIG Sync Established Complete event + * 0x0000000020000000 LE BIG Sync lost event + */ + mask |= 0x0000000030000000; + } +#endif + cmd.event_mask = htole64(mask); rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, diff --git a/nimble/host/src/ble_iso.c b/nimble/host/src/ble_iso.c index 5c797836f..271bd27cb 100644 --- a/nimble/host/src/ble_iso.c +++ b/nimble/host/src/ble_iso.c @@ -17,12 +17,9 @@ * under the License. */ -#include <inttypes.h> #include "syscfg/syscfg.h" -#include "ble_hs_mbuf_priv.h" #if MYNEWT_VAL(BLE_ISO) - #include "os/os_mbuf.h" #include "host/ble_hs_log.h" #include "host/ble_hs.h" @@ -31,22 +28,85 @@ #include "sys/queue.h" #include "ble_hs_priv.h" #include "ble_hs_hci_priv.h" +#include "ble_hs_mbuf_priv.h" + +#define ble_iso_big_conn_handles_init(_big, _handles, _num_handles) \ + do { \ + struct ble_iso_conn *conn = SLIST_FIRST(&ble_iso_conns); \ + \ + for (uint8_t i = 0; i < (_num_handles); i++) { \ + while (conn != NULL) { \ + if (conn->type == BLE_ISO_CONN_BIS) { \ + struct ble_iso_bis *bis; \ + \ + bis = CONTAINER_OF(conn, struct ble_iso_bis, conn); \ + if (bis->big == (_big)) { \ + conn->handle = le16toh((_handles)[i]); \ + conn = SLIST_NEXT(conn, next); \ + break; \ + } \ + } \ + \ + conn = SLIST_NEXT(conn, next); \ + } \ + } \ + } while (0); + +enum ble_iso_conn_type { + BLE_ISO_CONN_BIS, +}; struct ble_iso_big { SLIST_ENTRY(ble_iso_big) next; uint8_t handle; uint16_t max_pdu; - uint8_t num_bis; - uint16_t conn_handles[MYNEWT_VAL(BLE_MAX_BIS)]; + uint8_t bis_cnt; ble_iso_event_fn *cb; void *cb_arg; }; +struct ble_iso_conn { + SLIST_ENTRY(ble_iso_conn) next; + enum ble_iso_conn_type type; + uint8_t handle; + + struct ble_iso_rx_data_info rx_info; + struct os_mbuf *rx_buf; + + ble_iso_event_fn *cb; + void *cb_arg; +}; + +struct ble_iso_bis { + struct ble_iso_conn conn; + struct ble_iso_big *big; +}; + static SLIST_HEAD(, ble_iso_big) ble_iso_bigs; +static SLIST_HEAD(, ble_iso_conn) ble_iso_conns; 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_bis_pool; +static os_membuf_t ble_iso_bis_mem[ + OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_MAX_BIS), sizeof (struct ble_iso_bis))]; + +static void +ble_iso_conn_append(struct ble_iso_conn *conn) +{ + struct ble_iso_conn *entry, *prev = NULL; + + SLIST_FOREACH(entry, &ble_iso_conns, next) { + prev = entry; + } + + if (prev == NULL) { + SLIST_INSERT_HEAD(&ble_iso_conns, conn, next); + } else { + SLIST_INSERT_AFTER(prev, conn, next); + } +} static int ble_iso_big_handle_set(struct ble_iso_big *big) @@ -106,6 +166,27 @@ ble_iso_big_alloc(void) return new_big; } +static struct ble_iso_bis * +ble_iso_bis_alloc(struct ble_iso_big *big) +{ + struct ble_iso_bis *new_bis; + + new_bis = os_memblock_get(&ble_iso_bis_pool); + if (new_bis == NULL) { + BLE_HS_LOG_ERROR("No more memory in pool\n"); + /* Out of memory. */ + return NULL; + } + + memset(new_bis, 0, sizeof *new_bis); + new_bis->conn.type = BLE_ISO_CONN_BIS; + new_bis->big = big; + + ble_iso_conn_append(&new_bis->conn); + + return new_bis; +} + static struct ble_iso_big * ble_iso_big_find_by_handle(uint8_t big_handle) { @@ -123,6 +204,22 @@ ble_iso_big_find_by_handle(uint8_t big_handle) static int ble_iso_big_free(struct ble_iso_big *big) { + struct ble_iso_conn *conn; + + SLIST_FOREACH(conn, &ble_iso_conns, next) { + struct ble_iso_bis *bis; + + if (conn->type != BLE_ISO_CONN_BIS) { + continue; + } + + bis = CONTAINER_OF(conn, struct ble_iso_bis, conn); + if (bis->big == big) { + SLIST_REMOVE(&ble_iso_conns, conn, ble_iso_conn, next); + os_memblock_put(&ble_iso_bis_pool, bis); + } + } + SLIST_REMOVE(&ble_iso_bigs, big, ble_iso_big, next); os_memblock_put(&ble_iso_big_pool, big); return 0; @@ -145,9 +242,21 @@ ble_iso_create_big(const struct ble_iso_create_big_params *create_params, return BLE_HS_ENOMEM; } + big->bis_cnt = create_params->bis_cnt; big->cb = create_params->cb; big->cb_arg = create_params->cb_arg; + for (uint8_t i = 0; i < create_params->bis_cnt; i++) { + struct ble_iso_bis *bis; + + bis = ble_iso_bis_alloc(big); + if (bis == NULL) { + ble_iso_big_free(big); + return BLE_HS_ENOMEM; + } + } + + cp.adv_handle = create_params->adv_handle; cp.num_bis = create_params->bis_cnt; put_le24(cp.sdu_interval, big_params->sdu_interval); cp.max_sdu = big_params->max_sdu; @@ -202,6 +311,12 @@ ble_iso_init(void) ble_iso_big_mem, "ble_iso_big_pool"); SYSINIT_PANIC_ASSERT(rc == 0); + rc = os_mempool_init(&ble_iso_bis_pool, + MYNEWT_VAL(BLE_MAX_BIS), + sizeof (struct ble_iso_bis), + ble_iso_bis_mem, "ble_iso_bis_pool"); + SYSINIT_PANIC_ASSERT(rc == 0); + return 0; } @@ -209,9 +324,7 @@ void ble_iso_rx_create_big_complete(const struct ble_hci_ev_le_subev_create_big_complete *ev) { struct ble_iso_event event; - struct ble_iso_big *big; - int i; big = ble_iso_big_find_by_handle(ev->big_handle); if (big == NULL) { @@ -219,29 +332,38 @@ ble_iso_rx_create_big_complete(const struct ble_hci_ev_le_subev_create_big_compl return; } - big->num_bis = ev->num_bis; - - for (i = 0; i < ev->num_bis; i++) { - big->conn_handles[i] = ev->conn_handle[i]; - } + memset(&event, 0, sizeof(event)); + event.type = BLE_ISO_EVENT_BIG_CREATE_COMPLETE; + event.big_created.status = ev->status; - big->max_pdu = ev->max_pdu; + if (event.big_created.status != 0) { + ble_iso_big_free(big); + } else { + if (big->bis_cnt != ev->num_bis) { + BLE_HS_LOG_ERROR("Unexpected num_bis=%d != bis_cnt=%d\n", + ev->num_bis, big->bis_cnt); + /* XXX: Should we destroy the group? */ + } - event.type = BLE_ISO_EVENT_BIG_CREATE_COMPLETE; - event.big_created.desc.big_handle = ev->big_handle; - event.big_created.desc.big_sync_delay = get_le24(ev->big_sync_delay); - event.big_created.desc.transport_latency_big = - get_le24(ev->transport_latency_big); - event.big_created.desc.phy = ev->phy; - event.big_created.desc.nse = ev->nse; - event.big_created.desc.bn = ev->bn; - event.big_created.desc.pto = ev->pto; - event.big_created.desc.irc = ev->irc; - event.big_created.desc.max_pdu = ev->max_pdu; - event.big_created.desc.iso_interval = ev->iso_interval; - event.big_created.desc.num_bis = ev->num_bis; - memcpy(event.big_created.desc.conn_handle, ev->conn_handle, - ev->num_bis * sizeof(uint16_t)); + ble_iso_big_conn_handles_init(big, ev->conn_handle, ev->num_bis); + + big->max_pdu = ev->max_pdu; + + event.big_created.desc.big_handle = ev->big_handle; + event.big_created.desc.big_sync_delay = get_le24(ev->big_sync_delay); + event.big_created.desc.transport_latency_big = + get_le24(ev->transport_latency_big); + event.big_created.desc.nse = ev->nse; + event.big_created.desc.bn = ev->bn; + event.big_created.desc.pto = ev->pto; + event.big_created.desc.irc = ev->irc; + event.big_created.desc.max_pdu = ev->max_pdu; + event.big_created.desc.iso_interval = ev->iso_interval; + event.big_created.desc.num_bis = ev->num_bis; + memcpy(event.big_created.desc.conn_handle, ev->conn_handle, + ev->num_bis * sizeof(uint16_t)); + event.big_created.phy = ev->phy; + } if (big->cb != NULL) { big->cb(&event, big->cb_arg); @@ -375,4 +497,440 @@ ble_iso_tx(uint16_t conn_handle, void *data, uint16_t data_len) return rc; } -#endif + +#if MYNEWT_VAL(BLE_ISO_BROADCAST_SINK) +static struct ble_iso_conn * +ble_iso_conn_lookup_handle(uint16_t handle) +{ + struct ble_iso_conn *conn; + + SLIST_FOREACH(conn, &ble_iso_conns, next) { + if (conn->handle == handle) { + return conn; + } + } + + return NULL; +} + +int +ble_iso_big_sync_create(const struct ble_iso_big_sync_create_params *param, + uint8_t *big_handle) +{ + struct ble_hci_le_big_create_sync_cp *cp; + uint8_t buf[sizeof(*cp) + MYNEWT_VAL(BLE_MAX_BIS)]; + struct ble_iso_big *big; + int rc; + + big = ble_iso_big_alloc(); + if (big == NULL) { + return BLE_HS_ENOMEM; + } + + big->bis_cnt = param->bis_cnt; + big->cb = param->cb; + big->cb_arg = param->cb_arg; + + cp = (void *)buf; + cp->big_handle = big->handle; + put_le16(&cp->sync_handle, param->sync_handle); + + if (param->broadcast_code != NULL) { + cp->encryption = BLE_HCI_ISO_BIG_ENCRYPTION_ENCRYPTED; + memcpy(cp->broadcast_code, param->broadcast_code, sizeof(cp->broadcast_code)); + } else { + cp->encryption = BLE_HCI_ISO_BIG_ENCRYPTION_UNENCRYPTED; + memset(cp->broadcast_code, 0, sizeof(cp->broadcast_code)); + } + + cp->mse = param->mse; + put_le16(&cp->sync_timeout, param->sync_timeout); + cp->num_bis = param->bis_cnt; + + for (uint8_t i = 0; i < param->bis_cnt; i++) { + struct ble_iso_bis *bis; + + bis = ble_iso_bis_alloc(big); + if (bis == NULL) { + ble_iso_big_free(big); + return BLE_HS_ENOMEM; + } + + bis->conn.cb = param->bis_params[i].cb; + bis->conn.cb_arg = param->bis_params[i].cb_arg; + + cp->bis[i] = param->bis_params[i].bis_index; + } + + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_BIG_CREATE_SYNC), + cp, sizeof(*cp) + cp->num_bis, NULL, 0); + if (rc != 0) { + ble_iso_big_free(big); + } else { + *big_handle = big->handle; + } + + return rc; +} + +int +ble_iso_big_sync_terminate(uint8_t big_handle) +{ + struct ble_hci_le_big_terminate_sync_cp cp; + struct ble_hci_le_big_terminate_sync_rp rp; + struct ble_iso_big *big; + int rc; + + big = ble_iso_big_find_by_handle(big_handle); + if (big == NULL) { + BLE_HS_LOG_ERROR("No BIG with handle=%d\n", big_handle); + return BLE_HS_ENOENT; + } + + 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), &rp, sizeof(rp)); + if (rc == 0) { + struct ble_iso_event event; + ble_iso_event_fn *cb; + void *cb_arg; + + event.type = BLE_ISO_EVENT_BIG_SYNC_TERMINATED; + event.big_terminated.big_handle = big_handle; + event.big_terminated.reason = BLE_ERR_CONN_TERM_LOCAL; + + cb = big->cb; + cb_arg = big->cb_arg; + + ble_iso_big_free(big); + + if (cb != NULL) { + cb(&event, cb_arg); + } + } + + return rc; +} + +void +ble_iso_rx_big_sync_established(const struct ble_hci_ev_le_subev_big_sync_established *ev) +{ + struct ble_iso_event event; + struct ble_iso_big *big; + ble_iso_event_fn *cb; + void *cb_arg; + + big = ble_iso_big_find_by_handle(ev->big_handle); + if (big == NULL) { + return; + } + + cb = big->cb; + cb_arg = big->cb_arg; + + memset(&event, 0, sizeof(event)); + event.type = BLE_ISO_EVENT_BIG_SYNC_ESTABLISHED; + event.big_sync_established.status = ev->status; + + if (event.big_sync_established.status != 0) { + ble_iso_big_free(big); + } else { + if (big->bis_cnt != ev->num_bis) { + BLE_HS_LOG_ERROR("Unexpected num_bis=%d != bis_cnt=%d\n", + ev->num_bis, big->bis_cnt); + /* XXX: Should we destroy the group? */ + } + + ble_iso_big_conn_handles_init(big, ev->conn_handle, ev->num_bis); + + event.big_sync_established.desc.big_handle = ev->big_handle; + event.big_sync_established.desc.transport_latency_big = + get_le24(ev->transport_latency_big); + event.big_sync_established.desc.nse = ev->nse; + event.big_sync_established.desc.bn = ev->bn; + event.big_sync_established.desc.pto = ev->pto; + event.big_sync_established.desc.irc = ev->irc; + event.big_sync_established.desc.max_pdu = le16toh(ev->max_pdu); + event.big_sync_established.desc.iso_interval = le16toh(ev->iso_interval); + event.big_sync_established.desc.num_bis = ev->num_bis; + memcpy(event.big_sync_established.desc.conn_handle, ev->conn_handle, + ev->num_bis * sizeof(uint16_t)); + } + + if (cb != NULL) { + cb(&event, cb_arg); + } +} + +void +ble_iso_rx_big_sync_lost(const struct ble_hci_ev_le_subev_big_sync_lost *ev) +{ + struct ble_iso_event event; + struct ble_iso_big *big; + ble_iso_event_fn *cb; + void *cb_arg; + + big = ble_iso_big_find_by_handle(ev->big_handle); + if (big == NULL) { + BLE_HS_LOG_ERROR("No BIG with handle=%d\n", ev->big_handle); + return; + } + + event.type = BLE_ISO_EVENT_BIG_SYNC_TERMINATED; + event.big_terminated.big_handle = ev->big_handle; + event.big_terminated.reason = ev->reason; + + cb = big->cb; + cb_arg = big->cb_arg; + + ble_iso_big_free(big); + + if (cb != NULL) { + cb(&event, cb_arg); + } +} + +static int +ble_iso_rx_data_info_parse(struct os_mbuf *om, bool ts_available, + struct ble_iso_rx_data_info *info) +{ + struct ble_hci_iso_data *iso_data; + uint16_t u16; + + if (ts_available) { + if (os_mbuf_len(om) < sizeof(info->ts)) { + BLE_HS_LOG_DEBUG("Data missing\n"); + return BLE_HS_EMSGSIZE; + } + + info->ts = get_le32(om->om_data); + os_mbuf_adj(om, sizeof(info->ts)); + } else { + info->ts = 0; + } + + if (os_mbuf_len(om) < sizeof(*iso_data)) { + BLE_HS_LOG_DEBUG("Data missing\n"); + return BLE_HS_EMSGSIZE; + } + + iso_data = (void *)(om->om_data); + + info->seq_num = le16toh(iso_data->packet_seq_num); + u16 = le16toh(iso_data->sdu_len); + info->sdu_len = BLE_HCI_ISO_SDU_LENGTH(u16); + info->status = BLE_HCI_ISO_PKT_STATUS_FLAG(u16); + info->ts_valid = ts_available; + + os_mbuf_adj(om, sizeof(*iso_data)); + + return 0; +} + +static void +ble_iso_conn_rx_reset(struct ble_iso_conn *conn) +{ + memset(&conn->rx_info, 0, sizeof(conn->rx_info)); + conn->rx_buf = NULL; +} + +static void +ble_iso_conn_rx_data_discard(struct ble_iso_conn *conn) +{ + os_mbuf_free_chain(conn->rx_buf); + ble_iso_conn_rx_reset(conn); +} + +static void +ble_iso_event_iso_rx_emit(struct ble_iso_conn *conn) +{ + struct ble_iso_event event = { + .type = BLE_ISO_EVENT_ISO_RX, + .iso_rx.conn_handle = conn->handle, + .iso_rx.info = &conn->rx_info, + .iso_rx.om = conn->rx_buf, + }; + + if (conn->cb != NULL) { + conn->cb(&event, conn->cb_arg); + } +} + +static int +ble_iso_conn_rx_data_load(struct ble_iso_conn *conn, struct os_mbuf *frag, + uint8_t pb_flag, bool ts_available, void *arg) +{ + int len_remaining; + int rc; + + switch (pb_flag) { + case BLE_HCI_ISO_PB_FIRST: + case BLE_HCI_ISO_PB_COMPLETE: + if (conn->rx_buf != NULL) { + /* Previous data packet never completed. Discard old packet. */ + ble_iso_conn_rx_data_discard(conn); + } + + rc = ble_iso_rx_data_info_parse(frag, ts_available, &conn->rx_info); + if (rc != 0) { + return rc; + } + + conn->rx_buf = frag; + break; + + case BLE_HCI_ISO_PB_CONTINUATION: + case BLE_HCI_ISO_PB_LAST: + if (conn->rx_buf == NULL) { + /* Last fragment without the start. Discard new packet. */ + return BLE_HS_EBADDATA; + } + + /* Determine whether the total length won't exceed the declared SDU length */ + len_remaining = conn->rx_info.sdu_len - OS_MBUF_PKTLEN(conn->rx_buf); + if (len_remaining - os_mbuf_len(frag) < 0) { + /* SDU Length exceeded. Discard all packets. */ + ble_iso_conn_rx_data_discard(conn); + return BLE_HS_EBADDATA; + } + + os_mbuf_concat(conn->rx_buf, frag); + break; + + default: + BLE_HS_LOG_ERROR("Invalid pb_flag %d\n", pb_flag); + return BLE_HS_EBADDATA; + } + + if (pb_flag == BLE_HCI_ISO_PB_COMPLETE || pb_flag == BLE_HCI_ISO_PB_LAST) { + ble_iso_event_iso_rx_emit(conn); + ble_iso_conn_rx_reset(conn); + } + + return 0; +} + +int +ble_iso_rx_data(struct os_mbuf *om, void *arg) +{ + struct ble_iso_conn *conn; + struct ble_hci_iso *hci_iso; + uint16_t conn_handle; + uint16_t length; + uint16_t pb_flag; + uint16_t ts_flag; + uint16_t u16; + int rc; + + if (os_mbuf_len(om) < sizeof(*hci_iso)) { + BLE_HS_LOG_DEBUG("Data missing\n"); + os_mbuf_free_chain(om); + return BLE_HS_EMSGSIZE; + } + + hci_iso = (void *)om->om_data; + + u16 = le16toh(hci_iso->handle); + conn_handle = BLE_HCI_ISO_CONN_HANDLE(u16); + pb_flag = BLE_HCI_ISO_PB_FLAG(u16); + ts_flag = BLE_HCI_ISO_TS_FLAG(u16); + length = BLE_HCI_ISO_LENGTH(le16toh(hci_iso->length)); + + os_mbuf_adj(om, sizeof(*hci_iso)); + + if (os_mbuf_len(om) < length) { + BLE_HS_LOG_DEBUG("Data missing\n"); + os_mbuf_free_chain(om); + return BLE_HS_EMSGSIZE; + } + + conn = ble_iso_conn_lookup_handle(conn_handle); + if (conn == NULL) { + BLE_HS_LOG_DEBUG("Unknown handle=%d\n", conn_handle); + os_mbuf_free_chain(om); + return BLE_HS_EMSGSIZE; + } + + rc = ble_iso_conn_rx_data_load(conn, om, pb_flag, ts_flag > 0, arg); + if (rc != 0) { + os_mbuf_free_chain(om); + return rc; + } + + return 0; +} +#endif /* BLE_ISO_BROADCAST_SINK */ + +int +ble_iso_data_path_setup(const struct ble_iso_data_path_setup_params *param) +{ + struct ble_hci_le_setup_iso_data_path_rp rp; + struct ble_hci_le_setup_iso_data_path_cp *cp; + uint8_t buf[sizeof(*cp) + UINT8_MAX]; + int rc; + + if (param->codec_config_len > 0 && param->codec_config == NULL) { + BLE_HS_LOG_ERROR("Missing codec_config\n"); + return BLE_HS_EINVAL; + } + + cp = (void *)buf; + put_le16(&cp->conn_handle, param->conn_handle); + cp->data_path_dir = 0; + + if (param->data_path_dir & BLE_ISO_DATA_DIR_TX) { + /* Input (Host to Controller) */ + cp->data_path_dir |= BLE_HCI_ISO_DATA_PATH_DIR_INPUT; + } + + if (param->data_path_dir & BLE_ISO_DATA_DIR_RX) { + /* Output (Controller to Host) */ + cp->data_path_dir |= BLE_HCI_ISO_DATA_PATH_DIR_OUTPUT; + } + + cp->data_path_id = param->data_path_id; + cp->codec_id[0] = param->codec_id.format; + put_le16(&cp->codec_id[1], param->codec_id.company_id); + put_le16(&cp->codec_id[3], param->codec_id.vendor_specific); + put_le24(cp->controller_delay, param->ctrl_delay); + + cp->codec_config_len = param->codec_config_len; + memcpy(cp->codec_config, param->codec_config, cp->codec_config_len); + + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_SETUP_ISO_DATA_PATH), + cp, sizeof(*cp) + cp->codec_config_len, &rp, + sizeof(rp)); + + return rc; +} + +int +ble_iso_data_path_remove(const struct ble_iso_data_path_remove_params *param) +{ + struct ble_hci_le_remove_iso_data_path_rp rp; + struct ble_hci_le_remove_iso_data_path_cp cp = { 0 }; + int rc; + + put_le16(&cp.conn_handle, param->conn_handle); + + if (param->data_path_dir & BLE_ISO_DATA_DIR_TX) { + /* Input (Host to Controller) */ + cp.data_path_dir |= BLE_HCI_ISO_DATA_PATH_DIR_INPUT; + } + + if (param->data_path_dir & BLE_ISO_DATA_DIR_RX) { + /* Output (Controller to Host) */ + cp.data_path_dir |= BLE_HCI_ISO_DATA_PATH_DIR_OUTPUT; + } + + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_REMOVE_ISO_DATA_PATH), + &cp, sizeof(cp), &rp, sizeof(rp)); + + return rc; +} +#endif /* BLE_ISO */ diff --git a/nimble/host/src/ble_iso_priv.h b/nimble/host/src/ble_iso_priv.h index fb0a6aab3..9005b5067 100644 --- a/nimble/host/src/ble_iso_priv.h +++ b/nimble/host/src/ble_iso_priv.h @@ -31,6 +31,15 @@ ble_iso_rx_create_big_complete(const struct ble_hci_ev_le_subev_create_big_compl void ble_iso_rx_terminate_big_complete(const struct ble_hci_ev_le_subev_terminate_big_complete *ev); +void +ble_iso_rx_big_sync_established(const struct ble_hci_ev_le_subev_big_sync_established *ev); + +void +ble_iso_rx_big_sync_lost(const struct ble_hci_ev_le_subev_big_sync_lost *ev); + +int +ble_iso_rx_data(struct os_mbuf *om, void *arg); + #ifdef __cplusplus } #endif diff --git a/nimble/include/nimble/hci_common.h b/nimble/include/nimble/hci_common.h index 62c62d0fb..fb74c24aa 100644 --- a/nimble/include/nimble/hci_common.h +++ b/nimble/include/nimble/hci_common.h @@ -2087,31 +2087,45 @@ struct hci_data_hdr #define BLE_HCI_PB_FIRST_FLUSH 2 #define BLE_HCI_PB_FULL 3 -#define BLE_HCI_ISO_CONN_HANDLE_MASK (0x07ff) -#define BLE_HCI_ISO_PB_FLAG_MASK (0x3000) -#define BLE_HCI_ISO_TS_FLAG_MASK (0x4000) -#define BLE_HCI_ISO_LENGTH_MASK (0x7fff) +#define BLE_HCI_ISO_CONN_HANDLE_MASK (0x07ff) +#define BLE_HCI_ISO_PB_FLAG_MASK (0x3000) +#define BLE_HCI_ISO_TS_FLAG_MASK (0x4000) +#define BLE_HCI_ISO_LENGTH_MASK (0x7fff) +#define BLE_HCI_ISO_SDU_LENGTH_MASK (0x0fff) +#define BLE_HCI_ISO_PKT_STATUS_FLAG_MASK (0xC000) #define BLE_HCI_ISO_HANDLE(ch, pb, ts) ((ch) | ((pb) << 12) | ((ts) << 14)) #define BLE_HCI_ISO_CONN_HANDLE(h) ((h) & BLE_HCI_ISO_CONN_HANDLE_MASK) #define BLE_HCI_ISO_PB_FLAG(h) (((h) & BLE_HCI_ISO_PB_FLAG_MASK) >> 12) -#define BLE_HCI_ISO_TS_FLAG(h) ((h) & BLE_HCI_ISO_TS_FLAG_MASK) +#define BLE_HCI_ISO_TS_FLAG(h) (((h) & BLE_HCI_ISO_TS_FLAG_MASK) >> 14) #define BLE_HCI_ISO_LENGTH(l) ((l) & BLE_HCI_ISO_LENGTH_MASK) +#define BLE_HCI_ISO_SDU_LENGTH(l) ((l) & BLE_HCI_ISO_SDU_LENGTH_MASK) +#define BLE_HCI_ISO_PKT_STATUS_FLAG(l) (((l) & BLE_HCI_ISO_PKT_STATUS_FLAG_MASK) >> 14) #define BLE_HCI_ISO_PB_FIRST (0) #define BLE_HCI_ISO_PB_CONTINUATION (1) #define BLE_HCI_ISO_PB_COMPLETE (2) #define BLE_HCI_ISO_PB_LAST (3) +#define BLE_HCI_ISO_PKT_STATUS_VALID 0x00 +#define BLE_HCI_ISO_PKT_STATUS_INVALID 0x01 +#define BLE_HCI_ISO_PKT_STATUS_LOST 0x10 + #define BLE_HCI_ISO_BIG_HANDLE_MIN 0x00 #define BLE_HCI_ISO_BIG_HANDLE_MAX 0xEF +#define BLE_HCI_ISO_BIG_ENCRYPTION_UNENCRYPTED 0x00 +#define BLE_HCI_ISO_BIG_ENCRYPTION_ENCRYPTED 0x01 + +#define BLE_HCI_ISO_DATA_PATH_DIR_INPUT 0x00 +#define BLE_HCI_ISO_DATA_PATH_DIR_OUTPUT 0x01 + struct ble_hci_iso { uint16_t handle; uint16_t length; uint8_t data[0]; -}; +} __attribute__((packed)); #define BLE_HCI_ISO_HDR_SDU_LENGTH_MASK (0x07ff) @@ -2119,7 +2133,7 @@ struct ble_hci_iso_data { uint16_t packet_seq_num; uint16_t sdu_len; uint8_t data[0]; -}; +} __attribute__((packed)); #ifdef __cplusplus } diff --git a/nimble/transport/socket/src/ble_hci_socket.c b/nimble/transport/socket/src/ble_hci_socket.c index 75a813dba..0d52ec584 100644 --- a/nimble/transport/socket/src/ble_hci_socket.c +++ b/nimble/transport/socket/src/ble_hci_socket.c @@ -103,11 +103,13 @@ STATS_SECT_START(hci_sock_stats) STATS_SECT_ENTRY(icmd) STATS_SECT_ENTRY(ievt) STATS_SECT_ENTRY(iacl) + STATS_SECT_ENTRY(iiso) STATS_SECT_ENTRY(ibytes) STATS_SECT_ENTRY(ierr) STATS_SECT_ENTRY(imem) STATS_SECT_ENTRY(omsg) STATS_SECT_ENTRY(oacl) + STATS_SECT_ENTRY(oiso) STATS_SECT_ENTRY(ocmd) STATS_SECT_ENTRY(oevt) STATS_SECT_ENTRY(obytes) @@ -120,11 +122,13 @@ STATS_NAME_START(hci_sock_stats) STATS_NAME(hci_sock_stats, icmd) STATS_NAME(hci_sock_stats, ievt) STATS_NAME(hci_sock_stats, iacl) + STATS_NAME(hci_sock_stats, iiso) STATS_NAME(hci_sock_stats, ibytes) STATS_NAME(hci_sock_stats, ierr) STATS_NAME(hci_sock_stats, imem) STATS_NAME(hci_sock_stats, omsg) STATS_NAME(hci_sock_stats, oacl) + STATS_NAME(hci_sock_stats, oiso) STATS_NAME(hci_sock_stats, ocmd) STATS_NAME(hci_sock_stats, oevt) STATS_NAME(hci_sock_stats, obytes) @@ -142,6 +146,7 @@ STATS_NAME_END(hci_sock_stats) #define BLE_HCI_UART_H4_ACL 0x02 #define BLE_HCI_UART_H4_SCO 0x03 #define BLE_HCI_UART_H4_EVT 0x04 +#define BLE_HCI_UART_H4_ISO 0x05 #define BLE_HCI_UART_H4_SYNC_LOSS 0x80 #define BLE_HCI_UART_H4_SKIP_CMD 0x81 #define BLE_HCI_UART_H4_SKIP_ACL 0x82 @@ -483,6 +488,39 @@ ble_hci_sock_rx_msg(void) ble_transport_to_ll_acl(m); #else ble_transport_to_hs_acl(m); +#endif + OS_EXIT_CRITICAL(sr); + break; + case BLE_HCI_UART_H4_ISO: + if (bhss->rx_off < BLE_HCI_DATA_HDR_SZ) { + return -1; + } + len = 1 + BLE_HCI_DATA_HDR_SZ + (bhss->rx_data[4] << 8) + + bhss->rx_data[3]; + if (bhss->rx_off < len) { + return -1; + } + STATS_INC(hci_sock_stats, imsg); + STATS_INC(hci_sock_stats, iiso); +#if MYNEWT_VAL(BLE_CONTROLLER) + m = ble_transport_alloc_iso_from_hs(); +#else + m = ble_transport_alloc_iso_from_ll(); +#endif + if (!m) { + STATS_INC(hci_sock_stats, imem); + break; + } + if (os_mbuf_append(m, &bhss->rx_data[1], len - 1)) { + STATS_INC(hci_sock_stats, imem); + os_mbuf_free_chain(m); + break; + } + OS_ENTER_CRITICAL(sr); +#if MYNEWT_VAL(BLE_CONTROLLER) + ble_transport_to_ll_iso(m); +#else + ble_transport_to_hs_iso(m); #endif OS_EXIT_CRITICAL(sr); break;
