1 /*
2 * Copyright (c) 2019 Oticon A/S
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 /**
8 * @brief HCI interface application
9 */
10
11 #include <zephyr/kernel.h>
12
13 #include <zephyr/settings/settings.h>
14
15 #include <zephyr/sys/byteorder.h>
16 #include <zephyr/debug/stack.h>
17
18 #include <zephyr/net_buf.h>
19
20 #include <zephyr/bluetooth/bluetooth.h>
21 #include <zephyr/bluetooth/l2cap.h>
22 #include <zephyr/bluetooth/hci_vs.h>
23 #include <zephyr/bluetooth/hci_raw.h>
24 #include <zephyr/bluetooth/iso.h>
25
26 #include "edtt_driver.h"
27 #include "bs_tracing.h"
28 #include "commands.h"
29
30 #if defined(CONFIG_BT_HCI_CORE_LOG_LEVEL_DBG)
31 #define LOG_LEVEL LOG_LEVEL_DBG
32 #else
33 #define LOG_LEVEL CONFIG_BT_LOG_LEVEL
34 #endif
35
36 #include <zephyr/logging/log.h>
37 LOG_MODULE_REGISTER(hci_test_app);
38
39 static uint16_t waiting_opcode;
40 static enum commands_t waiting_response;
41 static uint8_t m_events;
42
43 struct EdttIxit {
44 uint8_t refMajor;
45 uint8_t refMinor;
46 uint16_t len;
47 uint8_t *pVal;
48 };
49
50 /*! \brief Implementation eXtra Information for Test (IXIT) definitions */
51 #if defined(CONFIG_BT_CTLR_CONN_ISO_STREAMS_MAX_NSE)
52 const uint8_t TSPX_max_cis_nse = CONFIG_BT_CTLR_CONN_ISO_STREAMS_MAX_NSE;
53 #endif
54
55 /*! \brief Persistent LL IXIT values. */
56 static struct EdttIxit llIxits[] = {
57 #if defined(CONFIG_BT_CTLR_CONN_ISO_STREAMS_MAX_NSE)
58 {7, 14, 1, &TSPX_max_cis_nse},
59 #endif
60 };
61
62 /**
63 * @brief Clean out excess bytes from the input buffer
64 */
read_excess_bytes(uint16_t size)65 static void read_excess_bytes(uint16_t size)
66 {
67 if (size > 0) {
68 uint8_t buffer[size];
69
70 edtt_read((uint8_t *)buffer, size, EDTTT_BLOCK);
71 LOG_ERR("command size wrong! (%u extra bytes removed)", size);
72 }
73 }
74
75 /**
76 * @brief Provide an error response when an HCI command send failed
77 */
error_response(int error)78 static void error_response(int error)
79 {
80 uint16_t response = sys_cpu_to_le16(waiting_response);
81 int le_error = sys_cpu_to_le32(error);
82 uint16_t size = sys_cpu_to_le16(sizeof(le_error));
83
84 edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK);
85 edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK);
86 edtt_write((uint8_t *)&le_error, sizeof(le_error), EDTTT_BLOCK);
87 waiting_response = CMD_NOTHING;
88 waiting_opcode = 0;
89 }
90
91 /**
92 * @brief Allocate buffer for HCI command and fill in opCode for the command
93 */
hci_cmd_create(uint16_t opcode,uint8_t param_len)94 static struct net_buf *hci_cmd_create(uint16_t opcode, uint8_t param_len)
95 {
96 struct bt_hci_cmd_hdr *hdr;
97 struct net_buf *buf;
98
99 buf = bt_buf_get_tx(BT_BUF_CMD, K_FOREVER, NULL, 0);
100 __ASSERT_NO_MSG(buf);
101
102 hdr = net_buf_add(buf, sizeof(*hdr));
103 hdr->opcode = sys_cpu_to_le16(opcode);
104 hdr->param_len = param_len;
105
106 return buf;
107 }
108
109 /**
110 * @brief Allocate buffer for ACL Data Package and fill in Header
111 */
acl_data_create(struct bt_hci_acl_hdr * le_hdr)112 static struct net_buf *acl_data_create(struct bt_hci_acl_hdr *le_hdr)
113 {
114 struct net_buf *buf;
115 struct bt_hci_acl_hdr *hdr;
116
117 buf = bt_buf_get_tx(BT_BUF_ACL_OUT, K_FOREVER, NULL, 0);
118 __ASSERT_NO_MSG(buf);
119
120 hdr = net_buf_add(buf, sizeof(*hdr));
121 *hdr = *le_hdr;
122
123 return buf;
124 }
125
126 #if defined(CONFIG_BT_ISO)
127 /**
128 * @brief Allocate buffer for ISO Data Package and fill in Header
129 */
iso_data_create(struct bt_hci_iso_hdr * le_hdr)130 static struct net_buf *iso_data_create(struct bt_hci_iso_hdr *le_hdr)
131 {
132 struct net_buf *buf;
133 struct bt_hci_iso_hdr *hdr;
134
135 buf = bt_buf_get_tx(BT_BUF_ISO_OUT, K_FOREVER, NULL, 0);
136 __ASSERT_NO_MSG(buf);
137
138 hdr = net_buf_add(buf, sizeof(*hdr));
139 *hdr = *le_hdr;
140
141 return buf;
142 }
143 #endif /* defined(CONFIG_BT_ISO) */
144
145 /**
146 * @brief Allocate buffer for HCI command, fill in parameters and send the
147 * command...
148 */
send_hci_command(uint16_t opcode,uint8_t param_len,uint16_t response)149 static int send_hci_command(uint16_t opcode, uint8_t param_len, uint16_t response)
150 {
151 struct net_buf *buf;
152 void *cp;
153 int err = 0;
154
155 waiting_response = response;
156 buf = hci_cmd_create(waiting_opcode = opcode, param_len);
157 if (buf) {
158 if (param_len) {
159 cp = net_buf_add(buf, param_len);
160 edtt_read((uint8_t *)cp, param_len, EDTTT_BLOCK);
161 }
162 err = bt_send(buf);
163 if (err) {
164 LOG_ERR("Failed to send HCI command %d (err %d)",
165 opcode, err);
166 error_response(err);
167 }
168 } else {
169 LOG_ERR("Failed to create buffer for HCI command 0x%04x",
170 opcode);
171 error_response(-1);
172 }
173 return err;
174 }
175
176 /**
177 * @brief Echo function - echo input received...
178 */
echo(uint16_t size)179 static void echo(uint16_t size)
180 {
181 uint16_t response = sys_cpu_to_le16(CMD_ECHO_RSP);
182 uint16_t le_size = sys_cpu_to_le16(size);
183
184 edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK);
185 edtt_write((uint8_t *)&le_size, sizeof(le_size), EDTTT_BLOCK);
186
187 if (size > 0) {
188 uint8_t buff[size];
189
190 edtt_read(buff, size, EDTTT_BLOCK);
191 edtt_write(buff, size, EDTTT_BLOCK);
192 }
193 }
194
195 NET_BUF_POOL_FIXED_DEFINE(event_pool, 32, BT_BUF_RX_SIZE + 4, 4, NULL);
196 static K_FIFO_DEFINE(event_queue);
197 static K_FIFO_DEFINE(rx_queue);
198 NET_BUF_POOL_FIXED_DEFINE(data_pool, CONFIG_BT_CTLR_RX_BUFFERS + 14,
199 BT_BUF_ACL_SIZE(CONFIG_BT_BUF_ACL_TX_SIZE) + 4, 4, NULL);
200 static K_FIFO_DEFINE(data_queue);
201 #if defined(CONFIG_BT_ISO)
202 NET_BUF_POOL_FIXED_DEFINE(iso_data_pool, CONFIG_BT_ISO_RX_BUF_COUNT + 14,
203 BT_ISO_SDU_BUF_SIZE(CONFIG_BT_ISO_RX_MTU) +
204 sizeof(uint32_t), 8, NULL);
205 static K_FIFO_DEFINE(iso_data_queue);
206 #endif /* CONFIG_BT_ISO */
207
208 /**
209 * @brief Handle Command Complete HCI event...
210 */
command_complete(struct net_buf * buf)211 static void command_complete(struct net_buf *buf)
212 {
213 struct bt_hci_evt_cmd_complete *evt = (void *)buf->data;
214 uint16_t opcode = sys_le16_to_cpu(evt->opcode);
215 uint16_t response = sys_cpu_to_le16(waiting_response);
216 struct net_buf_simple_state state;
217 uint16_t size;
218
219 net_buf_simple_save(&buf->b, &state);
220
221 net_buf_pull(buf, sizeof(*evt));
222 size = sys_cpu_to_le16(buf->len);
223
224 if (opcode == waiting_opcode) {
225 LOG_DBG("Command complete for 0x%04x", waiting_opcode);
226
227 edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK);
228 edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK);
229 edtt_write((uint8_t *)buf->data, buf->len, EDTTT_BLOCK);
230 waiting_opcode = 0;
231 } else {
232 LOG_WRN("Not waiting for 0x(%04x) command status,"
233 " expected 0x(%04x)", opcode, waiting_opcode);
234 }
235
236 net_buf_simple_restore(&buf->b, &state);
237 }
238
239 /**
240 * @brief Handle Command Status HCI event...
241 */
command_status(struct net_buf * buf)242 static void command_status(struct net_buf *buf)
243 {
244 struct bt_hci_evt_cmd_status *evt = (void *)buf->data;
245 uint16_t opcode = sys_le16_to_cpu(evt->opcode);
246 uint16_t response = sys_cpu_to_le16(waiting_response);
247 struct net_buf_simple_state state;
248 uint8_t status = evt->status;
249 uint16_t size;
250
251 net_buf_simple_save(&buf->b, &state);
252
253 net_buf_pull(buf, sizeof(*evt));
254 size = sys_cpu_to_le16(buf->len) + 1;
255
256 if (opcode == waiting_opcode) {
257 LOG_DBG("Command status for 0x%04x", waiting_opcode);
258
259 edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK);
260 edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK);
261 edtt_write((uint8_t *)&status, sizeof(status), EDTTT_BLOCK);
262 edtt_write((uint8_t *)buf->data, buf->len, EDTTT_BLOCK);
263 waiting_opcode = 0;
264 } else {
265 LOG_WRN("Not waiting for 0x(%04x) command status,"
266 " expected 0x(%04x)", opcode, waiting_opcode);
267 }
268
269 net_buf_simple_restore(&buf->b, &state);
270 }
271
272 /**
273 * @brief Remove an event from the event queue
274 */
discard_event(void)275 static void discard_event(void)
276 {
277 struct net_buf *buf = k_fifo_get(&event_queue, K_FOREVER);
278
279 net_buf_unref(buf);
280 m_events--;
281 }
282
283 /**
284 * @brief Allocate and store an event in the event queue
285 */
queue_event(struct net_buf * buf)286 static struct net_buf *queue_event(struct net_buf *buf)
287 {
288 struct net_buf *evt;
289
290 evt = net_buf_alloc(&event_pool, K_NO_WAIT);
291 if (evt) {
292 bt_buf_set_type(evt, BT_BUF_EVT);
293 net_buf_add_le32(evt, sys_cpu_to_le32(k_uptime_get()));
294 net_buf_add_mem(evt, buf->data, buf->len);
295 k_fifo_put(&event_queue, evt);
296 m_events++;
297 }
298 return evt;
299 }
300
301 /**
302 * @brief Thread to service events and ACL/ISO data packets from the HCI input queue
303 */
service_events(void * p1,void * p2,void * p3)304 static void service_events(void *p1, void *p2, void *p3)
305 {
306 struct net_buf *buf, *evt;
307
308 while (1) {
309 buf = k_fifo_get(&rx_queue, K_FOREVER);
310 if (bt_buf_get_type(buf) == BT_BUF_EVT) {
311
312 evt = queue_event(buf);
313 if (!evt) {
314 bs_trace_raw_time(4,
315 "Failed to allocated buffer "
316 "for event!\n");
317 LOG_WRN("No event in queue");
318 }
319
320 struct bt_hci_evt_hdr *hdr = (void *)buf->data;
321
322 net_buf_pull(buf, sizeof(*hdr));
323
324 switch (hdr->evt) {
325 case BT_HCI_EVT_CMD_COMPLETE:
326 if (!evt) {
327 discard_event();
328 evt = queue_event(buf);
329 }
330 command_complete(buf);
331 break;
332 case BT_HCI_EVT_CMD_STATUS:
333 if (!evt) {
334 discard_event();
335 evt = queue_event(buf);
336 }
337 command_status(buf);
338 break;
339 default:
340 break;
341 }
342 } else if (bt_buf_get_type(buf) == BT_BUF_ACL_IN) {
343 struct net_buf *data;
344
345 data = net_buf_alloc(&data_pool, K_NO_WAIT);
346 if (data) {
347 bt_buf_set_type(data, BT_BUF_ACL_IN);
348 net_buf_add_le32(data,
349 sys_cpu_to_le32(k_uptime_get()));
350 net_buf_add_mem(data, buf->data, buf->len);
351 k_fifo_put(&data_queue, data);
352 }
353 #if defined(CONFIG_BT_ISO)
354 } else if (bt_buf_get_type(buf) == BT_BUF_ISO_IN) {
355 struct net_buf *data;
356
357 data = net_buf_alloc(&iso_data_pool, K_NO_WAIT);
358 if (data) {
359 bt_buf_set_type(data, BT_BUF_ISO_IN);
360 net_buf_add_le32(data,
361 sys_cpu_to_le32(k_uptime_get()));
362 net_buf_add_mem(data, buf->data, buf->len);
363 k_fifo_put(&iso_data_queue, data);
364 }
365 #endif /* CONFIG_BT_ISO */
366 }
367 net_buf_unref(buf);
368
369 k_yield();
370 }
371 }
372
373 /**
374 * @brief Flush all HCI events from the input-copy queue
375 */
flush_events(uint16_t size)376 static void flush_events(uint16_t size)
377 {
378 uint16_t response = sys_cpu_to_le16(CMD_FLUSH_EVENTS_RSP);
379 struct net_buf *buf;
380
381 while ((buf = k_fifo_get(&event_queue, K_NO_WAIT))) {
382 net_buf_unref(buf);
383 m_events--;
384 }
385 read_excess_bytes(size);
386 size = 0;
387
388 edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK);
389 edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK);
390 }
391
392 /**
393 * @brief Get next available HCI event from the input-copy queue
394 */
get_event(uint16_t size)395 static void get_event(uint16_t size)
396 {
397 uint16_t response = sys_cpu_to_le16(CMD_GET_EVENT_RSP);
398 struct net_buf *buf;
399
400 read_excess_bytes(size);
401 size = 0;
402
403 edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK);
404 buf = k_fifo_get(&event_queue, K_FOREVER);
405 if (buf) {
406 size = sys_cpu_to_le16(buf->len);
407 edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK);
408 edtt_write((uint8_t *)buf->data, buf->len, EDTTT_BLOCK);
409 net_buf_unref(buf);
410 m_events--;
411 } else {
412 edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK);
413 }
414 }
415
416 /**
417 * @brief Get next available HCI events from the input-copy queue
418 */
get_events(uint16_t size)419 static void get_events(uint16_t size)
420 {
421 uint16_t response = sys_cpu_to_le16(CMD_GET_EVENT_RSP);
422 struct net_buf *buf;
423 uint8_t count = m_events;
424
425 read_excess_bytes(size);
426 size = 0;
427
428 edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK);
429 edtt_write((uint8_t *)&count, sizeof(count), EDTTT_BLOCK);
430 while (count--) {
431 buf = k_fifo_get(&event_queue, K_FOREVER);
432 size = sys_cpu_to_le16(buf->len);
433 edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK);
434 edtt_write((uint8_t *)buf->data, buf->len, EDTTT_BLOCK);
435 net_buf_unref(buf);
436 m_events--;
437 }
438 }
439
440 /**
441 * @brief Check whether an HCI event is available in the input-copy queue
442 */
has_event(uint16_t size)443 static void has_event(uint16_t size)
444 {
445 struct has_event_resp {
446 uint16_t response;
447 uint16_t size;
448 uint8_t count;
449 } __packed;
450 struct has_event_resp le_response = {
451 .response = sys_cpu_to_le16(CMD_HAS_EVENT_RSP),
452 .size = sys_cpu_to_le16(1),
453 .count = m_events
454 };
455
456 if (size > 0) {
457 read_excess_bytes(size);
458 }
459 edtt_write((uint8_t *)&le_response, sizeof(le_response), EDTTT_BLOCK);
460 }
461
462 /**
463 * @brief Flush all ACL Data Packages from the input-copy queue
464 */
le_flush_data(uint16_t size)465 static void le_flush_data(uint16_t size)
466 {
467 uint16_t response = sys_cpu_to_le16(CMD_LE_FLUSH_DATA_RSP);
468 struct net_buf *buf;
469
470 while ((buf = k_fifo_get(&data_queue, K_NO_WAIT))) {
471 net_buf_unref(buf);
472 }
473 read_excess_bytes(size);
474 size = 0;
475
476 edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK);
477 edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK);
478 }
479
480 /**
481 * @brief Check whether an ACL Data Package is available in the input-copy queue
482 */
le_data_ready(uint16_t size)483 static void le_data_ready(uint16_t size)
484 {
485 struct has_data_resp {
486 uint16_t response;
487 uint16_t size;
488 uint8_t empty;
489 } __packed;
490 struct has_data_resp le_response = {
491 .response = sys_cpu_to_le16(CMD_LE_DATA_READY_RSP),
492 .size = sys_cpu_to_le16(1),
493 .empty = 0
494 };
495
496 if (size > 0) {
497 read_excess_bytes(size);
498 }
499 if (k_fifo_is_empty(&data_queue)) {
500 le_response.empty = 1;
501 }
502 edtt_write((uint8_t *)&le_response, sizeof(le_response), EDTTT_BLOCK);
503 }
504
505 /**
506 * @brief Get next available HCI Data Package from the input-copy queue
507 */
le_data_read(uint16_t size)508 static void le_data_read(uint16_t size)
509 {
510 uint16_t response = sys_cpu_to_le16(CMD_LE_DATA_READ_RSP);
511 struct net_buf *buf;
512
513 read_excess_bytes(size);
514 size = 0;
515
516 edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK);
517 buf = k_fifo_get(&data_queue, K_FOREVER);
518 if (buf) {
519 size = sys_cpu_to_le16(buf->len);
520 edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK);
521 edtt_write((uint8_t *)buf->data, buf->len, EDTTT_BLOCK);
522 net_buf_unref(buf);
523 } else {
524 edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK);
525 }
526 }
527
528 /**
529 * @brief Write ACL Data Package to the Controller...
530 */
le_data_write(uint16_t size)531 static void le_data_write(uint16_t size)
532 {
533 struct data_write_resp {
534 uint16_t code;
535 uint16_t size;
536 uint8_t status;
537 } __packed;
538 struct data_write_resp response = {
539 .code = sys_cpu_to_le16(CMD_LE_DATA_WRITE_RSP),
540 .size = sys_cpu_to_le16(1),
541 .status = 0
542 };
543 struct net_buf *buf;
544 struct bt_hci_acl_hdr hdr;
545 int err;
546
547 if (size >= sizeof(hdr)) {
548 edtt_read((uint8_t *)&hdr, sizeof(hdr), EDTTT_BLOCK);
549 size -= sizeof(hdr);
550 buf = acl_data_create(&hdr);
551 if (buf) {
552 uint16_t hdr_length = sys_le16_to_cpu(hdr.len);
553 uint8_t *pdata = net_buf_add(buf, hdr_length);
554
555 if (size >= hdr_length) {
556 edtt_read(pdata, hdr_length, EDTTT_BLOCK);
557 size -= hdr_length;
558 }
559 err = bt_send(buf);
560 if (err) {
561 LOG_ERR("Failed to send ACL Data (err %d)",
562 err);
563 }
564 } else {
565 err = -2; /* Failed to allocate data buffer */
566 LOG_ERR("Failed to create buffer for ACL Data.");
567 }
568 } else {
569 /* Size too small for header (handle and data length) */
570 err = -3;
571 }
572 read_excess_bytes(size);
573
574 response.status = sys_cpu_to_le32(err);
575 edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK);
576 }
577
578 #if defined(CONFIG_BT_ISO)
579 /**
580 * @brief Flush all ISO Data Packages from the input-copy queue
581 */
le_flush_iso_data(uint16_t size)582 static void le_flush_iso_data(uint16_t size)
583 {
584 uint16_t response = sys_cpu_to_le16(CMD_LE_FLUSH_ISO_DATA_RSP);
585 struct net_buf *buf;
586
587 while ((buf = k_fifo_get(&iso_data_queue, K_NO_WAIT))) {
588 net_buf_unref(buf);
589 }
590 read_excess_bytes(size);
591 size = 0;
592
593 edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK);
594 edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK);
595 }
596
597 /**
598 * @brief Check whether an ISO Data Package is available in the input-copy queue
599 */
le_iso_data_ready(uint16_t size)600 static void le_iso_data_ready(uint16_t size)
601 {
602 struct has_iso_data_resp {
603 uint16_t response;
604 uint16_t size;
605 uint8_t empty;
606 } __packed;
607 struct has_iso_data_resp le_response = {
608 .response = sys_cpu_to_le16(CMD_LE_ISO_DATA_READY_RSP),
609 .size = sys_cpu_to_le16(1),
610 .empty = 0
611 };
612
613 if (size > 0) {
614 read_excess_bytes(size);
615 }
616 if (k_fifo_is_empty(&iso_data_queue)) {
617 le_response.empty = 1;
618 }
619 edtt_write((uint8_t *)&le_response, sizeof(le_response), EDTTT_BLOCK);
620 }
621
622 /**
623 * @brief Get next available ISO Data Package from the input-copy queue
624 */
le_iso_data_read(uint16_t size)625 static void le_iso_data_read(uint16_t size)
626 {
627 uint16_t response = sys_cpu_to_le16(CMD_LE_ISO_DATA_READ_RSP);
628 struct net_buf *buf;
629
630 read_excess_bytes(size);
631 size = 0;
632
633 edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK);
634 buf = k_fifo_get(&iso_data_queue, K_FOREVER);
635 if (buf) {
636 size = sys_cpu_to_le16(buf->len);
637 edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK);
638 edtt_write((uint8_t *)buf->data, buf->len, EDTTT_BLOCK);
639 net_buf_unref(buf);
640 } else {
641 edtt_write((uint8_t *)&size, sizeof(size), EDTTT_BLOCK);
642 }
643 }
644
645 /**
646 * @brief Write ISO Data Package to the Controller...
647 */
le_iso_data_write(uint16_t size)648 static void le_iso_data_write(uint16_t size)
649 {
650 struct iso_data_write_resp {
651 uint16_t code;
652 uint16_t size;
653 uint8_t status;
654 } __packed;
655 struct iso_data_write_resp response = {
656 .code = sys_cpu_to_le16(CMD_LE_ISO_DATA_WRITE_RSP),
657 .size = sys_cpu_to_le16(1),
658 .status = 0
659 };
660 struct net_buf *buf;
661 struct bt_hci_iso_hdr hdr;
662 int err;
663
664 if (size >= sizeof(hdr)) {
665 edtt_read((uint8_t *)&hdr, sizeof(hdr), EDTTT_BLOCK);
666 size -= sizeof(hdr);
667 buf = iso_data_create(&hdr);
668 if (buf) {
669 uint16_t hdr_length = sys_le16_to_cpu(hdr.len);
670 uint8_t *pdata = net_buf_add(buf, hdr_length);
671
672 if (size >= hdr_length) {
673 edtt_read(pdata, hdr_length, EDTTT_BLOCK);
674 size -= hdr_length;
675 }
676 err = bt_send(buf);
677 if (err) {
678 LOG_ERR("Failed to send ISO Data (err %d)",
679 err);
680 }
681 } else {
682 err = -2; /* Failed to allocate data buffer */
683 LOG_ERR("Failed to create buffer for ISO Data.");
684 }
685 } else {
686 /* Size too small for header (handle and data length) */
687 err = -3;
688 }
689 read_excess_bytes(size);
690
691 response.status = sys_cpu_to_le32(err);
692 edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK);
693 }
694 #endif /* CONFIG_BT_ISO */
695
696 /**
697 * @brief Read 'Implementation eXtra Information for Test' value
698 */
le_ixit_value_read(uint16_t size)699 static void le_ixit_value_read(uint16_t size)
700 {
701 uint8_t profileId;
702 uint8_t refMajor;
703 uint8_t refMinor;
704 struct EdttIxit *pIxitArray;
705 int ixitArraySize;
706 struct EdttIxit *pIxitElement = NULL;
707
708 /*
709 * CMD_GET_IXIT_VALUE_REQ payload layout
710 *
711 * ...
712 * [ 4] PROFILE_ID[0]
713 * [ 5] IXIT_Reference_Major
714 * [ 6] IXIT_Reference_Minor
715 */
716 edtt_read((uint8_t *)&profileId, sizeof(profileId), EDTTT_BLOCK);
717 edtt_read((uint8_t *)&refMajor, sizeof(refMajor), EDTTT_BLOCK);
718 edtt_read((uint8_t *)&refMinor, sizeof(refMinor), EDTTT_BLOCK);
719
720 switch (profileId) {
721 case PROFILE_ID_LL:
722 pIxitArray = llIxits;
723 ixitArraySize = ARRAY_SIZE(llIxits);
724 break;
725 default:
726 pIxitArray = NULL;
727 ixitArraySize = 0;
728 }
729 for (int i = 0; i < ixitArraySize; i++) {
730 if (pIxitArray[i].refMajor == refMajor && pIxitArray[i].refMinor == refMinor) {
731 pIxitElement = &pIxitArray[i];
732 break;
733 }
734 }
735
736 struct ixit_value_get_resp {
737 uint16_t code;
738 uint16_t size;
739 uint8_t data[];
740 } __packed;
741 struct ixit_value_get_resp response = {
742 .code = sys_cpu_to_le16(CMD_GET_IXIT_VALUE_RSP),
743 .size = sys_cpu_to_le16(pIxitElement ? pIxitElement->len : 0),
744 };
745 edtt_write((uint8_t *)&response, sizeof(response), EDTTT_BLOCK);
746 if (pIxitElement) {
747 edtt_write(pIxitElement->pVal, pIxitElement->len, EDTTT_BLOCK);
748 }
749 }
750
751 static K_THREAD_STACK_DEFINE(service_events_stack,
752 CONFIG_BT_HCI_TX_STACK_SIZE);
753 static struct k_thread service_events_data;
754
755 /**
756 * @brief Zephyr application main entry...
757 */
main(void)758 int main(void)
759 {
760 int err;
761 uint16_t command;
762 uint16_t size;
763 uint16_t opcode;
764 /**
765 * Initialize HCI command opcode and response variables...
766 */
767 waiting_opcode = 0;
768 waiting_response = CMD_NOTHING;
769 m_events = 0;
770 /**
771 * Initialize Bluetooth stack in raw mode...
772 */
773 err = bt_enable_raw(&rx_queue);
774 if (err) {
775 LOG_ERR("Bluetooth initialization failed (err %d)", err);
776 return 0;
777 }
778 /**
779 * Initialize and start EDTT system...
780 */
781 #if defined(CONFIG_ARCH_POSIX)
782 enable_edtt_mode();
783 set_edtt_autoshutdown(true);
784 #endif
785 edtt_start();
786 /**
787 * Initialize and start thread to service HCI events and ACL data...
788 */
789 k_thread_create(&service_events_data, service_events_stack,
790 K_THREAD_STACK_SIZEOF(service_events_stack),
791 service_events, NULL, NULL, NULL, K_PRIO_COOP(7),
792 0, K_NO_WAIT);
793
794 while (1) {
795 /**
796 * Wait for a command to arrive - then read and execute command
797 */
798 edtt_read((uint8_t *)&command, sizeof(command), EDTTT_BLOCK);
799 command = sys_le16_to_cpu(command);
800 edtt_read((uint8_t *)&size, sizeof(size), EDTTT_BLOCK);
801 size = sys_le16_to_cpu(size);
802 bs_trace_raw_time(4, "command 0x%04X received (size %u) "
803 "events=%u\n",
804 command, size, m_events);
805
806 switch (command) {
807 case CMD_ECHO_REQ:
808 echo(size);
809 break;
810 case CMD_FLUSH_EVENTS_REQ:
811 flush_events(size);
812 break;
813 case CMD_HAS_EVENT_REQ:
814 has_event(size);
815 break;
816 case CMD_GET_EVENT_REQ:
817 {
818 uint8_t multiple;
819
820 edtt_read((uint8_t *)&multiple, sizeof(multiple),
821 EDTTT_BLOCK);
822 if (multiple) {
823 get_events(--size);
824 } else {
825 get_event(--size);
826 }
827 }
828 break;
829 case CMD_LE_FLUSH_DATA_REQ:
830 le_flush_data(size);
831 break;
832 case CMD_LE_DATA_READY_REQ:
833 le_data_ready(size);
834 break;
835 case CMD_LE_DATA_WRITE_REQ:
836 le_data_write(size);
837 break;
838 case CMD_LE_DATA_READ_REQ:
839 le_data_read(size);
840 break;
841 #if defined(CONFIG_BT_ISO)
842 case CMD_LE_FLUSH_ISO_DATA_REQ:
843 le_flush_iso_data(size);
844 break;
845 case CMD_LE_ISO_DATA_READY_REQ:
846 le_iso_data_ready(size);
847 break;
848 case CMD_LE_ISO_DATA_WRITE_REQ:
849 le_iso_data_write(size);
850 break;
851 case CMD_LE_ISO_DATA_READ_REQ:
852 le_iso_data_read(size);
853 break;
854 #endif /* CONFIG_BT_ISO */
855
856 case CMD_GET_IXIT_VALUE_REQ:
857 le_ixit_value_read(size);
858 break;
859
860 default:
861 if (size >= 2) {
862 edtt_read((uint8_t *)&opcode, sizeof(opcode),
863 EDTTT_BLOCK);
864 send_hci_command(sys_le16_to_cpu(opcode),
865 size - 2, command + 1);
866 }
867 }
868 }
869 return 0;
870 }
871
