1 /*
2 * Copyright (c) 2020 Demant
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <soc.h>
8 #include <zephyr/kernel.h>
9 #include <zephyr/sys/byteorder.h>
10 #include <zephyr/bluetooth/hci_types.h>
11 #include <zephyr/bluetooth/buf.h>
12
13 #include "hal/cpu.h"
14 #include "hal/ccm.h"
15 #include "hal/ticker.h"
16
17 #include "util/util.h"
18 #include "util/mem.h"
19 #include "util/memq.h"
20 #include "util/mfifo.h"
21 #include "util/mayfly.h"
22 #include "util/dbuf.h"
23
24 #include "pdu_df.h"
25 #include "lll/pdu_vendor.h"
26 #include "pdu.h"
27
28 #include "lll.h"
29 #include "lll/lll_adv_types.h"
30 #include "lll_adv.h"
31 #include "lll/lll_adv_pdu.h"
32 #include "lll_adv_iso.h"
33 #include "lll/lll_df_types.h"
34 #include "lll_sync.h"
35 #include "lll_sync_iso.h"
36 #include "lll_conn.h"
37 #include "lll_conn_iso.h"
38 #include "lll_iso_tx.h"
39
40 #include "ll_sw/ull_tx_queue.h"
41
42 #include "isoal.h"
43
44 #include "ull_adv_types.h"
45 #include "ull_sync_types.h"
46 #include "ull_conn_types.h"
47 #include "ull_iso_types.h"
48 #include "ull_conn_iso_types.h"
49 #include "ull_llcp.h"
50
51 #include "ull_internal.h"
52 #include "ull_adv_internal.h"
53 #include "ull_conn_internal.h"
54 #include "ull_iso_internal.h"
55 #include "ull_sync_iso_internal.h"
56 #include "ull_conn_iso_internal.h"
57
58 #include "ll_feat.h"
59
60 #include "hal/debug.h"
61
62 #define LOG_LEVEL CONFIG_BT_HCI_DRIVER_LOG_LEVEL
63 #include <zephyr/logging/log.h>
64 LOG_MODULE_REGISTER(bt_ctlr_ull_iso);
65
66 #if defined(CONFIG_BT_CTLR_CONN_ISO_STREAMS)
67 #define BT_CTLR_CONN_ISO_STREAMS CONFIG_BT_CTLR_CONN_ISO_STREAMS
68 #else /* !CONFIG_BT_CTLR_CONN_ISO_STREAMS */
69 #define BT_CTLR_CONN_ISO_STREAMS 0
70 #endif /* !CONFIG_BT_CTLR_CONN_ISO_STREAMS */
71
72 #if defined(CONFIG_BT_CTLR_ADV_ISO_STREAM_COUNT)
73 #define BT_CTLR_ADV_ISO_STREAMS (CONFIG_BT_CTLR_ADV_ISO_STREAM_COUNT)
74 #else /* !CONFIG_BT_CTLR_ADV_ISO_STREAM_COUNT */
75 #define BT_CTLR_ADV_ISO_STREAMS 0
76 #endif /* CONFIG_BT_CTLR_ADV_ISO_STREAM_COUNT */
77
78 #if defined(CONFIG_BT_CTLR_SYNC_ISO_STREAM_COUNT)
79 #define BT_CTLR_SYNC_ISO_STREAMS (CONFIG_BT_CTLR_SYNC_ISO_STREAM_COUNT)
80 #else /* !CONFIG_BT_CTLR_SYNC_ISO_STREAM_COUNT */
81 #define BT_CTLR_SYNC_ISO_STREAMS 0
82 #endif /* CONFIG_BT_CTLR_SYNC_ISO_STREAM_COUNT */
83
84 static int init_reset(void);
85
86 #if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
87 static isoal_status_t ll_iso_pdu_alloc(struct isoal_pdu_buffer *pdu_buffer);
88 static isoal_status_t ll_iso_pdu_write(struct isoal_pdu_buffer *pdu_buffer,
89 const size_t offset,
90 const uint8_t *sdu_payload,
91 const size_t consume_len);
92 static isoal_status_t ll_iso_pdu_emit(struct node_tx_iso *node_tx,
93 const uint16_t handle);
94 static isoal_status_t ll_iso_pdu_release(struct node_tx_iso *node_tx,
95 const uint16_t handle,
96 const isoal_status_t status);
97 #endif /* CONFIG_BT_CTLR_ADV_ISO || CONFIG_BT_CTLR_CONN_ISO */
98
99 /* Allocate data path pools for RX/TX directions for each stream */
100 #define BT_CTLR_ISO_STREAMS ((2 * (BT_CTLR_CONN_ISO_STREAMS)) + \
101 BT_CTLR_ADV_ISO_STREAMS + \
102 BT_CTLR_SYNC_ISO_STREAMS)
103 #if BT_CTLR_ISO_STREAMS
104 static struct ll_iso_datapath datapath_pool[BT_CTLR_ISO_STREAMS];
105 #endif /* BT_CTLR_ISO_STREAMS */
106
107 static void *datapath_free;
108
109 #if defined(CONFIG_BT_CTLR_SYNC_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
110 #define NODE_RX_HEADER_SIZE (offsetof(struct node_rx_pdu, pdu))
111 /* ISO LL conformance tests require a PDU size of maximum 251 bytes + header */
112 #define ISO_RX_BUFFER_SIZE (2 + 251)
113
114 /* Declare the ISO rx node RXFIFO. This is a composite pool-backed MFIFO for
115 * rx_nodes. The declaration constructs the following data structures:
116 * - mfifo_iso_rx: FIFO with pointers to PDU buffers
117 * - mem_iso_rx: Backing data pool for PDU buffer elements
118 * - mem_link_iso_rx: Pool of memq_link_t elements
119 *
120 * One extra rx buffer is reserved for empty ISO PDU reception.
121 * Two extra links are reserved for use by the ll_iso_rx and ull_iso_rx memq.
122 */
123 static RXFIFO_DEFINE(iso_rx, ((NODE_RX_HEADER_SIZE) + (ISO_RX_BUFFER_SIZE)),
124 (CONFIG_BT_CTLR_ISO_RX_BUFFERS + 1U), 2U);
125
126 static MEMQ_DECLARE(ll_iso_rx);
127 #if defined(CONFIG_BT_CTLR_ISO_VENDOR_DATA_PATH)
128 static MEMQ_DECLARE(ull_iso_rx);
129 static void iso_rx_demux(void *param);
130 #endif /* CONFIG_BT_CTLR_ISO_VENDOR_DATA_PATH */
131 #endif /* CONFIG_BT_CTLR_SYNC_ISO) || CONFIG_BT_CTLR_CONN_ISO */
132
133 #define ISO_TEST_PACKET_COUNTER_SIZE 4U
134
135 #if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
136 void ll_iso_link_tx_release(void *link);
137 void ll_iso_tx_mem_release(void *node_tx);
138
139 #define NODE_TX_BUFFER_SIZE MROUND(offsetof(struct node_tx_iso, pdu) + \
140 offsetof(struct pdu_iso, payload) + \
141 MAX(LL_BIS_OCTETS_TX_MAX, \
142 LL_CIS_OCTETS_TX_MAX))
143
144 #define ISO_TEST_TX_BUFFER_SIZE 32U
145
146 static struct {
147 void *free;
148 uint8_t pool[NODE_TX_BUFFER_SIZE * BT_CTLR_ISO_TX_BUFFERS];
149 } mem_iso_tx;
150
151 static struct {
152 void *free;
153 uint8_t pool[sizeof(memq_link_t) * BT_CTLR_ISO_TX_BUFFERS];
154 } mem_link_iso_tx;
155
156 #endif /* CONFIG_BT_CTLR_ADV_ISO || CONFIG_BT_CTLR_CONN_ISO */
157
ll_read_iso_tx_sync(uint16_t handle,uint16_t * seq,uint32_t * timestamp,uint32_t * offset)158 uint8_t ll_read_iso_tx_sync(uint16_t handle, uint16_t *seq,
159 uint32_t *timestamp, uint32_t *offset)
160 {
161 if (IS_CIS_HANDLE(handle)) {
162 struct ll_iso_datapath *dp = NULL;
163 struct ll_conn_iso_stream *cis;
164
165 cis = ll_conn_iso_stream_get(handle);
166
167 if (cis) {
168 dp = cis->hdr.datapath_in;
169 }
170
171 if (dp &&
172 isoal_tx_get_sync_info(dp->source_hdl, seq,
173 timestamp, offset) == ISOAL_STATUS_OK) {
174 return BT_HCI_ERR_SUCCESS;
175 }
176
177 return BT_HCI_ERR_CMD_DISALLOWED;
178
179 } else if (IS_ADV_ISO_HANDLE(handle)) {
180 const struct lll_adv_iso_stream *adv_stream;
181 uint16_t stream_handle;
182
183 stream_handle = LL_BIS_ADV_IDX_FROM_HANDLE(handle);
184 adv_stream = ull_adv_iso_stream_get(stream_handle);
185 if (!adv_stream || !adv_stream->dp ||
186 isoal_tx_get_sync_info(adv_stream->dp->source_hdl, seq,
187 timestamp, offset) != ISOAL_STATUS_OK) {
188 return BT_HCI_ERR_CMD_DISALLOWED;
189 }
190
191 return BT_HCI_ERR_SUCCESS;
192
193 } else if (IS_SYNC_ISO_HANDLE(handle)) {
194 return BT_HCI_ERR_CMD_DISALLOWED;
195 }
196
197 return BT_HCI_ERR_UNKNOWN_CONN_ID;
198 }
199
path_is_vendor_specific(uint8_t path_id)200 static inline bool path_is_vendor_specific(uint8_t path_id)
201 {
202 return (path_id >= BT_HCI_DATAPATH_ID_VS &&
203 path_id <= BT_HCI_DATAPATH_ID_VS_END);
204 }
205
ll_setup_iso_path(uint16_t handle,uint8_t path_dir,uint8_t path_id,uint8_t coding_format,uint16_t company_id,uint16_t vs_codec_id,uint32_t controller_delay,uint8_t codec_config_len,uint8_t * codec_config)206 uint8_t ll_setup_iso_path(uint16_t handle, uint8_t path_dir, uint8_t path_id,
207 uint8_t coding_format, uint16_t company_id,
208 uint16_t vs_codec_id, uint32_t controller_delay,
209 uint8_t codec_config_len, uint8_t *codec_config)
210 {
211 struct lll_sync_iso_stream *sync_stream = NULL;
212 struct lll_adv_iso_stream *adv_stream = NULL;
213 struct ll_conn_iso_stream *cis = NULL;
214 struct ll_iso_datapath *dp;
215 uint32_t stream_sync_delay;
216 uint32_t group_sync_delay;
217 uint8_t flush_timeout;
218 uint16_t iso_interval;
219 uint32_t sdu_interval;
220 uint8_t burst_number;
221 uint8_t max_octets;
222 uint8_t framed;
223 uint8_t role;
224
225 ARG_UNUSED(controller_delay);
226 ARG_UNUSED(codec_config);
227
228 if (false) {
229
230 #if defined(CONFIG_BT_CTLR_CONN_ISO)
231 } else if (IS_CIS_HANDLE(handle)) {
232 struct ll_conn_iso_group *cig;
233 struct ll_conn *conn;
234
235 /* If the Host attempts to set a data path with a Connection
236 * Handle that does not exist or that is not for a CIS or a BIS,
237 * the Controller shall return the error code Unknown Connection
238 * Identifier (0x02)
239 */
240 cis = ll_conn_iso_stream_get(handle);
241 if (!cis || !cis->group) {
242 /* CIS does not belong to a CIG */
243 return BT_HCI_ERR_UNKNOWN_CONN_ID;
244 }
245
246 conn = ll_connected_get(cis->lll.acl_handle);
247 if (conn) {
248 /* If we're still waiting for accept/response from
249 * host, path setup is premature and we must return
250 * disallowed status.
251 */
252 #if defined(CONFIG_BT_CTLR_PERIPHERAL_ISO)
253 const uint8_t cis_waiting = ull_cp_cc_awaiting_reply(conn);
254
255 if (cis_waiting) {
256 return BT_HCI_ERR_CMD_DISALLOWED;
257 }
258 #endif /* CONFIG_BT_CTLR_PERIPHERAL_ISO */
259 }
260
261 if ((path_dir == BT_HCI_DATAPATH_DIR_HOST_TO_CTLR && cis->hdr.datapath_in) ||
262 (path_dir == BT_HCI_DATAPATH_DIR_CTLR_TO_HOST && cis->hdr.datapath_out)) {
263 /* Data path has been set up, can only do setup once */
264 return BT_HCI_ERR_CMD_DISALLOWED;
265 }
266
267 cig = cis->group;
268
269 role = cig->lll.role;
270 iso_interval = cig->iso_interval;
271 group_sync_delay = cig->sync_delay;
272 stream_sync_delay = cis->sync_delay;
273 framed = cis->framed;
274
275 if (path_dir == BT_HCI_DATAPATH_DIR_CTLR_TO_HOST) {
276 /* Create sink for RX data path */
277 burst_number = cis->lll.rx.bn;
278 flush_timeout = cis->lll.rx.ft;
279 max_octets = cis->lll.rx.max_pdu;
280
281 if (role) {
282 /* peripheral */
283 sdu_interval = cig->c_sdu_interval;
284 } else {
285 /* central */
286 sdu_interval = cig->p_sdu_interval;
287 }
288 } else {
289 /* path_dir == BT_HCI_DATAPATH_DIR_HOST_TO_CTLR */
290 burst_number = cis->lll.tx.bn;
291 flush_timeout = cis->lll.tx.ft;
292 max_octets = cis->lll.tx.max_pdu;
293
294 if (role) {
295 /* peripheral */
296 sdu_interval = cig->p_sdu_interval;
297 } else {
298 /* central */
299 sdu_interval = cig->c_sdu_interval;
300 }
301 }
302 #endif /* CONFIG_BT_CTLR_CONN_ISO */
303
304 #if defined(CONFIG_BT_CTLR_ADV_ISO)
305 } else if (IS_ADV_ISO_HANDLE(handle)) {
306 struct ll_adv_iso_set *adv_iso;
307 struct lll_adv_iso *lll_iso;
308 uint16_t stream_handle;
309
310 stream_handle = LL_BIS_ADV_IDX_FROM_HANDLE(handle);
311 adv_stream = ull_adv_iso_stream_get(stream_handle);
312 if (!adv_stream || adv_stream->dp) {
313 return BT_HCI_ERR_CMD_DISALLOWED;
314 }
315
316 adv_iso = ull_adv_iso_by_stream_get(stream_handle);
317 lll_iso = &adv_iso->lll;
318
319 role = ISOAL_ROLE_BROADCAST_SOURCE;
320 iso_interval = lll_iso->iso_interval;
321 sdu_interval = lll_iso->sdu_interval;
322 burst_number = lll_iso->bn;
323 flush_timeout = 0U; /* Not used for Broadcast ISO */
324 group_sync_delay = ull_iso_big_sync_delay(lll_iso->num_bis, lll_iso->bis_spacing,
325 lll_iso->nse, lll_iso->sub_interval,
326 lll_iso->phy, lll_iso->max_pdu,
327 lll_iso->enc);
328 stream_sync_delay = group_sync_delay - stream_handle * lll_iso->bis_spacing;
329 framed = lll_iso->framing;
330 max_octets = lll_iso->max_pdu;
331 #endif /* CONFIG_BT_CTLR_ADV_ISO */
332
333 #if defined(CONFIG_BT_CTLR_SYNC_ISO)
334 } else if (IS_SYNC_ISO_HANDLE(handle)) {
335 struct ll_sync_iso_set *sync_iso;
336 struct lll_sync_iso *lll_iso;
337 uint16_t stream_handle;
338
339 stream_handle = LL_BIS_SYNC_IDX_FROM_HANDLE(handle);
340 sync_stream = ull_sync_iso_stream_get(stream_handle);
341 if (!sync_stream || sync_stream->dp) {
342 return BT_HCI_ERR_CMD_DISALLOWED;
343 }
344
345 sync_iso = ull_sync_iso_by_stream_get(stream_handle);
346 lll_iso = &sync_iso->lll;
347
348 role = ISOAL_ROLE_BROADCAST_SINK;
349 iso_interval = lll_iso->iso_interval;
350 sdu_interval = lll_iso->sdu_interval;
351 burst_number = lll_iso->bn;
352
353 group_sync_delay = ull_iso_big_sync_delay(lll_iso->num_bis, lll_iso->bis_spacing,
354 lll_iso->nse, lll_iso->sub_interval,
355 lll_iso->phy, lll_iso->max_pdu,
356 lll_iso->enc);
357 stream_sync_delay = group_sync_delay - stream_handle * lll_iso->bis_spacing;
358 framed = lll_iso->framing;
359 max_octets = lll_iso->max_pdu;
360 flush_timeout = 0U; /* Not used for Broadcast ISO */
361 #endif /* CONFIG_BT_CTLR_SYNC_ISO */
362
363 } else {
364 return BT_HCI_ERR_UNKNOWN_CONN_ID;
365 }
366
367 if (path_is_vendor_specific(path_id) &&
368 (!IS_ENABLED(CONFIG_BT_CTLR_ISO_VENDOR_DATA_PATH) ||
369 !ll_data_path_configured(path_dir, path_id))) {
370 /* Data path must be configured prior to setup */
371 return BT_HCI_ERR_CMD_DISALLOWED;
372 }
373
374 /* If Codec_Configuration_Length non-zero and Codec_ID set to
375 * transparent air mode, the Controller shall return the error code
376 * Invalid HCI Command Parameters (0x12).
377 */
378 if (codec_config_len &&
379 (vs_codec_id == BT_HCI_CODING_FORMAT_TRANSPARENT)) {
380 return BT_HCI_ERR_INVALID_PARAM;
381 }
382
383 /* Allocate and configure datapath */
384 dp = mem_acquire(&datapath_free);
385 if (!dp) {
386 return BT_HCI_ERR_CMD_DISALLOWED;
387 }
388
389 dp->path_dir = path_dir;
390 dp->path_id = path_id;
391 dp->coding_format = coding_format;
392 dp->company_id = company_id;
393
394 /* TODO dp->sync_delay = controller_delay; ?*/
395
396 if (false) {
397
398 #if defined(CONFIG_BT_CTLR_SYNC_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
399 } else if ((path_dir == BT_HCI_DATAPATH_DIR_CTLR_TO_HOST) &&
400 (cis || sync_stream)) {
401 isoal_sink_handle_t sink_handle;
402 isoal_status_t err;
403
404 if (path_id == BT_HCI_DATAPATH_ID_HCI) {
405 /* Not vendor specific, thus alloc and emit functions
406 * known
407 */
408 err = isoal_sink_create(handle, role, framed,
409 burst_number, flush_timeout,
410 sdu_interval, iso_interval,
411 stream_sync_delay,
412 group_sync_delay,
413 sink_sdu_alloc_hci,
414 sink_sdu_emit_hci,
415 sink_sdu_write_hci,
416 &sink_handle);
417 } else {
418 /* Set up vendor specific data path */
419 isoal_sink_sdu_alloc_cb sdu_alloc;
420 isoal_sink_sdu_emit_cb sdu_emit;
421 isoal_sink_sdu_write_cb sdu_write;
422
423 /* Request vendor sink callbacks for path */
424 if (IS_ENABLED(CONFIG_BT_CTLR_ISO_VENDOR_DATA_PATH) &&
425 ll_data_path_sink_create(handle, dp, &sdu_alloc,
426 &sdu_emit, &sdu_write)) {
427 err = isoal_sink_create(handle, role, framed,
428 burst_number,
429 flush_timeout,
430 sdu_interval,
431 iso_interval,
432 stream_sync_delay,
433 group_sync_delay,
434 sdu_alloc, sdu_emit,
435 sdu_write,
436 &sink_handle);
437 } else {
438 ull_iso_datapath_release(dp);
439
440 return BT_HCI_ERR_CMD_DISALLOWED;
441 }
442 }
443
444 if (!err) {
445 if (cis) {
446 cis->hdr.datapath_out = dp;
447 }
448
449 if (sync_stream) {
450 sync_stream->dp = dp;
451 }
452
453 dp->sink_hdl = sink_handle;
454 isoal_sink_enable(sink_handle);
455 } else {
456 ull_iso_datapath_release(dp);
457
458 return BT_HCI_ERR_CMD_DISALLOWED;
459 }
460 #else /* !CONFIG_BT_CTLR_SYNC_ISO && !CONFIG_BT_CTLR_CONN_ISO */
461 ARG_UNUSED(sync_stream);
462 #endif /* !CONFIG_BT_CTLR_SYNC_ISO && !CONFIG_BT_CTLR_CONN_ISO */
463
464 #if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
465 } else if ((path_dir == BT_HCI_DATAPATH_DIR_HOST_TO_CTLR) &&
466 (cis || adv_stream)) {
467 isoal_source_handle_t source_handle;
468 isoal_status_t err;
469
470 /* Create source for TX data path */
471 isoal_source_pdu_alloc_cb pdu_alloc;
472 isoal_source_pdu_write_cb pdu_write;
473 isoal_source_pdu_emit_cb pdu_emit;
474 isoal_source_pdu_release_cb pdu_release;
475
476 if (path_is_vendor_specific(path_id)) {
477 if (!IS_ENABLED(CONFIG_BT_CTLR_ISO_VENDOR_DATA_PATH) ||
478 !ll_data_path_source_create(handle, dp,
479 &pdu_alloc, &pdu_write,
480 &pdu_emit,
481 &pdu_release)) {
482 ull_iso_datapath_release(dp);
483
484 return BT_HCI_ERR_CMD_DISALLOWED;
485 }
486 } else {
487 /* Set default callbacks when not vendor specific
488 * or that the vendor specific path is the same.
489 */
490 pdu_alloc = ll_iso_pdu_alloc;
491 pdu_write = ll_iso_pdu_write;
492 pdu_emit = ll_iso_pdu_emit;
493 pdu_release = ll_iso_pdu_release;
494 }
495
496 err = isoal_source_create(handle, role, framed, burst_number,
497 flush_timeout, max_octets,
498 sdu_interval, iso_interval,
499 stream_sync_delay, group_sync_delay,
500 pdu_alloc, pdu_write, pdu_emit,
501 pdu_release, &source_handle);
502
503 if (!err) {
504 if (IS_ENABLED(CONFIG_BT_CTLR_CONN_ISO) && cis != NULL) {
505 cis->hdr.datapath_in = dp;
506 }
507
508 if (IS_ENABLED(CONFIG_BT_CTLR_ADV_ISO) && adv_stream != NULL) {
509 adv_stream->dp = dp;
510 }
511
512 dp->source_hdl = source_handle;
513 isoal_source_enable(source_handle);
514 } else {
515 ull_iso_datapath_release(dp);
516
517 return BT_HCI_ERR_CMD_DISALLOWED;
518 }
519
520 #else /* !CONFIG_BT_CTLR_ADV_ISO && !CONFIG_BT_CTLR_CONN_ISO */
521 ARG_UNUSED(adv_stream);
522 #endif /* !CONFIG_BT_CTLR_ADV_ISO && !CONFIG_BT_CTLR_CONN_ISO */
523
524 } else {
525 return BT_HCI_ERR_CMD_DISALLOWED;
526 }
527
528 return BT_HCI_ERR_SUCCESS;
529 }
530
ll_remove_iso_path(uint16_t handle,uint8_t path_dir)531 uint8_t ll_remove_iso_path(uint16_t handle, uint8_t path_dir)
532 {
533 /* If the Host issues this command with a Connection_Handle that does
534 * not exist or is not for a CIS or a BIS, the Controller shall return
535 * the error code Unknown Connection Identifier (0x02).
536 */
537 if (false) {
538
539 #if defined(CONFIG_BT_CTLR_CONN_ISO)
540 } else if (IS_CIS_HANDLE(handle)) {
541 struct ll_conn_iso_stream *cis;
542 struct ll_iso_stream_hdr *hdr;
543 struct ll_iso_datapath *dp;
544
545 cis = ll_conn_iso_stream_get(handle);
546 hdr = &cis->hdr;
547
548 if (path_dir & BIT(BT_HCI_DATAPATH_DIR_HOST_TO_CTLR)) {
549 dp = hdr->datapath_in;
550 if (dp) {
551 isoal_source_destroy(dp->source_hdl);
552
553 hdr->datapath_in = NULL;
554 ull_iso_datapath_release(dp);
555 } else {
556 /* Datapath was not previously set up */
557 return BT_HCI_ERR_CMD_DISALLOWED;
558 }
559 }
560
561 if (path_dir & BIT(BT_HCI_DATAPATH_DIR_CTLR_TO_HOST)) {
562 dp = hdr->datapath_out;
563 if (dp) {
564 isoal_sink_destroy(dp->sink_hdl);
565
566 hdr->datapath_out = NULL;
567 ull_iso_datapath_release(dp);
568 } else {
569 /* Datapath was not previously set up */
570 return BT_HCI_ERR_CMD_DISALLOWED;
571 }
572 }
573 #endif /* CONFIG_BT_CTLR_CONN_ISO */
574
575 #if defined(CONFIG_BT_CTLR_ADV_ISO)
576 } else if (IS_ADV_ISO_HANDLE(handle)) {
577 struct lll_adv_iso_stream *adv_stream;
578 struct ll_iso_datapath *dp;
579 uint16_t stream_handle;
580
581 if (!(path_dir & BIT(BT_HCI_DATAPATH_DIR_HOST_TO_CTLR))) {
582 return BT_HCI_ERR_CMD_DISALLOWED;
583 }
584
585 stream_handle = LL_BIS_ADV_IDX_FROM_HANDLE(handle);
586 adv_stream = ull_adv_iso_stream_get(stream_handle);
587 if (!adv_stream) {
588 return BT_HCI_ERR_CMD_DISALLOWED;
589 }
590
591 dp = adv_stream->dp;
592 if (dp) {
593 adv_stream->dp = NULL;
594 isoal_source_destroy(dp->source_hdl);
595 ull_iso_datapath_release(dp);
596 } else {
597 /* Datapath was not previously set up */
598 return BT_HCI_ERR_CMD_DISALLOWED;
599 }
600 #endif /* CONFIG_BT_CTLR_ADV_ISO */
601
602 #if defined(CONFIG_BT_CTLR_SYNC_ISO)
603 } else if (IS_SYNC_ISO_HANDLE(handle)) {
604 struct lll_sync_iso_stream *sync_stream;
605 struct ll_iso_datapath *dp;
606 uint16_t stream_handle;
607
608 if (!(path_dir & BIT(BT_HCI_DATAPATH_DIR_CTLR_TO_HOST))) {
609 return BT_HCI_ERR_CMD_DISALLOWED;
610 }
611
612 stream_handle = LL_BIS_SYNC_IDX_FROM_HANDLE(handle);
613 sync_stream = ull_sync_iso_stream_get(stream_handle);
614 if (!sync_stream) {
615 return BT_HCI_ERR_CMD_DISALLOWED;
616 }
617
618 dp = sync_stream->dp;
619 if (dp) {
620 sync_stream->dp = NULL;
621 isoal_sink_destroy(dp->sink_hdl);
622 ull_iso_datapath_release(dp);
623 } else {
624 /* Datapath was not previously set up */
625 return BT_HCI_ERR_CMD_DISALLOWED;
626 }
627 #endif /* CONFIG_BT_CTLR_SYNC_ISO */
628
629 } else {
630 return BT_HCI_ERR_CMD_DISALLOWED;
631 }
632
633 return 0;
634 }
635
636 #if defined(CONFIG_BT_CTLR_SYNC_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
637 /* The sdu_alloc function is called before combining PDUs into an SDU. Here we
638 * store the paylaod number associated with the first PDU, for unframed usecase.
639 */
ll_iso_test_sdu_alloc(const struct isoal_sink * sink_ctx,const struct isoal_pdu_rx * valid_pdu,struct isoal_sdu_buffer * sdu_buffer)640 static isoal_status_t ll_iso_test_sdu_alloc(const struct isoal_sink *sink_ctx,
641 const struct isoal_pdu_rx *valid_pdu,
642 struct isoal_sdu_buffer *sdu_buffer)
643 {
644 uint16_t handle;
645
646 handle = sink_ctx->session.handle;
647
648 if (IS_CIS_HANDLE(handle)) {
649 if (!sink_ctx->session.framed) {
650 struct ll_conn_iso_stream *cis;
651
652 cis = ll_iso_stream_connected_get(sink_ctx->session.handle);
653 LL_ASSERT(cis);
654
655 /* For unframed, SDU counter is the payload number */
656 cis->hdr.test_mode.rx.sdu_counter =
657 (uint32_t)valid_pdu->meta->payload_number;
658 }
659 } else if (IS_SYNC_ISO_HANDLE(handle)) {
660 if (!sink_ctx->session.framed) {
661 struct lll_sync_iso_stream *sync_stream;
662 uint16_t stream_handle;
663
664 stream_handle = LL_BIS_SYNC_IDX_FROM_HANDLE(handle);
665 sync_stream = ull_sync_iso_stream_get(stream_handle);
666 LL_ASSERT(sync_stream);
667
668 sync_stream->test_mode->sdu_counter =
669 (uint32_t)valid_pdu->meta->payload_number;
670 }
671 }
672
673 return sink_sdu_alloc_hci(sink_ctx, valid_pdu, sdu_buffer);
674 }
675
676 /* The sdu_emit function is called whenever an SDU is combined and ready to be sent
677 * further in the data path. This injected implementation performs statistics on
678 * the SDU and then discards it.
679 */
ll_iso_test_sdu_emit(const struct isoal_sink * sink_ctx,const struct isoal_emitted_sdu_frag * sdu_frag,const struct isoal_emitted_sdu * sdu)680 static isoal_status_t ll_iso_test_sdu_emit(const struct isoal_sink *sink_ctx,
681 const struct isoal_emitted_sdu_frag *sdu_frag,
682 const struct isoal_emitted_sdu *sdu)
683 {
684 struct ll_iso_rx_test_mode *test_mode_rx;
685 isoal_sdu_len_t length;
686 isoal_status_t status;
687 struct net_buf *buf;
688 uint32_t sdu_counter;
689 uint16_t max_sdu;
690 uint16_t handle;
691 uint8_t framed;
692
693 handle = sink_ctx->session.handle;
694 buf = (struct net_buf *)sdu_frag->sdu.contents.dbuf;
695
696 if (IS_CIS_HANDLE(handle)) {
697 struct ll_conn_iso_stream *cis;
698
699 cis = ll_iso_stream_connected_get(sink_ctx->session.handle);
700 LL_ASSERT(cis);
701
702 test_mode_rx = &cis->hdr.test_mode.rx;
703 max_sdu = cis->c_max_sdu;
704 #if defined(CONFIG_BT_CTLR_SYNC_ISO)
705 } else if (IS_SYNC_ISO_HANDLE(handle)) {
706 struct lll_sync_iso_stream *sync_stream;
707 struct ll_sync_iso_set *sync_iso;
708 uint16_t stream_handle;
709
710 stream_handle = LL_BIS_SYNC_IDX_FROM_HANDLE(handle);
711 sync_stream = ull_sync_iso_stream_get(stream_handle);
712 LL_ASSERT(sync_stream);
713
714 sync_iso = ull_sync_iso_by_stream_get(stream_handle);
715
716 test_mode_rx = sync_stream->test_mode;
717 max_sdu = sync_iso->lll.max_sdu;
718 #endif /* CONFIG_BT_CTLR_SYNC_ISO */
719 } else {
720 /* Handle is out of range */
721 status = ISOAL_STATUS_ERR_SDU_EMIT;
722 net_buf_unref(buf);
723
724 return status;
725 }
726
727 length = sink_ctx->sdu_production.sdu_written;
728 framed = sink_ctx->session.framed;
729
730 /* In BT_HCI_ISO_TEST_ZERO_SIZE_SDU mode, all SDUs must have length 0 and there is
731 * no sdu_counter field. In the other modes, the first 4 bytes must contain a
732 * packet counter, which is used as SDU counter. The sdu_counter is extracted
733 * regardless of mode as a sanity check, unless the length does not allow it.
734 */
735 if (length >= ISO_TEST_PACKET_COUNTER_SIZE) {
736 sdu_counter = sys_get_le32(buf->data);
737 } else {
738 sdu_counter = 0U;
739 }
740
741 switch (sdu_frag->sdu.status) {
742 case ISOAL_SDU_STATUS_VALID:
743 if (framed && test_mode_rx->sdu_counter == 0U) {
744 /* BT 5.3, Vol 6, Part B, section 7.2:
745 * When using framed PDUs the expected value of the SDU counter
746 * shall be initialized with the value of the SDU counter of the
747 * first valid received SDU.
748 */
749 test_mode_rx->sdu_counter = sdu_counter;
750 }
751
752 switch (test_mode_rx->payload_type) {
753 case BT_HCI_ISO_TEST_ZERO_SIZE_SDU:
754 if (length == 0) {
755 test_mode_rx->received_cnt++;
756 } else {
757 test_mode_rx->failed_cnt++;
758 }
759 break;
760
761 case BT_HCI_ISO_TEST_VARIABLE_SIZE_SDU:
762 if ((length >= ISO_TEST_PACKET_COUNTER_SIZE) &&
763 (length <= max_sdu) &&
764 (sdu_counter == test_mode_rx->sdu_counter)) {
765 test_mode_rx->received_cnt++;
766 } else {
767 test_mode_rx->failed_cnt++;
768 }
769 break;
770
771 case BT_HCI_ISO_TEST_MAX_SIZE_SDU:
772 if ((length == max_sdu) &&
773 (sdu_counter == test_mode_rx->sdu_counter)) {
774 test_mode_rx->received_cnt++;
775 } else {
776 test_mode_rx->failed_cnt++;
777 }
778 break;
779
780 default:
781 LL_ASSERT(0);
782 return ISOAL_STATUS_ERR_SDU_EMIT;
783 }
784 break;
785
786 case ISOAL_SDU_STATUS_ERRORS:
787 case ISOAL_SDU_STATUS_LOST_DATA:
788 test_mode_rx->missed_cnt++;
789 break;
790 }
791
792 /* In framed mode, we may start incrementing the SDU counter when rx_sdu_counter
793 * becomes non zero (initial state), or in case of zero-based counting, if zero
794 * is actually the first valid SDU counter received.
795 */
796 if (framed && (test_mode_rx->sdu_counter ||
797 (sdu_frag->sdu.status == ISOAL_SDU_STATUS_VALID))) {
798 test_mode_rx->sdu_counter++;
799 }
800
801 status = ISOAL_STATUS_OK;
802 net_buf_unref(buf);
803
804 return status;
805 }
806
ll_iso_receive_test(uint16_t handle,uint8_t payload_type)807 uint8_t ll_iso_receive_test(uint16_t handle, uint8_t payload_type)
808 {
809 struct ll_iso_rx_test_mode *test_mode_rx;
810 isoal_sink_handle_t sink_handle;
811 struct ll_iso_datapath *dp;
812 uint32_t sdu_interval;
813 isoal_status_t err;
814
815 struct ll_iso_datapath **stream_dp;
816
817 uint32_t stream_sync_delay;
818 uint32_t group_sync_delay;
819 #if defined(CONFIG_BT_CTLR_SYNC_ISO)
820 uint16_t stream_handle;
821 #endif /* CONFIG_BT_CTLR_SYNC_ISO */
822 uint16_t iso_interval;
823 uint8_t framed;
824 uint8_t role;
825 uint8_t ft;
826 uint8_t bn;
827
828 if (IS_CIS_HANDLE(handle)) {
829 struct ll_conn_iso_stream *cis;
830 struct ll_conn_iso_group *cig;
831
832 cis = ll_iso_stream_connected_get(handle);
833 if (!cis) {
834 /* CIS is not connected */
835 return BT_HCI_ERR_UNKNOWN_CONN_ID;
836 }
837
838 if (cis->lll.rx.bn == 0) {
839 /* CIS is not configured for RX */
840 return BT_HCI_ERR_UNSUPP_FEATURE_PARAM_VAL;
841 }
842
843 test_mode_rx = &cis->hdr.test_mode.rx;
844 stream_dp = &cis->hdr.datapath_out;
845 cig = cis->group;
846
847 if (cig->lll.role == BT_HCI_ROLE_PERIPHERAL) {
848 /* peripheral */
849 sdu_interval = cig->c_sdu_interval;
850 } else {
851 /* central */
852 sdu_interval = cig->p_sdu_interval;
853 }
854
855 role = cig->lll.role;
856 framed = cis->framed;
857 bn = cis->lll.rx.bn;
858 ft = cis->lll.rx.ft;
859 iso_interval = cig->iso_interval;
860 stream_sync_delay = cis->sync_delay;
861 group_sync_delay = cig->sync_delay;
862 #if defined(CONFIG_BT_CTLR_SYNC_ISO)
863 } else if (IS_SYNC_ISO_HANDLE(handle)) {
864 /* Get the sync stream from the handle */
865 struct lll_sync_iso_stream *sync_stream;
866 struct ll_sync_iso_set *sync_iso;
867 struct lll_sync_iso *lll_iso;
868
869 stream_handle = LL_BIS_SYNC_IDX_FROM_HANDLE(handle);
870 sync_stream = ull_sync_iso_stream_get(stream_handle);
871 if (!sync_stream) {
872 return BT_HCI_ERR_UNKNOWN_CONN_ID;
873 }
874
875 if (sync_stream->dp) {
876 /* Data path already set up */
877 return BT_HCI_ERR_CMD_DISALLOWED;
878 }
879
880 sync_iso = ull_sync_iso_by_stream_get(stream_handle);
881 lll_iso = &sync_iso->lll;
882
883 test_mode_rx = sync_stream->test_mode;
884 stream_dp = &sync_stream->dp;
885
886 /* BT Core v5.4 - Vol 6, Part B, Section 4.4.6.4:
887 * BIG_Sync_Delay = (Num_BIS – 1) × BIS_Spacing
888 * + (NSE – 1) × Sub_Interval + MPT.
889 */
890 group_sync_delay = ull_iso_big_sync_delay(lll_iso->num_bis, lll_iso->bis_spacing,
891 lll_iso->nse, lll_iso->sub_interval,
892 lll_iso->phy, lll_iso->max_pdu,
893 lll_iso->enc);
894 stream_sync_delay = group_sync_delay - stream_handle * lll_iso->bis_spacing;
895
896 role = ISOAL_ROLE_BROADCAST_SINK;
897 framed = lll_iso->framing;
898 bn = lll_iso->bn;
899 ft = 0;
900 sdu_interval = lll_iso->sdu_interval;
901 iso_interval = lll_iso->iso_interval;
902 #endif /* CONFIG_BT_CTLR_SYNC_ISO */
903 } else {
904 /* Handle is out of range */
905 return BT_HCI_ERR_UNKNOWN_CONN_ID;
906 }
907
908 if (*stream_dp) {
909 /* Data path already set up */
910 return BT_HCI_ERR_CMD_DISALLOWED;
911 }
912
913 if (payload_type > BT_HCI_ISO_TEST_MAX_SIZE_SDU) {
914 return BT_HCI_ERR_INVALID_LL_PARAM;
915 }
916
917 /* Allocate and configure test datapath */
918 dp = mem_acquire(&datapath_free);
919 if (!dp) {
920 return BT_HCI_ERR_CMD_DISALLOWED;
921 }
922
923 dp->path_dir = BT_HCI_DATAPATH_DIR_CTLR_TO_HOST;
924 dp->path_id = BT_HCI_DATAPATH_ID_HCI;
925
926 *stream_dp = dp;
927 memset(test_mode_rx, 0, sizeof(struct ll_iso_rx_test_mode));
928
929 err = isoal_sink_create(handle, role, framed, bn, ft,
930 sdu_interval, iso_interval,
931 stream_sync_delay, group_sync_delay,
932 ll_iso_test_sdu_alloc,
933 ll_iso_test_sdu_emit,
934 sink_sdu_write_hci, &sink_handle);
935 if (err) {
936 /* Error creating test source - cleanup source and
937 * datapath
938 */
939 isoal_sink_destroy(sink_handle);
940 ull_iso_datapath_release(dp);
941 *stream_dp = NULL;
942
943 return BT_HCI_ERR_CMD_DISALLOWED;
944 }
945
946 dp->sink_hdl = sink_handle;
947 isoal_sink_enable(sink_handle);
948
949 /* Enable Receive Test Mode */
950 test_mode_rx->enabled = 1;
951 test_mode_rx->payload_type = payload_type;
952
953 return BT_HCI_ERR_SUCCESS;
954 }
955
ll_iso_read_test_counters(uint16_t handle,uint32_t * received_cnt,uint32_t * missed_cnt,uint32_t * failed_cnt)956 uint8_t ll_iso_read_test_counters(uint16_t handle, uint32_t *received_cnt,
957 uint32_t *missed_cnt,
958 uint32_t *failed_cnt)
959 {
960 struct ll_iso_rx_test_mode *test_mode_rx;
961
962 *received_cnt = 0U;
963 *missed_cnt = 0U;
964 *failed_cnt = 0U;
965
966 if (IS_CIS_HANDLE(handle)) {
967 struct ll_conn_iso_stream *cis;
968
969 cis = ll_iso_stream_connected_get(handle);
970 if (!cis) {
971 /* CIS is not connected */
972 return BT_HCI_ERR_UNKNOWN_CONN_ID;
973 }
974
975 test_mode_rx = &cis->hdr.test_mode.rx;
976
977 } else if (IS_SYNC_ISO_HANDLE(handle)) {
978 /* Get the sync stream from the handle */
979 struct lll_sync_iso_stream *sync_stream;
980 uint16_t stream_handle;
981
982 stream_handle = LL_BIS_SYNC_IDX_FROM_HANDLE(handle);
983 sync_stream = ull_sync_iso_stream_get(stream_handle);
984 if (!sync_stream) {
985 return BT_HCI_ERR_UNKNOWN_CONN_ID;
986 }
987
988 test_mode_rx = sync_stream->test_mode;
989
990 } else {
991 /* Handle is out of range */
992 return BT_HCI_ERR_UNKNOWN_CONN_ID;
993 }
994
995 if (!test_mode_rx->enabled) {
996 /* ISO receive Test is not active */
997 return BT_HCI_ERR_UNSUPP_FEATURE_PARAM_VAL;
998 }
999
1000 /* Return SDU statistics */
1001 *received_cnt = test_mode_rx->received_cnt;
1002 *missed_cnt = test_mode_rx->missed_cnt;
1003 *failed_cnt = test_mode_rx->failed_cnt;
1004
1005 return BT_HCI_ERR_SUCCESS;
1006 }
1007
1008 #if defined(CONFIG_BT_CTLR_READ_ISO_LINK_QUALITY)
ll_read_iso_link_quality(uint16_t handle,uint32_t * tx_unacked_packets,uint32_t * tx_flushed_packets,uint32_t * tx_last_subevent_packets,uint32_t * retransmitted_packets,uint32_t * crc_error_packets,uint32_t * rx_unreceived_packets,uint32_t * duplicate_packets)1009 uint8_t ll_read_iso_link_quality(uint16_t handle,
1010 uint32_t *tx_unacked_packets,
1011 uint32_t *tx_flushed_packets,
1012 uint32_t *tx_last_subevent_packets,
1013 uint32_t *retransmitted_packets,
1014 uint32_t *crc_error_packets,
1015 uint32_t *rx_unreceived_packets,
1016 uint32_t *duplicate_packets)
1017 {
1018 uint8_t status;
1019
1020 *tx_unacked_packets = 0;
1021 *tx_flushed_packets = 0;
1022 *tx_last_subevent_packets = 0;
1023 *retransmitted_packets = 0;
1024 *crc_error_packets = 0;
1025 *rx_unreceived_packets = 0;
1026 *duplicate_packets = 0;
1027
1028 status = BT_HCI_ERR_SUCCESS;
1029
1030 if (IS_CIS_HANDLE(handle)) {
1031 struct ll_conn_iso_stream *cis;
1032
1033 cis = ll_iso_stream_connected_get(handle);
1034
1035 if (!cis) {
1036 /* CIS is not connected */
1037 return BT_HCI_ERR_UNKNOWN_CONN_ID;
1038 }
1039
1040 *tx_unacked_packets = cis->hdr.link_quality.tx_unacked_packets;
1041 *tx_flushed_packets = cis->hdr.link_quality.tx_flushed_packets;
1042 *tx_last_subevent_packets = cis->hdr.link_quality.tx_last_subevent_packets;
1043 *retransmitted_packets = cis->hdr.link_quality.retransmitted_packets;
1044 *crc_error_packets = cis->hdr.link_quality.crc_error_packets;
1045 *rx_unreceived_packets = cis->hdr.link_quality.rx_unreceived_packets;
1046 *duplicate_packets = cis->hdr.link_quality.duplicate_packets;
1047
1048 } else if (IS_SYNC_ISO_HANDLE(handle)) {
1049 /* FIXME: Implement for sync receiver */
1050 status = BT_HCI_ERR_CMD_DISALLOWED;
1051 } else {
1052 /* Handle is out of range */
1053 status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1054 }
1055
1056 return status;
1057 }
1058 #endif /* CONFIG_BT_CTLR_READ_ISO_LINK_QUALITY */
1059
1060 #endif /* CONFIG_BT_CTLR_SYNC_ISO || CONFIG_BT_CTLR_CONN_ISO */
1061
1062 #if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
ll_iso_test_pdu_release(struct node_tx_iso * node_tx,const uint16_t handle,const isoal_status_t status)1063 static isoal_status_t ll_iso_test_pdu_release(struct node_tx_iso *node_tx,
1064 const uint16_t handle,
1065 const isoal_status_t status)
1066 {
1067 /* Release back to memory pool */
1068 if (node_tx->link) {
1069 ll_iso_link_tx_release(node_tx->link);
1070 }
1071 ll_iso_tx_mem_release(node_tx);
1072
1073 return ISOAL_STATUS_OK;
1074 }
1075
1076 #if defined(CONFIG_BT_CTLR_CONN_ISO)
ll_iso_transmit_test_send_sdu(uint16_t handle,uint32_t ticks_at_expire)1077 void ll_iso_transmit_test_send_sdu(uint16_t handle, uint32_t ticks_at_expire)
1078 {
1079 isoal_source_handle_t source_handle;
1080 struct isoal_sdu_tx sdu;
1081 isoal_status_t err;
1082 uint8_t tx_buffer[ISO_TEST_TX_BUFFER_SIZE];
1083 uint64_t next_payload_number;
1084 uint16_t remaining_tx;
1085 uint32_t sdu_counter;
1086
1087 if (IS_CIS_HANDLE(handle)) {
1088 struct ll_conn_iso_stream *cis;
1089 struct ll_conn_iso_group *cig;
1090 uint32_t rand_max_sdu;
1091 uint8_t event_offset;
1092 uint8_t max_sdu;
1093 uint8_t rand_8;
1094
1095 cis = ll_iso_stream_connected_get(handle);
1096 LL_ASSERT(cis);
1097
1098 if (!cis->hdr.test_mode.tx.enabled) {
1099 /* Transmit Test Mode not enabled */
1100 return;
1101 }
1102
1103 cig = cis->group;
1104 source_handle = cis->hdr.datapath_in->source_hdl;
1105
1106 max_sdu = IS_PERIPHERAL(cig) ? cis->p_max_sdu : cis->c_max_sdu;
1107
1108 switch (cis->hdr.test_mode.tx.payload_type) {
1109 case BT_HCI_ISO_TEST_ZERO_SIZE_SDU:
1110 remaining_tx = 0;
1111 break;
1112
1113 case BT_HCI_ISO_TEST_VARIABLE_SIZE_SDU:
1114 /* Randomize the length [4..max_sdu] */
1115 lll_rand_get(&rand_8, sizeof(rand_8));
1116 rand_max_sdu = rand_8 * (max_sdu - ISO_TEST_PACKET_COUNTER_SIZE);
1117 remaining_tx = ISO_TEST_PACKET_COUNTER_SIZE + (rand_max_sdu >> 8);
1118 break;
1119
1120 case BT_HCI_ISO_TEST_MAX_SIZE_SDU:
1121 LL_ASSERT(max_sdu > ISO_TEST_PACKET_COUNTER_SIZE);
1122 remaining_tx = max_sdu;
1123 break;
1124
1125 default:
1126 LL_ASSERT(0);
1127 return;
1128 }
1129
1130 if (remaining_tx > ISO_TEST_TX_BUFFER_SIZE) {
1131 sdu.sdu_state = BT_ISO_START;
1132 } else {
1133 sdu.sdu_state = BT_ISO_SINGLE;
1134 }
1135
1136 /* Configure SDU similarly to one delivered via HCI */
1137 sdu.packet_sn = 0;
1138 sdu.dbuf = tx_buffer;
1139
1140 /* We must ensure sufficient time for ISO-AL to fragment SDU and
1141 * deliver PDUs to the TX queue. By checking ull_ref_get, we
1142 * know if we are within the subevents of an ISO event. If so,
1143 * we can assume that we have enough time to deliver in the next
1144 * ISO event. If we're not active within the ISO event, we don't
1145 * know if there is enough time to deliver in the next event,
1146 * and for safety we set the target to current event + 2.
1147 *
1148 * For FT > 1, we have the opportunity to retransmit in later
1149 * event(s), in which case we have the option to target an
1150 * earlier event (this or next) because being late does not
1151 * instantly flush the payload.
1152 */
1153 event_offset = ull_ref_get(&cig->ull) ? 1 : 2;
1154 if (cis->lll.tx.ft > 1) {
1155 /* FT > 1, target an earlier event */
1156 event_offset -= 1;
1157 }
1158
1159 sdu.grp_ref_point = isoal_get_wrapped_time_us(cig->cig_ref_point,
1160 (event_offset * cig->iso_interval *
1161 ISO_INT_UNIT_US));
1162 sdu.target_event = cis->lll.event_count + event_offset;
1163 sdu.iso_sdu_length = remaining_tx;
1164
1165 /* Send all SDU fragments */
1166 do {
1167 sdu.cntr_time_stamp = HAL_TICKER_TICKS_TO_US(ticks_at_expire);
1168 sdu.time_stamp = sdu.cntr_time_stamp;
1169 sdu.size = MIN(remaining_tx, ISO_TEST_TX_BUFFER_SIZE);
1170 memset(tx_buffer, 0, sdu.size);
1171
1172 /* If this is the first fragment of a framed SDU, inject the SDU
1173 * counter.
1174 */
1175 if ((sdu.size >= ISO_TEST_PACKET_COUNTER_SIZE) &&
1176 ((sdu.sdu_state == BT_ISO_START) || (sdu.sdu_state == BT_ISO_SINGLE))) {
1177 if (cis->framed) {
1178 sdu_counter = (uint32_t)cis->hdr.test_mode.tx.sdu_counter;
1179 } else {
1180 /* Unframed. Get the next payload counter.
1181 *
1182 * BT 5.3, Vol 6, Part B, Section 7.1:
1183 * When using unframed PDUs, the SDU counter shall be equal
1184 * to the payload counter.
1185 */
1186 isoal_tx_unframed_get_next_payload_number(source_handle,
1187 &sdu,
1188 &next_payload_number);
1189 sdu_counter = (uint32_t)next_payload_number;
1190 }
1191
1192 sys_put_le32(sdu_counter, tx_buffer);
1193 }
1194
1195 /* Send to ISOAL */
1196 err = isoal_tx_sdu_fragment(source_handle, &sdu);
1197 LL_ASSERT(!err);
1198
1199 remaining_tx -= sdu.size;
1200
1201 if (remaining_tx > ISO_TEST_TX_BUFFER_SIZE) {
1202 sdu.sdu_state = BT_ISO_CONT;
1203 } else {
1204 sdu.sdu_state = BT_ISO_END;
1205 }
1206 } while (remaining_tx);
1207
1208 cis->hdr.test_mode.tx.sdu_counter++;
1209
1210 } else if (IS_ADV_ISO_HANDLE(handle)) {
1211 /* FIXME: Implement for broadcaster */
1212 } else {
1213 LL_ASSERT(0);
1214 }
1215 }
1216 #endif /* CONFIG_BT_CTLR_CONN_ISO */
1217
ll_iso_transmit_test(uint16_t handle,uint8_t payload_type)1218 uint8_t ll_iso_transmit_test(uint16_t handle, uint8_t payload_type)
1219 {
1220 isoal_source_handle_t source_handle;
1221 struct ll_iso_datapath *dp;
1222 uint32_t sdu_interval;
1223 isoal_status_t err;
1224 uint8_t status;
1225
1226 status = BT_HCI_ERR_SUCCESS;
1227
1228 if (IS_CIS_HANDLE(handle)) {
1229 struct ll_conn_iso_stream *cis;
1230 struct ll_conn_iso_group *cig;
1231
1232 cis = ll_iso_stream_connected_get(handle);
1233 if (!cis) {
1234 /* CIS is not connected */
1235 return BT_HCI_ERR_UNKNOWN_CONN_ID;
1236 }
1237
1238 if (cis->lll.tx.bn == 0U) {
1239 /* CIS is not configured for TX */
1240 return BT_HCI_ERR_UNSUPP_FEATURE_PARAM_VAL;
1241 }
1242
1243 if (cis->hdr.datapath_in) {
1244 /* Data path already set up */
1245 return BT_HCI_ERR_CMD_DISALLOWED;
1246 }
1247
1248 if (payload_type > BT_HCI_ISO_TEST_MAX_SIZE_SDU) {
1249 return BT_HCI_ERR_INVALID_LL_PARAM;
1250 }
1251
1252 /* Allocate and configure test datapath */
1253 dp = mem_acquire(&datapath_free);
1254 if (!dp) {
1255 return BT_HCI_ERR_CMD_DISALLOWED;
1256 }
1257
1258 dp->path_dir = BT_HCI_DATAPATH_DIR_HOST_TO_CTLR;
1259 dp->path_id = BT_HCI_DATAPATH_ID_HCI;
1260
1261 cis->hdr.datapath_in = dp;
1262 cig = cis->group;
1263
1264 sdu_interval = IS_PERIPHERAL(cig) ? cig->p_sdu_interval : cig->c_sdu_interval;
1265
1266 /* Setup the test source */
1267 err = isoal_source_create(handle, cig->lll.role, cis->framed,
1268 cis->lll.tx.bn, cis->lll.tx.ft,
1269 cis->lll.tx.max_pdu, sdu_interval,
1270 cig->iso_interval, cis->sync_delay,
1271 cig->sync_delay, ll_iso_pdu_alloc,
1272 ll_iso_pdu_write, ll_iso_pdu_emit,
1273 ll_iso_test_pdu_release,
1274 &source_handle);
1275
1276 if (err) {
1277 /* Error creating test source - cleanup source and datapath */
1278 isoal_source_destroy(source_handle);
1279 ull_iso_datapath_release(dp);
1280 cis->hdr.datapath_in = NULL;
1281
1282 return BT_HCI_ERR_CMD_DISALLOWED;
1283 }
1284
1285 dp->source_hdl = source_handle;
1286 isoal_source_enable(source_handle);
1287
1288 /* Enable Transmit Test Mode */
1289 cis->hdr.test_mode.tx.enabled = 1;
1290 cis->hdr.test_mode.tx.payload_type = payload_type;
1291
1292 } else if (IS_ADV_ISO_HANDLE(handle)) {
1293 struct lll_adv_iso_stream *stream;
1294 uint16_t stream_handle;
1295
1296 stream_handle = LL_BIS_ADV_IDX_FROM_HANDLE(handle);
1297 stream = ull_adv_iso_stream_get(stream_handle);
1298 if (!stream) {
1299 return BT_HCI_ERR_UNKNOWN_CONN_ID;
1300 }
1301
1302 /* FIXME: Implement use of common header in stream to enable code sharing
1303 * between CIS and BIS for test commands (and other places).
1304 */
1305 status = BT_HCI_ERR_CMD_DISALLOWED;
1306 } else {
1307 /* Handle is out of range */
1308 status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1309 }
1310
1311 return status;
1312 }
1313 #endif /* CONFIG_BT_CTLR_ADV_ISO || CONFIG_BT_CTLR_CONN_ISO */
1314
ll_iso_test_end(uint16_t handle,uint32_t * received_cnt,uint32_t * missed_cnt,uint32_t * failed_cnt)1315 uint8_t ll_iso_test_end(uint16_t handle, uint32_t *received_cnt,
1316 uint32_t *missed_cnt, uint32_t *failed_cnt)
1317 {
1318 *received_cnt = 0U;
1319 *missed_cnt = 0U;
1320 *failed_cnt = 0U;
1321
1322 if (IS_CIS_HANDLE(handle)) {
1323 struct ll_conn_iso_stream *cis;
1324
1325 cis = ll_iso_stream_connected_get(handle);
1326 if (!cis) {
1327 /* CIS is not connected */
1328 return BT_HCI_ERR_UNKNOWN_CONN_ID;
1329 }
1330
1331 if (!cis->hdr.test_mode.rx.enabled && !cis->hdr.test_mode.tx.enabled) {
1332 /* Test Mode is not active */
1333 return BT_HCI_ERR_UNSUPP_FEATURE_PARAM_VAL;
1334 }
1335
1336 if (cis->hdr.test_mode.rx.enabled) {
1337 isoal_sink_destroy(cis->hdr.datapath_out->sink_hdl);
1338 ull_iso_datapath_release(cis->hdr.datapath_out);
1339 cis->hdr.datapath_out = NULL;
1340
1341 /* Return SDU statistics */
1342 *received_cnt = cis->hdr.test_mode.rx.received_cnt;
1343 *missed_cnt = cis->hdr.test_mode.rx.missed_cnt;
1344 *failed_cnt = cis->hdr.test_mode.rx.failed_cnt;
1345 }
1346
1347 if (cis->hdr.test_mode.tx.enabled) {
1348 /* Tear down source and datapath */
1349 isoal_source_destroy(cis->hdr.datapath_in->source_hdl);
1350 ull_iso_datapath_release(cis->hdr.datapath_in);
1351 cis->hdr.datapath_in = NULL;
1352 }
1353
1354 /* Disable Test Mode */
1355 (void)memset(&cis->hdr.test_mode, 0U, sizeof(cis->hdr.test_mode));
1356
1357 } else if (IS_ADV_ISO_HANDLE(handle)) {
1358 /* FIXME: Implement for broadcaster */
1359 return BT_HCI_ERR_CMD_DISALLOWED;
1360
1361 } else if (IS_SYNC_ISO_HANDLE(handle)) {
1362 struct lll_sync_iso_stream *sync_stream;
1363 uint16_t stream_handle;
1364
1365 stream_handle = LL_BIS_SYNC_IDX_FROM_HANDLE(handle);
1366 sync_stream = ull_sync_iso_stream_get(stream_handle);
1367 if (!sync_stream) {
1368 return BT_HCI_ERR_UNKNOWN_CONN_ID;
1369 }
1370
1371 if (!sync_stream->test_mode->enabled || !sync_stream->dp) {
1372 /* Test Mode is not active */
1373 return BT_HCI_ERR_UNSUPP_FEATURE_PARAM_VAL;
1374 }
1375
1376 isoal_sink_destroy(sync_stream->dp->sink_hdl);
1377 ull_iso_datapath_release(sync_stream->dp);
1378 sync_stream->dp = NULL;
1379
1380 /* Return SDU statistics */
1381 *received_cnt = sync_stream->test_mode->received_cnt;
1382 *missed_cnt = sync_stream->test_mode->missed_cnt;
1383 *failed_cnt = sync_stream->test_mode->failed_cnt;
1384
1385 (void)memset(&sync_stream->test_mode, 0U, sizeof(sync_stream->test_mode));
1386
1387 } else {
1388 /* Handle is out of range */
1389 return BT_HCI_ERR_UNKNOWN_CONN_ID;
1390 }
1391
1392 return BT_HCI_ERR_SUCCESS;
1393 }
1394
1395 #if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
ll_iso_tx_mem_acquire(void)1396 void *ll_iso_tx_mem_acquire(void)
1397 {
1398 return mem_acquire(&mem_iso_tx.free);
1399 }
1400
ll_iso_tx_mem_release(void * node_tx)1401 void ll_iso_tx_mem_release(void *node_tx)
1402 {
1403 mem_release(node_tx, &mem_iso_tx.free);
1404 }
1405
ll_iso_tx_mem_enqueue(uint16_t handle,void * node_tx,void * link)1406 int ll_iso_tx_mem_enqueue(uint16_t handle, void *node_tx, void *link)
1407 {
1408 if (IS_ENABLED(CONFIG_BT_CTLR_CONN_ISO) &&
1409 IS_CIS_HANDLE(handle)) {
1410 struct ll_conn_iso_stream *cis;
1411
1412 cis = ll_conn_iso_stream_get(handle);
1413 memq_enqueue(link, node_tx, &cis->lll.memq_tx.tail);
1414
1415 } else if (IS_ENABLED(CONFIG_BT_CTLR_ADV_ISO) &&
1416 IS_ADV_ISO_HANDLE(handle)) {
1417 struct lll_adv_iso_stream *stream;
1418 uint16_t stream_handle;
1419
1420 stream_handle = LL_BIS_ADV_IDX_FROM_HANDLE(handle);
1421 stream = ull_adv_iso_stream_get(stream_handle);
1422 memq_enqueue(link, node_tx, &stream->memq_tx.tail);
1423
1424 } else {
1425 return -EINVAL;
1426 }
1427
1428 return 0;
1429 }
1430 #endif /* CONFIG_BT_CTLR_ADV_ISO || CONFIG_BT_CTLR_CONN_ISO */
1431
ull_iso_init(void)1432 int ull_iso_init(void)
1433 {
1434 int err;
1435
1436 err = init_reset();
1437 if (err) {
1438 return err;
1439 }
1440
1441 return 0;
1442 }
1443
ull_iso_reset(void)1444 int ull_iso_reset(void)
1445 {
1446 int err;
1447
1448 err = init_reset();
1449 if (err) {
1450 return err;
1451 }
1452
1453 return 0;
1454 }
1455
1456 #if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
ull_iso_lll_ack_enqueue(uint16_t handle,struct node_tx_iso * node_tx)1457 void ull_iso_lll_ack_enqueue(uint16_t handle, struct node_tx_iso *node_tx)
1458 {
1459 struct ll_iso_datapath *dp = NULL;
1460
1461 if (IS_ENABLED(CONFIG_BT_CTLR_CONN_ISO) && IS_CIS_HANDLE(handle)) {
1462 struct ll_conn_iso_stream *cis;
1463
1464 cis = ll_conn_iso_stream_get(handle);
1465 dp = cis->hdr.datapath_in;
1466
1467 if (dp) {
1468 isoal_tx_pdu_release(dp->source_hdl, node_tx);
1469 } else {
1470 /* Race with Data Path remove */
1471 /* FIXME: ll_tx_ack_put is not LLL callable as it is
1472 * used by ACL connections in ULL context to dispatch
1473 * ack.
1474 */
1475 ll_tx_ack_put(handle, (void *)node_tx);
1476 ll_rx_sched();
1477 }
1478 } else if (IS_ENABLED(CONFIG_BT_CTLR_ADV_ISO) && IS_ADV_ISO_HANDLE(handle)) {
1479 /* Process as TX ack. TODO: Can be unified with CIS and use
1480 * ISOAL.
1481 */
1482 /* FIXME: ll_tx_ack_put is not LLL callable as it is
1483 * used by ACL connections in ULL context to dispatch
1484 * ack.
1485 */
1486 ll_tx_ack_put(handle, (void *)node_tx);
1487 ll_rx_sched();
1488 } else {
1489 LL_ASSERT(0);
1490 }
1491 }
1492
ull_iso_lll_event_prepare(uint16_t handle,uint64_t event_count)1493 void ull_iso_lll_event_prepare(uint16_t handle, uint64_t event_count)
1494 {
1495 if (IS_CIS_HANDLE(handle)) {
1496 struct ll_iso_datapath *dp = NULL;
1497 struct ll_conn_iso_stream *cis;
1498
1499 cis = ll_iso_stream_connected_get(handle);
1500
1501 if (cis) {
1502 dp = cis->hdr.datapath_in;
1503 }
1504
1505 if (dp) {
1506 isoal_tx_event_prepare(dp->source_hdl, event_count);
1507 }
1508 } else if (IS_ADV_ISO_HANDLE(handle)) {
1509 struct ll_iso_datapath *dp = NULL;
1510 struct lll_adv_iso_stream *stream;
1511 uint16_t stream_handle;
1512
1513 stream_handle = LL_BIS_ADV_IDX_FROM_HANDLE(handle);
1514 stream = ull_adv_iso_stream_get(stream_handle);
1515
1516 if (stream) {
1517 dp = stream->dp;
1518 }
1519
1520 if (dp) {
1521 isoal_tx_event_prepare(dp->source_hdl, event_count);
1522 }
1523 } else {
1524 LL_ASSERT(0);
1525 }
1526 }
1527 #endif /* CONFIG_BT_CTLR_ADV_ISO || CONFIG_BT_CTLR_CONN_ISO */
1528
1529 #if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_SYNC_ISO)
ull_iso_big_sync_delay(uint8_t num_bis,uint32_t bis_spacing,uint8_t nse,uint32_t sub_interval,uint8_t phy,uint8_t max_pdu,bool enc)1530 uint32_t ull_iso_big_sync_delay(uint8_t num_bis, uint32_t bis_spacing, uint8_t nse,
1531 uint32_t sub_interval, uint8_t phy, uint8_t max_pdu, bool enc)
1532 {
1533 /* BT Core v5.4 - Vol 6, Part B, Section 4.4.6.4:
1534 * BIG_Sync_Delay = (Num_BIS – 1) × BIS_Spacing + (NSE – 1) × Sub_Interval + MPT.
1535 */
1536 return (num_bis - 1) * bis_spacing + (nse - 1) * sub_interval +
1537 BYTES2US(PDU_OVERHEAD_SIZE(phy) + max_pdu + (enc ? 4 : 0), phy);
1538 }
1539 #endif /* CONFIG_BT_CTLR_ADV_ISO || CONFIG_BT_CTLR_SYNC_ISO */
1540
1541 #if defined(CONFIG_BT_CTLR_SYNC_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
ull_iso_pdu_rx_alloc_peek(uint8_t count)1542 void *ull_iso_pdu_rx_alloc_peek(uint8_t count)
1543 {
1544 if (count > MFIFO_AVAIL_COUNT_GET(iso_rx)) {
1545 return NULL;
1546 }
1547
1548 return MFIFO_DEQUEUE_PEEK(iso_rx);
1549 }
1550
ull_iso_pdu_rx_alloc(void)1551 void *ull_iso_pdu_rx_alloc(void)
1552 {
1553 return MFIFO_DEQUEUE(iso_rx);
1554 }
1555
1556 #if defined(CONFIG_BT_CTLR_ISO_VENDOR_DATA_PATH)
ull_iso_rx_put(memq_link_t * link,void * rx)1557 void ull_iso_rx_put(memq_link_t *link, void *rx)
1558 {
1559 /* Enqueue the Rx object */
1560 memq_enqueue(link, rx, &memq_ull_iso_rx.tail);
1561 }
1562
ull_iso_rx_sched(void)1563 void ull_iso_rx_sched(void)
1564 {
1565 static memq_link_t link;
1566 static struct mayfly mfy = {0, 0, &link, NULL, iso_rx_demux};
1567
1568 /* Kick the ULL (using the mayfly, tailchain it) */
1569 mayfly_enqueue(TICKER_USER_ID_LLL, TICKER_USER_ID_ULL_HIGH, 1, &mfy);
1570 }
1571
1572 #if defined(CONFIG_BT_CTLR_CONN_ISO)
iso_rx_cig_ref_point_update(struct ll_conn_iso_group * cig,const struct ll_conn_iso_stream * cis,const struct node_rx_iso_meta * meta)1573 static void iso_rx_cig_ref_point_update(struct ll_conn_iso_group *cig,
1574 const struct ll_conn_iso_stream *cis,
1575 const struct node_rx_iso_meta *meta)
1576 {
1577 uint32_t cig_sync_delay;
1578 uint32_t cis_sync_delay;
1579 uint64_t event_count;
1580 uint8_t burst_number;
1581 uint8_t role;
1582
1583 role = cig->lll.role;
1584 cig_sync_delay = cig->sync_delay;
1585 cis_sync_delay = cis->sync_delay;
1586 burst_number = cis->lll.rx.bn;
1587 event_count = cis->lll.event_count;
1588
1589 if (role) {
1590 /* Peripheral */
1591
1592 /* Check if this is the first payload received for this cis in
1593 * this event
1594 */
1595 if (meta->payload_number == (burst_number * event_count)) {
1596 /* Update the CIG reference point based on the CIS
1597 * anchor point
1598 */
1599 cig->cig_ref_point = isoal_get_wrapped_time_us(meta->timestamp,
1600 cis_sync_delay - cig_sync_delay);
1601 }
1602 }
1603 }
1604 #endif /* CONFIG_BT_CTLR_CONN_ISO */
1605
iso_rx_demux(void * param)1606 static void iso_rx_demux(void *param)
1607 {
1608 #if defined(CONFIG_BT_CTLR_CONN_ISO) || \
1609 defined(CONFIG_BT_CTLR_SYNC_ISO)
1610 struct ll_iso_datapath *dp;
1611 #endif /* CONFIG_BT_CTLR_CONN_ISO || CONFIG_BT_CTLR_SYNC_ISO */
1612 struct node_rx_pdu *rx_pdu;
1613 struct node_rx_hdr *rx;
1614 memq_link_t *link;
1615 uint16_t handle;
1616
1617 do {
1618 link = memq_peek(memq_ull_iso_rx.head, memq_ull_iso_rx.tail,
1619 (void **)&rx);
1620 if (link) {
1621 /* Demux Rx objects */
1622 switch (rx->type) {
1623 case NODE_RX_TYPE_RELEASE:
1624 (void)memq_dequeue(memq_ull_iso_rx.tail,
1625 &memq_ull_iso_rx.head, NULL);
1626 ll_iso_rx_put(link, rx);
1627 ll_rx_sched();
1628 break;
1629
1630 case NODE_RX_TYPE_ISO_PDU:
1631 /* Remove from receive-queue; ULL has received this now */
1632 (void)memq_dequeue(memq_ull_iso_rx.tail, &memq_ull_iso_rx.head,
1633 NULL);
1634
1635 rx_pdu = (struct node_rx_pdu *)rx;
1636 handle = rx_pdu->hdr.handle;
1637 dp = NULL;
1638
1639 if (false) {
1640 #if defined(CONFIG_BT_CTLR_CONN_ISO)
1641 } else if (IS_CIS_HANDLE(handle)) {
1642 struct ll_conn_iso_stream *cis;
1643 struct ll_conn_iso_group *cig;
1644
1645 cis = ll_conn_iso_stream_get(handle);
1646 cig = cis->group;
1647 dp = cis->hdr.datapath_out;
1648
1649 iso_rx_cig_ref_point_update(cig, cis,
1650 &rx_pdu->hdr.rx_iso_meta);
1651 #endif /* CONFIG_BT_CTLR_CONN_ISO */
1652 #if defined(CONFIG_BT_CTLR_SYNC_ISO)
1653 } else if (IS_SYNC_ISO_HANDLE(handle)) {
1654 struct lll_sync_iso_stream *sync_stream;
1655 uint16_t stream_handle;
1656
1657 stream_handle = LL_BIS_SYNC_IDX_FROM_HANDLE(handle);
1658 sync_stream = ull_sync_iso_stream_get(stream_handle);
1659 dp = sync_stream ? sync_stream->dp : NULL;
1660 #endif /* CONFIG_BT_CTLR_SYNC_ISO */
1661 }
1662
1663 #if defined(CONFIG_BT_CTLR_CONN_ISO) || defined(CONFIG_BT_CTLR_SYNC_ISO)
1664 if (dp && dp->path_id != BT_HCI_DATAPATH_ID_HCI) {
1665 /* If vendor specific datapath pass to ISO AL here,
1666 * in case of HCI destination it will be passed in
1667 * HCI context.
1668 */
1669 struct isoal_pdu_rx pckt_meta = {
1670 .meta = &rx_pdu->rx_iso_meta,
1671 .pdu = (struct pdu_iso *)&rx_pdu->pdu[0]
1672 };
1673
1674 /* Pass the ISO PDU through ISO-AL */
1675 const isoal_status_t err =
1676 isoal_rx_pdu_recombine(dp->sink_hdl, &pckt_meta);
1677
1678 LL_ASSERT(err == ISOAL_STATUS_OK); /* TODO handle err */
1679 }
1680 #endif /* CONFIG_BT_CTLR_CONN_ISO || CONFIG_BT_CTLR_SYNC_ISO */
1681
1682 /* Let ISO PDU start its long journey upwards */
1683 ll_iso_rx_put(link, rx);
1684 ll_rx_sched();
1685 break;
1686
1687 default:
1688 LL_ASSERT(0);
1689 break;
1690 }
1691 }
1692 } while (link);
1693 }
1694 #endif /* CONFIG_BT_CTLR_ISO_VENDOR_DATA_PATH */
1695
ll_iso_rx_put(memq_link_t * link,void * rx)1696 void ll_iso_rx_put(memq_link_t *link, void *rx)
1697 {
1698 /* Enqueue the Rx object */
1699 memq_enqueue(link, rx, &memq_ll_iso_rx.tail);
1700 }
1701
ll_iso_rx_get(void)1702 void *ll_iso_rx_get(void)
1703 {
1704 struct node_rx_hdr *rx;
1705 memq_link_t *link;
1706
1707 link = memq_peek(memq_ll_iso_rx.head, memq_ll_iso_rx.tail, (void **)&rx);
1708 while (link) {
1709 /* Do not send up buffers to Host thread that are
1710 * marked for release
1711 */
1712 if (rx->type == NODE_RX_TYPE_RELEASE) {
1713 (void)memq_dequeue(memq_ll_iso_rx.tail,
1714 &memq_ll_iso_rx.head, NULL);
1715 mem_release(link, &mem_link_iso_rx.free);
1716 mem_release(rx, &mem_pool_iso_rx.free);
1717 RXFIFO_ALLOC(iso_rx, 1);
1718
1719 link = memq_peek(memq_ll_iso_rx.head, memq_ll_iso_rx.tail, (void **)&rx);
1720 continue;
1721 }
1722 return rx;
1723 }
1724
1725 return NULL;
1726 }
1727
ll_iso_rx_dequeue(void)1728 void ll_iso_rx_dequeue(void)
1729 {
1730 struct node_rx_hdr *rx = NULL;
1731 memq_link_t *link;
1732
1733 link = memq_dequeue(memq_ll_iso_rx.tail, &memq_ll_iso_rx.head,
1734 (void **)&rx);
1735 LL_ASSERT(link);
1736
1737 mem_release(link, &mem_link_iso_rx.free);
1738
1739 /* Handle object specific clean up */
1740 switch (rx->type) {
1741 case NODE_RX_TYPE_ISO_PDU:
1742 break;
1743 default:
1744 LL_ASSERT(0);
1745 break;
1746 }
1747 }
1748
ll_iso_rx_mem_release(void ** node_rx)1749 void ll_iso_rx_mem_release(void **node_rx)
1750 {
1751 struct node_rx_hdr *rx;
1752
1753 rx = *node_rx;
1754 while (rx) {
1755 struct node_rx_hdr *rx_free;
1756
1757 rx_free = rx;
1758 rx = rx->next;
1759
1760 switch (rx_free->type) {
1761 case NODE_RX_TYPE_ISO_PDU:
1762 mem_release(rx_free, &mem_pool_iso_rx.free);
1763 break;
1764 default:
1765 /* Ignore other types as node may have been initialized due to
1766 * race with HCI reset.
1767 */
1768 break;
1769 }
1770 }
1771
1772 *node_rx = rx;
1773
1774 RXFIFO_ALLOC(iso_rx, UINT8_MAX);
1775 }
1776 #endif /* CONFIG_BT_CTLR_SYNC_ISO) || CONFIG_BT_CTLR_CONN_ISO */
1777
ull_iso_datapath_alloc(void)1778 struct ll_iso_datapath *ull_iso_datapath_alloc(void)
1779 {
1780 return mem_acquire(&datapath_free);
1781 }
1782
ull_iso_datapath_release(struct ll_iso_datapath * dp)1783 void ull_iso_datapath_release(struct ll_iso_datapath *dp)
1784 {
1785 mem_release(dp, &datapath_free);
1786 }
1787
1788 #if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
ll_iso_link_tx_release(void * link)1789 void ll_iso_link_tx_release(void *link)
1790 {
1791 mem_release(link, &mem_link_iso_tx.free);
1792 }
1793
1794 /**
1795 * Allocate a PDU from the LL and store the details in the given buffer. Allocation
1796 * is not expected to fail as there must always be sufficient PDU buffers. Any
1797 * failure will trigger the assert.
1798 * @param[in] pdu_buffer Buffer to store PDU details in
1799 * @return Error status of operation
1800 */
ll_iso_pdu_alloc(struct isoal_pdu_buffer * pdu_buffer)1801 static isoal_status_t ll_iso_pdu_alloc(struct isoal_pdu_buffer *pdu_buffer)
1802 {
1803 struct node_tx_iso *node_tx;
1804
1805 node_tx = ll_iso_tx_mem_acquire();
1806 if (!node_tx) {
1807 LOG_ERR("Tx Buffer Overflow");
1808 /* TODO: Report overflow to HCI and remove assert
1809 * data_buf_overflow(evt, BT_OVERFLOW_LINK_ISO)
1810 */
1811 LL_ASSERT(0);
1812 return ISOAL_STATUS_ERR_PDU_ALLOC;
1813 }
1814
1815 node_tx->link = NULL;
1816
1817 /* node_tx handle will be required to emit the PDU later */
1818 pdu_buffer->handle = (void *)node_tx;
1819 pdu_buffer->pdu = (void *)node_tx->pdu;
1820
1821 /* Use TX buffer size as the limit here. Actual size will be decided in
1822 * the ISOAL based on the minimum of the buffer size and the respective
1823 * Max_PDU_C_To_P or Max_PDU_P_To_C.
1824 */
1825 pdu_buffer->size = MAX(LL_BIS_OCTETS_TX_MAX, LL_CIS_OCTETS_TX_MAX);
1826
1827 return ISOAL_STATUS_OK;
1828 }
1829
1830 /**
1831 * Write the given SDU payload to the target PDU buffer at the given offset.
1832 * @param[in,out] pdu_buffer Target PDU buffer
1833 * @param[in] pdu_offset Offset / current write position within PDU
1834 * @param[in] sdu_payload Location of source data
1835 * @param[in] consume_len Length of data to copy
1836 * @return Error status of write operation
1837 */
ll_iso_pdu_write(struct isoal_pdu_buffer * pdu_buffer,const size_t pdu_offset,const uint8_t * sdu_payload,const size_t consume_len)1838 static isoal_status_t ll_iso_pdu_write(struct isoal_pdu_buffer *pdu_buffer,
1839 const size_t pdu_offset,
1840 const uint8_t *sdu_payload,
1841 const size_t consume_len)
1842 {
1843 ARG_UNUSED(pdu_offset);
1844 ARG_UNUSED(consume_len);
1845
1846 LL_ASSERT(pdu_buffer);
1847 LL_ASSERT(pdu_buffer->pdu);
1848 LL_ASSERT(sdu_payload);
1849
1850 if ((pdu_offset + consume_len) > pdu_buffer->size) {
1851 /* Exceeded PDU buffer */
1852 return ISOAL_STATUS_ERR_UNSPECIFIED;
1853 }
1854
1855 /* Copy source to destination at given offset */
1856 memcpy(&pdu_buffer->pdu->payload[pdu_offset], sdu_payload, consume_len);
1857
1858 return ISOAL_STATUS_OK;
1859 }
1860
1861 /**
1862 * Emit the encoded node to the transmission queue
1863 * @param node_tx TX node to enqueue
1864 * @param handle CIS/BIS handle
1865 * @return Error status of enqueue operation
1866 */
ll_iso_pdu_emit(struct node_tx_iso * node_tx,const uint16_t handle)1867 static isoal_status_t ll_iso_pdu_emit(struct node_tx_iso *node_tx,
1868 const uint16_t handle)
1869 {
1870 memq_link_t *link;
1871
1872 link = mem_acquire(&mem_link_iso_tx.free);
1873 LL_ASSERT(link);
1874
1875 if (ll_iso_tx_mem_enqueue(handle, node_tx, link)) {
1876 return ISOAL_STATUS_ERR_PDU_EMIT;
1877 }
1878
1879 return ISOAL_STATUS_OK;
1880 }
1881
1882 #if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
1883 /**
1884 * Release the given payload back to the memory pool.
1885 * @param node_tx TX node to release or forward
1886 * @param handle CIS/BIS handle
1887 * @param status Reason for release
1888 * @return Error status of release operation
1889 */
ll_iso_pdu_release(struct node_tx_iso * node_tx,const uint16_t handle,const isoal_status_t status)1890 static isoal_status_t ll_iso_pdu_release(struct node_tx_iso *node_tx,
1891 const uint16_t handle,
1892 const isoal_status_t status)
1893 {
1894 if (status == ISOAL_STATUS_OK) {
1895 /* Process as TX ack, we are in LLL execution context here.
1896 * status == ISOAL_STATUS_OK when an ISO PDU has been acked.
1897 *
1898 * Call Path:
1899 * ull_iso_lll_ack_enqueue() --> isoal_tx_pdu_release() -->
1900 * pdu_release() == ll_iso_pdu_release() (this function).
1901 */
1902 /* FIXME: ll_tx_ack_put is not LLL callable as it is used by
1903 * ACL connections in ULL context to dispatch ack.
1904 */
1905 ll_tx_ack_put(handle, (void *)node_tx);
1906 ll_rx_sched();
1907 } else {
1908 /* Release back to memory pool, we are in Thread context
1909 * Callers:
1910 * isoal_source_deallocate() with ISOAL_STATUS_ERR_PDU_EMIT
1911 * isoal_tx_pdu_emit with status != ISOAL_STATUS_OK
1912 */
1913 if (node_tx->link) {
1914 ll_iso_link_tx_release(node_tx->link);
1915 }
1916 ll_iso_tx_mem_release(node_tx);
1917 }
1918
1919 return ISOAL_STATUS_OK;
1920 }
1921 #endif /* CONFIG_BT_CTLR_CONN_ISO */
1922 #endif /* CONFIG_BT_CTLR_ADV_ISO || CONFIG_BT_CTLR_CONN_ISO */
1923
init_reset(void)1924 static int init_reset(void)
1925 {
1926 #if defined(CONFIG_BT_CTLR_SYNC_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
1927 memq_link_t *link;
1928
1929 RXFIFO_INIT(iso_rx);
1930
1931 /* Acquire a link to initialize ull rx memq */
1932 link = mem_acquire(&mem_link_iso_rx.free);
1933 LL_ASSERT(link);
1934
1935 #if defined(CONFIG_BT_CTLR_ISO_VENDOR_DATA_PATH)
1936 /* Initialize ull rx memq */
1937 MEMQ_INIT(ull_iso_rx, link);
1938 #endif /* CONFIG_BT_CTLR_ISO_VENDOR_DATA_PATH */
1939
1940 /* Acquire a link to initialize ll_iso_rx memq */
1941 link = mem_acquire(&mem_link_iso_rx.free);
1942 LL_ASSERT(link);
1943
1944 /* Initialize ll_iso_rx memq */
1945 MEMQ_INIT(ll_iso_rx, link);
1946
1947 RXFIFO_ALLOC(iso_rx, UINT8_MAX);
1948 #endif /* CONFIG_BT_CTLR_SYNC_ISO) || CONFIG_BT_CTLR_CONN_ISO */
1949
1950 #if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
1951 /* Initialize tx pool. */
1952 mem_init(mem_iso_tx.pool, NODE_TX_BUFFER_SIZE, BT_CTLR_ISO_TX_BUFFERS,
1953 &mem_iso_tx.free);
1954
1955 /* Initialize tx link pool. */
1956 mem_init(mem_link_iso_tx.pool, sizeof(memq_link_t),
1957 BT_CTLR_ISO_TX_BUFFERS, &mem_link_iso_tx.free);
1958 #endif /* CONFIG_BT_CTLR_ADV_ISO || CONFIG_BT_CTLR_CONN_ISO */
1959
1960 #if BT_CTLR_ISO_STREAMS
1961 /* Initialize ISO Datapath pool */
1962 mem_init(datapath_pool, sizeof(struct ll_iso_datapath),
1963 sizeof(datapath_pool) / sizeof(struct ll_iso_datapath), &datapath_free);
1964 #endif /* BT_CTLR_ISO_STREAMS */
1965
1966 /* Initialize ISO Adaptation Layer */
1967 isoal_init();
1968
1969 return 0;
1970 }
1971
