1 /* userchan.c - HCI User Channel based Bluetooth driver */
2
3 /*
4 * Copyright (c) 2018 Intel Corporation
5 * Copyright (c) 2023 Victor Chavez
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #include <zephyr/kernel.h>
10 #include <zephyr/device.h>
11 #include <zephyr/init.h>
12 #include <zephyr/sys/util.h>
13
14 #include <errno.h>
15 #include <stddef.h>
16 #include <stdlib.h>
17 #include <poll.h>
18 #include <errno.h>
19 #include <sys/socket.h>
20 #include <string.h>
21 #include <unistd.h>
22 #include <stdio.h>
23 #include <limits.h>
24 #include <netinet/in.h>
25 #include <arpa/inet.h>
26 #include <zephyr/sys/byteorder.h>
27
28 #include "soc.h"
29 #include "cmdline.h" /* native_posix command line options header */
30
31 #include <zephyr/bluetooth/bluetooth.h>
32 #include <zephyr/bluetooth/hci.h>
33 #include <zephyr/drivers/bluetooth.h>
34
35 #define LOG_LEVEL CONFIG_BT_HCI_DRIVER_LOG_LEVEL
36 #include <zephyr/logging/log.h>
37 LOG_MODULE_REGISTER(bt_driver);
38
39 #define DT_DRV_COMPAT zephyr_bt_hci_userchan
40
41 struct uc_data {
42 int fd;
43 bt_hci_recv_t recv;
44
45 };
46
47 #define BTPROTO_HCI 1
48 struct sockaddr_hci {
49 sa_family_t hci_family;
50 unsigned short hci_dev;
51 unsigned short hci_channel;
52 };
53 #define HCI_CHANNEL_USER 1
54
55 #define SOL_HCI 0
56
57 static K_KERNEL_STACK_DEFINE(rx_thread_stack,
58 CONFIG_ARCH_POSIX_RECOMMENDED_STACK_SIZE);
59 static struct k_thread rx_thread_data;
60
61 static unsigned short bt_dev_index;
62
63 #define TCP_ADDR_BUFF_SIZE 16
64 static bool hci_socket;
65 static char ip_addr[TCP_ADDR_BUFF_SIZE];
66 static unsigned int port;
67 static bool arg_found;
68
get_rx(const uint8_t * buf)69 static struct net_buf *get_rx(const uint8_t *buf)
70 {
71 bool discardable = false;
72 k_timeout_t timeout = K_FOREVER;
73
74 switch (buf[0]) {
75 case BT_HCI_H4_EVT:
76 if (buf[1] == BT_HCI_EVT_LE_META_EVENT &&
77 (buf[3] == BT_HCI_EVT_LE_ADVERTISING_REPORT)) {
78 discardable = true;
79 timeout = K_NO_WAIT;
80 }
81
82 return bt_buf_get_evt(buf[1], discardable, timeout);
83 case BT_HCI_H4_ACL:
84 return bt_buf_get_rx(BT_BUF_ACL_IN, K_FOREVER);
85 case BT_HCI_H4_ISO:
86 if (IS_ENABLED(CONFIG_BT_ISO)) {
87 return bt_buf_get_rx(BT_BUF_ISO_IN, K_FOREVER);
88 }
89 __fallthrough;
90 default:
91 LOG_ERR("Unknown packet type: %u", buf[0]);
92 }
93
94 return NULL;
95 }
96
97 /**
98 * @brief Decode the length of an HCI H4 packet and check it's complete
99 * @details Decodes packet length according to Bluetooth spec v5.4 Vol 4 Part E
100 * @param buf Pointer to a HCI packet buffer
101 * @param buf_len Bytes available in the buffer
102 * @return Length of the complete HCI packet in bytes, -1 if cannot find an HCI
103 * packet, 0 if more data required.
104 */
hci_packet_complete(const uint8_t * buf,uint16_t buf_len)105 static int32_t hci_packet_complete(const uint8_t *buf, uint16_t buf_len)
106 {
107 uint16_t payload_len = 0;
108 const uint8_t type = buf[0];
109 uint8_t header_len = sizeof(type);
110 const uint8_t *hdr = &buf[sizeof(type)];
111
112 switch (type) {
113 case BT_HCI_H4_CMD: {
114 if (buf_len < header_len + BT_HCI_CMD_HDR_SIZE) {
115 return 0;
116 }
117 const struct bt_hci_cmd_hdr *cmd = (const struct bt_hci_cmd_hdr *)hdr;
118
119 /* Parameter Total Length */
120 payload_len = cmd->param_len;
121 header_len += BT_HCI_CMD_HDR_SIZE;
122 break;
123 }
124 case BT_HCI_H4_ACL: {
125 if (buf_len < header_len + BT_HCI_ACL_HDR_SIZE) {
126 return 0;
127 }
128 const struct bt_hci_acl_hdr *acl = (const struct bt_hci_acl_hdr *)hdr;
129
130 /* Data Total Length */
131 payload_len = sys_le16_to_cpu(acl->len);
132 header_len += BT_HCI_ACL_HDR_SIZE;
133 break;
134 }
135 case BT_HCI_H4_SCO: {
136 if (buf_len < header_len + BT_HCI_SCO_HDR_SIZE) {
137 return 0;
138 }
139 const struct bt_hci_sco_hdr *sco = (const struct bt_hci_sco_hdr *)hdr;
140
141 /* Data_Total_Length */
142 payload_len = sco->len;
143 header_len += BT_HCI_SCO_HDR_SIZE;
144 break;
145 }
146 case BT_HCI_H4_EVT: {
147 if (buf_len < header_len + BT_HCI_EVT_HDR_SIZE) {
148 return 0;
149 }
150 const struct bt_hci_evt_hdr *evt = (const struct bt_hci_evt_hdr *)hdr;
151
152 /* Parameter Total Length */
153 payload_len = evt->len;
154 header_len += BT_HCI_EVT_HDR_SIZE;
155 break;
156 }
157 case BT_HCI_H4_ISO: {
158 if (buf_len < header_len + BT_HCI_ISO_HDR_SIZE) {
159 return 0;
160 }
161 const struct bt_hci_iso_hdr *iso = (const struct bt_hci_iso_hdr *)hdr;
162
163 /* ISO_Data_Load_Length parameter */
164 payload_len = bt_iso_hdr_len(sys_le16_to_cpu(iso->len));
165 header_len += BT_HCI_ISO_HDR_SIZE;
166 break;
167 }
168 /* If no valid packet type found */
169 default:
170 LOG_WRN("Unknown packet type 0x%02x", type);
171 return -1;
172 }
173
174 /* Request more data */
175 if (buf_len < header_len + payload_len) {
176 return 0;
177 }
178
179 return (int32_t)header_len + payload_len;
180 }
181
uc_ready(int fd)182 static bool uc_ready(int fd)
183 {
184 struct pollfd pollfd = { .fd = fd, .events = POLLIN };
185
186 return (poll(&pollfd, 1, 0) == 1);
187 }
188
rx_thread(void * p1,void * p2,void * p3)189 static void rx_thread(void *p1, void *p2, void *p3)
190 {
191 const struct device *dev = p1;
192 struct uc_data *uc = dev->data;
193
194 ARG_UNUSED(p2);
195 ARG_UNUSED(p3);
196
197 LOG_DBG("started");
198
199 ssize_t frame_size = 0;
200
201 while (1) {
202 static uint8_t frame[512];
203 struct net_buf *buf;
204 size_t buf_tailroom;
205 size_t buf_add_len;
206 ssize_t len;
207 const uint8_t *frame_start = frame;
208
209 if (!uc_ready(uc->fd)) {
210 k_sleep(K_MSEC(1));
211 continue;
212 }
213
214 LOG_DBG("calling read()");
215
216 len = read(uc->fd, frame + frame_size, sizeof(frame) - frame_size);
217 if (len < 0) {
218 if (errno == EINTR) {
219 k_yield();
220 continue;
221 }
222
223 LOG_ERR("Reading socket failed, errno %d", errno);
224 close(uc->fd);
225 uc->fd = -1;
226 return;
227 }
228
229 frame_size += len;
230
231 while (frame_size > 0) {
232 const uint8_t *buf_add;
233 const uint8_t packet_type = frame_start[0];
234 const int32_t decoded_len = hci_packet_complete(frame_start, frame_size);
235
236 if (decoded_len == -1) {
237 LOG_ERR("HCI Packet type is invalid, length could not be decoded");
238 frame_size = 0; /* Drop buffer */
239 break;
240 }
241
242 if (decoded_len == 0) {
243 if (frame_size == sizeof(frame)) {
244 LOG_ERR("HCI Packet (%d bytes) is too big for frame (%d "
245 "bytes)",
246 decoded_len, sizeof(frame));
247 frame_size = 0; /* Drop buffer */
248 break;
249 }
250 if (frame_start != frame) {
251 memmove(frame, frame_start, frame_size);
252 }
253 /* Read more */
254 break;
255 }
256
257 buf_add = frame_start + sizeof(packet_type);
258 buf_add_len = decoded_len - sizeof(packet_type);
259
260 buf = get_rx(frame_start);
261
262 frame_size -= decoded_len;
263 frame_start += decoded_len;
264
265 if (!buf) {
266 LOG_DBG("Discard adv report due to insufficient buf");
267 continue;
268 }
269
270 buf_tailroom = net_buf_tailroom(buf);
271 if (buf_tailroom < buf_add_len) {
272 LOG_ERR("Not enough space in buffer %zu/%zu",
273 buf_add_len, buf_tailroom);
274 net_buf_unref(buf);
275 continue;
276 }
277
278 net_buf_add_mem(buf, buf_add, buf_add_len);
279
280 LOG_DBG("Calling bt_recv(%p)", buf);
281
282 uc->recv(dev, buf);
283 }
284
285 k_yield();
286 }
287 }
288
uc_send(const struct device * dev,struct net_buf * buf)289 static int uc_send(const struct device *dev, struct net_buf *buf)
290 {
291 struct uc_data *uc = dev->data;
292
293 LOG_DBG("buf %p type %u len %u", buf, bt_buf_get_type(buf), buf->len);
294
295 if (uc->fd < 0) {
296 LOG_ERR("User channel not open");
297 return -EIO;
298 }
299
300 switch (bt_buf_get_type(buf)) {
301 case BT_BUF_ACL_OUT:
302 net_buf_push_u8(buf, BT_HCI_H4_ACL);
303 break;
304 case BT_BUF_CMD:
305 net_buf_push_u8(buf, BT_HCI_H4_CMD);
306 break;
307 case BT_BUF_ISO_OUT:
308 if (IS_ENABLED(CONFIG_BT_ISO)) {
309 net_buf_push_u8(buf, BT_HCI_H4_ISO);
310 break;
311 }
312 __fallthrough;
313 default:
314 LOG_ERR("Unknown buffer type");
315 return -EINVAL;
316 }
317
318 if (write(uc->fd, buf->data, buf->len) < 0) {
319 return -errno;
320 }
321
322 net_buf_unref(buf);
323 return 0;
324 }
325
user_chan_open(void)326 static int user_chan_open(void)
327 {
328 int fd;
329
330 if (hci_socket) {
331 struct sockaddr_hci addr;
332
333 fd = socket(PF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC | SOCK_NONBLOCK,
334 BTPROTO_HCI);
335 if (fd < 0) {
336 return -errno;
337 }
338
339 (void)memset(&addr, 0, sizeof(addr));
340 addr.hci_family = AF_BLUETOOTH;
341 addr.hci_dev = bt_dev_index;
342 addr.hci_channel = HCI_CHANNEL_USER;
343
344 if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
345 int err = -errno;
346
347 close(fd);
348 return err;
349 }
350 } else {
351 struct sockaddr_in addr;
352
353 fd = socket(AF_INET, SOCK_STREAM, 0);
354 if (fd < 0) {
355 return -errno;
356 }
357
358 addr.sin_family = AF_INET;
359 addr.sin_port = htons(port);
360 if (inet_pton(AF_INET, ip_addr, &(addr.sin_addr)) <= 0) {
361 int err = -errno;
362
363 close(fd);
364 return err;
365 }
366
367 if (connect(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
368 int err = -errno;
369
370 close(fd);
371 return err;
372 }
373 }
374
375 return fd;
376 }
377
uc_open(const struct device * dev,bt_hci_recv_t recv)378 static int uc_open(const struct device *dev, bt_hci_recv_t recv)
379 {
380 struct uc_data *uc = dev->data;
381
382 if (hci_socket) {
383 LOG_DBG("hci%d", bt_dev_index);
384 } else {
385 LOG_DBG("hci %s:%d", ip_addr, port);
386 }
387
388 uc->fd = user_chan_open();
389 if (uc->fd < 0) {
390 return uc->fd;
391 }
392
393 uc->recv = recv;
394
395 LOG_DBG("User Channel opened as fd %d", uc->fd);
396
397 k_thread_create(&rx_thread_data, rx_thread_stack,
398 K_KERNEL_STACK_SIZEOF(rx_thread_stack),
399 rx_thread, (void *)dev, NULL, NULL,
400 K_PRIO_COOP(CONFIG_BT_DRIVER_RX_HIGH_PRIO),
401 0, K_NO_WAIT);
402
403 LOG_DBG("returning");
404
405 return 0;
406 }
407
408 static DEVICE_API(bt_hci, uc_drv_api) = {
409 .open = uc_open,
410 .send = uc_send,
411 };
412
uc_init(const struct device * dev)413 static int uc_init(const struct device *dev)
414 {
415 if (!arg_found) {
416 posix_print_warning("Warning: Bluetooth device missing.\n"
417 "Specify either a local hci interface --bt-dev=hciN\n"
418 "or a valid hci tcp server --bt-dev=ip_address:port\n");
419 return -ENODEV;
420 }
421
422 return 0;
423 }
424
425 #define UC_DEVICE_INIT(inst) \
426 static struct uc_data uc_data_##inst = { \
427 .fd = -1, \
428 }; \
429 DEVICE_DT_INST_DEFINE(inst, uc_init, NULL, &uc_data_##inst, NULL, \
430 POST_KERNEL, CONFIG_KERNEL_INIT_PRIORITY_DEVICE, &uc_drv_api)
431
DT_INST_FOREACH_STATUS_OKAY(UC_DEVICE_INIT)432 DT_INST_FOREACH_STATUS_OKAY(UC_DEVICE_INIT)
433
434 static void cmd_bt_dev_found(char *argv, int offset)
435 {
436 arg_found = true;
437 if (strncmp(&argv[offset], "hci", 3) == 0 && strlen(&argv[offset]) >= 4) {
438 long arg_hci_idx = strtol(&argv[offset + 3], NULL, 10);
439
440 if (arg_hci_idx >= 0 && arg_hci_idx <= USHRT_MAX) {
441 bt_dev_index = arg_hci_idx;
442 hci_socket = true;
443 } else {
444 posix_print_error_and_exit("Invalid argument value for --bt-dev. "
445 "hci idx must be within range 0 to 65536.\n");
446 }
447 } else if (sscanf(&argv[offset], "%15[^:]:%d", ip_addr, &port) == 2) {
448 if (port > USHRT_MAX) {
449 posix_print_error_and_exit("Error: IP port for bluetooth "
450 "hci tcp server is out of range.\n");
451 }
452 struct in_addr addr;
453
454 if (inet_pton(AF_INET, ip_addr, &addr) != 1) {
455 posix_print_error_and_exit("Error: IP address for bluetooth "
456 "hci tcp server is incorrect.\n");
457 }
458 } else {
459 posix_print_error_and_exit("Invalid option %s for --bt-dev. "
460 "An hci interface or hci tcp server is expected.\n",
461 &argv[offset]);
462 }
463 }
464
add_btuserchan_arg(void)465 static void add_btuserchan_arg(void)
466 {
467 static struct args_struct_t btuserchan_args[] = {
468 /*
469 * Fields:
470 * manual, mandatory, switch,
471 * option_name, var_name ,type,
472 * destination, callback,
473 * description
474 */
475 { false, true, false,
476 "bt-dev", "hciX", 's',
477 NULL, cmd_bt_dev_found,
478 "A local HCI device to be used for Bluetooth (e.g. hci0) "
479 "or an HCI TCP Server (e.g. 127.0.0.1:9000)"},
480 ARG_TABLE_ENDMARKER
481 };
482
483 native_add_command_line_opts(btuserchan_args);
484 }
485
486 NATIVE_TASK(add_btuserchan_arg, PRE_BOOT_1, 10);
487
