diff --git a/nimble/controller/include/controller/ble_ll_iso.h b/nimble/controller/include/controller/ble_ll_iso.h index 2944b07476..e2b3a2a85b 100644 --- a/nimble/controller/include/controller/ble_ll_iso.h +++ b/nimble/controller/include/controller/ble_ll_iso.h @@ -26,7 +26,6 @@ extern "C" { #endif -int ble_ll_iso_read_tx_sync(const uint8_t *cmdbuf, uint8_t len); int ble_ll_iso_set_cig_param(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf, uint8_t *rsplen); int ble_ll_iso_set_cig_param_test(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf, uint8_t *rsplen); int ble_ll_iso_create_cis(const uint8_t *cmdbuf, uint8_t len); diff --git a/nimble/controller/src/ble_ll_hci.c b/nimble/controller/src/ble_ll_hci.c index c2b3af71ab..098ec3f0ce 100644 --- a/nimble/controller/src/ble_ll_hci.c +++ b/nimble/controller/src/ble_ll_hci.c @@ -1276,10 +1276,7 @@ ble_ll_hci_le_cmd_proc(const uint8_t *cmdbuf, uint8_t len, uint16_t ocf, break; #endif /* BLE_LL_ISO_BROADCASTER */ #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_ISO) - case BLE_HCI_OCF_LE_READ_ISO_TX_SYNC: - rc = ble_ll_iso_read_tx_sync(cmdbuf, len); - break; - case BLE_HCI_OCF_LE_SET_CIG_PARAM: + case BLE_HCI_OCF_LE_SET_CIG_PARAMS: rc = ble_ll_iso_set_cig_param(cmdbuf, len, rspbuf, rsplen); break; case BLE_HCI_OCF_LE_CREATE_CIS: diff --git a/nimble/controller/src/ble_ll_iso.c b/nimble/controller/src/ble_ll_iso.c index a6186fe1a5..7d12fbf0e1 100644 --- a/nimble/controller/src/ble_ll_iso.c +++ b/nimble/controller/src/ble_ll_iso.c @@ -25,12 +25,6 @@ #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_ISO) -int -ble_ll_iso_read_tx_sync(const uint8_t *cmdbuf, uint8_t len) -{ - return BLE_ERR_UNSUPPORTED; -} - int ble_ll_iso_set_cig_param(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf, uint8_t *rsplen) diff --git a/nimble/host/include/host/ble_iso.h b/nimble/host/include/host/ble_iso.h new file mode 100644 index 0000000000..41ebc10159 --- /dev/null +++ b/nimble/host/include/host/ble_iso.h @@ -0,0 +1,131 @@ +/* + * 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. + */ + +#ifndef H_BLE_ISO_ +#define H_BLE_ISO_ +#include "syscfg/syscfg.h" + +#if MYNEWT_VAL(BLE_ISO) + +#include + +enum { + BLE_ISO_CONNECT_REQUEST_EVENT, + BLE_ISO_CIS_ESTABLISHED_EVENT, + BLE_ISO_CIS_DISCONNECTED_EVENT, + BLE_ISO_BIG_CREATE_COMPLETE_EVENT, + BLE_ISO_BIG_ESTABLISHED_EVENT, + BLE_ISO_BIG_TERMINATE_EVENT, + BLE_ISO_BIG_SYNC_LOST_EVENT, + BLE_ISO_DATA_EVENT, +}; + +struct ble_iso_event { + + uint8_t type; + + union { + /* BLE ISO CONNECT REQUEST */ + struct { + uint16_t conn_handle; + uint16_t cis_handle; + } cis_connect_req; + + /* BLE_ISO_CIS_ESTABLISHED_EVENT */ + struct { + uint8_t status; + uint16_t cis_handle; + } cis_established; + + /* BLE_ISO_CIS_DISCONNECTED_EVENT */ + struct { + uint8_t status; + uint16_t cis_handle; + } cis_disconnected; + + /* BLE_ISO_BIG_CREATE_COMPLETE_EVENT, + * BLE_ISO_BIG_ESTABLISHED_EVENT*/ + struct { + uint8_t status; + uint8_t big_handle; + uint8_t bis_cnt; + uint16_t bis[0]; + } big_complete; + + /*BLE_ISO_BIG_SYNC_LOST_EVENT */ + struct { + uint8_t big_handle; + uint8_t reason; + } big_terminate_lost; + + /* BLE_ISO_DATA_EVENT - for both cis/bis_handles */ + struct { + struct os_mbuf *om; + uint16_t handle; + } iso_data; + }; +}; + +typedef int ble_iso_event_fn(struct ble_iso_event *event, void *arg); +int ble_iso_server_register_le_audio(ble_iso_event_fn *cb, void *cb_arg); + +struct ble_iso_cig_params { + uint32_t sdu_c_to_p_itvl; + uint32_t sdu_p_to_c_itvl; + uint8_t sca; + uint8_t packing; + uint8_t framing; + uint8_t max_c_to_p_latency; + uint8_t max_p_to_c_latency; +#if MYNEWT_VAL(BLE_ISO_TEST) + uint8_t ft_c_to_p; + uint8_t ft_p_to_c; + uint16_t iso_itvl; +#endif +}; + +struct ble_iso_cis_params { + uint16_t max_sdu_c_to_p; + uint16_t max_sdu_p_to_c; + uint8_t phy_c_to_p; + uint8_t phy_p_to_c; + uint8_t rnt_c_to_p; + uint8_t rnt_p_to_c; +}; + +int ble_iso_client_create_cig(struct ble_iso_cig_params *cig_params, + uint8_t cis_cnt, + struct ble_iso_cis_params *cis_params, + uint8_t *cig, uint8_t *cis_handles); +int ble_iso_client_remove_group(uint8_t cig_id); +int ble_iso_client_create_cis(uint8_t cig, uint8_t cis_cnt, + struct ble_hci_le_create_cis_params *params, + ble_iso_event_fn *cb, void *cb_arg); + +int ble_iso_create_big_sync(uint8_t *out_big_handle, uint16_t sync_handle, + bool encrypted, uint8_t *broadcast_code, + uint8_t mse, uint16_t sync_timeout, + uint8_t num_of_bis, uint8_t *bis, + ble_iso_event_fn *cb, void *cb_arg); +int ble_iso_terminate_big_sync(uint8_t big_handle); + +int ble_iso_init(void); +int ble_iso_tx(uint8_t cig_id, struct os_mbuf *om); +#endif +#endif diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c index 4736415a37..bee335fb81 100644 --- a/nimble/host/src/ble_gap.c +++ b/nimble/host/src/ble_gap.c @@ -26,6 +26,7 @@ #include "host/ble_hs_hci.h" #include "ble_hs_priv.h" #include "ble_gap_priv.h" +#include "ble_iso_priv.h" #ifndef min #define min(a, b) ((a) < (b) ? (a) : (b)) @@ -1206,7 +1207,10 @@ ble_gap_conn_broken(uint16_t conn_handle, int reason) rc = ble_gap_find_snapshot(conn_handle, &snap); if (rc != 0) { +#if MYNEWT_VAL(BLE_ISO) /* No longer connected. */ + ble_iso_disconnected_event(conn_handle, reason, false); +#endif return; } @@ -1230,7 +1234,10 @@ ble_gap_conn_broken(uint16_t conn_handle, int reason) ble_sm_connection_broken(conn_handle); ble_gatts_connection_broken(conn_handle); ble_gattc_connection_broken(conn_handle); - ble_hs_flow_connection_broken(conn_handle);; + ble_hs_flow_connection_broken(conn_handle); +#if MYNEWT_VAL(BLE_ISO) + ble_iso_disconnected_event(conn_handle, reason, true); +#endif ble_hs_atomic_conn_delete(conn_handle); diff --git a/nimble/host/src/ble_hs.c b/nimble/host/src/ble_hs.c index bab8fe1116..ddbf10ca04 100644 --- a/nimble/host/src/ble_hs.c +++ b/nimble/host/src/ble_hs.c @@ -74,6 +74,7 @@ static struct ble_npl_callout ble_hs_timer; static struct ble_npl_eventq *ble_hs_evq; static struct ble_mqueue ble_hs_rx_q; +static struct ble_mqueue ble_hs_rx_iso_q; static struct ble_npl_mutex ble_hs_mutex; @@ -224,6 +225,16 @@ ble_hs_process_rx_data_queue(void) } } +void +ble_hs_process_rx_iso_data_queue(void) +{ + struct os_mbuf *om; + + while ((om = ble_mqueue_get(&ble_hs_rx_iso_q)) != NULL) { + ble_hs_hci_evt_iso_process(om); + } +} + static int ble_hs_wakeup_tx_conn(struct ble_hs_conn *conn) { @@ -301,6 +312,10 @@ ble_hs_clear_rx_queue(void) while ((om = ble_mqueue_get(&ble_hs_rx_q)) != NULL) { os_mbuf_free_chain(om); } + + while ((om = ble_mqueue_get(&ble_hs_rx_iso_q)) != NULL) { + os_mbuf_free_chain(om); + } } int @@ -511,6 +526,12 @@ ble_hs_event_rx_data(struct ble_npl_event *ev) ble_hs_process_rx_data_queue(); } +static void +ble_hs_event_rx_iso_data(struct ble_npl_event *ev) +{ + ble_hs_process_rx_iso_data_queue(); +} + static void ble_hs_event_reset(struct ble_npl_event *ev) { @@ -675,6 +696,24 @@ ble_hs_rx_data(struct os_mbuf *om, void *arg) return 0; } +static int +ble_hs_rx_iso(struct os_mbuf *om, void *arg) +{ + int rc; + + /* If flow control is enabled, mark this packet with its corresponding + * connection handle. + */ + + rc = ble_mqueue_put(&ble_hs_rx_iso_q, ble_hs_evq, om); + if (rc != 0) { + os_mbuf_free_chain(om); + return BLE_HS_EOS; + } + + return 0; +} + /** * Enqueues an ACL data packet for transmission. This function consumes the * supplied mbuf, regardless of the outcome. @@ -755,6 +794,7 @@ ble_hs_init(void) ble_hs_stop_init(); ble_mqueue_init(&ble_hs_rx_q, ble_hs_event_rx_data, NULL); + ble_mqueue_init(&ble_hs_rx_iso_q, ble_hs_event_rx_iso_data, NULL); rc = stats_init_and_reg( STATS_HDR(ble_hs_stats), STATS_SIZE_INIT_PARMS(ble_hs_stats, @@ -805,9 +845,7 @@ ble_transport_to_hs_acl_impl(struct os_mbuf *om) int ble_transport_to_hs_iso_impl(struct os_mbuf *om) { - os_mbuf_free_chain(om); - - return 0; + return ble_hs_rx_iso(om, NULL); } void diff --git a/nimble/host/src/ble_hs_hci_evt.c b/nimble/host/src/ble_hs_hci_evt.c index a128e3ab6b..b019fc739b 100644 --- a/nimble/host/src/ble_hs_hci_evt.c +++ b/nimble/host/src/ble_hs_hci_evt.c @@ -24,6 +24,7 @@ #include "nimble/hci_common.h" #include "host/ble_gap.h" #include "ble_hs_priv.h" +#include "ble_iso_priv.h" _Static_assert(sizeof (struct hci_data_hdr) == BLE_HCI_DATA_HDR_SZ, "struct hci_data_hdr must be 4 bytes"); @@ -73,6 +74,14 @@ static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_transmit_power_report; #if MYNEWT_VAL(BLE_CONN_SUBRATING) static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_subrate_change; #endif +#if NIMBLE_BLE_CONNECT +static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_cis_request; +static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_cis_established; +static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_big_completed; +static ble_hs_hci_evt_le_fn ble_hs_hci_le_big_terminate_complete; +static ble_hs_hci_evt_le_fn ble_hs_hci_le_big_sync_established; +static ble_hs_hci_evt_le_fn ble_hs_hci_le_big_sync_lost; +#endif /* Statistics */ struct host_hci_stats { @@ -141,6 +150,14 @@ static ble_hs_hci_evt_le_fn * const ble_hs_hci_evt_le_dispatch[] = { #if MYNEWT_VAL(BLE_CONN_SUBRATING) [BLE_HCI_LE_SUBEV_SUBRATE_CHANGE] = ble_hs_hci_evt_le_subrate_change, #endif +#if NIMBLE_BLE_CONNECT + [BLE_HCI_LE_SUBEV_CIS_ESTABLISHED] = ble_hs_hci_evt_le_cis_established, + [BLE_HCI_LE_SUBEV_CIS_REQUEST] = ble_hs_hci_evt_le_cis_request, + [BLE_HCI_LE_SUBEV_CREATE_BIG_COMPLETE] = ble_hs_hci_evt_le_big_completed, + [BLE_HCI_LE_SUBEV_TERMINATE_BIG_COMPLETE] = ble_hs_hci_le_big_terminate_complete, + [BLE_HCI_LE_SUBEV_BIG_SYNC_ESTABLISHED] = ble_hs_hci_le_big_sync_established, + [BLE_HCI_LE_SUBEV_BIG_SYNC_LOST] = ble_hs_hci_le_big_sync_lost +#endif }; #define BLE_HS_HCI_EVT_LE_DISPATCH_SZ \ @@ -824,6 +841,108 @@ ble_hs_hci_evt_le_subrate_change(uint8_t subevent, const void *data, #endif #if NIMBLE_BLE_CONNECT +static int +ble_hs_hci_evt_le_cis_established(uint8_t subevent, const void *data, + unsigned int len) +{ +#if MYNEWT_VAL(BLE_ISO) + const struct ble_hci_ev_le_subev_cis_established *ev = data; + + if (len != sizeof(*ev)) { + return BLE_HS_ECONTROLLER; + } + + return ble_iso_rx_hci_evt_le_cis_established(ev); +#else + return BLE_HS_ENOTSUP; +#endif +} + +static int +ble_hs_hci_evt_le_cis_request(uint8_t subevent, const void *data, + unsigned int len) +{ +#if MYNEWT_VAL(BLE_ISO) + const struct ble_hci_ev_le_subev_cis_request *ev = data; + + if (len != sizeof(*ev)) { + return BLE_HS_ECONTROLLER; + } + + return ble_iso_rx_hci_evt_le_cis_request(ev); +#else + return BLE_HS_ENOTSUP; +#endif +} + +static int +ble_hs_hci_evt_le_big_completed(uint8_t subevent, const void *data, + unsigned int len) +{ +#if MYNEWT_VAL(BLE_ISO) + const struct ble_hci_ev_le_subev_create_big_complete *ev = data; + + if (len != sizeof(*ev)) { + return BLE_HS_ECONTROLLER; + } + + return ble_iso_rx_hci_evt_le_big_completed(ev); +#else + return BLE_HS_ENOTSUP; +#endif +} + +static int +ble_hs_hci_le_big_terminate_complete(uint8_t subevent, const void *data, + unsigned int len) +{ +#if MYNEWT_VAL(BLE_ISO) + const struct ble_hci_ev_le_subev_terminate_big_complete *ev = data; + + if (len != sizeof(*ev)) { + return BLE_HS_ECONTROLLER; + } + + return ble_iso_rx_hci_le_big_terminate_complete(ev); +#else + return BLE_HS_ENOTSUP; +#endif +} + +static int +ble_hs_hci_le_big_sync_established(uint8_t subevent, const void *data, + unsigned int len) +{ +#if MYNEWT_VAL(BLE_ISO) + const struct ble_hci_ev_le_subev_big_sync_established *ev = data; + + if (len < sizeof(*ev)) { + return BLE_HS_ECONTROLLER; + } + + return ble_iso_hci_le_big_sync_established(ev); +#else + return BLE_HS_ENOTSUP; +#endif +} + +static int +ble_hs_hci_le_big_sync_lost(uint8_t subevent, const void *data, + unsigned int len) +{ +#if MYNEWT_VAL(BLE_ISO) + const struct ble_hci_ev_le_subev_big_sync_lost *ev = data; + + if (len != sizeof(*ev)) { + return BLE_HS_ECONTROLLER; + } + + return ble_iso_hci_le_big_sync_lost(ev); +#else + return BLE_HS_ENOTSUP; +#endif +} + static int ble_hs_hci_evt_le_conn_upd_complete(uint8_t subevent, const void *data, unsigned int len) @@ -930,6 +1049,15 @@ ble_hs_hci_evt_process(struct ble_hci_ev *ev) return rc; } +int +ble_hs_hci_evt_iso_process(struct os_mbuf *om) +{ +#if MYNEWT_VAL(BLE_ISO) + ble_hs_log_mbuf(om); + ble_iso_rx(om); +#endif + return 0; +} /** * Called when a data packet is received from the controller. This function * consumes the supplied mbuf, regardless of the outcome. diff --git a/nimble/host/src/ble_hs_priv.h b/nimble/host/src/ble_hs_priv.h index 393b4d4323..5e63d75e10 100644 --- a/nimble/host/src/ble_hs_priv.h +++ b/nimble/host/src/ble_hs_priv.h @@ -101,12 +101,14 @@ extern uint16_t ble_hs_max_client_configs; void ble_hs_process_rx_data_queue(void); int ble_hs_tx_data(struct os_mbuf *om); +int ble_hs_tx_iso_data(struct os_mbuf *om); void ble_hs_wakeup_tx(void); void ble_hs_enqueue_hci_event(uint8_t *hci_evt); void ble_hs_event_enqueue(struct os_event *ev); int ble_hs_hci_rx_evt(uint8_t *hci_ev, void *arg); int ble_hs_hci_evt_acl_process(struct os_mbuf *om); +int ble_hs_hci_evt_iso_process(struct os_mbuf *om); int ble_hs_misc_conn_chan_find(uint16_t conn_handle, uint16_t cid, struct ble_hs_conn **out_conn, diff --git a/nimble/host/src/ble_hs_startup.c b/nimble/host/src/ble_hs_startup.c index 75e98ebc43..57207e4784 100644 --- a/nimble/host/src/ble_hs_startup.c +++ b/nimble/host/src/ble_hs_startup.c @@ -22,6 +22,7 @@ #include "host/ble_hs.h" #include "host/ble_hs_hci.h" #include "ble_hs_priv.h" +#include "ble_iso_priv.h" #if !MYNEWT_VAL(BLE_CONTROLLER) static int @@ -86,6 +87,29 @@ ble_hs_startup_le_read_sup_f_tx(void) return 0; } +#if MYNEWT_VAL(BLE_ISO) +static int +ble_hs_startup_le_read_buf_sz_v2_tx(uint16_t *out_pktlen, uint8_t *out_max_pkts, + uint16_t *out_iso_pktlen, uint8_t *out_iso_max_pkts) +{ + struct ble_hci_le_rd_buf_size_v2_rp rsp; + int rc; + + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_RD_BUF_SIZE_V2), NULL, 0, + &rsp, sizeof(rsp)); + if (rc != 0) { + return rc; + } + + *out_pktlen = le16toh(rsp.data_len); + *out_max_pkts = rsp.data_packets; + *out_iso_pktlen = le16toh(rsp.data_len); + *out_iso_max_pkts = rsp.data_packets; + return 0; +} +#endif + #if MYNEWT_VAL(BLE_ROLE_CENTRAL) || MYNEWT_VAL(BLE_ROLE_PERIPHERAL) static int ble_hs_startup_le_read_buf_sz_tx(uint16_t *out_pktlen, uint8_t *out_max_pkts) @@ -132,9 +156,21 @@ ble_hs_startup_read_buf_sz(void) uint16_t max_pkts = 0; uint16_t pktlen = 0; uint8_t le_max_pkts = 0; +#if MYNEWT_VAL(BLE_ISO) + uint16_t iso_pktlen = 0; + uint8_t iso_max_pkts = 0; +#endif int rc; +#if MYNEWT_VAL(BLE_ISO) + rc = ble_hs_startup_le_read_buf_sz_v2_tx(&le_pktlen, &le_max_pkts, &iso_pktlen, &iso_max_pkts); + if (rc != 0) { + /* Fallback to v1 */ + rc = ble_hs_startup_le_read_buf_sz_tx(&le_pktlen, &le_max_pkts); + } +#else rc = ble_hs_startup_le_read_buf_sz_tx(&le_pktlen, &le_max_pkts); +#endif if (rc != 0) { return rc; } @@ -154,6 +190,13 @@ ble_hs_startup_read_buf_sz(void) return rc; } +#if MYNEWT_VAL(BLE_ISO) + rc = ble_iso_hci_set_buf_sz(iso_pktlen, iso_max_pkts); + if (rc != 0) { + return rc; + } +#endif + return 0; } #endif @@ -230,6 +273,29 @@ ble_hs_startup_le_set_evmask_tx(void) mask |= 0x00000000000ff800; } + if (version >= BLE_HCI_VER_BCS_5_2) { + /** + * Enable the following LE events: + * 0x0000000040000000 LE Request Peer SCA Complete event + */ + mask |= 0x0000000040000000; + } + +#if MYNEWT_VAL(BLE_ISO) + if (version >= BLE_HCI_VER_BCS_5_2) { + /* Enable BLE ISO releated features. + * + * 0x0000000001000000 LE CIS Established + * 0x0000000002000000 LE CIS Request + * 0x0000000004000000 LE Create BIG Complete + * 0x0000000008000000 LE Terminate BIG Complete + * 0x0000000010000000 LE BIG Sync Established Complete + * 0x0000000020000000 LE BIG Sync lost + **/ + mask |= 0x000000003f000000; + } +#endif + #if MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_TRANSFER) if (version >= BLE_HCI_VER_BCS_5_1) { /** diff --git a/nimble/host/src/ble_iso.c b/nimble/host/src/ble_iso.c new file mode 100644 index 0000000000..725e3cb08a --- /dev/null +++ b/nimble/host/src/ble_iso.c @@ -0,0 +1,1181 @@ +/* + * 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 +#include "syscfg/syscfg.h" + +#if MYNEWT_VAL(BLE_ISO) + +#include "os/os_mbuf.h" +#include "host/ble_hs_log.h" +#include "host/ble_hs.h" +#include "host/ble_iso.h" +#include "nimble/hci_common.h" +#include "sys/queue.h" +#include "ble_iso_priv.h" +#include "ble_hs_hci_priv.h" +#include "ble_hs_priv.h" + +#define BLE_ISO_INVALID_CIG_ID (0xFF) + +#define BLE_ISO_CIS_F_IS_MASTER (0x01) +#define BLE_ISO_CIS_F_CONNECTED (0x02) +#define BLE_ISO_IS_BIS (0x04) + +#define BLE_ISO_SET_FLAG(flags, flag) (flags |= (1 << flag)) +#define BLE_ISO_CLEAR_FLAG(flags, flag) (flags &= ~(1 << flag)) + +#define BLE_ISO_CIG_IDS_IDX (MYNEWT_VAL(BLE_MAX_CIG) / 32 + 1) + +/* This should be in ble_iso_hci.c */ +static uint16_t ble_iso_hci_buf_sz; +static uint8_t ble_iso_hci_max_pkts; +static uint8_t ble_iso_hci_avail_pkts; + +static uint32_t ble_iso_group_ids[BLE_ISO_CIG_IDS_IDX]; + +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_cig; + STAILQ_ENTRY(ble_iso_group) free_cig; + }; + + struct ble_iso_active_cig *list; +}; + +struct ble_iso_conn { + union { + SLIST_ENTRY(ble_iso_conn) next; + STAILQ_ENTRY(ble_iso_conn) free_cis; + STAILQ_ENTRY(ble_iso_conn) pend_cis; + }; + + /* 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; +}; + +/* For now support only one server */ +static ble_iso_event_fn *ble_iso_leaudio_server_cb; +static void *ble_iso_leaudio_server_cb_arg; + +static struct ble_iso_group g_ble_iso_groups_ids[MYNEWT_VAL(BLE_MAX_CIG)]; + +SLIST_HEAD(ble_iso_active_cig, ble_iso_group); +STAILQ_HEAD(ble_iso_free_groups, ble_iso_group); +static struct ble_iso_active_cig g_ble_iso_act_slave_cig_list; +static struct ble_iso_active_cig g_ble_iso_act_master_cig_list; +static struct ble_iso_free_groups g_ble_iso_free_group_list; + +static struct ble_iso_active_cig g_ble_iso_big_list; + +STAILQ_HEAD(ble_iso_pending_cis_conn, ble_iso_conn); +static struct ble_iso_pending_cis_conn g_ble_iso_pend_cis_conn_list; + +static struct os_mempool ble_iso_conn_pool; +static os_membuf_t ble_iso_conn_elem_mem[ + OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_MAX_CIS_CONNECTIONS), + sizeof (struct ble_iso_conn)) +]; + +static struct ble_iso_cis_params g_ble_cis_params_dflt = { + .max_sdu_c_to_p = 100, + .max_sdu_p_to_c = 100, + .phy_c_to_p = 0, + .phy_p_to_c = 0, + .rnt_c_to_p = 2, + .rnt_p_to_c = 2, +}; + +static struct ble_iso_group * +ble_iso_find_group(uint8_t group_id, struct ble_iso_active_cig *active_cig_list) +{ + struct ble_iso_group *group; + + ble_hs_lock(); + SLIST_FOREACH(group, active_cig_list, active_cig) { + if (group_id == group->id) { + ble_hs_unlock(); + return group; + } + } + ble_hs_unlock(); + return NULL; +} + +static struct ble_iso_conn * +ble_iso_find_by_iso_handle(uint16_t iso_handle) +{ + struct ble_iso_group *cig; + struct ble_iso_conn *cis_conn; + + ble_hs_lock(); + SLIST_FOREACH(cig, &g_ble_iso_act_master_cig_list, active_cig) { + SLIST_FOREACH(cis_conn, &cig->iso_head, next) { + if (cis_conn->iso_handle == iso_handle) { + ble_hs_unlock(); + return cis_conn; + } + } + } + + SLIST_FOREACH(cig, &g_ble_iso_act_slave_cig_list, active_cig) { + SLIST_FOREACH(cis_conn, &cig->iso_head, next) { + if (cis_conn->iso_handle == iso_handle) { + ble_hs_unlock(); + return cis_conn; + } + } + } + ble_hs_unlock(); + return NULL; +} + +static int +ble_iso_pick_and_set_group_id(struct ble_iso_group *cig) +{ + int i; + int bit; + + for (i = 0; i < BLE_ISO_CIG_IDS_IDX; i++) { + bit = __builtin_ffs(~(unsigned int)(ble_iso_group_ids[i])); + if (bit < 32) { + break; + } + } + + if (i == BLE_ISO_CIG_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 struct ble_iso_group * +ble_iso_get_new_cig_and_put_to_active(struct ble_iso_active_cig *active_list) +{ + struct ble_iso_group *cig; + + cig = STAILQ_FIRST(&g_ble_iso_free_group_list); + if (cig) { + STAILQ_REMOVE_HEAD(&g_ble_iso_free_group_list, free_cig); + SLIST_INSERT_HEAD(active_list, cig, active_cig); + cig->list = active_list; + } + + return cig; +} + +static void +ble_iso_release_group(struct ble_iso_group * group) +{ + struct ble_iso_conn *iso_conn; + int idx; + + if (group->list == &g_ble_iso_act_master_cig_list) { + idx = group->id / 32; + BLE_ISO_CLEAR_FLAG(ble_iso_group_ids[idx], (group->id - idx * 32)); + } + + 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_cig); + STAILQ_INSERT_TAIL(&g_ble_iso_free_group_list, group, free_cig); +} + +static int +ble_iso_num_of_available_conns(void) +{ + return ble_iso_conn_pool.mp_num_free; +} + +struct ble_iso_conn * +ble_iso_conn_alloc(uint8_t iso_handle) +{ + struct ble_iso_conn *iso_conn; + + iso_conn = os_memblock_get(&ble_iso_conn_pool); + assert(iso_conn); + iso_conn->id = iso_handle; + iso_conn->seq_num = 0; + iso_conn->last_timestamp = 0; + + return iso_conn; +} + +int +ble_iso_client_create_cig(struct ble_iso_cig_params *cig_params, + uint8_t cis_cnt, struct ble_iso_cis_params *cis_params, + uint8_t *cig_id, uint8_t *cis_handles) +{ + struct ble_iso_group *cig; + struct ble_iso_conn *cis_conn; + struct ble_hci_le_set_cig_params_cp *cmd; + uint8_t cmd_buf[(sizeof(*cmd)) + BLE_HCI_LE_SET_CIG_CIS_MAX_NUM * sizeof(*cis_params)]; + struct ble_hci_le_set_cig_params_rp *ev; + uint8_t ev_buf[sizeof(*ev) + BLE_HCI_LE_SET_CIG_CIS_MAX_NUM * sizeof(uint16_t)]; + uint32_t tmp; + int i; + int rc; + + if (ble_iso_num_of_available_conns() < cis_cnt) { + return BLE_HS_ENOMEM; + } + + cig = ble_iso_get_new_cig_and_put_to_active(&g_ble_iso_act_master_cig_list); + if (!cig) { + return BLE_HS_ENOMEM; + } + + cmd = (struct ble_hci_le_set_cig_params_cp *)&cmd_buf[0]; + + cmd->cig_id = ble_iso_pick_and_set_group_id(cig); + assert(cmd->cig_id >= 0); + *cig_id = cmd->cig_id; + + tmp = htole32(cig_params->sdu_c_to_p_itvl); + memcpy(cmd->sdu_interval_c_to_p, &tmp, 3); + + tmp = htole32(cig_params->sdu_c_to_p_itvl); + memcpy(cmd->sdu_interval_p_to_c, &tmp, 3); + + cmd->worst_sca = cig_params->sca; + cmd->packing = cig_params->packing; + cmd->framing = cig_params->framing; + put_le16((uint8_t *)&cmd->max_latency_c_to_p, cig_params->max_c_to_p_latency); + put_le16((uint8_t *)&cmd->max_latency_p_to_c, cig_params->max_p_to_c_latency); + + cmd->cis_count = cis_cnt; + for (i = 0; i < cis_cnt; i++) { + if (!cis_params) { + cis_params = &g_ble_cis_params_dflt; + } + + cmd->cis[i].cis_id = i; + cmd->cis[i].max_sdu_c_to_p = cis_params->max_sdu_c_to_p; + cmd->cis[i].max_sdu_p_to_c = cis_params->max_sdu_p_to_c; + cmd->cis[i].phy_c_to_p = cis_params->phy_c_to_p; + cmd->cis[i].phy_p_to_c = cis_params->phy_p_to_c; + cmd->cis[i].rnt_c_to_p = cis_params->rnt_c_to_p; + cmd->cis[i].rnt_p_to_c = cis_params->rnt_p_to_c; + } + + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_SET_CIG_PARAMS), + cmd, sizeof(*cmd) + cis_cnt * sizeof(*cis_params), + ev_buf, sizeof(*ev) + cis_cnt * sizeof(uint16_t)); + if (rc) { + rc = BLE_HS_HCI_ERR(rc); + goto error; + } + + ev = (struct ble_hci_le_set_cig_params_rp *)ev_buf; + assert(cis_cnt == ev->cis_count); + + for (i = 0; i < cis_cnt; i++) { + cis_conn = ble_iso_conn_alloc(i); + assert(cis_conn); + SLIST_INSERT_HEAD(&cig->iso_head, cis_conn, next); + cis_conn->iso_handle = ev->conn_handle[i]; + cis_conn->cig_id = cig->id; + } + + cig->iso_num = cis_cnt; + + return 0; + +error: + ble_iso_release_group(cig); + + return rc; +} + +int +ble_iso_client_remove_group(uint8_t cig_id) +{ + struct ble_hci_le_remove_cig_cp cmd; + struct ble_iso_group *cig; + int rc; + + cig = ble_iso_find_group(cig_id, &g_ble_iso_act_master_cig_list); + if (!cig) { + return BLE_HS_ENOENT; + } + + cmd.cig_id = cig_id; + + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_REMOVE_CIG), &cmd, sizeof(cmd), + NULL, 0); + + if (rc == 0) { + ble_iso_release_group(cig); + } + + return rc; +} + +int +ble_iso_client_create_cis(uint8_t cig_id, uint8_t cis_cnt, + struct ble_hci_le_create_cis_params *params, + ble_iso_event_fn *cb, void *cb_arg) +{ + struct ble_hci_le_create_cis_cp *cmd; + uint8_t cmd_buf[sizeof(*cmd) + cis_cnt * sizeof(*params)]; + struct ble_iso_group *cig; + struct ble_iso_conn *cis_conn; + int rc; + int i; + + if (cis_cnt > BLE_HCI_LE_SET_CIG_CIS_MAX_NUM) { + return BLE_HS_EINVAL; + } + + cig = ble_iso_find_group(cig_id, &g_ble_iso_act_master_cig_list); + if (!cig || cig->iso_num < cis_cnt) { + return BLE_HS_ENOENT; + } + + cmd = (struct ble_hci_le_create_cis_cp *)cmd_buf; + cmd->cis_count = cis_cnt; + memcpy(cmd->cis, params, cis_cnt * + sizeof(struct ble_hci_le_create_cis_params)); + + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_CREATE_CIS), + cmd, sizeof(*cmd) + cis_cnt * sizeof(*params), + NULL, 0); + + if (rc == 0) { + for (i = 0; i < cis_cnt; i++) { + cis_conn = SLIST_FIRST(&cig->iso_head); + cis_conn->cb = cb; + cis_conn->cb_arg = cb_arg; + + /* TODO We need to check if ACL is really in Master Role */ + BLE_ISO_SET_FLAG(cis_conn->flags, BLE_ISO_CIS_F_IS_MASTER); + + SLIST_REMOVE_HEAD(&cig->iso_head, next); + STAILQ_INSERT_HEAD(&g_ble_iso_pend_cis_conn_list, cis_conn, pend_cis); + } + } + + return rc; +} + +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_big_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_create_big_sync(uint8_t *out_big_handle, uint16_t sync_handle, + bool encrypted, uint8_t *broadcast_code, + uint8_t mse, uint16_t sync_timeout, + 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_cig_and_put_to_active(&g_ble_iso_big_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); + cmd->encryption = encrypted; + if (encrypted) { + memcpy(cmd->broadcast_code, broadcast_code, 16); + } + cmd->sync_timeout = sync_timeout; + 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 int +ble_iso_set_hci_data_path(uint16_t handle, bool endpoint_input, bool endpoint_output) +{ + struct ble_hci_le_setup_iso_data_path_cp cmd = {0}; + int rc = 0; + + put_le16(&cmd.conn_handle, handle); + + if (endpoint_input) { + /* For now we support only HCI path */ + cmd.data_path_dir = 1; /* Controller to Host */ + cmd.data_path_id = 0; /* hci */ + + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_SETUP_ISO_DATA_PATH), + &cmd, sizeof(cmd), NULL, 0); + assert(rc == 0); + } + + if (endpoint_output) { + /* For now we support only HCI path */ + cmd.data_path_dir = 0; /* Host to Controller */ + cmd.data_path_id = 0; /* hci */ + + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_SETUP_ISO_DATA_PATH), + &cmd, sizeof(cmd), NULL, 0); + assert(rc == 0); + } + return rc; +} + +int +ble_iso_rx_hci_evt_le_cis_established(const struct ble_hci_ev_le_subev_cis_established *ev) +{ + struct ble_iso_group *cig; + struct ble_iso_conn *cis_conn_prev; + struct ble_iso_conn *cis_conn; + struct ble_iso_event event = {0}; + + cis_conn = STAILQ_FIRST(&g_ble_iso_pend_cis_conn_list); + assert(cis_conn); + + if (cis_conn->iso_handle != le16toh(ev->conn_handle)) { + while (STAILQ_NEXT(cis_conn, pend_cis)) { + cis_conn_prev = cis_conn; + cis_conn = STAILQ_NEXT(cis_conn, pend_cis); + if (cis_conn->iso_handle == le16toh(ev->conn_handle)) { + STAILQ_REMOVE_AFTER(&g_ble_iso_pend_cis_conn_list, cis_conn_prev, pend_cis); + break; + } + } + } + + assert(cis_conn); + + if (cis_conn->flags & BLE_ISO_CIS_F_IS_MASTER) { + cig = ble_iso_find_group(cis_conn->cig_id, &g_ble_iso_act_master_cig_list); + } else { + cig = ble_iso_find_group(cis_conn->cig_id, &g_ble_iso_act_slave_cig_list); + } + assert(cig); + + event.type = BLE_ISO_CIS_ESTABLISHED_EVENT; + event.cis_established.cis_handle = cis_conn->iso_handle; + event.cis_established.status = ev->status; + + cis_conn->cb(&event, cis_conn->cb_arg); + + if (event.cis_established.status != BLE_ERR_SUCCESS) { + os_memblock_put(&ble_iso_conn_pool, cis_conn); + return 0; + } + + if (cis_conn->flags & BLE_ISO_CIS_F_IS_MASTER) { + cis_conn->max_pdu_output = ev->max_pdu_c_to_p; + cis_conn->max_pdu_input = ev->max_pdu_p_to_c; + } else { + cis_conn->max_pdu_input = ev->max_pdu_c_to_p; + cis_conn->max_pdu_output = ev->max_pdu_p_to_c; + } + + ble_iso_set_hci_data_path(ev->conn_handle, + !!cis_conn->max_pdu_input, + !!cis_conn->max_pdu_output); + SLIST_INSERT_HEAD(&cig->iso_head, cis_conn, next); + + return 0; +} + +static void +ble_iso_cis_reject_rsp(uint16_t cis_handle, uint8_t reason) +{ + struct ble_hci_le_reject_cis_request_cp cmd; + + put_le16(&cmd.conn_handle, cis_handle); + cmd.reason = reason; + + if (ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_REJECT_CIS_REQ), &cmd, sizeof(cmd), + NULL, 0)) { + assert(0); + } +} + +static void +ble_iso_cis_accept_rsp(uint16_t cis_handle) +{ + struct ble_hci_le_accept_cis_request_cp cmd; + + put_le16(&cmd.conn_handle, cis_handle); + + if (ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_ACCEPT_CIS_REQ), &cmd, sizeof(cmd), + NULL, 0)) { + assert(0); + } +} + +int +ble_iso_rx_hci_evt_le_cis_request(const struct ble_hci_ev_le_subev_cis_request *ev) +{ + struct ble_iso_group *cig; + struct ble_iso_conn *cis_conn; + struct ble_iso_event event = {0}; + int rc; + + if (!ble_iso_leaudio_server_cb) { + ble_iso_cis_reject_rsp(ev->cis_conn_handle, BLE_ERR_UNSUPPORTED); + return 0; + } + + cig = ble_iso_find_group(ev->cig_id, &g_ble_iso_act_slave_cig_list); + if (!cig) { + /* This is new CIG - lets us create it */ + cig = ble_iso_get_new_cig_and_put_to_active(&g_ble_iso_act_slave_cig_list); + if (!cig) { + ble_iso_cis_reject_rsp(ev->cis_conn_handle, BLE_ERR_CONN_LIMIT); + return 0; + } + cig->id = ev->cig_id; + } + + cis_conn = ble_iso_conn_alloc(ev->cis_id); + if (!cis_conn) { + ble_iso_cis_reject_rsp(ev->cis_conn_handle, BLE_ERR_CONN_LIMIT); + ble_iso_release_group(cig); + return 0; + } + + cis_conn->iso_handle = le16toh(ev->cis_conn_handle); + cis_conn->acl_handle = le16toh(ev->acl_conn_handle); + cis_conn->cig_id = cig->id; + + event.type = BLE_ISO_CONNECT_REQUEST_EVENT; + event.cis_connect_req.cis_handle = ev->cis_conn_handle; + event.cis_connect_req.conn_handle = ev->acl_conn_handle; + + cis_conn->cb = ble_iso_leaudio_server_cb; + cis_conn->cb_arg = ble_iso_leaudio_server_cb_arg; + + rc = cis_conn->cb(&event, cis_conn->cb_arg); + if (rc) { + ble_iso_cis_reject_rsp(ev->cis_conn_handle, rc); + ble_iso_release_group(cig); + } else { + ble_iso_cis_accept_rsp(ev->cis_conn_handle); + STAILQ_INSERT_HEAD(&g_ble_iso_pend_cis_conn_list, cis_conn, pend_cis); + } + + return 0; +} + +static void +ble_iso_sent_big_complete_event(struct ble_iso_group *big, + uint8_t type, uint8_t status, + uint8_t bis_cnt, const uint16_t *bis) +{ + struct ble_iso_event *event; + uint8_t buf[sizeof(*event) + bis_cnt * sizeof(uint16_t)]; + + event = (struct ble_iso_event *)buf; + memset(event, 0, sizeof(*event) + bis_cnt * sizeof(uint16_t)); + + event->type = type; + event->big_complete.status = status; + event->big_complete.big_handle = big->id; + event->big_complete.bis_cnt = bis_cnt; + memcpy(event->big_complete.bis, bis, bis_cnt * sizeof(uint16_t)); + + big->cb(event, big->cb_arg); +} + +static void +ble_iso_sent_big_terminate_event(struct ble_iso_group *big, + uint8_t type, uint8_t reason) +{ + struct ble_iso_event event = {0}; + + event.type = type; + event.big_terminate_lost.big_handle = big->id; + event.big_terminate_lost.reason = reason; + + big->cb(&event, big->cb_arg); +} + +int +ble_iso_rx_hci_evt_le_big_completed(const struct ble_hci_ev_le_subev_create_big_complete *ev) +{ + struct ble_iso_group *big; + struct ble_iso_conn *iso_conn; + uint16_t bis_handles[ev->num_bis]; + int i; + + big = ble_iso_find_group(ev->big_handle, &g_ble_iso_big_list); + if (!big) { + BLE_HS_LOG_ERROR("Could not find big 0x%02x\n", ev->big_handle); + return 0; + } + + if (ev->status == 0) { + for (i = 0; i < ev->num_bis; i++) { + iso_conn = ble_iso_conn_alloc(ev->conn_handle[i]); + assert(iso_conn); + iso_conn->acl_handle = BLE_HS_CONN_HANDLE_NONE; + iso_conn->cb = big->cb; + iso_conn->cb_arg = big->cb_arg; + SLIST_INSERT_HEAD(&big->iso_head, iso_conn, next); + BLE_ISO_SET_FLAG(iso_conn->flags, BLE_ISO_IS_BIS); + bis_handles[i] = ev->conn_handle[i]; + } + } + + ble_iso_sent_big_complete_event(big, BLE_ISO_BIG_CREATE_COMPLETE_EVENT, ev->status, ev->num_bis, bis_handles); + + return 0; +} + +int +ble_iso_rx_hci_le_big_terminate_complete(const struct ble_hci_ev_le_subev_terminate_big_complete *ev) +{ + struct ble_iso_group *big; + + big = ble_iso_find_group(ev->big_handle, &g_ble_iso_big_list); + if (!big) { + BLE_HS_LOG_ERROR("Could not find big 0x%02x\n", ev->big_handle); + return 0; + } + + ble_iso_sent_big_terminate_event(big, BLE_ISO_BIG_TERMINATE_EVENT, ev->reason); + ble_iso_release_group(big); + return 0; +} + +int +ble_iso_hci_le_big_sync_established(const struct ble_hci_ev_le_subev_big_sync_established *ev) +{ + struct ble_iso_group *big; + struct ble_iso_conn *iso_conn; + uint16_t bis_handles[ev->num_bis]; + int i; + + big = ble_iso_find_group(ev->big_handle, &g_ble_iso_big_list); + if (!big) { + BLE_HS_LOG_ERROR("Could not find big 0x%02x\n", ev->big_handle); + return 0; + } + + if (ev->status == 0) { + for (i = 0; i < ev->num_bis; i++) { + iso_conn = ble_iso_conn_alloc(ev->conn_handle[i]); + assert(iso_conn); + iso_conn->acl_handle = BLE_HS_CONN_HANDLE_NONE; + iso_conn->cb = big->cb; + iso_conn->cb_arg = big->cb_arg; + SLIST_INSERT_HEAD(&big->iso_head, iso_conn, next); + BLE_ISO_SET_FLAG(iso_conn->flags, BLE_ISO_IS_BIS); + iso_conn->iso_handle = ev->conn_handle[i]; + bis_handles[i] = ev->conn_handle[i]; + } + } + + ble_iso_sent_big_complete_event(big, BLE_ISO_BIG_ESTABLISHED_EVENT, + ev->status, ev->num_bis, bis_handles); + return 0; +} + +int +ble_iso_hci_le_big_sync_lost(const struct ble_hci_ev_le_subev_big_sync_lost *ev) +{ + struct ble_iso_group *big; + + big = ble_iso_find_group(ev->big_handle, &g_ble_iso_big_list); + if (!big) { + BLE_HS_LOG_ERROR("Could not find big 0x%02x\n", ev->big_handle); + return 0; + } + + ble_iso_sent_big_terminate_event(big, BLE_ISO_BIG_SYNC_LOST_EVENT, ev->reason); + ble_iso_release_group(big); + return 0; +} + +int +ble_iso_server_register_le_audio(ble_iso_event_fn *cb, void *cb_arg) +{ + if (ble_iso_leaudio_server_cb) { + return BLE_HS_EBUSY; + } + + ble_iso_leaudio_server_cb = cb; + ble_iso_leaudio_server_cb_arg = cb_arg; + + return 0; +} + +static void +ble_iso_send_disconnected_event(struct ble_iso_conn *cis_conn, uint8_t reason) +{ + struct ble_iso_event event; + + event.type = BLE_ISO_CIS_DISCONNECTED_EVENT; + event.cis_disconnected.status = reason; + event.cis_disconnected.cis_handle = cis_conn->iso_handle; + cis_conn->cb(&event, cis_conn->cb_arg); + + BLE_ISO_CLEAR_FLAG(cis_conn->flags, BLE_ISO_CIS_F_CONNECTED); +} + +void +ble_iso_disconnect_cis(uint16_t handle) +{ + struct ble_hci_lc_disconnect_cp cmd; + int rc; + + put_le16(&cmd.conn_handle, handle); + cmd.reason = BLE_ERR_REM_USER_CONN_TERM; + + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LINK_CTRL, + BLE_HCI_OCF_DISCONNECT_CMD), + &cmd, sizeof(cmd), NULL, 0); + if (rc != 0) { + BLE_HS_LOG_INFO("Cis already disconnected? handle =0x%04x", handle); + } +} + +static void +ble_iso_acl_disconnected(uint16_t acl_handle, uint8_t reason) +{ + struct ble_iso_conn *cis_conn; + struct ble_iso_group *cig; + bool done = false; + + ble_hs_lock(); + SLIST_FOREACH(cig, &g_ble_iso_act_master_cig_list, active_cig) { + SLIST_FOREACH(cis_conn, &cig->iso_head, next) { + if (cis_conn->acl_handle == acl_handle) { + ble_iso_disconnect_cis(cis_conn->iso_handle); + + done = true; + } + } + } + + if (done) { + ble_hs_unlock(); + return; + } + + SLIST_FOREACH(cig, &g_ble_iso_act_slave_cig_list, active_cig) { + SLIST_FOREACH(cis_conn, &cig->iso_head, next) { + if (cis_conn->acl_handle == acl_handle) { + ble_iso_disconnect_cis(cis_conn->iso_handle); + os_memblock_put(&ble_iso_conn_pool, cis_conn); + } + } + } + ble_hs_unlock(); +} + +static void +ble_iso_cis_disconnected(uint16_t cis_handle, uint8_t reason) +{ + struct ble_iso_conn *cis_conn; + struct ble_iso_group *cig; + + ble_hs_lock(); + SLIST_FOREACH(cig, &g_ble_iso_act_master_cig_list, active_cig) { + SLIST_FOREACH(cis_conn, &cig->iso_head, next) { + if (cis_conn->iso_handle == cis_handle) { + ble_iso_send_disconnected_event(cis_conn, reason); + SLIST_REMOVE(&cig->iso_head, cis_conn, ble_iso_conn, next); + os_memblock_put(&ble_iso_conn_pool, cis_conn); + ble_hs_unlock(); + return; + } + } + } + + SLIST_FOREACH(cig, &g_ble_iso_act_slave_cig_list, active_cig) { + SLIST_FOREACH(cis_conn, &cig->iso_head, next) { + if (cis_conn->iso_handle == cis_handle) { + ble_iso_send_disconnected_event(cis_conn, reason); + SLIST_REMOVE(&cig->iso_head, cis_conn, ble_iso_conn, next); + os_memblock_put(&ble_iso_conn_pool, cis_conn); + if (SLIST_EMPTY((&cig->iso_head))) { + ble_iso_release_group(cig); + } + ble_hs_unlock(); + return; + } + } + } + ble_hs_unlock(); +} + +void +ble_iso_disconnected_event(uint16_t conn_handle, uint8_t reason, bool is_acl) +{ + if (is_acl) { + ble_iso_acl_disconnected(conn_handle, reason); + } else { + ble_iso_cis_disconnected(conn_handle, reason); + } +} + +static void +ble_iso_send_iso_data_event(struct ble_iso_conn *conn, struct os_mbuf *om) +{ + struct ble_iso_event event; + + if (!conn->cb) { + return; + } + + event.type = BLE_ISO_DATA_EVENT; + event.iso_data.handle = conn->iso_handle; + event.iso_data.om = om; + conn->cb(&event, conn->cb_arg); +} + +int +ble_iso_hci_util_data_hdr_strip(struct os_mbuf *om, + struct hci_iso_hdr *out_hdr) +{ + int rc; + /* TODO THis should be part of ble_iso_hci */ + rc = os_mbuf_copydata(om, 0, sizeof(*out_hdr), out_hdr); + if (rc != 0) { + return BLE_HS_ECONTROLLER; + } + + out_hdr->handle_pb_ts = get_le16(&out_hdr->handle_pb_ts); + out_hdr->len = BLE_HCI_ISO_DATA_LEN(get_le16(&out_hdr->len)); + + if (BLE_HCI_ISO_DATA_TS(out_hdr->handle_pb_ts)) { + out_hdr->with_ts.ts = get_le32(&out_hdr->with_ts.ts); + out_hdr->with_ts.seq_num = get_le16(&out_hdr->with_ts.seq_num); + out_hdr->with_ts.sdu_len_psf = get_le16(&out_hdr->with_ts.sdu_len_psf); + + /* Strip HCI ACL data header from the front of the packet. */ + os_mbuf_adj(om, BLE_HCI_ISO_HDR_SIZE_WITH_TS); + + } else { + out_hdr->no_ts.seq_num = get_le16(&out_hdr->no_ts.seq_num); + out_hdr->no_ts.sdu_len_psf = BLE_HCI_ISO_DATA_SDU_LEN(get_le16(&out_hdr->no_ts.sdu_len_psf)); + /* Strip HCI ACL data header from the front of the packet. */ + os_mbuf_adj(om, BLE_HCI_ISO_HDR_SIZE_NO_TS); + } + + return 0; +} + +static uint16_t +ble_iso_hci_util_handle_pb_ts_join(uint16_t handle, uint8_t pb, uint8_t ts) +{ + BLE_HS_DBG_ASSERT(handle <= 0x0fff); + BLE_HS_DBG_ASSERT(pb <= 0x03); + BLE_HS_DBG_ASSERT(ts <= 0x01); + + return (handle << 0) | + (pb << 12) | + (ts << 14); +} + +static int +ble_iso_tx_iso(struct ble_iso_conn *iso_conn, struct os_mbuf *om) +{ + struct os_mbuf *om2; + struct hci_iso_hdr *hci_iso_hdr; + uint16_t sdu_len = OS_MBUF_PKTLEN(om); + uint32_t ts; + + om2 = os_mbuf_prepend(om, BLE_HCI_ISO_HDR_SIZE_NO_TS); + assert(om2); + + /* Make sure we have linear memory for header */ + om2 = os_mbuf_pullup(om2, BLE_HCI_ISO_HDR_SIZE_NO_TS); + assert(om2); + + hci_iso_hdr = (struct hci_iso_hdr *)&om2->om_data[0]; + + /* TODO Add fragmentation but this is going to be in ble_iso_hci */ + hci_iso_hdr->handle_pb_ts = + ble_iso_hci_util_handle_pb_ts_join(iso_conn->iso_handle, + BLE_HCI_ISO_PB_COMPLETE, 0); + put_le16(&hci_iso_hdr->len, sdu_len + 4); + + ts = os_cputime_get32(); + iso_conn->last_timestamp = ts; + + /* TODO seq_num should probably be increased every SDU interval */ + put_le16(&hci_iso_hdr->no_ts.seq_num, iso_conn->seq_num++); + put_le16(&hci_iso_hdr->no_ts.sdu_len_psf, sdu_len); + + /* TODO: Verify credits */ + return ble_hs_tx_iso_data(om2); +} + +static struct ble_iso_conn * +ble_iso_iso_conn_output(uint8_t cig_id, bool master) +{ + struct ble_iso_group *cig; + struct ble_iso_conn *iso_conn; + + if (master) { + cig = ble_iso_find_group(cig_id, &g_ble_iso_act_master_cig_list); + } else { + cig = ble_iso_find_group(cig_id, &g_ble_iso_act_slave_cig_list); + } + + assert(cig); + + ble_hs_lock(); + SLIST_FOREACH(iso_conn, &cig->iso_head, next) { + if (iso_conn->max_pdu_output) { + ble_hs_unlock(); + return iso_conn; + } + } + ble_hs_unlock(); + + return NULL; +} + +void +ble_iso_rx(struct os_mbuf *om) +{ + uint16_t handle; + struct hci_iso_hdr hci_iso_hdr; + struct ble_iso_conn *iso_conn; + struct ble_iso_conn *iso_conn_loop; + int rc; + bool is_ts; + uint8_t ps_flag; + + rc = ble_iso_hci_util_data_hdr_strip(om, &hci_iso_hdr); + if (rc != 0) { + goto done; + } + + handle = BLE_HCI_ISO_DATA_HANDLE(hci_iso_hdr.handle_pb_ts); + is_ts = BLE_HCI_ISO_DATA_TS(hci_iso_hdr.handle_pb_ts); + + if ((is_ts && hci_iso_hdr.with_ts.sdu_len_psf == 0) || + (!is_ts && hci_iso_hdr.no_ts.sdu_len_psf == 0)) { + goto done; + } + + if (is_ts) { + ps_flag = BLE_HCI_ISO_DATA_PS_FLAG(hci_iso_hdr.with_ts.sdu_len_psf); + } else { + ps_flag = BLE_HCI_ISO_DATA_PS_FLAG(hci_iso_hdr.no_ts.sdu_len_psf); + } + + if (ps_flag) { + BLE_HS_LEAUDIO_LOG_DEBUG("ps_flag: 0x%02x, buf size %d\n", ps_flag, om->om_len); + if (om->om_len == 0) { + goto done; + } + } + + iso_conn = ble_iso_find_by_iso_handle(handle); + if (!iso_conn) { + BLE_HS_LEAUDIO_LOG_ERROR("Iso not there ? handle=0x%04x\n", handle); + goto done; + } + + if (MYNEWT_VAL(BLE_ISO_LOOPBACK) == 1) { + if (iso_conn->max_pdu_output == 0) { + /* ISO is not bi directional. Check if there is any output ISO. If yes, use it */ + iso_conn_loop = ble_iso_iso_conn_output(iso_conn->cig_id, (iso_conn->flags & BLE_ISO_CIS_F_IS_MASTER)); + if (iso_conn_loop) { + rc = ble_iso_tx_iso(iso_conn_loop, om); + } else { + BLE_HS_LEAUDIO_LOG_ERROR("No Output ISO\n"); + } + } else { + /* bi directional ISO, */ + rc = ble_iso_tx_iso(iso_conn, om); + } + /* In case of loop, we just return here */ + return; + } + + ble_iso_send_iso_data_event(iso_conn, om); + +done: + os_mbuf_free_chain(om); +} + +int +ble_iso_tx_encoded(uint8_t cig_id, uint16_t iso_handle, struct os_mbuf *om) +{ + struct ble_iso_group *cig; + struct ble_iso_conn *iso_conn; + + /*TODO CIG ID can be same for master and slave right?*/ + cig = ble_iso_find_group(cig_id, &g_ble_iso_act_slave_cig_list); + if (!cig) { + cig = ble_iso_find_group(cig_id, &g_ble_iso_act_master_cig_list); + } + + if (!cig) { + return BLE_HS_ENOTCONN; + } + + /* For now we se*/ + ble_hs_lock(); + SLIST_FOREACH(iso_conn, &cig->iso_head, next) { + if (iso_handle == iso_conn->iso_handle) { + if (iso_conn->max_pdu_output == 0) { + ble_hs_unlock(); + return BLE_HS_EBADDATA; + } + ble_hs_unlock(); + ble_iso_tx_iso(iso_conn, om); + ble_hs_lock(); + } + } + ble_hs_unlock(); + return 0; +} + +int +ble_iso_hci_set_buf_sz(uint16_t iso_pktlen, uint16_t iso_max_pkts) +{ + /* TODO This should be in ble_iso_hci.c */ + ble_iso_hci_buf_sz = iso_pktlen; + ble_iso_hci_max_pkts = iso_max_pkts; + ble_iso_hci_avail_pkts = iso_max_pkts; + + return 0; +} + +int +ble_iso_init(void) +{ + struct ble_iso_group *cig; + int rc; + int i; + + rc = os_mempool_init(&ble_iso_conn_pool, MYNEWT_VAL(BLE_MAX_CIS_CONNECTIONS), + sizeof (struct ble_iso_conn), + ble_iso_conn_elem_mem, "ble_iso_conn_pool"); + if (rc != 0) { + return BLE_HS_EOS; + } + + SLIST_INIT(&g_ble_iso_act_slave_cig_list); + SLIST_INIT(&g_ble_iso_act_master_cig_list); + SLIST_INIT(&g_ble_iso_big_list); + STAILQ_INIT(&g_ble_iso_pend_cis_conn_list); + STAILQ_INIT(&g_ble_iso_free_group_list); + + for (i = 0; i < MYNEWT_VAL(BLE_MAX_CIG); i++) { + cig = &g_ble_iso_groups_ids[i]; + cig->id = BLE_ISO_INVALID_CIG_ID; + SLIST_INIT(&cig->iso_head); + STAILQ_INSERT_TAIL(&g_ble_iso_free_group_list, cig, free_cig); + } + + return 0; +} +#endif diff --git a/nimble/host/src/ble_iso_priv.h b/nimble/host/src/ble_iso_priv.h new file mode 100644 index 0000000000..68fbb3573f --- /dev/null +++ b/nimble/host/src/ble_iso_priv.h @@ -0,0 +1,39 @@ +/* + * 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. + */ + +#ifndef H_BLE_ISO_PRIV_ +#define H_BLE_ISO_PRIV_ + +#include +#include "nimble/hci_common.h" + +int ble_iso_rx_hci_evt_le_cis_established(const struct ble_hci_ev_le_subev_cis_established *ev); +int ble_iso_rx_hci_evt_le_cis_request(const struct ble_hci_ev_le_subev_cis_request *ev); +int ble_iso_rx_hci_evt_le_big_completed(const struct ble_hci_ev_le_subev_create_big_complete *ev); +int ble_iso_rx_hci_le_big_terminate_complete(const struct ble_hci_ev_le_subev_terminate_big_complete *ev); +int ble_iso_hci_le_big_sync_established(const struct ble_hci_ev_le_subev_big_sync_established *ev); +int ble_iso_hci_le_big_sync_lost(const struct ble_hci_ev_le_subev_big_sync_lost *ev); + +void ble_iso_disconnected_event(uint16_t conn_handle, uint8_t reason, bool is_acl); +int ble_iso_hci_set_buf_sz(uint16_t iso_pktlen, uint16_t iso_max_pkts); +/* Data handling */ +void ble_iso_rx(struct os_mbuf *om); +void ble_iso_disconnect_cis(uint16_t handle); + +#endif diff --git a/nimble/host/syscfg.yml b/nimble/host/syscfg.yml index 8218b9e769..a54756d6e7 100644 --- a/nimble/host/syscfg.yml +++ b/nimble/host/syscfg.yml @@ -481,6 +481,16 @@ syscfg.defs: supported by host. value: 0 + BLE_MAX_CIS_CONNECTIONS: + description: > + Number of supported CISs + value: 4 + + BLE_MAX_CIG: + desciptrion: > + Number of available CIGs + value: 1 + ### Log settings. BLE_HS_LOG_MOD: @@ -497,10 +507,21 @@ syscfg.defs: description: 'Minimum level for the BLE EATT log.' value: 1 + BLE_HS_LEAUDIO_MOD: + description: 'Numeric module ID to use for BLE LEAudio log messages.' + value: 28 + BLE_HS_LEAUDIO_LOG_LVL: + description: 'Minimum level for the BLE LEAudio logs.' + value: 1 + syscfg.logs: BLE_HS_LOG: module: MYNEWT_VAL(BLE_HS_LOG_MOD) level: MYNEWT_VAL(BLE_HS_LOG_LVL) + + BLE_HS_LEAUDIO_LOG: + module: MYNEWT_VAL(BLE_HS_LEAUDIO_MOD) + level: MYNEWT_VAL(BLE_HS_LEAUDIO_LOG_LVL) BLE_EATT_LOG: module: MYNEWT_VAL(BLE_EATT_LOG_MOD) diff --git a/nimble/include/nimble/hci_common.h b/nimble/include/nimble/hci_common.h index 742824d877..7304341dfb 100644 --- a/nimble/include/nimble/hci_common.h +++ b/nimble/include/nimble/hci_common.h @@ -846,6 +846,7 @@ struct ble_hci_le_read_iso_tx_sync_rp { uint8_t time_offset[3]; } __attribute__((packed)); +#define BLE_HCI_LE_SET_CIG_CIS_MAX_NUM (0x1F) #define BLE_HCI_OCF_LE_SET_CIG_PARAMS (0x0062) struct ble_hci_le_cis_params { uint8_t cis_id; @@ -2103,6 +2104,37 @@ struct ble_hci_iso_data { uint8_t data[0]; }; +#if MYNEWT_VAL(BLE_ISO) + +#define BLE_HCI_ISO_DATA_HANDLE(handle_pb_ts) (((handle_pb_ts) & 0x0fff) >> 0) +#define BLE_HCI_ISO_DATA_TS(handle_pb_ts) (((handle_pb_ts) & 0x4000) >> 14) +#define BLE_HCI_ISO_DATA_LEN(len_rfu) ((len_rfu) & 0x3fff) +#define BLE_HCI_ISO_DATA_SDU_LEN(sdu_len_psf) ((sdu_len_psf) & 0x0fff) +#define BLE_HCI_ISO_DATA_PS_FLAG(sdu_len_psf) (((sdu_len_psf) & 0xc000) >> 14) + +#define BLE_HCI_ISO_HDR_SIZE_WITH_TS (12) +struct hci_iso_data_with_ts_hdr { + uint32_t ts; + uint16_t seq_num; + uint16_t sdu_len_psf; +}; + +#define BLE_HCI_ISO_HDR_SIZE_NO_TS (8) +struct hci_iso_data_no_ts_hdr { + uint16_t seq_num; + uint16_t sdu_len_psf; +}; + +struct hci_iso_hdr { + uint16_t handle_pb_ts; + uint16_t len; + union { + struct hci_iso_data_with_ts_hdr with_ts; + struct hci_iso_data_no_ts_hdr no_ts; + }; +}; +#endif + #ifdef __cplusplus } #endif diff --git a/nimble/syscfg.yml b/nimble/syscfg.yml index ce43a00b1d..dd56885ac2 100644 --- a/nimble/syscfg.yml +++ b/nimble/syscfg.yml @@ -102,6 +102,10 @@ syscfg.defs: value: 0 restrictions: - 'BLE_ISO if 1' + BLE_ISO_LOOPBACK: + decription: > + Enable ISO loopback - mynewt will just send back ISO data to the remote + value: 0 BLE_HCI_VS: description: > diff --git a/nimble/transport/socket/src/ble_hci_socket.c b/nimble/transport/socket/src/ble_hci_socket.c index 75a813dba3..1916662c60 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 @@ -173,6 +178,48 @@ static int s_ble_hci_device = MYNEWT_VAL(BLE_SOCK_LINUX_DEV); static int s_ble_hci_device = 0; #endif +static int +ble_hci_sock_iso_tx(struct os_mbuf *om) +{ + struct msghdr msg; + struct iovec iov[8]; + int i; + struct os_mbuf *m; + uint8_t ch; + + memset(&msg, 0, sizeof(msg)); + memset(iov, 0, sizeof(iov)); + + msg.msg_iov = iov; + + ch = BLE_HCI_UART_H4_ISO; + iov[0].iov_len = 1; + iov[0].iov_base = &ch; + i = 1; + for (m = om; m; m = SLIST_NEXT(m, om_next)) { + iov[i].iov_base = m->om_data; + iov[i].iov_len = m->om_len; + i++; + } + msg.msg_iovlen = i; + + STATS_INC(hci_sock_stats, omsg); + STATS_INC(hci_sock_stats, oiso); + STATS_INCN(hci_sock_stats, obytes, OS_MBUF_PKTLEN(om) + 1); + i = sendmsg(ble_hci_sock_state.sock, &msg, 0); + os_mbuf_free_chain(om); + if (i != OS_MBUF_PKTLEN(om) + 1) { + if (i < 0) { + dprintf(1, "sendmsg() failed : %d\n", errno); + } else { + dprintf(1, "sendmsg() partial write: %d\n", i); + } + STATS_INC(hci_sock_stats, oerr); + return BLE_ERR_MEM_CAPACITY; + } + return 0; +} + #if MYNEWT_VAL(BLE_SOCK_USE_LINUX_BLUE) static int ble_hci_sock_acl_tx(struct os_mbuf *om) @@ -486,6 +533,37 @@ ble_hci_sock_rx_msg(void) #endif OS_EXIT_CRITICAL(sr); break; +#if MYNEWT_VAL(BLE_ISO) + 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) + ble_transport_to_ll_acl(m); +#else + ble_transport_to_hs_acl(m); +#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); + rc = ble_transport_to_ll_iso(m); + OS_EXIT_CRITICAL(sr); + break; +#endif default: STATS_INC(hci_sock_stats, ierr); break; @@ -735,6 +813,20 @@ ble_hci_trans_hs_acl_tx(struct os_mbuf *om) return ble_hci_sock_acl_tx(om); } +/** + * Sends ISO data from host to controller. + * + * @param om The ISO data packet to send. + * + * @return 0 on success; + * A BLE_ERR_[...] error code on failure. + */ +int +ble_hci_trans_hs_iso_tx(struct os_mbuf *om) +{ + return ble_hci_sock_iso_tx(om); +} + /** * Resets the HCI UART transport to a clean state. Frees all buffers and * reconfigures the UART. diff --git a/nimble/transport/src/monitor_priv.h b/nimble/transport/src/monitor_priv.h index 938da8f5ba..0cc7dd5b41 100644 --- a/nimble/transport/src/monitor_priv.h +++ b/nimble/transport/src/monitor_priv.h @@ -49,6 +49,8 @@ extern "C" { #define BLE_MONITOR_EXTHDR_SCO_TX_DROPS 6 #define BLE_MONITOR_EXTHDR_OTHER_DROPS 7 #define BLE_MONITOR_EXTHDR_TS32 8 +#define BLE_MONITOR_EXTHDR_ISO_RX_DROPS 9 +#define BLE_MONITOR_EXTHDR_ISO_TX_DROPS 10 struct ble_monitor_hdr { uint16_t data_len;