This is an automated email from the ASF dual-hosted git repository. andk pushed a commit to branch master in repository https://gitbox.apache.org/repos/asf/mynewt-nimble.git
commit c1cfe3decc3e1348cb4c61aecd9b61d07290f11b Author: Andrzej Kaczmarek <[email protected]> AuthorDate: Tue Jan 31 14:37:01 2023 +0100 nimble/ll: Initial ISOAL implementation This adds some ISOAL code that for now works with unframed PDUs only. It's enough to pass qualification test cases that do not require framed PDUs and even stream some actual content if host supports LC3 codec. --- nimble/controller/include/controller/ble_ll.h | 5 + .../controller/include/controller/ble_ll_iso_big.h | 7 + .../controller/include/controller/ble_ll_isoal.h | 84 +++++ nimble/controller/src/ble_ll.c | 12 + nimble/controller/src/ble_ll_hci.c | 20 +- nimble/controller/src/ble_ll_iso_big.c | 104 +++++- nimble/controller/src/ble_ll_isoal.c | 403 +++++++++++++++++++++ nimble/include/nimble/ble.h | 5 + nimble/include/nimble/hci_common.h | 31 ++ 9 files changed, 661 insertions(+), 10 deletions(-) diff --git a/nimble/controller/include/controller/ble_ll.h b/nimble/controller/include/controller/ble_ll.h index fcfdff20..408f03b6 100644 --- a/nimble/controller/include/controller/ble_ll.h +++ b/nimble/controller/include/controller/ble_ll.h @@ -118,6 +118,11 @@ struct ble_ll_obj uint16_t ll_acl_pkt_size; #endif +#if MYNEWT_VAL(BLE_LL_ISO) + uint8_t ll_num_iso_pkts; + uint16_t ll_iso_pkt_size; +#endif + /* Preferred PHY's */ uint8_t ll_pref_tx_phys; uint8_t ll_pref_rx_phys; diff --git a/nimble/controller/include/controller/ble_ll_iso_big.h b/nimble/controller/include/controller/ble_ll_iso_big.h index c9052126..30cf1b46 100644 --- a/nimble/controller/include/controller/ble_ll_iso_big.h +++ b/nimble/controller/include/controller/ble_ll_iso_big.h @@ -27,11 +27,18 @@ extern "C" { #if MYNEWT_VAL(BLE_LL_ISO_BROADCASTER) struct ble_ll_iso_big; +struct ble_ll_iso_bis; int ble_ll_iso_big_biginfo_copy(struct ble_ll_iso_big *big, uint8_t *dptr, uint32_t base_ticks, uint8_t base_rem_us); int ble_ll_iso_big_biginfo_len(struct ble_ll_iso_big *big); +struct ble_ll_iso_bis *ble_ll_iso_big_find_bis_by_handle(uint16_t conn_handle); +struct ble_ll_isoal_mux *ble_ll_iso_big_find_mux_by_handle(uint16_t conn_handle); +int ble_ll_iso_big_last_tx_timestamp_get(struct ble_ll_iso_bis *bis, + uint16_t *packet_seq_num, + uint32_t *timestamp); + void ble_ll_iso_big_chan_map_update(void); void ble_ll_iso_big_halt(void); diff --git a/nimble/controller/include/controller/ble_ll_isoal.h b/nimble/controller/include/controller/ble_ll_isoal.h new file mode 100644 index 00000000..5104ea97 --- /dev/null +++ b/nimble/controller/include/controller/ble_ll_isoal.h @@ -0,0 +1,84 @@ +/* + * 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_LL_ISOAL_ +#define H_BLE_LL_ISOAL_ + +#ifdef __cplusplus +extern "C" { +#endif + +#if MYNEWT_VAL(BLE_LL_ISO) + +struct ble_ll_isoal_mux { + /* Max PDU length */ + uint8_t max_pdu; + /* Number of expected SDUs per ISO interval */ + uint8_t sdu_per_interval; + /* Number of expected PDUs per SDU */ + uint8_t pdu_per_sdu; + /* Number of SDUs required to fill complete BIG/CIG event (i.e. with pt) */ + uint8_t sdu_per_event; + /* Number of SDUs available for current event */ + uint8_t sdu_in_event; + + STAILQ_HEAD(, os_mbuf_pkthdr) sdu_q; + + struct os_mbuf *frag; + + uint32_t sdu_counter; + + uint32_t event_tx_timestamp; + uint32_t last_tx_timestamp; + uint16_t last_tx_packet_seq_num; +}; + +void +ble_ll_isoal_mux_init(struct ble_ll_isoal_mux *mux, uint8_t max_pdu, + uint32_t iso_interval_us, uint32_t sdu_interval_us, + uint8_t bn, uint8_t pte); +void ble_ll_isoal_mux_free(struct ble_ll_isoal_mux *mux); + +int ble_ll_isoal_mux_event_start(struct ble_ll_isoal_mux *mux, + uint32_t timestamp); +int ble_ll_isoal_mux_event_done(struct ble_ll_isoal_mux *mux); + +int +ble_ll_isoal_mux_unframed_get(struct ble_ll_isoal_mux *mux, uint8_t idx, + uint8_t *llid, void *dptr); + +/* HCI command handlers */ +int ble_ll_isoal_hci_setup_iso_data_path(const uint8_t *cmdbuf, uint8_t cmdlen, + uint8_t *rspbuf, uint8_t *rsplen); +int ble_ll_isoal_hci_remove_iso_data_path(const uint8_t *cmdbuf, uint8_t cmdlen, + uint8_t *rspbuf, uint8_t *rsplen); +int ble_ll_isoal_hci_read_tx_sync(const uint8_t *cmdbuf, uint8_t cmdlen, + uint8_t *rspbuf, uint8_t *rsplen); + +void ble_ll_isoal_init(void); +void ble_ll_isoal_reset(void); +int ble_ll_isoal_data_in(struct os_mbuf *om); + +#endif /* BLE_LL_ISO */ + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_LL_ISOAL_ */ diff --git a/nimble/controller/src/ble_ll.c b/nimble/controller/src/ble_ll.c index de30b892..4ccab7b5 100644 --- a/nimble/controller/src/ble_ll.c +++ b/nimble/controller/src/ble_ll.c @@ -46,6 +46,7 @@ #include "controller/ble_ll_trace.h" #include "controller/ble_ll_sync.h" #include "controller/ble_fem.h" +#include "controller/ble_ll_isoal.h" #include "controller/ble_ll_iso_big.h" #if MYNEWT_VAL(BLE_LL_EXT) #include "controller/ble_ll_ext.h" @@ -1679,6 +1680,9 @@ ble_ll_reset(void) ble_fem_lna_init(); #endif +#if MYNEWT_VAL(BLE_LL_ISO) + ble_ll_isoal_reset(); +#endif #if MYNEWT_VAL(BLE_LL_ISO_BROADCASTER) ble_ll_iso_big_reset(); #endif @@ -1811,6 +1815,11 @@ ble_ll_init(void) lldata->ll_acl_pkt_size = MYNEWT_VAL(BLE_TRANSPORT_ACL_SIZE); #endif +#if MYNEWT_VAL(BLE_LL_ISO) + lldata->ll_num_iso_pkts = MYNEWT_VAL(BLE_TRANSPORT_ISO_FROM_HS_COUNT); + lldata->ll_iso_pkt_size = MYNEWT_VAL(BLE_TRANSPORT_ISO_SIZE); +#endif + /* Initialize eventq */ ble_npl_eventq_init(&lldata->ll_evq); @@ -1950,6 +1959,9 @@ ble_ll_init(void) ble_ll_hci_vs_init(); #endif +#if MYNEWT_VAL(BLE_LL_ISO) + ble_ll_isoal_init(); +#endif #if MYNEWT_VAL(BLE_LL_ISO_BROADCASTER) ble_ll_iso_big_init(); #endif diff --git a/nimble/controller/src/ble_ll_hci.c b/nimble/controller/src/ble_ll_hci.c index b693f5ca..c2b3af71 100644 --- a/nimble/controller/src/ble_ll_hci.c +++ b/nimble/controller/src/ble_ll_hci.c @@ -34,6 +34,7 @@ #include "controller/ble_ll_resolv.h" #include "controller/ble_ll_sync.h" #include <controller/ble_ll_utils.h> +#include "controller/ble_ll_isoal.h" #include "controller/ble_ll_iso.h" #include "controller/ble_ll_iso_big.h" #include "ble_ll_priv.h" @@ -356,9 +357,8 @@ ble_ll_hci_le_read_bufsize_v2(uint8_t *rspbuf, uint8_t *rsplen) rp->data_len = htole16(g_ble_ll_data.ll_acl_pkt_size); rp->data_packets = g_ble_ll_data.ll_num_acl_pkts; - /* XXX for testing only */ - rp->iso_data_len = 512; - rp->iso_data_packets = 10; + rp->iso_data_len = htole16(g_ble_ll_data.ll_iso_pkt_size); + rp->iso_data_packets = g_ble_ll_data.ll_num_iso_pkts; *rsplen = sizeof(*rp); return BLE_ERR_SUCCESS; @@ -1300,14 +1300,17 @@ ble_ll_hci_le_cmd_proc(const uint8_t *cmdbuf, uint8_t len, uint16_t ocf, case BLE_HCI_OCF_LE_BIG_TERMINATE_SYNC: rc = ble_ll_iso_big_terminate_sync(cmdbuf,len); break; +#endif +#if MYNEWT_VAL(BLE_LL_ISO) case BLE_HCI_OCF_LE_SETUP_ISO_DATA_PATH: - rc = ble_ll_iso_setup_iso_data_path(cmdbuf, len); + rc = ble_ll_isoal_hci_setup_iso_data_path(cmdbuf, len, rspbuf, rsplen); break; case BLE_HCI_OCF_LE_REMOVE_ISO_DATA_PATH: - rc = ble_ll_iso_remove_iso_data_path(cmdbuf, len); + rc = ble_ll_isoal_hci_remove_iso_data_path(cmdbuf, len, rspbuf, rsplen); + break; + case BLE_HCI_OCF_LE_READ_ISO_TX_SYNC: + rc = ble_ll_isoal_hci_read_tx_sync(cmdbuf, len, rspbuf, rsplen); break; -#endif -#if MYNEWT_VAL(BLE_LL_ISO) case BLE_HCI_OCF_LE_RD_BUF_SIZE_V2: if (len == 0) { rc = ble_ll_hci_le_read_bufsize_v2(rspbuf, rsplen); @@ -1970,8 +1973,7 @@ int ble_ll_hci_iso_rx(struct os_mbuf *om) { #if MYNEWT_VAL(BLE_LL_ISO) - /* FIXME send to isoal... */ - os_mbuf_free_chain(om); + ble_ll_isoal_data_in(om); #else os_mbuf_free_chain(om); #endif diff --git a/nimble/controller/src/ble_ll_iso_big.c b/nimble/controller/src/ble_ll_iso_big.c index 594ff4a1..096d3a98 100644 --- a/nimble/controller/src/ble_ll_iso_big.c +++ b/nimble/controller/src/ble_ll_iso_big.c @@ -27,6 +27,7 @@ #include <controller/ble_ll_adv.h> #include <controller/ble_ll_crypto.h> #include <controller/ble_ll_hci.h> +#include <controller/ble_ll_isoal.h> #include <controller/ble_ll_iso_big.h> #include <controller/ble_ll_pdu.h> #include <controller/ble_ll_sched.h> @@ -68,6 +69,9 @@ struct ble_ll_iso_bis { uint8_t g; } tx; + struct ble_ll_isoal_mux mux; + uint16_t num_completed_pkt; + STAILQ_ENTRY(ble_ll_iso_bis) bis_q_next; }; @@ -160,6 +164,58 @@ static uint8_t bis_pool_free = BIS_POOL_SIZE; static struct ble_ll_iso_big *big_pending; static struct ble_ll_iso_big *big_active; +struct ble_ll_iso_bis * +ble_ll_iso_big_find_bis_by_handle(uint16_t conn_handle) +{ + struct ble_ll_iso_bis *bis; + uint8_t bis_idx; + + if (!BLE_LL_CONN_HANDLE_IS_BIS(conn_handle)) { + return NULL; + } + + bis_idx = BLE_LL_CONN_HANDLE_IDX(conn_handle); + + if (bis_idx >= BIS_POOL_SIZE) { + return NULL; + } + + bis = &bis_pool[bis_idx]; + if (!bis->big) { + return NULL; + } + + return bis; +} + +struct ble_ll_isoal_mux * +ble_ll_iso_big_find_mux_by_handle(uint16_t conn_handle) +{ + struct ble_ll_iso_bis *bis; + + bis = ble_ll_iso_big_find_bis_by_handle(conn_handle); + if (bis) { + return &bis->mux; + } + + return NULL; +} + +int +ble_ll_iso_big_last_tx_timestamp_get(struct ble_ll_iso_bis *bis, + uint16_t *packet_seq_num, uint32_t *timestamp) +{ + struct ble_ll_iso_big *big; + + big = bis->big; + + *packet_seq_num = big->bis_counter; + *timestamp = (uint64_t)big->event_start * 1000000 / 32768 + + big->event_start_us; + + return 0; +} + static void ble_ll_iso_big_biginfo_calc(struct ble_ll_iso_big *big, uint32_t seed_aa) { @@ -281,6 +337,7 @@ ble_ll_iso_big_free(struct ble_ll_iso_big *big) ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &big->event_done); STAILQ_FOREACH(bis, &big->bis_q, bis_q_next) { + ble_ll_isoal_mux_free(&bis->mux); bis->big = NULL; bis_pool_free++; } @@ -339,6 +396,10 @@ ble_ll_iso_big_update_event_start(struct ble_ll_iso_big *big) static void ble_ll_iso_big_event_done(struct ble_ll_iso_big *big) { + struct ble_ll_iso_bis *bis; + struct ble_hci_ev *hci_ev; + struct ble_hci_ev_num_comp_pkts *hci_ev_ncp; + int num_completed_pkt; int rc; ble_ll_rfmgmt_release(); @@ -348,6 +409,37 @@ ble_ll_iso_big_event_done(struct ble_ll_iso_big *big) ble_ll_iso_big_hci_evt_complete(); } + hci_ev = ble_transport_alloc_evt(1); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_NUM_COMP_PKTS; + hci_ev->length = sizeof(*hci_ev_ncp); + hci_ev_ncp = (void *)hci_ev->data; + hci_ev_ncp->count = 0; + } + + STAILQ_FOREACH(bis, &big->bis_q, bis_q_next) { + num_completed_pkt = ble_ll_isoal_mux_event_done(&bis->mux); + if (hci_ev && num_completed_pkt) { + hci_ev_ncp->completed[hci_ev_ncp->count].handle = + htole16(bis->conn_handle); + hci_ev_ncp->completed[hci_ev_ncp->count].packets = + htole16(num_completed_pkt + bis->num_completed_pkt); + bis->num_completed_pkt = 0; + hci_ev_ncp->count++; + } else { + bis->num_completed_pkt += num_completed_pkt; + } + } + + if (hci_ev) { + if (hci_ev_ncp->count) { + hci_ev->length = sizeof(*hci_ev_ncp) + + hci_ev_ncp->count * sizeof(hci_ev_ncp->completed[0]); + ble_transport_to_hs_evt(hci_ev); + } else { + ble_transport_free(hci_ev); + } + } big->sch.start_time = big->event_start; big->sch.remainder = big->event_start_us; @@ -532,6 +624,9 @@ ble_ll_iso_big_subevent_pdu_cb(uint8_t *dptr, void *arg, uint8_t *hdr_byte) ble_phy_encrypt_counter_set(big->bis_counter + idx, 1); } +#if 1 + pdu_len = ble_ll_isoal_mux_unframed_get(&bis->mux, idx, &llid, dptr); +#else llid = 0; pdu_len = big->max_pdu; /* XXX dummy data for testing */ @@ -549,6 +644,7 @@ ble_ll_iso_big_subevent_pdu_cb(uint8_t *dptr, void *arg, uint8_t *hdr_byte) dptr[11] = 'P'; } dptr[12] = 0xff; +#endif *hdr_byte = llid | (big->cssn << 2) | (big->cstf << 5); @@ -669,6 +765,10 @@ ble_ll_iso_big_event_sched_cb(struct ble_ll_sched_item *sch) /* XXX calculate this in advance at the end of previous event? */ big->tx.subevents_rem = big->num_bis * big->nse; STAILQ_FOREACH(bis, &big->bis_q, bis_q_next) { + ble_ll_isoal_mux_event_start(&bis->mux, (uint64_t)big->event_start * + 1000000 / 32768 + + big->event_start_us); + bis->tx.subevent_num = 0; bis->tx.n = 0; bis->tx.g = 0; @@ -820,7 +920,6 @@ ble_ll_iso_big_create(uint8_t big_handle, uint8_t adv_handle, uint8_t num_bis, /* Calculate number of additional events for pre-transmissions */ /* Core 5.3, Vol 6, Part B, 4.4.6.6 */ gc = bp->nse / bp->bn; - (void)pte; if (bp->irc == gc) { pte = 0; } else { @@ -844,6 +943,9 @@ ble_ll_iso_big_create(uint8_t big_handle, uint8_t adv_handle, uint8_t num_bis, bis->crc_init = (big->crc_init << 8) | (big->num_bis); BLE_LL_ASSERT(!big->framed); + + ble_ll_isoal_mux_init(&bis->mux, bp->max_pdu, bp->iso_interval * 1250, + bp->sdu_interval, bp->bn, pte); } big_pool_free--; diff --git a/nimble/controller/src/ble_ll_isoal.c b/nimble/controller/src/ble_ll_isoal.c new file mode 100644 index 00000000..b0288443 --- /dev/null +++ b/nimble/controller/src/ble_ll_isoal.c @@ -0,0 +1,403 @@ +/* + * 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 <stdint.h> +#include <syscfg/syscfg.h> +#include <nimble/hci_common.h> +#include <controller/ble_ll.h> +#include <controller/ble_ll_isoal.h> +#include <controller/ble_ll_iso_big.h> + +#if MYNEWT_VAL(BLE_LL_ISO) + +STAILQ_HEAD(ble_ll_iso_tx_q, os_mbuf_pkthdr); + +static struct ble_npl_event ll_isoal_tx_pkt_in; +static struct ble_ll_iso_tx_q ll_isoal_tx_q; + +void +ble_ll_isoal_mux_init(struct ble_ll_isoal_mux *mux, uint8_t max_pdu, + uint32_t iso_interval_us, uint32_t sdu_interval_us, + uint8_t bn, uint8_t pte) +{ + memset(mux, 0, sizeof(*mux)); + + mux->max_pdu = max_pdu; + /* Core 5.3, Vol 6, Part G, 2.1 */ + mux->sdu_per_interval = iso_interval_us / sdu_interval_us; + mux->pdu_per_sdu = bn / mux->sdu_per_interval; + + mux->sdu_per_event = (1 + pte) * mux->sdu_per_interval; + + STAILQ_INIT(&mux->sdu_q); +} + +void +ble_ll_isoal_mux_free(struct ble_ll_isoal_mux *mux) +{ + struct os_mbuf_pkthdr *pkthdr; + struct os_mbuf *om; + struct os_mbuf *om_next; + + pkthdr = STAILQ_FIRST(&mux->sdu_q); + while (pkthdr) { + om = OS_MBUF_PKTHDR_TO_MBUF(pkthdr); + + while (om) { + om_next = SLIST_NEXT(om, om_next); + os_mbuf_free(om); + om = om_next; + } + + STAILQ_REMOVE_HEAD(&mux->sdu_q, omp_next); + pkthdr = STAILQ_FIRST(&mux->sdu_q); + } + + STAILQ_INIT(&mux->sdu_q); +} + +static void +ble_ll_isoal_mux_tx_pkt_in(struct ble_ll_isoal_mux *mux, struct os_mbuf *om, + uint8_t pb, uint32_t timestamp) +{ + struct os_mbuf_pkthdr *pkthdr; + struct ble_mbuf_hdr *blehdr; + os_sr_t sr; + + BLE_LL_ASSERT(mux); + + switch (pb) { + case BLE_HCI_ISO_PB_FIRST: + BLE_LL_ASSERT(!mux->frag); + mux->frag = om; + om = NULL; + break; + case BLE_HCI_ISO_PB_CONTINUATION: + BLE_LL_ASSERT(mux->frag); + os_mbuf_concat(mux->frag, om); + om = NULL; + break; + case BLE_HCI_ISO_PB_COMPLETE: + BLE_LL_ASSERT(!mux->frag); + break; + case BLE_HCI_ISO_PB_LAST: + BLE_LL_ASSERT(mux->frag); + os_mbuf_concat(mux->frag, om); + om = mux->frag; + mux->frag = NULL; + break; + default: + BLE_LL_ASSERT(0); + break; + } + + if (!om) { + return; + } + + blehdr = BLE_MBUF_HDR_PTR(om); + blehdr->txiso.packet_seq_num = ++mux->sdu_counter; + + OS_ENTER_CRITICAL(sr); + pkthdr = OS_MBUF_PKTHDR(om); + STAILQ_INSERT_TAIL(&mux->sdu_q, pkthdr, omp_next); + OS_EXIT_CRITICAL(sr); +} + +int +ble_ll_isoal_mux_event_start(struct ble_ll_isoal_mux *mux, uint32_t timestamp) +{ + struct os_mbuf_pkthdr *pkthdr; + uint8_t num_sdu; + + num_sdu = mux->sdu_per_event; + + pkthdr = STAILQ_FIRST(&mux->sdu_q); + while (pkthdr && num_sdu--) { + pkthdr = STAILQ_NEXT(pkthdr, omp_next); + } + + mux->sdu_in_event = mux->sdu_per_event - num_sdu; + mux->event_tx_timestamp = timestamp; + + return mux->sdu_in_event; +} + +int +ble_ll_isoal_mux_event_done(struct ble_ll_isoal_mux *mux) +{ + struct os_mbuf_pkthdr *pkthdr; + struct ble_mbuf_hdr *blehdr; + struct os_mbuf *om; + struct os_mbuf *om_next; + uint8_t num_sdu; + int pkt_freed = 0; + + num_sdu = min(mux->sdu_in_event, mux->sdu_per_interval); + + pkthdr = STAILQ_FIRST(&mux->sdu_q); + if (pkthdr) { + om = OS_MBUF_PKTHDR_TO_MBUF(pkthdr); + blehdr = BLE_MBUF_HDR_PTR(om); + mux->last_tx_timestamp = mux->event_tx_timestamp; + mux->last_tx_packet_seq_num = blehdr->txiso.packet_seq_num; + } + + while (pkthdr && num_sdu--) { + om = OS_MBUF_PKTHDR_TO_MBUF(pkthdr); + + while (om) { + om_next = SLIST_NEXT(om, om_next); + os_mbuf_free(om); + pkt_freed++; + om = om_next; + } + + STAILQ_REMOVE_HEAD(&mux->sdu_q, omp_next); + pkthdr = STAILQ_FIRST(&mux->sdu_q); + } + + mux->sdu_in_event = 0; + + return pkt_freed; +} + +int +ble_ll_isoal_mux_unframed_get(struct ble_ll_isoal_mux *mux, uint8_t idx, + uint8_t *llid, void *dptr) +{ + struct os_mbuf_pkthdr *pkthdr; + struct os_mbuf *om; + uint8_t sdu_idx; + uint8_t pdu_idx; + uint16_t sdu_offset; + uint16_t rem_len; + uint8_t pdu_len; + + sdu_idx = idx / mux->pdu_per_sdu; + pdu_idx = idx - sdu_idx * mux->pdu_per_sdu; + + if (sdu_idx >= mux->sdu_in_event) { + *llid = 1; + return 0; + } + + pkthdr = STAILQ_FIRST(&mux->sdu_q); + while (pkthdr && sdu_idx--) { + pkthdr = STAILQ_NEXT(pkthdr, omp_next); + } + + if (!pkthdr) { + *llid = 1; + return 0; + } + + om = OS_MBUF_PKTHDR_TO_MBUF(pkthdr); + sdu_offset = pdu_idx * mux->max_pdu; + rem_len = OS_MBUF_PKTLEN(om) - sdu_offset; + + if ((int32_t)rem_len <= 0) { + *llid = 1; + pdu_len = 0; + } else { + *llid = (pdu_idx < mux->pdu_per_sdu - 1); + pdu_len = min(mux->max_pdu, rem_len); + } + + os_mbuf_copydata(om, sdu_offset, pdu_len, dptr); + + return pdu_len; +} + +static void +ble_ll_isoal_tx_pkt_in(struct ble_npl_event *ev) +{ + struct os_mbuf *om; + struct os_mbuf_pkthdr *pkthdr; + struct ble_hci_iso *hci_iso; + struct ble_hci_iso_data *hci_iso_data; + struct ble_ll_isoal_mux *mux; + uint16_t data_hdr_len; + uint16_t handle; + uint16_t conn_handle; + uint16_t length; + uint16_t pb_flag; + uint16_t ts_flag; + uint32_t timestamp = 0; + os_sr_t sr; + + while (STAILQ_FIRST(&ll_isoal_tx_q)) { + pkthdr = STAILQ_FIRST(&ll_isoal_tx_q); + om = OS_MBUF_PKTHDR_TO_MBUF(pkthdr); + + OS_ENTER_CRITICAL(sr); + STAILQ_REMOVE_HEAD(&ll_isoal_tx_q, omp_next); + OS_EXIT_CRITICAL(sr); + + hci_iso = (void *)om->om_data; + + handle = le16toh(hci_iso->handle); + conn_handle = BLE_HCI_ISO_CONN_HANDLE(handle); + pb_flag = BLE_HCI_ISO_PB_FLAG(handle); + ts_flag = BLE_HCI_ISO_TS_FLAG(handle); + length = BLE_HCI_ISO_LENGTH(le16toh(hci_iso->length)); + + data_hdr_len = 0; + if ((pb_flag == BLE_HCI_ISO_PB_FIRST) || + (pb_flag == BLE_HCI_ISO_PB_COMPLETE)) { + if (ts_flag) { + timestamp = get_le32(om->om_data + sizeof(*hci_iso)); + data_hdr_len += sizeof(uint32_t); + } + + hci_iso_data = (void *)(om->om_data + sizeof(*hci_iso) + data_hdr_len); + data_hdr_len += sizeof(*hci_iso_data); + } + os_mbuf_adj(om, sizeof(*hci_iso) + data_hdr_len); + + if (OS_MBUF_PKTLEN(om) != length - data_hdr_len) { + os_mbuf_free_chain(om); + continue; + } + + switch (BLE_LL_CONN_HANDLE_TYPE(conn_handle)) { + case BLE_LL_CONN_HANDLE_TYPE_BIS: + mux = ble_ll_iso_big_find_mux_by_handle(conn_handle); + ble_ll_isoal_mux_tx_pkt_in(mux, om, pb_flag, timestamp); + break; + default: + os_mbuf_free_chain(om); + break; + } + } +} + +int +ble_ll_isoal_hci_setup_iso_data_path(const uint8_t *cmdbuf, uint8_t cmdlen, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_le_setup_iso_data_path_cp *cmd = (const void *)cmdbuf; + struct ble_hci_le_setup_iso_data_path_rp *rsp = (void *)rspbuf; + struct ble_ll_iso_bis *bis; + uint16_t conn_handle; + + conn_handle = le16toh(cmd->conn_handle); + switch (BLE_LL_CONN_HANDLE_TYPE(conn_handle)) { + case BLE_LL_CONN_HANDLE_TYPE_BIS: + bis = ble_ll_iso_big_find_bis_by_handle(conn_handle); + if (bis) { + break; + } + default: + return BLE_ERR_UNK_CONN_ID; + } + + /* Only input for now since we only support BIS */ + if (cmd->data_path_dir) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* We do not (yet) support any vendor-specific data path */ + if (cmd->data_path_id) { + return BLE_ERR_CMD_DISALLOWED; + } + + rsp->conn_handle = cmd->conn_handle; + *rsplen = sizeof(*rsp); + + return 0; +} + +int +ble_ll_isoal_hci_remove_iso_data_path(const uint8_t *cmdbuf, uint8_t cmdlen, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_le_remove_iso_data_path_cp *cmd = (const void *)cmdbuf; + struct ble_hci_le_remove_iso_data_path_rp *rsp = (void *)rspbuf; + + /* XXX accepts anything for now */ + rsp->conn_handle = cmd->conn_handle; + *rsplen = sizeof(*rsp); + + return 0; +} + +int +ble_ll_isoal_hci_read_tx_sync(const uint8_t *cmdbuf, uint8_t cmdlen, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_le_read_iso_tx_sync_cp *cmd = (const void *)cmdbuf; + struct ble_hci_le_read_iso_tx_sync_rp *rsp = (void *)rspbuf; + struct ble_ll_isoal_mux *mux; + uint16_t handle; + + handle = le16toh(cmd->conn_handle); + switch (BLE_LL_CONN_HANDLE_TYPE(handle)) { + case BLE_LL_CONN_HANDLE_TYPE_BIS: + mux = ble_ll_iso_big_find_mux_by_handle(handle); + if (!mux) { + return BLE_ERR_UNK_CONN_ID; + } + break; + default: + return BLE_ERR_UNK_CONN_ID; + } + + rsp->conn_handle = cmd->conn_handle; + rsp->packet_seq_num = htole16(mux->last_tx_packet_seq_num); + rsp->tx_timestamp = htole32(mux->last_tx_timestamp); + put_le24(rsp->time_offset, 0); + + *rsplen = sizeof(*rsp); + + return 0; +} + +void +ble_ll_isoal_init(void) +{ + STAILQ_INIT(&ll_isoal_tx_q); + ble_npl_event_init(&ll_isoal_tx_pkt_in, ble_ll_isoal_tx_pkt_in, NULL); +} + +void +ble_ll_isoal_reset(void) +{ + STAILQ_INIT(&ll_isoal_tx_q); + ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &ll_isoal_tx_pkt_in); +} + +int +ble_ll_isoal_data_in(struct os_mbuf *om) +{ + struct os_mbuf_pkthdr *hdr; + os_sr_t sr; + + hdr = OS_MBUF_PKTHDR(om); + + OS_ENTER_CRITICAL(sr); + STAILQ_INSERT_TAIL(&ll_isoal_tx_q, hdr, omp_next); + OS_EXIT_CRITICAL(sr); + + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &ll_isoal_tx_pkt_in); + + return 0; +} + +#endif /* BLE_LL_ISO */ diff --git a/nimble/include/nimble/ble.h b/nimble/include/nimble/ble.h index 0aa96251..6979e945 100644 --- a/nimble/include/nimble/ble.h +++ b/nimble/include/nimble/ble.h @@ -118,11 +118,16 @@ struct ble_mbuf_hdr_txinfo uint16_t pyld_len; }; +struct ble_mbuf_hdr_txiso { + uint16_t packet_seq_num; +}; + struct ble_mbuf_hdr { union { struct ble_mbuf_hdr_rxinfo rxinfo; struct ble_mbuf_hdr_txinfo txinfo; + struct ble_mbuf_hdr_txiso txiso; }; uint32_t beg_cputime; uint32_t rem_usecs; diff --git a/nimble/include/nimble/hci_common.h b/nimble/include/nimble/hci_common.h index ca0c219e..93429a2f 100644 --- a/nimble/include/nimble/hci_common.h +++ b/nimble/include/nimble/hci_common.h @@ -2070,6 +2070,37 @@ 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_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_LENGTH(l) ((l) & BLE_HCI_ISO_LENGTH_MASK) + +#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) + +struct ble_hci_iso { + uint16_t handle; + uint16_t length; + uint8_t data[0]; +}; + +#define BLE_HCI_ISO_HDR_SDU_LENGTH_MASK (0x07ff) + +struct ble_hci_iso_data { + uint16_t packet_seq_num; + uint16_t sdu_len; + uint8_t data[0]; +}; + #ifdef __cplusplus } #endif
