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