nimble/ll: BIG improvements

nimble/controller/include/controller/ble_ll_isoal.h

@@ -39,6 +39,7 @@ struct ble_ll_isoal_mux {
     uint8_t sdu_in_event;
 
     STAILQ_HEAD(, os_mbuf_pkthdr) sdu_q;
+    uint16_t sdu_q_len;
 
     struct os_mbuf *frag;
 

nimble/controller/src/ble_ll_iso_big.c

@@ -144,6 +144,10 @@ struct ble_ll_iso_big {
 
     struct ble_ll_iso_bis_q bis_q;
 
+#if MYNEWT_VAL(BLE_LL_ISO_HCI_FEEDBACK_INTERVAL_MS)
+    uint32_t last_feedback;
+#endif
+
     uint8_t cstf : 1;
     uint8_t cssn : 4;
     uint8_t control_active : 3;
@@ -398,8 +402,16 @@ ble_ll_iso_big_event_done(struct ble_ll_iso_big *big)
 {
     struct ble_ll_iso_bis *bis;
     struct ble_hci_ev *hci_ev;
+#if MYNEWT_VAL(BLE_LL_ISO_HCI_FEEDBACK_INTERVAL_MS)
+    struct ble_hci_ev *fb_hci_ev = NULL;
+    struct ble_hci_ev_vs *fb_hci_ev_vs;
+    struct ble_hci_vs_subev_iso_hci_feedback *fb_hci_subev = NULL;
+    uint16_t exp;
+    uint32_t now;
+#endif
     struct ble_hci_ev_num_comp_pkts *hci_ev_ncp = NULL;
     int num_completed_pkt;
+    int idx;
     int rc;
 
     ble_ll_rfmgmt_release();
@@ -417,18 +429,49 @@ ble_ll_iso_big_event_done(struct ble_ll_iso_big *big)
         hci_ev_ncp->count = 0;
     }
 
+#if MYNEWT_VAL(BLE_LL_ISO_HCI_FEEDBACK_INTERVAL_MS)
+    now = os_time_get();
+    if (OS_TIME_TICK_GEQ(now, big->last_feedback +
+                              os_time_ms_to_ticks32(MYNEWT_VAL(BLE_LL_ISO_HCI_FEEDBACK_INTERVAL_MS)))) {
+        fb_hci_ev = ble_transport_alloc_evt(1);
+        if (fb_hci_ev) {
+            fb_hci_ev->opcode = BLE_HCI_EVCODE_VS;
+            fb_hci_ev->length = sizeof(*fb_hci_ev_vs) + sizeof(*fb_hci_subev);
+            fb_hci_ev_vs = (void *)fb_hci_ev->data;
+            fb_hci_ev_vs->id = BLE_HCI_VS_SUBEV_ISO_HCI_FEEDBACK;
+            fb_hci_subev = (void *)fb_hci_ev_vs->data;
+            fb_hci_subev->big_handle = big->handle;
+            fb_hci_subev->count = 0;
+        }
+    }
+#endif
+
     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);
+            idx = hci_ev_ncp->count++;
+            hci_ev_ncp->completed[idx].handle = htole16(bis->conn_handle);
+            hci_ev_ncp->completed[idx].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 MYNEWT_VAL(BLE_LL_ISO_HCI_FEEDBACK_INTERVAL_MS)
+        if (fb_hci_ev) {
+            /* Expected SDUs in queue after an event -> host should send
+             * sdu_per_interval SDUs until next event so there are sdu_per_event
+             * SDUs queued at next event. Feedback value is the difference between
+             * expected and actual SDUs count.
+             */
+            exp = bis->mux.sdu_per_event - bis->mux.sdu_per_interval;
+            idx = fb_hci_subev->count++;
+            fb_hci_subev->feedback[idx].handle = htole16(bis->conn_handle);
+            fb_hci_subev->feedback[idx].sdu_per_interval = bis->mux.sdu_per_interval;
+            fb_hci_subev->feedback[idx].diff = (int8_t)(bis->mux.sdu_q_len - exp);
+        }
+#endif
     }
 
     if (hci_ev) {
@@ -441,6 +484,15 @@ ble_ll_iso_big_event_done(struct ble_ll_iso_big *big)
         }
     }
 
+#if MYNEWT_VAL(BLE_LL_ISO_HCI_FEEDBACK_INTERVAL_MS)
+    if (fb_hci_ev) {
+        fb_hci_ev->length = sizeof(*fb_hci_ev_vs) + sizeof(*fb_hci_subev) +
+                            fb_hci_subev->count * sizeof(fb_hci_subev->feedback[0]);
+        ble_transport_to_hs_evt(fb_hci_ev);
+        big->last_feedback = now;
+    }
+#endif
+
     big->sch.start_time = big->event_start;
     big->sch.remainder = big->event_start_us;
 
@@ -760,6 +812,8 @@ ble_ll_iso_big_event_sched_cb(struct ble_ll_sched_item *sch)
     ble_phy_mode_set(phy_mode, phy_mode);
 #endif
 
+    ble_ll_tx_power_set(g_ble_ll_tx_power);
+
     BLE_LL_ASSERT(!big->framed);
 
     /* XXX calculate this in advance at the end of previous event? */

nimble/controller/src/ble_ll_isoal.c

@@ -46,6 +46,7 @@ ble_ll_isoal_mux_init(struct ble_ll_isoal_mux *mux, uint8_t max_pdu,
     mux->sdu_per_event = (1 + pte) * mux->sdu_per_interval;
 
     STAILQ_INIT(&mux->sdu_q);
+    mux->sdu_q_len = 0;
 }
 
 void
@@ -117,23 +118,14 @@ ble_ll_isoal_mux_tx_pkt_in(struct ble_ll_isoal_mux *mux, struct os_mbuf *om,
     OS_ENTER_CRITICAL(sr);
     pkthdr = OS_MBUF_PKTHDR(om);
     STAILQ_INSERT_TAIL(&mux->sdu_q, pkthdr, omp_next);
+    mux->sdu_q_len++;
     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->sdu_in_event = min(mux->sdu_q_len, mux->sdu_per_event);
     mux->event_tx_timestamp = timestamp;
 
     return mux->sdu_in_event;
@@ -148,6 +140,7 @@ ble_ll_isoal_mux_event_done(struct ble_ll_isoal_mux *mux)
     struct os_mbuf *om_next;
     uint8_t num_sdu;
     int pkt_freed = 0;
+    os_sr_t sr;
 
     num_sdu = min(mux->sdu_in_event, mux->sdu_per_interval);
 
@@ -159,17 +152,35 @@ ble_ll_isoal_mux_event_done(struct ble_ll_isoal_mux *mux)
         mux->last_tx_packet_seq_num = blehdr->txiso.packet_seq_num;
     }
 
+#if MYNEWT_VAL(BLE_LL_ISO_HCI_DISCARD_THRESHOLD)
+    /* Drop queued SDUs if number of queued SDUs exceeds defined threshold.
+     * Threshold is defined as number of ISO events. If number of queued SDUs
+     * exceeds number of SDUs required for single event (i.e. including pt)
+     * and number of subsequent ISO events defined by threshold value, we'll
+     * drop any excessive SDUs and notify host as if they were sent.
+     */
+    uint32_t thr = MYNEWT_VAL(BLE_LL_ISO_HCI_DISCARD_THRESHOLD);
+    if (mux->sdu_q_len > mux->sdu_per_event + thr * mux->sdu_per_interval) {
+        num_sdu = mux->sdu_q_len - mux->sdu_per_event -
+                  thr * mux->sdu_per_interval;
+    }
+#endif
+
     while (pkthdr && num_sdu--) {
-        om = OS_MBUF_PKTHDR_TO_MBUF(pkthdr);
+        OS_ENTER_CRITICAL(sr);
+        STAILQ_REMOVE_HEAD(&mux->sdu_q, omp_next);
+        BLE_LL_ASSERT(mux->sdu_q_len > 0);
+        mux->sdu_q_len--;
+        OS_EXIT_CRITICAL(sr);
 
+        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);
     }
 

nimble/controller/syscfg.yml

@@ -485,6 +485,35 @@ syscfg.defs:
             - BLE_LL_ISO if 1
         value: MYNEWT_VAL(BLE_ISO_BROADCASTER)
         state: experimental
+    BLE_LL_ISO_HCI_FEEDBACK_INTERVAL_MS:
+        description: >
+            Enables ISO synchronization feedback using vendor-specific HCI event.
+            The event is sent at configured interval after completed ISO event
+            and contains BIG handle, number of expected SDUs per ISO interval
+            and difference between expected vs actual number of SDUs queued in
+            controller.
+            The expected number of SDUs queued after each event is number of
+            SDUs required for each ISO event (i.e. including pre-transmissions)
+            minus number of SDUs expected per each ISO interval.
+            The host can use feedback to e.g. adjust for jitter between audio
+            clock and LL clock.
+            Set to 0 to disable.
+        value: 0
+        experimental: 1
+        restrictions:
+            - BLE_LL_HCI_VS || BLE_LL_ISO_HCI_FEEDBACK_INTERVAL_MS == 0
+    BLE_LL_ISO_HCI_DISCARD_THRESHOLD:
+        description: >
+            Enables automatic discarding of excessive ISO SDUs to avoid exhaustion
+            of HCI ISO buffers in case host sends too many SDUs.
+            Threshold is defined as number of ISO events. If number of queued
+            SDUs exceeds number of SDUs required for single event (i.e. including
+            pre-transmissions) and number of subsequent ISO events defined by
+            threshold value, the controller will drop any excessive SDUs and
+            notify to host as if they were already sent.
+            Set to 0 to disable.
+        value: 0
+        experimental: 1
 
     BLE_LL_SYSINIT_STAGE:
         description: >

nimble/include/nimble/hci_common.h

@@ -1614,6 +1614,18 @@ struct ble_hci_ev_vs_css_slot_changed {
     uint16_t slot_idx;
 };
 
+#define BLE_HCI_VS_SUBEV_ISO_HCI_FEEDBACK       (0x03)
+struct feedback_pkt {
+    uint16_t handle;
+    uint8_t sdu_per_interval;
+    int8_t diff;
+} __attribute__((packed));
+struct ble_hci_vs_subev_iso_hci_feedback {
+    uint8_t big_handle;
+    uint8_t count;
+    struct feedback_pkt feedback[0];
+} __attribute__((packed));
+
 #define BLE_HCI_VS_SUBEV_ID_LLCP_TRACE          (0x17)
 
 /* LE sub-event codes */