1 /*
2 * Copyright (c) 2023 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/kernel.h>
8 #include <zephyr/sys/byteorder.h>
9 #include <zephyr/sys/__assert.h>
10
11 #include <zephyr/net_buf.h>
12 #include <zephyr/bluetooth/buf.h>
13
14 #include <zephyr/bluetooth/bluetooth.h>
15 #include <zephyr/bluetooth/hci.h>
16 #include <zephyr/bluetooth/hci_raw.h>
17 #include <zephyr/bluetooth/hci_types.h>
18
19 #include "common/bt_str.h"
20
21 #include "host/conn_internal.h"
22 #include "host/l2cap_internal.h"
23
24 #include "utils.h"
25 #include "sync.h"
26 #include "bstests.h"
27 #include "NRF_HWLowL.h" /* for hwll_disconnect_phy(); */
28
29 #include <zephyr/logging/log.h>
30 LOG_MODULE_REGISTER(bt_tinyhost, LOG_LEVEL_INF);
31
32 #define BT_ATT_OP_MTU_REQ 0x02
33 #define BT_ATT_OP_MTU_RSP 0x03
34 #define BT_ATT_OP_WRITE_REQ 0x12
35 #define BT_ATT_OP_WRITE_RSP 0x13
36 #define BT_ATT_OP_NOTIFY 0x1b
37 #define BT_ATT_OP_INDICATE 0x1d
38 #define BT_ATT_OP_CONFIRM 0x1e
39 #define BT_ATT_OP_WRITE_CMD 0x52
40 #define BT_L2CAP_CID_ATT 0x0004
41
42 DEFINE_FLAG(is_connected);
43 DEFINE_FLAG(flag_data_length_updated);
44
45 static K_FIFO_DEFINE(rx_queue);
46
47 #define CMD_BUF_SIZE MAX(BT_BUF_EVT_RX_SIZE, BT_BUF_CMD_TX_SIZE)
48 NET_BUF_POOL_FIXED_DEFINE(hci_cmd_pool, CONFIG_BT_BUF_CMD_TX_COUNT,
49 CMD_BUF_SIZE, 8, NULL);
50
51 static K_SEM_DEFINE(cmd_sem, 1, 1);
52 static struct k_sem acl_pkts;
53 static uint16_t conn_handle;
54
55 static volatile uint16_t active_opcode = 0xFFFF;
56 static struct net_buf *cmd_rsp;
57
bt_hci_cmd_create(uint16_t opcode,uint8_t param_len)58 struct net_buf *bt_hci_cmd_create(uint16_t opcode, uint8_t param_len)
59 {
60 struct bt_hci_cmd_hdr *hdr;
61 struct net_buf *buf;
62
63 LOG_DBG("opcode 0x%04x param_len %u", opcode, param_len);
64
65 buf = net_buf_alloc(&hci_cmd_pool, K_FOREVER);
66 ASSERT(buf, "failed allocation");
67
68 LOG_DBG("buf %p", buf);
69
70 net_buf_reserve(buf, BT_BUF_RESERVE);
71
72 bt_buf_set_type(buf, BT_BUF_CMD);
73
74 hdr = net_buf_add(buf, sizeof(*hdr));
75 hdr->opcode = sys_cpu_to_le16(opcode);
76 hdr->param_len = param_len;
77
78 return buf;
79 }
80
handle_cmd_complete(struct net_buf * buf)81 static void handle_cmd_complete(struct net_buf *buf)
82 {
83 struct bt_hci_evt_hdr *hdr;
84 uint8_t status, ncmd;
85 uint16_t opcode;
86 struct net_buf_simple_state state;
87
88 net_buf_simple_save(&buf->b, &state);
89
90 hdr = net_buf_pull_mem(buf, sizeof(*hdr));
91
92 if (hdr->evt == BT_HCI_EVT_CMD_COMPLETE) {
93 struct bt_hci_evt_cmd_complete *evt;
94
95 evt = net_buf_pull_mem(buf, sizeof(*evt));
96 status = 0;
97 ncmd = evt->ncmd;
98 opcode = sys_le16_to_cpu(evt->opcode);
99
100 } else if (hdr->evt == BT_HCI_EVT_CMD_STATUS) {
101 struct bt_hci_evt_cmd_status *evt;
102
103 evt = net_buf_pull_mem(buf, sizeof(*evt));
104 status = buf->data[0];
105 ncmd = evt->ncmd;
106 opcode = sys_le16_to_cpu(evt->opcode);
107
108 } else {
109 FAIL("unhandled event 0x%x", hdr->evt);
110 }
111
112 LOG_DBG("opcode 0x%04x status %x", opcode, status);
113
114 ASSERT(status == 0x00, "cmd status: %x", status);
115
116 ASSERT(active_opcode == opcode, "unexpected opcode %x != %x", active_opcode, opcode);
117
118 if (active_opcode) {
119 active_opcode = 0xFFFF;
120 cmd_rsp = net_buf_ref(buf);
121 net_buf_simple_restore(&buf->b, &state);
122 }
123
124 if (ncmd) {
125 k_sem_give(&cmd_sem);
126 }
127 }
128
handle_meta_event(struct net_buf * buf)129 static void handle_meta_event(struct net_buf *buf)
130 {
131 uint8_t code = buf->data[2];
132
133 switch (code) {
134 case BT_HCI_EVT_LE_ENH_CONN_COMPLETE:
135 case BT_HCI_EVT_LE_ENH_CONN_COMPLETE_V2:
136 conn_handle = sys_get_le16(&buf->data[4]);
137 LOG_DBG("connected: handle: %d", conn_handle);
138 SET_FLAG(is_connected);
139 break;
140 case BT_HCI_EVT_LE_DATA_LEN_CHANGE:
141 SET_FLAG(flag_data_length_updated);
142 break;
143 case BT_HCI_EVT_LE_CHAN_SEL_ALGO:
144 /* do nothing */
145 break;
146 default:
147 LOG_ERR("unhandled meta event %x", code);
148 LOG_HEXDUMP_ERR(buf->data, buf->len, "HCI META EVT");
149 }
150 }
151
handle_ncp(struct net_buf * buf)152 static void handle_ncp(struct net_buf *buf)
153 {
154 struct bt_hci_evt_num_completed_packets *evt;
155 struct bt_hci_evt_hdr *hdr;
156 uint16_t handle, count;
157
158 hdr = net_buf_pull_mem(buf, sizeof(*hdr));
159
160 evt = (void *)buf->data;
161 handle = sys_le16_to_cpu(evt->h[0].handle);
162 count = sys_le16_to_cpu(evt->h[0].count);
163
164 LOG_DBG("sent %d packets", count);
165
166 while (count--) {
167 k_sem_give(&acl_pkts);
168 }
169 }
170
171 struct net_buf *alloc_l2cap_pdu(void);
172 static void send_l2cap_packet(struct net_buf *buf, uint16_t cid);
173
send_write_rsp(void)174 static void send_write_rsp(void)
175 {
176 struct net_buf *buf = alloc_l2cap_pdu();
177
178 net_buf_add_u8(buf, BT_ATT_OP_WRITE_RSP);
179 send_l2cap_packet(buf, BT_L2CAP_CID_ATT);
180 }
181
handle_att_write(struct net_buf * buf)182 static void handle_att_write(struct net_buf *buf)
183 {
184 uint16_t handle = net_buf_pull_le16(buf);
185
186 LOG_INF("Got write for 0x%04x len %d", handle, buf->len);
187 LOG_HEXDUMP_DBG(buf->data, buf->len, "payload");
188
189 static uint8_t ccc_write[2] = {0x03, 0x00};
190
191 ASSERT(buf->len == 2, "unexpected write length: %d\n", buf->len);
192 ASSERT(memcmp(buf->data, ccc_write, sizeof(ccc_write)) == 0, "bad data\n");
193
194 send_write_rsp();
195 }
196
handle_att(struct net_buf * buf)197 static void handle_att(struct net_buf *buf)
198 {
199 uint8_t op = net_buf_pull_u8(buf);
200
201 switch (op) {
202 case BT_ATT_OP_WRITE_REQ:
203 handle_att_write(buf);
204 return;
205 case BT_ATT_OP_MTU_RSP:
206 LOG_INF("got ATT MTU RSP");
207 return;
208 default:
209 LOG_HEXDUMP_ERR(buf->data, buf->len, "payload");
210 FAIL("unhandled opcode %x\n", op);
211 return;
212 }
213 }
214
handle_l2cap(struct net_buf * buf)215 static void handle_l2cap(struct net_buf *buf)
216 {
217 struct bt_l2cap_hdr *hdr;
218 uint16_t cid;
219
220 hdr = net_buf_pull_mem(buf, sizeof(*hdr));
221 cid = sys_le16_to_cpu(hdr->cid);
222
223 LOG_DBG("Packet for CID %u len %u", cid, buf->len);
224 LOG_HEXDUMP_DBG(buf->data, buf->len, "l2cap");
225
226 /* Make sure we don't have to recombine packets */
227 ASSERT(buf->len == hdr->len, "buflen = %d != hdrlen %d",
228 buf->len, hdr->len);
229
230 ASSERT(cid == BT_L2CAP_CID_ATT, "We only support (U)ATT");
231
232 /* (U)ATT PDU */
233 handle_att(buf);
234 }
235
handle_acl(struct net_buf * buf)236 static void handle_acl(struct net_buf *buf)
237 {
238 struct bt_hci_acl_hdr *hdr;
239 uint16_t len, handle;
240 uint8_t flags;
241
242 hdr = net_buf_pull_mem(buf, sizeof(*hdr));
243 len = sys_le16_to_cpu(hdr->len);
244 handle = sys_le16_to_cpu(hdr->handle);
245
246 flags = bt_acl_flags(handle);
247 handle = bt_acl_handle(handle);
248
249 ASSERT(flags == BT_ACL_START,
250 "Fragmentation not supported");
251
252 LOG_DBG("ACL: conn %d len %d flags %d", handle, len, flags);
253 LOG_HEXDUMP_DBG(buf->data, buf->len, "HCI ACL");
254
255 handle_l2cap(buf);
256 }
257
recv(struct net_buf * buf)258 static void recv(struct net_buf *buf)
259 {
260 LOG_HEXDUMP_DBG(buf->data, buf->len, "HCI RX");
261
262 uint8_t code = buf->data[0];
263
264 if (bt_buf_get_type(buf) == BT_BUF_EVT) {
265 switch (code) {
266 case BT_HCI_EVT_CMD_COMPLETE:
267 case BT_HCI_EVT_CMD_STATUS:
268 handle_cmd_complete(buf);
269 break;
270 case BT_HCI_EVT_LE_META_EVENT:
271 handle_meta_event(buf);
272 break;
273 case BT_HCI_EVT_DISCONN_COMPLETE:
274 UNSET_FLAG(is_connected);
275 break;
276 case BT_HCI_EVT_NUM_COMPLETED_PACKETS:
277 handle_ncp(buf);
278 break;
279 default:
280 LOG_ERR("unhandled msg %x", code);
281 LOG_HEXDUMP_ERR(buf->data, buf->len, "HCI EVT");
282 }
283
284 /* handlers should take a ref if they want to access the buffer
285 * later
286 */
287 net_buf_unref(buf);
288 return;
289 }
290
291 if (bt_buf_get_type(buf) == BT_BUF_ACL_IN) {
292 handle_acl(buf);
293 net_buf_unref(buf);
294 return;
295 }
296
297 LOG_ERR("HCI RX (not data or event)");
298 net_buf_unref(buf);
299 }
300
send_cmd(uint16_t opcode,struct net_buf * cmd,struct net_buf ** rsp)301 static void send_cmd(uint16_t opcode, struct net_buf *cmd, struct net_buf **rsp)
302 {
303 LOG_DBG("opcode %x", opcode);
304
305 if (!cmd) {
306 cmd = bt_hci_cmd_create(opcode, 0);
307 }
308
309 k_sem_take(&cmd_sem, K_FOREVER);
310 ASSERT(active_opcode == 0xFFFF, "");
311
312 active_opcode = opcode;
313
314 LOG_HEXDUMP_DBG(cmd->data, cmd->len, "HCI TX");
315 bt_send(cmd);
316
317 /* Wait until the command completes */
318 k_sem_take(&cmd_sem, K_FOREVER);
319 k_sem_give(&cmd_sem);
320
321 net_buf_unref(cmd);
322
323 /* return response. it's okay if cmd_rsp gets overwritten, since the app
324 * gets the ref to the underlying buffer when this fn returns.
325 */
326 if (rsp) {
327 *rsp = cmd_rsp;
328 } else {
329 net_buf_unref(cmd_rsp);
330 cmd_rsp = NULL;
331 }
332 }
333
334 static K_THREAD_STACK_DEFINE(rx_thread_stack, 1024);
335 static struct k_thread rx_thread_data;
336
rx_thread(void * p1,void * p2,void * p3)337 static void rx_thread(void *p1, void *p2, void *p3)
338 {
339 LOG_DBG("start HCI rx");
340
341 while (true) {
342 struct net_buf *buf;
343
344 /* Wait until a buffer is available */
345 buf = k_fifo_get(&rx_queue, K_FOREVER);
346 recv(buf);
347 }
348 }
349
le_read_buffer_size_complete(struct net_buf * rsp)350 static void le_read_buffer_size_complete(struct net_buf *rsp)
351 {
352 struct bt_hci_rp_le_read_buffer_size *rp = (void *)rsp->data;
353
354 LOG_DBG("status 0x%02x", rp->status);
355 LOG_DBG("max len %d max num %d", rp->le_max_len, rp->le_max_num);
356
357 k_sem_init(&acl_pkts, rp->le_max_num, rp->le_max_num);
358 net_buf_unref(rsp);
359 }
360
read_max_data_len(uint16_t * tx_octets,uint16_t * tx_time)361 static void read_max_data_len(uint16_t *tx_octets, uint16_t *tx_time)
362 {
363 struct bt_hci_rp_le_read_max_data_len *rp;
364 struct net_buf *rsp;
365
366 send_cmd(BT_HCI_OP_LE_READ_MAX_DATA_LEN, NULL, &rsp);
367
368 rp = (void *)rsp->data;
369 *tx_octets = sys_le16_to_cpu(rp->max_tx_octets);
370 *tx_time = sys_le16_to_cpu(rp->max_tx_time);
371 net_buf_unref(rsp);
372 }
373
write_default_data_len(uint16_t tx_octets,uint16_t tx_time)374 static void write_default_data_len(uint16_t tx_octets, uint16_t tx_time)
375 {
376 struct bt_hci_cp_le_write_default_data_len *cp;
377 struct net_buf *buf = bt_hci_cmd_create(BT_HCI_OP_LE_WRITE_DEFAULT_DATA_LEN, sizeof(*cp));
378
379 ASSERT(buf, "");
380
381 cp = net_buf_add(buf, sizeof(*cp));
382 cp->max_tx_octets = sys_cpu_to_le16(tx_octets);
383 cp->max_tx_time = sys_cpu_to_le16(tx_time);
384
385 send_cmd(BT_HCI_OP_LE_WRITE_DEFAULT_DATA_LEN, buf, NULL);
386 }
387
set_data_len(void)388 static void set_data_len(void)
389 {
390 uint16_t tx_octets, tx_time;
391
392 read_max_data_len(&tx_octets, &tx_time);
393 write_default_data_len(tx_octets, tx_time);
394 }
395
set_event_mask(uint16_t opcode)396 static void set_event_mask(uint16_t opcode)
397 {
398 struct bt_hci_cp_set_event_mask *cp_mask;
399 struct net_buf *buf;
400 uint64_t mask = 0U;
401
402 /* The two commands have the same length/params */
403 buf = bt_hci_cmd_create(opcode, sizeof(*cp_mask));
404 ASSERT(buf, "");
405
406 /* Forward all events */
407 cp_mask = net_buf_add(buf, sizeof(*cp_mask));
408 mask = UINT64_MAX;
409 sys_put_le64(mask, cp_mask->events);
410
411 send_cmd(opcode, buf, NULL);
412 }
413
set_random_address(void)414 static void set_random_address(void)
415 {
416 struct net_buf *buf;
417 bt_addr_le_t addr = {BT_ADDR_LE_RANDOM, {{0x0A, 0x89, 0x67, 0x45, 0x23, 0xC1}}};
418
419 LOG_DBG("%s", bt_addr_str(&addr.a));
420
421 buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_RANDOM_ADDRESS, sizeof(addr.a));
422 ASSERT(buf, "");
423
424 net_buf_add_mem(buf, &addr.a, sizeof(addr.a));
425 send_cmd(BT_HCI_OP_LE_SET_RANDOM_ADDRESS, buf, NULL);
426 }
427
start_adv(void)428 void start_adv(void)
429 {
430 struct bt_hci_cp_le_set_adv_param set_param;
431 struct net_buf *buf;
432 uint16_t interval = 60; /* Interval doesn't matter */
433
434 (void)memset(&set_param, 0, sizeof(set_param));
435
436 set_param.min_interval = sys_cpu_to_le16(interval);
437 set_param.max_interval = sys_cpu_to_le16(interval);
438 set_param.channel_map = 0x07;
439 set_param.filter_policy = BT_LE_ADV_FP_NO_FILTER;
440 set_param.type = BT_HCI_ADV_IND;
441 set_param.own_addr_type = BT_HCI_OWN_ADDR_RANDOM;
442
443 /* configure */
444 buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_ADV_PARAM, sizeof(set_param));
445 net_buf_add_mem(buf, &set_param, sizeof(set_param));
446 send_cmd(BT_HCI_OP_LE_SET_ADV_PARAM, buf, NULL);
447
448 /* start */
449 buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_ADV_ENABLE, 1);
450 net_buf_add_u8(buf, BT_HCI_LE_ADV_ENABLE);
451 send_cmd(BT_HCI_OP_LE_SET_ADV_ENABLE, buf, NULL);
452 }
453
454 NET_BUF_POOL_DEFINE(acl_tx_pool, 5, BT_L2CAP_BUF_SIZE(200), 8, NULL);
455
alloc_l2cap_pdu(void)456 struct net_buf *alloc_l2cap_pdu(void)
457 {
458 struct net_buf *buf;
459 uint16_t reserve;
460
461 buf = net_buf_alloc(&acl_tx_pool, K_FOREVER);
462 ASSERT(buf, "failed ACL allocation");
463
464 reserve = sizeof(struct bt_l2cap_hdr);
465 reserve += sizeof(struct bt_hci_acl_hdr) + BT_BUF_RESERVE;
466
467 net_buf_reserve(buf, reserve);
468
469 return buf;
470 }
471
send_acl(struct net_buf * buf)472 static int send_acl(struct net_buf *buf)
473 {
474 struct bt_hci_acl_hdr *hdr;
475 uint8_t flags = BT_ACL_START_NO_FLUSH;
476
477 hdr = net_buf_push(buf, sizeof(*hdr));
478 hdr->handle = sys_cpu_to_le16(bt_acl_handle_pack(conn_handle, flags));
479 hdr->len = sys_cpu_to_le16(buf->len - sizeof(*hdr));
480
481 bt_buf_set_type(buf, BT_BUF_ACL_OUT);
482
483 k_sem_take(&acl_pkts, K_FOREVER);
484
485 return bt_send(buf);
486 }
487
send_l2cap_packet(struct net_buf * buf,uint16_t cid)488 static void send_l2cap_packet(struct net_buf *buf, uint16_t cid)
489 {
490 struct bt_l2cap_hdr *hdr;
491
492 hdr = net_buf_push(buf, sizeof(*hdr));
493 hdr->len = sys_cpu_to_le16(buf->len - sizeof(*hdr));
494 hdr->cid = sys_cpu_to_le16(cid);
495
496 /* Always entire packets, no HCI fragmentation */
497 ASSERT(buf->len <= CONFIG_BT_BUF_ACL_TX_SIZE,
498 "Fragmentation not supported");
499
500 send_acl(buf);
501 }
502
gatt_notify(void)503 static void gatt_notify(void)
504 {
505 static uint8_t data[] = NOTIFICATION_PAYLOAD;
506 uint16_t handle = HVX_HANDLE;
507 struct net_buf *buf = alloc_l2cap_pdu();
508
509 net_buf_add_u8(buf, BT_ATT_OP_NOTIFY);
510 net_buf_add_le16(buf, handle);
511 net_buf_add_mem(buf, data, sizeof(data));
512
513 LOG_INF("send ATT notification");
514 send_l2cap_packet(buf, BT_L2CAP_CID_ATT);
515 }
516
prepare_controller(void)517 static void prepare_controller(void)
518 {
519 /* Initialize controller */
520 struct net_buf *rsp;
521
522 send_cmd(BT_HCI_OP_RESET, NULL, NULL);
523 send_cmd(BT_HCI_OP_LE_READ_BUFFER_SIZE, NULL, &rsp);
524 le_read_buffer_size_complete(rsp);
525
526 set_data_len();
527 set_event_mask(BT_HCI_OP_SET_EVENT_MASK);
528 set_event_mask(BT_HCI_OP_LE_SET_EVENT_MASK);
529 set_random_address();
530 }
531
init_tinyhost(void)532 static void init_tinyhost(void)
533 {
534 bt_enable_raw(&rx_queue);
535
536 /* Start the RX thread */
537 k_thread_create(&rx_thread_data, rx_thread_stack,
538 K_THREAD_STACK_SIZEOF(rx_thread_stack), rx_thread,
539 NULL, NULL, NULL, K_PRIO_PREEMPT(0), 0, K_NO_WAIT);
540 k_thread_name_set(&rx_thread_data, "HCI RX");
541
542 k_thread_priority_set(k_current_get(), K_PRIO_PREEMPT(0));
543
544 prepare_controller();
545 }
546
test_procedure_0(void)547 void test_procedure_0(void)
548 {
549 ASSERT(backchannel_init() == 0, "Failed to open backchannel\n");
550
551 init_tinyhost();
552
553 /* Start advertising & wait for a connection */
554 start_adv();
555 WAIT_FOR_FLAG(is_connected);
556 LOG_INF("connected");
557
558 /* We need this to be able to send whole L2CAP PDUs on-air. */
559 WAIT_FOR_FLAG(flag_data_length_updated);
560
561 LOG_INF("##################### START TEST #####################");
562
563 for (int n = 0; n < 3; n++) {
564 gatt_notify();
565 }
566
567 /* Wait until DUT starts sleeping */
568 backchannel_sync_wait();
569
570 /* Send some more, so DUT has some more data to process before having to
571 * handle the disconnect.
572 */
573 for (int n = 0; n < 3; n++) {
574 gatt_notify();
575 }
576
577 /* Wait >2 conn events, to be sure at least one more notification makes
578 * it to the other peer before breaking the link.
579 */
580 k_sleep(K_MSEC(50));
581
582 LOG_INF("kill radio");
583 hwll_disconnect_phy();
584
585 /* Pass has to be before the exit() for process to not error out */
586 PASS("Tester exit\n");
587 bs_trace_silent_exit(0);
588 }
589
test_tick(bs_time_t HW_device_time)590 void test_tick(bs_time_t HW_device_time)
591 {
592 bs_trace_debug_time(0, "Simulation ends now.\n");
593 if (bst_result != Passed) {
594 bst_result = Failed;
595 bs_trace_error("Test did not pass before simulation ended.\n");
596 }
597 }
598
test_init(void)599 void test_init(void)
600 {
601 bst_ticker_set_next_tick_absolute(TEST_TIMEOUT_SIMULATED);
602 bst_result = In_progress;
603 }
604
605 static const struct bst_test_instance test_to_add[] = {
606 {
607 .test_id = "tester",
608 .test_pre_init_f = test_init,
609 .test_tick_f = test_tick,
610 .test_main_f = test_procedure_0,
611 },
612 BSTEST_END_MARKER,
613 };
614
install(struct bst_test_list * tests)615 static struct bst_test_list *install(struct bst_test_list *tests)
616 {
617 return bst_add_tests(tests, test_to_add);
618 };
619
620 bst_test_install_t test_installers[] = {install, NULL};
621
622
main(void)623 int main(void)
624 {
625 bst_main();
626
627 return 0;
628 }
629
