1 // SPDX-License-Identifier: LGPL-2.1-or-later
4 * BlueZ - Bluetooth protocol stack for Linux
6 * Copyright (C) 2012-2014 Intel Corporation. All rights reserved.
22 #include <sys/socket.h>
23 #include <sys/ioctl.h>
27 #include "lib/bluetooth.h"
30 #include "monitor/bt.h"
31 #include "emulator/vhci.h"
32 #include "emulator/btdev.h"
33 #include "emulator/bthost.h"
34 #include "src/shared/util.h"
35 #include "src/shared/queue.h"
36 #include "emulator/hciemu.h"
38 struct hciemu_client {
49 enum btdev_type btdev_type;
51 struct queue *clients;
52 struct queue *post_command_hooks;
55 hciemu_debug_func_t debug_callback;
56 hciemu_destroy_func_t debug_destroy;
59 unsigned int flush_id;
62 struct hciemu_command_hook {
63 hciemu_command_func_t function;
67 static void destroy_command_hook(void *data)
69 struct hciemu_command_hook *hook = data;
80 static void run_command_hook(void *data, void *user_data)
82 struct hciemu_command_hook *hook = data;
83 struct run_data *run_data = user_data;
86 hook->function(run_data->opcode, run_data->data,
87 run_data->len, hook->user_data);
90 static void central_command_callback(uint16_t opcode,
91 const void *data, uint8_t len,
92 btdev_callback callback, void *user_data)
94 struct hciemu *hciemu = user_data;
95 struct run_data run_data = { .opcode = opcode,
96 .data = data, .len = len };
98 btdev_command_default(callback);
100 queue_foreach(hciemu->post_command_hooks, run_command_hook, &run_data);
103 static void client_command_callback(uint16_t opcode,
104 const void *data, uint8_t len,
105 btdev_callback callback, void *user_data)
107 btdev_command_default(callback);
110 static void writev_callback(const struct iovec *iov, int iovlen,
113 GIOChannel *channel = user_data;
117 fd = g_io_channel_unix_get_fd(channel);
119 written = writev(fd, iov, iovlen);
124 static gboolean receive_bthost(GIOChannel *channel, GIOCondition condition,
127 struct bthost *bthost = user_data;
128 unsigned char buf[4096];
132 if (condition & (G_IO_NVAL | G_IO_ERR | G_IO_HUP))
135 fd = g_io_channel_unix_get_fd(channel);
137 len = read(fd, buf, sizeof(buf));
141 bthost_receive_h4(bthost, buf, len);
146 static guint create_source_bthost(int fd, struct bthost *bthost)
151 channel = g_io_channel_unix_new(fd);
153 g_io_channel_set_close_on_unref(channel, TRUE);
154 g_io_channel_set_encoding(channel, NULL, NULL);
155 g_io_channel_set_buffered(channel, FALSE);
157 bthost_set_send_handler(bthost, writev_callback, channel);
159 source = g_io_add_watch_full(channel, G_PRIORITY_DEFAULT,
160 G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
161 receive_bthost, bthost, NULL);
163 g_io_channel_unref(channel);
168 static gboolean receive_btdev(GIOChannel *channel, GIOCondition condition,
171 struct btdev *btdev = user_data;
172 unsigned char buf[4096];
176 if (condition & (G_IO_NVAL | G_IO_ERR | G_IO_HUP))
179 fd = g_io_channel_unix_get_fd(channel);
181 len = read(fd, buf, sizeof(buf));
183 if (errno == EAGAIN || errno == EINTR)
197 btdev_receive_h4(btdev, buf, len);
204 static guint create_source_btdev(int fd, struct btdev *btdev)
209 channel = g_io_channel_unix_new(fd);
211 g_io_channel_set_close_on_unref(channel, TRUE);
212 g_io_channel_set_encoding(channel, NULL, NULL);
213 g_io_channel_set_buffered(channel, FALSE);
215 btdev_set_send_handler(btdev, writev_callback, channel);
217 source = g_io_add_watch_full(channel, G_PRIORITY_DEFAULT,
218 G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
219 receive_btdev, btdev, NULL);
221 g_io_channel_unref(channel);
226 static bool create_vhci(struct hciemu *hciemu)
230 vhci = vhci_open(hciemu->btdev_type);
234 btdev_set_command_handler(vhci_get_btdev(vhci),
235 central_command_callback, hciemu);
241 struct vhci *hciemu_get_vhci(struct hciemu *hciemu)
249 struct hciemu_client *hciemu_get_client(struct hciemu *hciemu, int num)
251 const struct queue_entry *entry;
256 for (entry = queue_get_entries(hciemu->clients); entry;
257 entry = entry->next, num--) {
265 struct bthost *hciemu_client_host(struct hciemu_client *client)
273 struct bthost *hciemu_client_get_host(struct hciemu *hciemu)
275 struct hciemu_client *client;
280 client = hciemu_get_client(hciemu, 0);
282 return hciemu_client_host(client);
285 static gboolean start_host(gpointer user_data)
287 struct hciemu_client *client = user_data;
289 client->start_source = 0;
291 bthost_start(client->host);
296 static void hciemu_client_destroy(void *data)
298 struct hciemu_client *client = data;
300 if (client->start_source)
301 g_source_remove(client->start_source);
303 g_source_remove(client->host_source);
304 g_source_remove(client->source);
306 bthost_destroy(client->host);
307 btdev_destroy(client->dev);
312 static struct hciemu_client *hciemu_client_new(struct hciemu *hciemu,
315 struct hciemu_client *client;
318 client = new0(struct hciemu_client, 1);
322 client->dev = btdev_create(hciemu->btdev_type, id++);
328 client->host = bthost_create();
330 btdev_destroy(client->dev);
335 btdev_set_command_handler(client->dev, client_command_callback, client);
337 if (socketpair(AF_UNIX, SOCK_SEQPACKET | SOCK_NONBLOCK | SOCK_CLOEXEC,
339 bthost_destroy(client->host);
340 btdev_destroy(client->dev);
344 client->sock[0] = sv[0];
345 client->sock[1] = sv[1];
347 client->source = create_source_btdev(sv[0], client->dev);
348 client->host_source = create_source_bthost(sv[1], client->host);
349 client->start_source = g_idle_add(start_host, client);
354 struct hciemu *hciemu_new_num(enum hciemu_type type, uint8_t num)
357 struct hciemu *hciemu;
363 hciemu = new0(struct hciemu, 1);
368 case HCIEMU_TYPE_BREDRLE:
369 hciemu->btdev_type = BTDEV_TYPE_BREDRLE;
371 case HCIEMU_TYPE_BREDR:
372 hciemu->btdev_type = BTDEV_TYPE_BREDR;
375 hciemu->btdev_type = BTDEV_TYPE_LE;
377 case HCIEMU_TYPE_LEGACY:
378 hciemu->btdev_type = BTDEV_TYPE_BREDR20;
380 case HCIEMU_TYPE_BREDRLE50:
381 hciemu->btdev_type = BTDEV_TYPE_BREDRLE50;
383 case HCIEMU_TYPE_BREDRLE52:
384 hciemu->btdev_type = BTDEV_TYPE_BREDRLE52;
390 hciemu->post_command_hooks = queue_new();
391 if (!hciemu->post_command_hooks) {
396 if (!create_vhci(hciemu)) {
397 queue_destroy(hciemu->post_command_hooks, NULL);
402 hciemu->clients = queue_new();
404 for (i = 0; i < num; i++) {
405 struct hciemu_client *client = hciemu_client_new(hciemu, i);
408 queue_destroy(hciemu->clients, hciemu_client_destroy);
412 queue_push_tail(hciemu->clients, client);
415 return hciemu_ref(hciemu);
418 struct hciemu *hciemu_new(enum hciemu_type type)
420 return hciemu_new_num(type, 1);
423 struct hciemu *hciemu_ref(struct hciemu *hciemu)
428 __sync_fetch_and_add(&hciemu->ref_count, 1);
433 void hciemu_unref(struct hciemu *hciemu)
438 if (__sync_sub_and_fetch(&hciemu->ref_count, 1))
441 queue_destroy(hciemu->post_command_hooks, destroy_command_hook);
442 queue_destroy(hciemu->clients, hciemu_client_destroy);
444 if (hciemu->flush_id)
445 g_source_remove(hciemu->flush_id);
447 vhci_close(hciemu->vhci);
452 static void bthost_print(const char *str, void *user_data)
454 struct hciemu *hciemu = user_data;
456 util_debug(hciemu->debug_callback, hciemu->debug_data,
460 static void vhci_debug(const char *str, void *user_data)
462 struct hciemu *hciemu = user_data;
464 util_debug(hciemu->debug_callback, hciemu->debug_data,
468 static void btdev_client_debug(const char *str, void *user_data)
470 struct hciemu *hciemu = user_data;
472 util_debug(hciemu->debug_callback, hciemu->debug_data,
476 static void hciemu_client_set_debug(void *data, void *user_data)
478 struct hciemu_client *client = data;
479 struct hciemu *hciemu = user_data;
481 btdev_set_debug(client->dev, btdev_client_debug, hciemu, NULL);
482 bthost_set_debug(client->host, bthost_print, hciemu, NULL);
485 bool hciemu_set_debug(struct hciemu *hciemu, hciemu_debug_func_t callback,
486 void *user_data, hciemu_destroy_func_t destroy)
491 if (hciemu->debug_destroy)
492 hciemu->debug_destroy(hciemu->debug_data);
494 hciemu->debug_callback = callback;
495 hciemu->debug_destroy = destroy;
496 hciemu->debug_data = user_data;
498 vhci_set_debug(hciemu->vhci, vhci_debug, hciemu, NULL);
500 queue_foreach(hciemu->clients, hciemu_client_set_debug, hciemu);
505 const char *hciemu_get_address(struct hciemu *hciemu)
510 if (!hciemu || !hciemu->vhci)
513 dev = vhci_get_btdev(hciemu->vhci);
517 addr = btdev_get_bdaddr(dev);
518 sprintf(hciemu->bdaddr_str, "%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X",
519 addr[5], addr[4], addr[3], addr[2], addr[1], addr[0]);
520 return hciemu->bdaddr_str;
523 uint8_t *hciemu_get_features(struct hciemu *hciemu)
527 if (!hciemu || !hciemu->vhci)
530 dev = vhci_get_btdev(hciemu->vhci);
534 return btdev_get_features(dev);
537 const uint8_t *hciemu_get_central_bdaddr(struct hciemu *hciemu)
541 if (!hciemu || !hciemu->vhci)
544 dev = vhci_get_btdev(hciemu->vhci);
548 return btdev_get_bdaddr(dev);
551 const uint8_t *hciemu_client_bdaddr(struct hciemu_client *client)
556 return btdev_get_bdaddr(client->dev);
559 const uint8_t *hciemu_get_client_bdaddr(struct hciemu *hciemu)
561 struct hciemu_client *client;
566 client = hciemu_get_client(hciemu, 0);
568 return hciemu_client_bdaddr(client);
571 uint8_t hciemu_get_central_scan_enable(struct hciemu *hciemu)
575 if (!hciemu || !hciemu->vhci)
578 dev = vhci_get_btdev(hciemu->vhci);
582 return btdev_get_scan_enable(dev);
585 uint8_t hciemu_get_central_le_scan_enable(struct hciemu *hciemu)
589 if (!hciemu || !hciemu->vhci)
592 dev = vhci_get_btdev(hciemu->vhci);
596 return btdev_get_le_scan_enable(dev);
599 void hciemu_set_central_le_states(struct hciemu *hciemu,
600 const uint8_t *le_states)
604 if (!hciemu || !hciemu->vhci)
607 dev = vhci_get_btdev(hciemu->vhci);
611 btdev_set_le_states(dev, le_states);
614 void hciemu_set_central_le_al_len(struct hciemu *hciemu, uint8_t len)
618 if (!hciemu || !hciemu->vhci)
621 dev = vhci_get_btdev(hciemu->vhci);
625 btdev_set_al_len(dev, len);
628 void hciemu_set_central_le_rl_len(struct hciemu *hciemu, uint8_t len)
632 if (!hciemu || !hciemu->vhci)
635 dev = vhci_get_btdev(hciemu->vhci);
639 btdev_set_rl_len(dev, len);
642 const uint8_t *hciemu_get_central_adv_addr(struct hciemu *hciemu,
647 if (!hciemu || !hciemu->vhci)
650 dev = vhci_get_btdev(hciemu->vhci);
654 return btdev_get_adv_addr(dev, handle);
657 bool hciemu_add_central_post_command_hook(struct hciemu *hciemu,
658 hciemu_command_func_t function, void *user_data)
660 struct hciemu_command_hook *hook;
665 hook = new0(struct hciemu_command_hook, 1);
669 hook->function = function;
670 hook->user_data = user_data;
672 if (!queue_push_tail(hciemu->post_command_hooks, hook)) {
680 bool hciemu_clear_central_post_command_hooks(struct hciemu *hciemu)
685 queue_remove_all(hciemu->post_command_hooks,
686 NULL, NULL, destroy_command_hook);
690 int hciemu_add_hook(struct hciemu *hciemu, enum hciemu_hook_type type,
691 uint16_t opcode, hciemu_hook_func_t function,
694 enum btdev_hook_type hook_type;
697 if (!hciemu || !hciemu->vhci)
700 dev = vhci_get_btdev(hciemu->vhci);
705 case HCIEMU_HOOK_PRE_CMD:
706 hook_type = BTDEV_HOOK_PRE_CMD;
708 case HCIEMU_HOOK_POST_CMD:
709 hook_type = BTDEV_HOOK_POST_CMD;
711 case HCIEMU_HOOK_PRE_EVT:
712 hook_type = BTDEV_HOOK_PRE_EVT;
714 case HCIEMU_HOOK_POST_EVT:
715 hook_type = BTDEV_HOOK_POST_EVT;
721 return btdev_add_hook(dev, hook_type, opcode, function, user_data);
724 bool hciemu_del_hook(struct hciemu *hciemu, enum hciemu_hook_type type,
727 enum btdev_hook_type hook_type;
730 if (!hciemu || !hciemu->vhci)
733 dev = vhci_get_btdev(hciemu->vhci);
738 case HCIEMU_HOOK_PRE_CMD:
739 hook_type = BTDEV_HOOK_PRE_CMD;
741 case HCIEMU_HOOK_POST_CMD:
742 hook_type = BTDEV_HOOK_POST_CMD;
744 case HCIEMU_HOOK_PRE_EVT:
745 hook_type = BTDEV_HOOK_PRE_EVT;
747 case HCIEMU_HOOK_POST_EVT:
748 hook_type = BTDEV_HOOK_POST_EVT;
754 return btdev_del_hook(dev, hook_type, opcode);
757 static bool client_is_pending(const void *data, const void *match_data)
759 struct hciemu_client *client = (struct hciemu_client *)data;
762 if (!client->source || !client->host_source)
765 for (i = 0; i < 2; ++i) {
766 if (!ioctl(client->sock[i], TIOCOUTQ, &used) && used > 0)
768 if (!ioctl(client->sock[i], TIOCINQ, &used) && used > 0)
775 static gboolean flush_client_events(gpointer user_data)
777 struct hciemu *hciemu = user_data;
779 if (queue_find(hciemu->clients, client_is_pending, NULL))
782 hciemu->flush_id = 0;
784 util_debug(hciemu->debug_callback, hciemu->debug_data, "vhci: resume");
786 vhci_pause_input(hciemu->vhci, false);
791 void hciemu_flush_client_events(struct hciemu *hciemu)
793 if (hciemu->flush_id || !hciemu->vhci)
796 util_debug(hciemu->debug_callback, hciemu->debug_data, "vhci: pause");
797 vhci_pause_input(hciemu->vhci, true);
798 hciemu->flush_id = g_idle_add(flush_client_events, hciemu);