1 /* l2cap.c - Bluetooth L2CAP Tester */
2
3 /*
4 * Copyright (c) 2016 Intel Corporation
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #include <zephyr/bluetooth/bluetooth.h>
10
11 #include <errno.h>
12 #include <zephyr/bluetooth/l2cap.h>
13 #include <zephyr/bluetooth/att.h>
14 #include <zephyr/sys/byteorder.h>
15
16 #include <zephyr/logging/log.h>
17 #define LOG_MODULE_NAME bttester_l2cap
18 LOG_MODULE_REGISTER(LOG_MODULE_NAME, CONFIG_BTTESTER_LOG_LEVEL);
19
20 #include "btp/btp.h"
21
22 #define DATA_MTU_INITIAL 128
23 #define DATA_MTU 256
24 #define DATA_BUF_SIZE BT_L2CAP_SDU_BUF_SIZE(DATA_MTU)
25 #define CHANNELS 2
26 #define SERVERS 1
27
28 NET_BUF_POOL_FIXED_DEFINE(data_pool, CHANNELS, DATA_BUF_SIZE, 8, NULL);
29
30 static bool authorize_flag;
31 static uint8_t req_keysize;
32
33 static struct channel {
34 uint8_t chan_id; /* Internal number that identifies L2CAP channel. */
35 struct bt_l2cap_le_chan le;
36 bool in_use;
37 bool hold_credit;
38 struct net_buf *pending_credit;
39 } channels[CHANNELS];
40
41 /* TODO Extend to support multiple servers */
42 static struct bt_l2cap_server servers[SERVERS];
43
alloc_buf_cb(struct bt_l2cap_chan * chan)44 static struct net_buf *alloc_buf_cb(struct bt_l2cap_chan *chan)
45 {
46 return net_buf_alloc(&data_pool, K_FOREVER);
47 }
48
49 static uint8_t recv_cb_buf[DATA_BUF_SIZE + sizeof(struct btp_l2cap_data_received_ev)];
50
recv_cb(struct bt_l2cap_chan * l2cap_chan,struct net_buf * buf)51 static int recv_cb(struct bt_l2cap_chan *l2cap_chan, struct net_buf *buf)
52 {
53 struct btp_l2cap_data_received_ev *ev = (void *) recv_cb_buf;
54 struct bt_l2cap_le_chan *l2cap_le_chan = CONTAINER_OF(
55 l2cap_chan, struct bt_l2cap_le_chan, chan);
56 struct channel *chan = CONTAINER_OF(l2cap_le_chan, struct channel, le);
57
58 ev->chan_id = chan->chan_id;
59 ev->data_length = sys_cpu_to_le16(buf->len);
60 memcpy(ev->data, buf->data, buf->len);
61
62 tester_event(BTP_SERVICE_ID_L2CAP, BTP_L2CAP_EV_DATA_RECEIVED,
63 recv_cb_buf, sizeof(*ev) + buf->len);
64
65 if (chan->hold_credit && !chan->pending_credit) {
66 /* no need for extra ref, as when returning EINPROGRESS user
67 * becomes owner of the netbuf
68 */
69 chan->pending_credit = buf;
70 return -EINPROGRESS;
71 }
72
73 return 0;
74 }
75
connected_cb(struct bt_l2cap_chan * l2cap_chan)76 static void connected_cb(struct bt_l2cap_chan *l2cap_chan)
77 {
78 struct btp_l2cap_connected_ev ev;
79 struct bt_l2cap_le_chan *l2cap_le_chan = CONTAINER_OF(
80 l2cap_chan, struct bt_l2cap_le_chan, chan);
81 struct channel *chan = CONTAINER_OF(l2cap_le_chan, struct channel, le);
82 struct bt_conn_info info;
83
84 ev.chan_id = chan->chan_id;
85 /* TODO: ev.psm */
86 if (!bt_conn_get_info(l2cap_chan->conn, &info)) {
87 switch (info.type) {
88 case BT_CONN_TYPE_LE:
89 ev.mtu_remote = sys_cpu_to_le16(chan->le.tx.mtu);
90 ev.mps_remote = sys_cpu_to_le16(chan->le.tx.mps);
91 ev.mtu_local = sys_cpu_to_le16(chan->le.rx.mtu);
92 ev.mps_local = sys_cpu_to_le16(chan->le.rx.mps);
93 bt_addr_le_copy(&ev.address, info.le.dst);
94 break;
95 case BT_CONN_TYPE_BR:
96 default:
97 /* TODO figure out how (if) want to handle BR/EDR */
98 return;
99 }
100 }
101
102 tester_event(BTP_SERVICE_ID_L2CAP, BTP_L2CAP_EV_CONNECTED, &ev, sizeof(ev));
103 }
104
disconnected_cb(struct bt_l2cap_chan * l2cap_chan)105 static void disconnected_cb(struct bt_l2cap_chan *l2cap_chan)
106 {
107 struct btp_l2cap_disconnected_ev ev;
108 struct bt_l2cap_le_chan *l2cap_le_chan = CONTAINER_OF(
109 l2cap_chan, struct bt_l2cap_le_chan, chan);
110 struct channel *chan = CONTAINER_OF(l2cap_le_chan, struct channel, le);
111 struct bt_conn_info info;
112
113 /* release netbuf on premature disconnection */
114 if (chan->pending_credit) {
115 net_buf_unref(chan->pending_credit);
116 chan->pending_credit = NULL;
117 }
118
119 (void)memset(&ev, 0, sizeof(struct btp_l2cap_disconnected_ev));
120
121 /* TODO: ev.result */
122 ev.chan_id = chan->chan_id;
123 /* TODO: ev.psm */
124 if (!bt_conn_get_info(l2cap_chan->conn, &info)) {
125 switch (info.type) {
126 case BT_CONN_TYPE_LE:
127 bt_addr_le_copy(&ev.address, info.le.dst);
128 break;
129 case BT_CONN_TYPE_BR:
130 default:
131 /* TODO figure out how (if) want to handle BR/EDR */
132 return;
133 }
134 }
135
136 chan->in_use = false;
137
138 tester_event(BTP_SERVICE_ID_L2CAP, BTP_L2CAP_EV_DISCONNECTED, &ev, sizeof(ev));
139 }
140
141 #if defined(CONFIG_BT_L2CAP_ECRED)
reconfigured_cb(struct bt_l2cap_chan * l2cap_chan)142 static void reconfigured_cb(struct bt_l2cap_chan *l2cap_chan)
143 {
144 struct btp_l2cap_reconfigured_ev ev;
145 struct bt_l2cap_le_chan *l2cap_le_chan = CONTAINER_OF(
146 l2cap_chan, struct bt_l2cap_le_chan, chan);
147 struct channel *chan = CONTAINER_OF(l2cap_le_chan, struct channel, le);
148
149 (void)memset(&ev, 0, sizeof(ev));
150
151 ev.chan_id = chan->chan_id;
152 ev.mtu_remote = sys_cpu_to_le16(chan->le.tx.mtu);
153 ev.mps_remote = sys_cpu_to_le16(chan->le.tx.mps);
154 ev.mtu_local = sys_cpu_to_le16(chan->le.rx.mtu);
155 ev.mps_local = sys_cpu_to_le16(chan->le.rx.mps);
156
157 tester_event(BTP_SERVICE_ID_L2CAP, BTP_L2CAP_EV_RECONFIGURED, &ev, sizeof(ev));
158 }
159 #endif
160
161 static const struct bt_l2cap_chan_ops l2cap_ops = {
162 .alloc_buf = alloc_buf_cb,
163 .recv = recv_cb,
164 .connected = connected_cb,
165 .disconnected = disconnected_cb,
166 #if defined(CONFIG_BT_L2CAP_ECRED)
167 .reconfigured = reconfigured_cb,
168 #endif
169 };
170
get_free_channel()171 static struct channel *get_free_channel()
172 {
173 uint8_t i;
174 struct channel *chan;
175
176 for (i = 0U; i < CHANNELS; i++) {
177 if (channels[i].in_use) {
178 continue;
179 }
180
181 chan = &channels[i];
182
183 (void)memset(chan, 0, sizeof(*chan));
184 chan->chan_id = i;
185
186 channels[i].in_use = true;
187
188 return chan;
189 }
190
191 return NULL;
192 }
193
connect(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)194 static uint8_t connect(const void *cmd, uint16_t cmd_len,
195 void *rsp, uint16_t *rsp_len)
196 {
197 const struct btp_l2cap_connect_cmd *cp = cmd;
198 struct btp_l2cap_connect_rp *rp = rsp;
199 struct bt_conn *conn;
200 struct channel *chan = NULL;
201 struct bt_l2cap_chan *allocated_channels[5] = {};
202 uint16_t mtu = sys_le16_to_cpu(cp->mtu);
203 uint16_t psm = sys_le16_to_cpu(cp->psm);
204 uint8_t i = 0;
205 bool ecfc = cp->options & BTP_L2CAP_CONNECT_OPT_ECFC;
206 int err;
207
208 if (cp->num == 0 || cp->num > CHANNELS || mtu > DATA_MTU_INITIAL) {
209 return BTP_STATUS_FAILED;
210 }
211
212 conn = bt_conn_lookup_addr_le(BT_ID_DEFAULT, &cp->address);
213 if (!conn) {
214 return BTP_STATUS_FAILED;
215 }
216
217 for (i = 0U; i < cp->num; i++) {
218 chan = get_free_channel();
219 if (!chan) {
220 goto fail;
221 }
222 chan->le.chan.ops = &l2cap_ops;
223 chan->le.rx.mtu = mtu;
224 rp->chan_id[i] = chan->chan_id;
225 allocated_channels[i] = &chan->le.chan;
226
227 chan->hold_credit = cp->options & BTP_L2CAP_CONNECT_OPT_HOLD_CREDIT;
228 }
229
230 if (cp->num == 1 && !ecfc) {
231 err = bt_l2cap_chan_connect(conn, &chan->le.chan, psm);
232 if (err < 0) {
233 goto fail;
234 }
235 } else if (ecfc) {
236 #if defined(CONFIG_BT_L2CAP_ECRED)
237 err = bt_l2cap_ecred_chan_connect(conn, allocated_channels,
238 psm);
239 if (err < 0) {
240 goto fail;
241 }
242 #else
243 goto fail;
244 #endif
245 } else {
246 LOG_ERR("Invalid 'num' parameter value");
247 goto fail;
248 }
249
250 rp->num = cp->num;
251 *rsp_len = sizeof(*rp) + (rp->num * sizeof(rp->chan_id[0]));
252
253 return BTP_STATUS_SUCCESS;
254
255 fail:
256 for (i = 0U; i < ARRAY_SIZE(allocated_channels); i++) {
257 if (allocated_channels[i]) {
258 channels[BT_L2CAP_LE_CHAN(allocated_channels[i])->ident].in_use = false;
259 }
260 }
261 return BTP_STATUS_FAILED;
262 }
263
disconnect(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)264 static uint8_t disconnect(const void *cmd, uint16_t cmd_len,
265 void *rsp, uint16_t *rsp_len)
266 {
267 const struct btp_l2cap_disconnect_cmd *cp = cmd;
268 struct channel *chan;
269 int err;
270
271 if (cp->chan_id >= CHANNELS) {
272 return BTP_STATUS_FAILED;
273 }
274
275 chan = &channels[cp->chan_id];
276
277 err = bt_l2cap_chan_disconnect(&chan->le.chan);
278 if (err) {
279 return BTP_STATUS_FAILED;
280 }
281
282 return BTP_STATUS_SUCCESS;
283 }
284
285 #if defined(CONFIG_BT_L2CAP_ECRED)
reconfigure(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)286 static uint8_t reconfigure(const void *cmd, uint16_t cmd_len,
287 void *rsp, uint16_t *rsp_len)
288 {
289 const struct btp_l2cap_reconfigure_cmd *cp = cmd;
290 uint16_t mtu;
291 struct bt_conn *conn;
292 int err;
293 struct bt_l2cap_chan *reconf_channels[CHANNELS + 1] = {};
294
295 if (cmd_len < sizeof(*cp) ||
296 cmd_len != sizeof(*cp) + cp->num) {
297 return BTP_STATUS_FAILED;
298 }
299
300 if (cp->num > CHANNELS) {
301 return BTP_STATUS_FAILED;
302 }
303
304 mtu = sys_le16_to_cpu(cp->mtu);
305 if (mtu > DATA_MTU) {
306 return BTP_STATUS_FAILED;
307 }
308
309 for (int i = 0; i < cp->num; i++) {
310 if (cp->chan_id[i] > CHANNELS) {
311 return BTP_STATUS_FAILED;
312 }
313
314 reconf_channels[i] = &channels[cp->chan_id[i]].le.chan;
315 }
316
317 conn = bt_conn_lookup_addr_le(BT_ID_DEFAULT, &cp->address);
318 if (!conn) {
319 LOG_ERR("Unknown connection");
320 return BTP_STATUS_FAILED;
321 }
322
323 err = bt_l2cap_ecred_chan_reconfigure(reconf_channels, mtu);
324 if (err) {
325 bt_conn_unref(conn);
326 return BTP_STATUS_FAILED;
327 }
328
329 bt_conn_unref(conn);
330 return BTP_STATUS_SUCCESS;
331 }
332 #endif
333
334 #if defined(CONFIG_BT_EATT)
disconnect_eatt_chans(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)335 static uint8_t disconnect_eatt_chans(const void *cmd, uint16_t cmd_len,
336 void *rsp, uint16_t *rsp_len)
337 {
338 const struct btp_l2cap_disconnect_eatt_chans_cmd *cp = cmd;
339 struct bt_conn *conn;
340 int err;
341
342 conn = bt_conn_lookup_addr_le(BT_ID_DEFAULT, &cp->address);
343 if (!conn) {
344 LOG_ERR("Unknown connection");
345 return BTP_STATUS_FAILED;
346 }
347
348 for (int i = 0; i < cp->count; i++) {
349 err = bt_eatt_disconnect_one(conn);
350 if (err) {
351 bt_conn_unref(conn);
352 return BTP_STATUS_FAILED;
353 }
354 }
355
356 bt_conn_unref(conn);
357 return BTP_STATUS_SUCCESS;
358 }
359 #endif
360
361
send_data(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)362 static uint8_t send_data(const void *cmd, uint16_t cmd_len,
363 void *rsp, uint16_t *rsp_len)
364 {
365 const struct btp_l2cap_send_data_cmd *cp = cmd;
366 struct channel *chan;
367 struct net_buf *buf;
368 uint16_t data_len;
369 int ret;
370
371 if (cmd_len < sizeof(*cp) ||
372 cmd_len != sizeof(*cp) + sys_le16_to_cpu(cp->data_len)) {
373 return BTP_STATUS_FAILED;
374 }
375
376 if (cp->chan_id >= CHANNELS) {
377 return BTP_STATUS_FAILED;
378 }
379
380 chan = &channels[cp->chan_id];
381 data_len = sys_le16_to_cpu(cp->data_len);
382
383
384 /* FIXME: For now, fail if data length exceeds buffer length */
385 if (data_len > DATA_MTU) {
386 return BTP_STATUS_FAILED;
387 }
388
389 /* FIXME: For now, fail if data length exceeds remote's L2CAP SDU */
390 if (data_len > chan->le.tx.mtu) {
391 return BTP_STATUS_FAILED;
392 }
393
394 buf = net_buf_alloc(&data_pool, K_FOREVER);
395 net_buf_reserve(buf, BT_L2CAP_SDU_CHAN_SEND_RESERVE);
396
397 net_buf_add_mem(buf, cp->data, data_len);
398 ret = bt_l2cap_chan_send(&chan->le.chan, buf);
399 if (ret < 0) {
400 LOG_ERR("Unable to send data: %d", -ret);
401 net_buf_unref(buf);
402 return BTP_STATUS_FAILED;
403 }
404
405 return BTP_STATUS_SUCCESS;
406 }
407
get_free_server(void)408 static struct bt_l2cap_server *get_free_server(void)
409 {
410 uint8_t i;
411
412 for (i = 0U; i < SERVERS ; i++) {
413 if (servers[i].psm) {
414 continue;
415 }
416
417 return &servers[i];
418 }
419
420 return NULL;
421 }
422
is_free_psm(uint16_t psm)423 static bool is_free_psm(uint16_t psm)
424 {
425 uint8_t i;
426
427 for (i = 0U; i < ARRAY_SIZE(servers); i++) {
428 if (servers[i].psm == psm) {
429 return false;
430 }
431 }
432
433 return true;
434 }
435
accept(struct bt_conn * conn,struct bt_l2cap_server * server,struct bt_l2cap_chan ** l2cap_chan)436 static int accept(struct bt_conn *conn, struct bt_l2cap_server *server,
437 struct bt_l2cap_chan **l2cap_chan)
438 {
439 struct channel *chan;
440
441 if (bt_conn_enc_key_size(conn) < req_keysize) {
442 return -EPERM;
443 }
444
445 if (authorize_flag) {
446 return -EACCES;
447 }
448
449 chan = get_free_channel();
450 if (!chan) {
451 return -ENOMEM;
452 }
453
454 chan->le.chan.ops = &l2cap_ops;
455 chan->le.rx.mtu = DATA_MTU_INITIAL;
456
457 *l2cap_chan = &chan->le.chan;
458
459 return 0;
460 }
461
listen(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)462 static uint8_t listen(const void *cmd, uint16_t cmd_len,
463 void *rsp, uint16_t *rsp_len)
464 {
465 const struct btp_l2cap_listen_cmd *cp = cmd;
466 struct bt_l2cap_server *server;
467 uint16_t psm = sys_le16_to_cpu(cp->psm);
468
469 /* TODO: Handle cmd->transport flag */
470
471 if (psm == 0 || !is_free_psm(psm)) {
472 return BTP_STATUS_FAILED;
473 }
474
475 server = get_free_server();
476 if (!server) {
477 return BTP_STATUS_FAILED;
478 }
479
480 server->accept = accept;
481 server->psm = psm;
482
483 switch (cp->response) {
484 case BTP_L2CAP_CONNECTION_RESPONSE_SUCCESS:
485 break;
486 case BTP_L2CAP_CONNECTION_RESPONSE_INSUFF_ENC_KEY:
487 /* TSPX_psm_encryption_key_size_required */
488 req_keysize = 16;
489 break;
490 case BTP_L2CAP_CONNECTION_RESPONSE_INSUFF_AUTHOR:
491 authorize_flag = true;
492 break;
493 case BTP_L2CAP_CONNECTION_RESPONSE_INSUFF_AUTHEN:
494 server->sec_level = BT_SECURITY_L3;
495 break;
496 case BTP_L2CAP_CONNECTION_RESPONSE_INSUFF_ENCRYPTION:
497 server->sec_level = BT_SECURITY_L2;
498 break;
499 default:
500 return BTP_STATUS_FAILED;
501 }
502
503 if (bt_l2cap_server_register(server) < 0) {
504 server->psm = 0U;
505 return BTP_STATUS_FAILED;
506 }
507
508 return BTP_STATUS_SUCCESS;
509 }
510
credits(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)511 static uint8_t credits(const void *cmd, uint16_t cmd_len,
512 void *rsp, uint16_t *rsp_len)
513 {
514 const struct btp_l2cap_credits_cmd *cp = cmd;
515 struct channel *chan;
516
517 if (cp->chan_id >= CHANNELS) {
518 return BTP_STATUS_FAILED;
519 }
520
521 chan = &channels[cp->chan_id];
522
523 if (!chan->in_use) {
524 return BTP_STATUS_FAILED;
525 }
526
527 if (chan->pending_credit) {
528 if (bt_l2cap_chan_recv_complete(&chan->le.chan,
529 chan->pending_credit) < 0) {
530 return BTP_STATUS_FAILED;
531 }
532
533 chan->pending_credit = NULL;
534 }
535
536 return BTP_STATUS_SUCCESS;
537 }
538
supported_commands(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)539 static uint8_t supported_commands(const void *cmd, uint16_t cmd_len,
540 void *rsp, uint16_t *rsp_len)
541 {
542 struct btp_l2cap_read_supported_commands_rp *rp = rsp;
543
544 /* octet 0 */
545 tester_set_bit(rp->data, BTP_L2CAP_READ_SUPPORTED_COMMANDS);
546 tester_set_bit(rp->data, BTP_L2CAP_CONNECT);
547 tester_set_bit(rp->data, BTP_L2CAP_DISCONNECT);
548 tester_set_bit(rp->data, BTP_L2CAP_SEND_DATA);
549 tester_set_bit(rp->data, BTP_L2CAP_LISTEN);
550 #if defined(CONFIG_BT_L2CAP_ECRED)
551 tester_set_bit(rp->data, BTP_L2CAP_RECONFIGURE);
552 #endif
553 /* octet 1 */
554 tester_set_bit(rp->data, BTP_L2CAP_CREDITS);
555 #if defined(CONFIG_BT_EATT)
556 tester_set_bit(rp->data, BTP_L2CAP_DISCONNECT_EATT_CHANS);
557 #endif
558
559 *rsp_len = sizeof(*rp) + 2;
560
561 return BTP_STATUS_SUCCESS;
562 }
563
564 static const struct btp_handler handlers[] = {
565 {
566 .opcode = BTP_L2CAP_READ_SUPPORTED_COMMANDS,
567 .index = BTP_INDEX_NONE,
568 .expect_len = 0,
569 .func = supported_commands,
570 },
571 {
572 .opcode = BTP_L2CAP_CONNECT,
573 .expect_len = sizeof(struct btp_l2cap_connect_cmd),
574 .func = connect,
575 },
576 {
577 .opcode = BTP_L2CAP_DISCONNECT,
578 .expect_len = sizeof(struct btp_l2cap_disconnect_cmd),
579 .func = disconnect,
580 },
581 {
582 .opcode = BTP_L2CAP_SEND_DATA,
583 .expect_len = BTP_HANDLER_LENGTH_VARIABLE,
584 .func = send_data,
585 },
586 {
587 .opcode = BTP_L2CAP_LISTEN,
588 .expect_len = sizeof(struct btp_l2cap_listen_cmd),
589 .func = listen,
590 },
591 {
592 .opcode = BTP_L2CAP_RECONFIGURE,
593 .expect_len = BTP_HANDLER_LENGTH_VARIABLE,
594 .func = reconfigure,
595 },
596 {
597 .opcode = BTP_L2CAP_CREDITS,
598 .expect_len = sizeof(struct btp_l2cap_credits_cmd),
599 .func = credits,
600 },
601 {
602 .opcode = BTP_L2CAP_DISCONNECT_EATT_CHANS,
603 .expect_len = sizeof(struct btp_l2cap_disconnect_eatt_chans_cmd),
604 .func = disconnect_eatt_chans,
605 },
606 };
607
tester_init_l2cap(void)608 uint8_t tester_init_l2cap(void)
609 {
610 tester_register_command_handlers(BTP_SERVICE_ID_L2CAP, handlers,
611 ARRAY_SIZE(handlers));
612
613 return BTP_STATUS_SUCCESS;
614 }
615
tester_unregister_l2cap(void)616 uint8_t tester_unregister_l2cap(void)
617 {
618 return BTP_STATUS_SUCCESS;
619 }
620
