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/hci_driver.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 BTPROTO_HCI 1
40 struct sockaddr_hci {
41 sa_family_t hci_family;
42 unsigned short hci_dev;
43 unsigned short hci_channel;
44 };
45 #define HCI_CHANNEL_USER 1
46
47 #define SOL_HCI 0
48
49 /* Bluetooth spec v5.4 Vol 4, Part A Table 2.1 */
50 #define H4_CMD 0x01
51 #define H4_ACL 0x02
52 #define H4_SCO 0x03
53 #define H4_EVT 0x04
54 #define H4_ISO 0x05
55
56 static K_KERNEL_STACK_DEFINE(rx_thread_stack,
57 CONFIG_ARCH_POSIX_RECOMMENDED_STACK_SIZE);
58 static struct k_thread rx_thread_data;
59
60 static int uc_fd = -1;
61
62 static unsigned short bt_dev_index;
63
64 #define TCP_ADDR_BUFF_SIZE 16
65 static bool hci_socket;
66 static char ip_addr[TCP_ADDR_BUFF_SIZE];
67 static unsigned int port;
68 static bool arg_found;
69
get_rx(const uint8_t * buf)70 static struct net_buf *get_rx(const uint8_t *buf)
71 {
72 bool discardable = false;
73 k_timeout_t timeout = K_FOREVER;
74
75 switch (buf[0]) {
76 case H4_EVT:
77 if (buf[1] == BT_HCI_EVT_LE_META_EVENT &&
78 (buf[3] == BT_HCI_EVT_LE_ADVERTISING_REPORT)) {
79 discardable = true;
80 timeout = K_NO_WAIT;
81 }
82
83 return bt_buf_get_evt(buf[1], discardable, timeout);
84 case H4_ACL:
85 return bt_buf_get_rx(BT_BUF_ACL_IN, K_FOREVER);
86 case H4_ISO:
87 if (IS_ENABLED(CONFIG_BT_ISO)) {
88 return bt_buf_get_rx(BT_BUF_ISO_IN, K_FOREVER);
89 }
90 __fallthrough;
91 default:
92 LOG_ERR("Unknown packet type: %u", buf[0]);
93 }
94
95 return NULL;
96 }
97
98 /**
99 * @brief Decode the length of an HCI H4 packet
100 * @details Decodes packet length according to Bluetooth spec v5.4 Vol 4 Part E
101 * @param buf Pointer to a HCI packet buffer
102 * @return Length of the HCI packet in bytes, zero if no valid packet found.
103 */
packet_len(const uint8_t * buf)104 static uint16_t packet_len(const uint8_t *buf)
105 {
106 uint16_t payload_len = 0;
107 uint8_t header_len = 0;
108 const uint8_t type = buf[0];
109 const uint8_t *hdr = &buf[sizeof(type)];
110
111 switch (type) {
112 case H4_CMD: {
113 const struct bt_hci_cmd_hdr *cmd = (const struct bt_hci_cmd_hdr *)hdr;
114
115 /* Parameter Total Length */
116 payload_len = cmd->param_len;
117 header_len = BT_HCI_CMD_HDR_SIZE;
118 break;
119 }
120 case H4_ACL: {
121 const struct bt_hci_acl_hdr *acl = (const struct bt_hci_acl_hdr *)hdr;
122
123 /* Data Total Length */
124 payload_len = sys_le16_to_cpu(acl->len);
125 header_len = BT_HCI_ACL_HDR_SIZE;
126 break;
127 }
128 case H4_SCO: {
129 const struct bt_hci_sco_hdr *sco = (const struct bt_hci_sco_hdr *)hdr;
130
131 /* Data_Total_Length */
132 payload_len = sco->len;
133 header_len = BT_HCI_SCO_HDR_SIZE;
134 break;
135 }
136 case H4_EVT: {
137 const struct bt_hci_evt_hdr *evt = (const struct bt_hci_evt_hdr *)hdr;
138
139 /* Parameter Total Length */
140 payload_len = evt->len;
141 header_len = BT_HCI_EVT_HDR_SIZE;
142 break;
143 }
144 case H4_ISO: {
145 const struct bt_hci_iso_hdr *iso = (const struct bt_hci_iso_hdr *)hdr;
146
147 /* ISO_Data_Load_Length parameter */
148 payload_len = bt_iso_hdr_len(sys_le16_to_cpu(iso->len));
149 header_len = BT_HCI_ISO_HDR_SIZE;
150 break;
151 }
152 /* If no valid packet type found */
153 default:
154 LOG_WRN("Unknown packet type 0x%02x", type);
155 return 0;
156 }
157
158 return sizeof(type) + header_len + payload_len;
159 }
160
uc_ready(void)161 static bool uc_ready(void)
162 {
163 struct pollfd pollfd = { .fd = uc_fd, .events = POLLIN };
164
165 return (poll(&pollfd, 1, 0) == 1);
166 }
167
rx_thread(void * p1,void * p2,void * p3)168 static void rx_thread(void *p1, void *p2, void *p3)
169 {
170 ARG_UNUSED(p1);
171 ARG_UNUSED(p2);
172 ARG_UNUSED(p3);
173
174 LOG_DBG("started");
175
176 while (1) {
177 static uint8_t frame[512];
178 struct net_buf *buf;
179 size_t buf_tailroom;
180 size_t buf_add_len;
181 ssize_t len;
182 const uint8_t *frame_start = frame;
183
184 if (!uc_ready()) {
185 k_sleep(K_MSEC(1));
186 continue;
187 }
188
189 LOG_DBG("calling read()");
190
191 len = read(uc_fd, frame, sizeof(frame));
192 if (len < 0) {
193 if (errno == EINTR) {
194 k_yield();
195 continue;
196 }
197
198 LOG_ERR("Reading socket failed, errno %d", errno);
199 close(uc_fd);
200 uc_fd = -1;
201 return;
202 }
203
204 while (len > 0) {
205
206 const uint8_t packet_type = frame_start[0];
207 const uint16_t decoded_len = packet_len(frame_start);
208
209 if (decoded_len == 0) {
210 LOG_ERR("HCI Packet type is invalid, length could not be decoded");
211 break;
212 }
213
214 if (decoded_len > len) {
215 LOG_ERR("Decoded HCI packet length (%d bytes) is greater "
216 "than buffer length (%d bytes)", decoded_len, len);
217 break;
218 }
219
220 buf = get_rx(frame_start);
221 if (!buf) {
222 LOG_DBG("Discard adv report due to insufficient buf");
223 continue;
224 }
225
226 buf_tailroom = net_buf_tailroom(buf);
227 buf_add_len = decoded_len - sizeof(packet_type);
228 if (buf_tailroom < buf_add_len) {
229 LOG_ERR("Not enough space in buffer %zu/%zu",
230 buf_add_len, buf_tailroom);
231 net_buf_unref(buf);
232 continue;
233 }
234
235 net_buf_add_mem(buf, frame_start + sizeof(packet_type), buf_add_len);
236
237 LOG_DBG("Calling bt_recv(%p)", buf);
238
239 bt_recv(buf);
240 len -= decoded_len;
241 frame_start += decoded_len;
242 }
243
244 k_yield();
245 }
246 }
247
uc_send(struct net_buf * buf)248 static int uc_send(struct net_buf *buf)
249 {
250 LOG_DBG("buf %p type %u len %u", buf, bt_buf_get_type(buf), buf->len);
251
252 if (uc_fd < 0) {
253 LOG_ERR("User channel not open");
254 return -EIO;
255 }
256
257 switch (bt_buf_get_type(buf)) {
258 case BT_BUF_ACL_OUT:
259 net_buf_push_u8(buf, H4_ACL);
260 break;
261 case BT_BUF_CMD:
262 net_buf_push_u8(buf, H4_CMD);
263 break;
264 case BT_BUF_ISO_OUT:
265 if (IS_ENABLED(CONFIG_BT_ISO)) {
266 net_buf_push_u8(buf, H4_ISO);
267 break;
268 }
269 __fallthrough;
270 default:
271 LOG_ERR("Unknown buffer type");
272 return -EINVAL;
273 }
274
275 if (write(uc_fd, buf->data, buf->len) < 0) {
276 return -errno;
277 }
278
279 net_buf_unref(buf);
280 return 0;
281 }
282
user_chan_open(void)283 static int user_chan_open(void)
284 {
285 int fd;
286
287 if (hci_socket) {
288 struct sockaddr_hci addr;
289
290 fd = socket(PF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC | SOCK_NONBLOCK,
291 BTPROTO_HCI);
292 if (fd < 0) {
293 return -errno;
294 }
295
296 (void)memset(&addr, 0, sizeof(addr));
297 addr.hci_family = AF_BLUETOOTH;
298 addr.hci_dev = bt_dev_index;
299 addr.hci_channel = HCI_CHANNEL_USER;
300
301 if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
302 int err = -errno;
303
304 close(fd);
305 return err;
306 }
307 } else {
308 struct sockaddr_in addr;
309
310 fd = socket(AF_INET, SOCK_STREAM, 0);
311 if (fd < 0) {
312 return -errno;
313 }
314
315 addr.sin_family = AF_INET;
316 addr.sin_port = htons(port);
317 if (inet_pton(AF_INET, ip_addr, &(addr.sin_addr)) <= 0) {
318 int err = -errno;
319
320 close(fd);
321 return err;
322 }
323
324 if (connect(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
325 int err = -errno;
326
327 close(fd);
328 return err;
329 }
330 }
331
332 return fd;
333 }
334
uc_open(void)335 static int uc_open(void)
336 {
337 if (hci_socket) {
338 LOG_DBG("hci%d", bt_dev_index);
339 } else {
340 LOG_DBG("hci %s:%d", ip_addr, port);
341 }
342
343
344 uc_fd = user_chan_open();
345 if (uc_fd < 0) {
346 return uc_fd;
347 }
348
349 LOG_DBG("User Channel opened as fd %d", uc_fd);
350
351 k_thread_create(&rx_thread_data, rx_thread_stack,
352 K_KERNEL_STACK_SIZEOF(rx_thread_stack),
353 rx_thread, NULL, NULL, NULL,
354 K_PRIO_COOP(CONFIG_BT_DRIVER_RX_HIGH_PRIO),
355 0, K_NO_WAIT);
356
357 LOG_DBG("returning");
358
359 return 0;
360 }
361
362 static const struct bt_hci_driver drv = {
363 .name = "HCI User Channel",
364 .bus = BT_HCI_DRIVER_BUS_UART,
365 .open = uc_open,
366 .send = uc_send,
367 };
368
bt_uc_init(void)369 static int bt_uc_init(void)
370 {
371
372 bt_hci_driver_register(&drv);
373
374 return 0;
375 }
376
377 SYS_INIT(bt_uc_init, POST_KERNEL, CONFIG_KERNEL_INIT_PRIORITY_DEVICE);
378
cmd_bt_dev_found(char * argv,int offset)379 static void cmd_bt_dev_found(char *argv, int offset)
380 {
381 arg_found = true;
382 if (strncmp(&argv[offset], "hci", 3) == 0 && strlen(&argv[offset]) >= 4) {
383 long arg_hci_idx = strtol(&argv[offset + 3], NULL, 10);
384
385 if (arg_hci_idx >= 0 && arg_hci_idx <= USHRT_MAX) {
386 bt_dev_index = arg_hci_idx;
387 hci_socket = true;
388 } else {
389 posix_print_error_and_exit("Invalid argument value for --bt-dev. "
390 "hci idx must be within range 0 to 65536.\n");
391 }
392 } else if (sscanf(&argv[offset], "%15[^:]:%d", ip_addr, &port) == 2) {
393 if (port > USHRT_MAX) {
394 posix_print_error_and_exit("Error: IP port for bluetooth "
395 "hci tcp server is out of range.\n");
396 }
397 struct in_addr addr;
398
399 if (inet_pton(AF_INET, ip_addr, &addr) != 1) {
400 posix_print_error_and_exit("Error: IP address for bluetooth "
401 "hci tcp server is incorrect.\n");
402 }
403 } else {
404 posix_print_error_and_exit("Invalid option %s for --bt-dev. "
405 "An hci interface or hci tcp server is expected.\n",
406 &argv[offset]);
407 }
408 }
409
add_btuserchan_arg(void)410 static void add_btuserchan_arg(void)
411 {
412 static struct args_struct_t btuserchan_args[] = {
413 /*
414 * Fields:
415 * manual, mandatory, switch,
416 * option_name, var_name ,type,
417 * destination, callback,
418 * description
419 */
420 { false, true, false,
421 "bt-dev", "hciX", 's',
422 NULL, cmd_bt_dev_found,
423 "A local HCI device to be used for Bluetooth (e.g. hci0) "
424 "or an HCI TCP Server (e.g. 127.0.0.1:9000)"},
425 ARG_TABLE_ENDMARKER
426 };
427
428 native_add_command_line_opts(btuserchan_args);
429 }
430
btuserchan_check_arg(void)431 static void btuserchan_check_arg(void)
432 {
433 if (!arg_found) {
434 posix_print_error_and_exit("Error: Bluetooth device missing.\n"
435 "Specify either a local hci interface --bt-dev=hciN\n"
436 "or a valid hci tcp server --bt-dev=ip_address:port\n");
437 }
438 }
439
440 NATIVE_TASK(add_btuserchan_arg, PRE_BOOT_1, 10);
441 NATIVE_TASK(btuserchan_check_arg, PRE_BOOT_2, 10);
442
