1 /*
2 * Copyright (c) 2016 Intel Corporation.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_REGISTER(net_bt, CONFIG_NET_L2_BT_LOG_LEVEL);
9
10 #include <zephyr/kernel.h>
11 #include <zephyr/toolchain.h>
12 #include <zephyr/linker/sections.h>
13 #include <string.h>
14 #include <errno.h>
15
16 #include <zephyr/device.h>
17 #include <zephyr/init.h>
18
19 #include <zephyr/net/net_pkt.h>
20 #include <zephyr/net/net_core.h>
21 #include <zephyr/net/net_l2.h>
22 #include <zephyr/net/net_if.h>
23 #include <zephyr/net/capture.h>
24 #include <zephyr/net/bt.h>
25 #include <6lo.h>
26
27 #include <zephyr/bluetooth/bluetooth.h>
28 #include <zephyr/bluetooth/hci.h>
29 #include <zephyr/bluetooth/conn.h>
30 #include <zephyr/bluetooth/uuid.h>
31 #include <zephyr/bluetooth/l2cap.h>
32
33 #include "net_private.h"
34 #include "ipv6.h"
35
36 #define BUF_TIMEOUT K_MSEC(50)
37
38 #define L2CAP_IPSP_PSM 0x0023
39 #define L2CAP_IPSP_MTU 1280
40
41 #define CHAN_CONN(_conn) CONTAINER_OF(_conn, struct bt_if_conn, ipsp_chan.chan)
42
43 #if defined(CONFIG_NET_L2_BT_MGMT)
44 static struct bt_conn *default_conn;
45 #endif
46
47 #if defined(CONFIG_NET_L2_BT_SHELL)
48 extern int net_bt_shell_init(void);
49 #else
50 #define net_bt_shell_init(...)
51 #endif
52
53 #if defined(CONFIG_NET_BUF_FIXED_DATA_SIZE)
54 #define IPSP_FRAG_LEN CONFIG_NET_BUF_DATA_SIZE
55 #else
56 #define IPSP_FRAG_LEN L2CAP_IPSP_MTU
57 #endif /* CONFIG_NET_BUF_FIXED_DATA_SIZE */
58
59 struct bt_if_conn {
60 struct net_if *iface;
61 struct bt_l2cap_le_chan ipsp_chan;
62 bt_addr_t src;
63 bt_addr_t dst;
64 };
65
66 struct bt_context {
67 struct bt_if_conn conns[CONFIG_BT_MAX_CONN];
68 };
69
net_bt_recv(struct net_if * iface,struct net_pkt * pkt)70 static enum net_verdict net_bt_recv(struct net_if *iface, struct net_pkt *pkt)
71 {
72 NET_DBG("iface %p pkt %p len %zu", iface, pkt, net_pkt_get_len(pkt));
73
74 if (!net_6lo_uncompress(pkt)) {
75 NET_DBG("Packet decompression failed");
76 return NET_DROP;
77 }
78
79 return NET_CONTINUE;
80 }
81
net_bt_get_conn(struct net_if * iface)82 static struct bt_if_conn *net_bt_get_conn(struct net_if *iface)
83 {
84 struct bt_context *ctxt = net_if_get_device(iface)->data;
85 int i;
86
87 for (i = 0; i < CONFIG_BT_MAX_CONN; i++) {
88 if (ctxt->conns[i].iface == iface) {
89 return &ctxt->conns[i];
90 }
91 }
92
93 return NULL;
94 }
95
net_bt_send(struct net_if * iface,struct net_pkt * pkt)96 static int net_bt_send(struct net_if *iface, struct net_pkt *pkt)
97 {
98 struct bt_if_conn *conn = net_bt_get_conn(iface);
99 struct net_buf *buffer;
100 int length;
101 int ret;
102
103 NET_DBG("iface %p pkt %p len %zu", iface, pkt, net_pkt_get_len(pkt));
104
105 /* Only accept IPv6 packets */
106 if (net_pkt_family(pkt) != AF_INET6) {
107 return -EINVAL;
108 }
109
110 ret = net_6lo_compress(pkt, true);
111 if (ret < 0) {
112 NET_DBG("Packet compression failed");
113 return ret;
114 }
115
116 length = net_pkt_get_len(pkt);
117
118 net_capture_pkt(iface, pkt);
119
120 /* Detach data fragments for packet */
121 buffer = pkt->buffer;
122 pkt->buffer = NULL;
123
124 ret = bt_l2cap_chan_send(&conn->ipsp_chan.chan, buffer);
125 if (ret < 0) {
126 NET_ERR("Unable to send packet: %d", ret);
127 bt_l2cap_chan_disconnect(&conn->ipsp_chan.chan);
128 net_buf_unref(buffer);
129 return ret;
130 }
131
132 net_pkt_unref(pkt);
133
134 return length;
135 }
136
net_bt_enable(struct net_if * iface,bool state)137 static int net_bt_enable(struct net_if *iface, bool state)
138 {
139 NET_DBG("iface %p %s", iface, state ? "up" : "down");
140
141 return 0;
142 }
143
net_bt_flags(struct net_if * iface)144 static enum net_l2_flags net_bt_flags(struct net_if *iface)
145 {
146 /* TODO: add NET_L2_MULTICAST_SKIP_JOIN_SOLICIT_NODE once the stack
147 * supports Address Registration Option for neighbor discovery.
148 */
149 return NET_L2_MULTICAST;
150 }
151
152 NET_L2_INIT(BLUETOOTH_L2, net_bt_recv, net_bt_send,
153 net_bt_enable, net_bt_flags);
154
ipsp_connected(struct bt_l2cap_chan * chan)155 static void ipsp_connected(struct bt_l2cap_chan *chan)
156 {
157 struct bt_if_conn *conn = CHAN_CONN(chan);
158 struct bt_conn_info info;
159 struct net_linkaddr ll;
160 struct in6_addr in6;
161
162 if (bt_conn_get_info(chan->conn, &info) < 0) {
163 NET_ERR("Unable to get connection info");
164 bt_l2cap_chan_disconnect(chan);
165 return;
166 }
167
168 if (CONFIG_NET_L2_BT_LOG_LEVEL >= LOG_LEVEL_DBG) {
169 char src[BT_ADDR_LE_STR_LEN];
170 char dst[BT_ADDR_LE_STR_LEN];
171
172 bt_addr_le_to_str(info.le.src, src, sizeof(src));
173 bt_addr_le_to_str(info.le.dst, dst, sizeof(dst));
174
175 NET_DBG("Channel %p Source %s connected to Destination %s",
176 chan, src, dst);
177 }
178
179 /* Swap bytes since net APIs expect big endian address */
180 sys_memcpy_swap(conn->src.val, info.le.src->a.val, sizeof(conn->src));
181 sys_memcpy_swap(conn->dst.val, info.le.dst->a.val, sizeof(conn->dst));
182
183 net_if_set_link_addr(conn->iface, conn->src.val, sizeof(conn->src.val),
184 NET_LINK_BLUETOOTH);
185
186 ll.addr = conn->dst.val;
187 ll.len = sizeof(conn->dst.val);
188 ll.type = NET_LINK_BLUETOOTH;
189
190 /* Add remote link-local address to the nbr cache to avoid sending ns:
191 * https://tools.ietf.org/html/rfc7668#section-3.2.3
192 * A Bluetooth LE 6LN MUST NOT register its link-local address.
193 */
194 net_ipv6_addr_create_iid(&in6, &ll);
195 net_ipv6_nbr_add(conn->iface, &in6, &ll, false,
196 NET_IPV6_NBR_STATE_STATIC);
197
198 /* Leave dormant state (iface goes up if set to admin up) */
199 net_if_dormant_off(conn->iface);
200 }
201
ipsp_disconnected(struct bt_l2cap_chan * chan)202 static void ipsp_disconnected(struct bt_l2cap_chan *chan)
203 {
204 struct bt_if_conn *conn = CHAN_CONN(chan);
205
206 NET_DBG("Channel %p disconnected", chan);
207
208 /* Enter dormant state (iface goes down) */
209 net_if_dormant_on(conn->iface);
210
211 #if defined(CONFIG_NET_L2_BT_MGMT)
212 if (chan->conn != default_conn) {
213 return;
214 }
215
216 bt_conn_unref(default_conn);
217 default_conn = NULL;
218 #endif
219 }
220
ipsp_recv(struct bt_l2cap_chan * chan,struct net_buf * buf)221 static int ipsp_recv(struct bt_l2cap_chan *chan, struct net_buf *buf)
222 {
223 struct bt_if_conn *conn = CHAN_CONN(chan);
224 struct net_pkt *pkt;
225
226 NET_DBG("Incoming data channel %p len %zu", chan,
227 net_buf_frags_len(buf));
228
229 /* Get packet for bearer / protocol related data */
230 pkt = net_pkt_rx_alloc_on_iface(conn->iface, BUF_TIMEOUT);
231 if (!pkt) {
232 return -ENOMEM;
233 }
234
235 /* Set destination address */
236 net_pkt_lladdr_dst(pkt)->addr = conn->src.val;
237 net_pkt_lladdr_dst(pkt)->len = sizeof(conn->src);
238 net_pkt_lladdr_dst(pkt)->type = NET_LINK_BLUETOOTH;
239
240 /* Set source address */
241 net_pkt_lladdr_src(pkt)->addr = conn->dst.val;
242 net_pkt_lladdr_src(pkt)->len = sizeof(conn->dst);
243 net_pkt_lladdr_src(pkt)->type = NET_LINK_BLUETOOTH;
244
245 /* Add data buffer as fragment of RX buffer, take a reference while
246 * doing so since L2CAP will unref the buffer after return.
247 */
248 net_pkt_append_buffer(pkt, net_buf_ref(buf));
249
250 if (net_recv_data(conn->iface, pkt) < 0) {
251 NET_DBG("Packet dropped by NET stack");
252 net_pkt_unref(pkt);
253 }
254
255 return 0;
256 }
257
ipsp_alloc_buf(struct bt_l2cap_chan * chan)258 static struct net_buf *ipsp_alloc_buf(struct bt_l2cap_chan *chan)
259 {
260 NET_DBG("Channel %p requires buffer", chan);
261
262 return net_pkt_get_reserve_rx_data(IPSP_FRAG_LEN, BUF_TIMEOUT);
263 }
264
265 static const struct bt_l2cap_chan_ops ipsp_ops = {
266 .alloc_buf = ipsp_alloc_buf,
267 .recv = ipsp_recv,
268 .connected = ipsp_connected,
269 .disconnected = ipsp_disconnected,
270 };
271
272 static struct bt_context bt_context_data = {
273 .conns[0 ... (CONFIG_BT_MAX_CONN - 1)] = {
274 .iface = NULL,
275 .ipsp_chan.chan.ops = &ipsp_ops,
276 .ipsp_chan.rx.mtu = L2CAP_IPSP_MTU,
277 }
278 };
279
bt_iface_init(struct net_if * iface)280 static void bt_iface_init(struct net_if *iface)
281 {
282 struct bt_context *ctxt = net_if_get_device(iface)->data;
283 struct bt_if_conn *conn = NULL;
284 int i;
285
286 NET_DBG("iface %p", iface);
287
288 /* Find unused slot to store the iface */
289 for (i = 0; i < CONFIG_BT_MAX_CONN; i++) {
290 if (!ctxt->conns[i].iface) {
291 conn = &ctxt->conns[i];
292 NET_DBG("[%d] alloc ctxt %p iface %p", i, ctxt, iface);
293 break;
294 }
295 }
296
297 if (!conn) {
298 NET_ERR("Unable to allocate iface");
299 return;
300 }
301
302 conn->iface = iface;
303
304 net_if_dormant_on(iface);
305
306 #if defined(CONFIG_NET_L2_BT_ZEP1656)
307 /* Workaround Linux bug, see:
308 * https://github.com/zephyrproject-rtos/zephyr/issues/3111
309 */
310 net_if_flag_set(iface, NET_IF_POINTOPOINT);
311 #endif
312 }
313
314 static struct net_if_api bt_if_api = {
315 .init = bt_iface_init,
316 };
317
ipsp_accept(struct bt_conn * conn,struct bt_l2cap_server * server,struct bt_l2cap_chan ** chan)318 static int ipsp_accept(struct bt_conn *conn, struct bt_l2cap_server *server,
319 struct bt_l2cap_chan **chan)
320 {
321 struct bt_if_conn *if_conn = NULL;
322 int i;
323
324 NET_DBG("Incoming conn %p", (void *)conn);
325
326 /* Find unused slot to store the iface */
327 for (i = 0; i < CONFIG_BT_MAX_CONN; i++) {
328 if (bt_context_data.conns[i].iface &&
329 !bt_context_data.conns[i].ipsp_chan.chan.conn) {
330 if_conn = &bt_context_data.conns[i];
331 break;
332 }
333 }
334
335 if (!if_conn) {
336 NET_ERR("No channels available");
337 return -ENOMEM;
338 }
339
340 *chan = &if_conn->ipsp_chan.chan;
341
342 return 0;
343 }
344
345 static struct bt_l2cap_server server = {
346 .psm = L2CAP_IPSP_PSM,
347 .sec_level = CONFIG_NET_L2_BT_SEC_LEVEL,
348 .accept = ipsp_accept,
349 };
350
351 #if defined(CONFIG_NET_L2_BT_MGMT)
352
353 #define DEVICE_NAME CONFIG_BT_DEVICE_NAME
354 #define DEVICE_NAME_LEN (sizeof(DEVICE_NAME) - 1)
355 #define UNKNOWN_APPEARANCE 0x0000
356
357 static const struct bt_data ad[] = {
358 BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
359 BT_DATA_BYTES(BT_DATA_UUID16_ALL, BT_UUID_16_ENCODE(BT_UUID_IPSS_VAL)),
360 };
361
362 static const struct bt_data sd[] = {
363 BT_DATA(BT_DATA_NAME_COMPLETE, DEVICE_NAME, DEVICE_NAME_LEN),
364 };
365
bt_advertise(uint32_t mgmt_request,struct net_if * iface,void * data,size_t len)366 static int bt_advertise(uint32_t mgmt_request, struct net_if *iface, void *data,
367 size_t len)
368 {
369 if (!strcmp(data, "on")) {
370 return bt_le_adv_start(BT_LE_ADV_CONN, ad, ARRAY_SIZE(ad),
371 sd, ARRAY_SIZE(sd));
372 } else if (!strcmp(data, "off")) {
373 return bt_le_adv_stop();
374 } else {
375 return -EINVAL;
376 }
377
378 return 0;
379 }
380
bt_connect(uint32_t mgmt_request,struct net_if * iface,void * data,size_t len)381 static int bt_connect(uint32_t mgmt_request, struct net_if *iface, void *data,
382 size_t len)
383 {
384 struct bt_if_conn *conn = net_bt_get_conn(iface);
385 bt_addr_le_t *addr = data;
386
387 if (len != sizeof(*addr)) {
388 NET_ERR("Invalid address");
389 return -EINVAL;
390 }
391
392 if (conn->ipsp_chan.chan.conn) {
393 NET_ERR("No channels available");
394 return -ENOMEM;
395 }
396
397 if (default_conn) {
398 return bt_l2cap_chan_connect(default_conn,
399 &conn->ipsp_chan.chan,
400 L2CAP_IPSP_PSM);
401 }
402
403 return bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN,
404 BT_LE_CONN_PARAM_DEFAULT, &default_conn);
405 }
406
eir_found(uint8_t type,const uint8_t * data,uint8_t data_len,void * user_data)407 static bool eir_found(uint8_t type, const uint8_t *data, uint8_t data_len,
408 void *user_data)
409 {
410 int i;
411
412 if (type != BT_DATA_UUID16_SOME && type != BT_DATA_UUID16_ALL) {
413 return false;
414 }
415
416 if (data_len % sizeof(uint16_t) != 0U) {
417 NET_ERR("AD malformed\n");
418 return false;
419 }
420
421 for (i = 0; i < data_len; i += sizeof(uint16_t)) {
422 struct bt_uuid *uuid;
423 uint16_t u16;
424
425 memcpy(&u16, &data[i], sizeof(u16));
426 uuid = BT_UUID_DECLARE_16(sys_le16_to_cpu(u16));
427 if (bt_uuid_cmp(uuid, BT_UUID_IPSS)) {
428 continue;
429 }
430
431 if (CONFIG_NET_L2_BT_LOG_LEVEL >= LOG_LEVEL_DBG) {
432 bt_addr_le_t *addr = user_data;
433 char dev[BT_ADDR_LE_STR_LEN];
434
435 bt_addr_le_to_str(addr, dev, sizeof(dev));
436 NET_DBG("[DEVICE]: %s", dev);
437 }
438
439 /* TODO: Notify device address found */
440 net_mgmt_event_notify(NET_EVENT_BT_SCAN_RESULT,
441 bt_context_data.conns[0].iface);
442
443 return true;
444 }
445
446 return false;
447 }
448
ad_parse(struct net_buf_simple * ad_buf,bool (* func)(uint8_t type,const uint8_t * data,uint8_t data_len,void * user_data),void * user_data)449 static bool ad_parse(struct net_buf_simple *ad_buf,
450 bool (*func)(uint8_t type, const uint8_t *data,
451 uint8_t data_len, void *user_data),
452 void *user_data)
453 {
454 while (ad_buf->len > 1) {
455 uint8_t len = net_buf_simple_pull_u8(ad_buf);
456 uint8_t type;
457
458 /* Check for early termination */
459 if (len == 0U) {
460 return false;
461 }
462
463 if (len > ad_buf->len) {
464 NET_ERR("AD malformed\n");
465 return false;
466 }
467
468 type = net_buf_simple_pull_u8(ad_buf);
469
470 if (func(type, ad_buf->data, len - 1, user_data)) {
471 return true;
472 }
473
474 net_buf_simple_pull(ad_buf, len - 1);
475 }
476
477 return false;
478 }
479
device_found(const bt_addr_le_t * addr,int8_t rssi,uint8_t type,struct net_buf_simple * ad_buf)480 static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
481 struct net_buf_simple *ad_buf)
482 {
483 /* We're only interested in connectable events */
484 if (type == BT_GAP_ADV_TYPE_ADV_IND ||
485 type == BT_GAP_ADV_TYPE_ADV_DIRECT_IND) {
486 ad_parse(ad_buf, eir_found, (void *)addr);
487 }
488 }
489
bt_active_scan(void)490 static void bt_active_scan(void)
491 {
492 int err;
493
494 err = bt_le_scan_start(BT_LE_SCAN_ACTIVE, device_found);
495 if (err) {
496 NET_ERR("Bluetooth set active scan failed (err %d)\n", err);
497 }
498 }
499
bt_passive_scan(void)500 static void bt_passive_scan(void)
501 {
502 int err;
503
504 err = bt_le_scan_start(BT_LE_SCAN_PASSIVE, device_found);
505 if (err) {
506 NET_ERR("Bluetooth set passive scan failed (err %d)\n", err);
507 }
508 }
509
bt_scan_off(void)510 static void bt_scan_off(void)
511 {
512 int err;
513
514 err = bt_le_scan_stop();
515 if (err) {
516 NET_ERR("Stopping scanning failed (err %d)\n", err);
517 }
518 }
519
bt_scan(uint32_t mgmt_request,struct net_if * iface,void * data,size_t len)520 static int bt_scan(uint32_t mgmt_request, struct net_if *iface, void *data,
521 size_t len)
522 {
523 if (!strcmp(data, "on") || !strcmp(data, "active")) {
524 bt_active_scan();
525 } else if (!strcmp(data, "passive")) {
526 bt_passive_scan();
527 } else if (!strcmp("off", data)) {
528 bt_scan_off();
529 } else {
530 return -EINVAL;
531 }
532
533 return 0;
534 }
535
bt_disconnect(uint32_t mgmt_request,struct net_if * iface,void * data,size_t len)536 static int bt_disconnect(uint32_t mgmt_request, struct net_if *iface,
537 void *data, size_t len)
538 {
539 struct bt_if_conn *conn = net_bt_get_conn(iface);
540
541 if (!conn->ipsp_chan.chan.conn) {
542 NET_ERR("Not connected");
543 return -ENOTCONN;
544 }
545
546 /* Release connect reference in case of central/router role */
547 if (default_conn) {
548 bt_conn_unref(default_conn);
549 default_conn = NULL;
550 }
551
552 return bt_l2cap_chan_disconnect(&conn->ipsp_chan.chan);
553 }
554
connected(struct bt_conn * conn,uint8_t err)555 static void connected(struct bt_conn *conn, uint8_t err)
556 {
557 int i;
558
559 if (err) {
560 if (CONFIG_NET_L2_BT_LOG_LEVEL >= LOG_LEVEL_DBG) {
561 char addr[BT_ADDR_LE_STR_LEN];
562
563 bt_addr_le_to_str(bt_conn_get_dst(conn), addr,
564 sizeof(addr));
565
566 NET_ERR("Failed to connect to %s (%u)\n",
567 addr, err);
568 }
569
570 return;
571 }
572
573 if (conn != default_conn) {
574 return;
575 }
576
577 for (i = 0; i < CONFIG_BT_MAX_CONN; i++) {
578 struct bt_if_conn *if_conn = &bt_context_data.conns[i];
579
580 if (if_conn->ipsp_chan.chan.conn == conn) {
581 bt_l2cap_chan_connect(conn, &if_conn->ipsp_chan.chan,
582 L2CAP_IPSP_PSM);
583 break;
584 }
585 }
586 }
587
disconnected(struct bt_conn * conn,uint8_t reason)588 static void disconnected(struct bt_conn *conn, uint8_t reason)
589 {
590 if (conn != default_conn) {
591 return;
592 }
593
594 if (CONFIG_NET_L2_BT_LOG_LEVEL >= LOG_LEVEL_DBG) {
595 char addr[BT_ADDR_LE_STR_LEN];
596
597 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
598
599 NET_DBG("Disconnected: %s (reason 0x%02x)\n",
600 addr, reason);
601 }
602
603 bt_conn_unref(default_conn);
604 default_conn = NULL;
605 }
606
607 BT_CONN_CB_DEFINE(conn_callbacks) = {
608 .connected = connected,
609 .disconnected = disconnected,
610 };
611 #endif /* CONFIG_NET_L2_BT_MGMT */
612
net_bt_init(const struct device * dev)613 static int net_bt_init(const struct device *dev)
614 {
615 int err;
616
617 NET_DBG("dev %p driver_data %p", dev, dev->data);
618
619 err = bt_l2cap_server_register(&server);
620 if (err) {
621 return err;
622 }
623
624 net_bt_shell_init();
625
626 return 0;
627 }
628
629 #if defined(CONFIG_NET_L2_BT_MGMT)
630 NET_MGMT_REGISTER_REQUEST_HANDLER(NET_REQUEST_BT_ADVERTISE, bt_advertise);
631 NET_MGMT_REGISTER_REQUEST_HANDLER(NET_REQUEST_BT_CONNECT, bt_connect);
632 NET_MGMT_REGISTER_REQUEST_HANDLER(NET_REQUEST_BT_SCAN, bt_scan);
633 NET_MGMT_REGISTER_REQUEST_HANDLER(NET_REQUEST_BT_DISCONNECT, bt_disconnect);
634 #endif
635
636 DEVICE_DEFINE(net_bt, "net_bt", net_bt_init, NULL, &bt_context_data, NULL,
637 POST_KERNEL, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT, &bt_if_api);
638 NET_L2_DATA_INIT(net_bt, 0, NET_L2_GET_CTX_TYPE(BLUETOOTH_L2));
639 NET_IF_INIT(net_bt, 0, BLUETOOTH_L2, L2CAP_IPSP_MTU, CONFIG_BT_MAX_CONN);
640
