b9b98fb19876420d988ab6bbcc4c3c7e240b0363
[platform/upstream/bluez.git] / emulator / hciemu.c
1 // SPDX-License-Identifier: LGPL-2.1-or-later
2 /*
3  *
4  *  BlueZ - Bluetooth protocol stack for Linux
5  *
6  *  Copyright (C) 2012-2014  Intel Corporation. All rights reserved.
7  *
8  *
9  */
10
11 #ifdef HAVE_CONFIG_H
12 #include <config.h>
13 #endif
14
15 #include <stdio.h>
16 #include <fcntl.h>
17 #include <unistd.h>
18 #include <stdlib.h>
19 #include <string.h>
20 #include <stdbool.h>
21 #include <errno.h>
22 #include <sys/socket.h>
23 #include <sys/ioctl.h>
24
25 #include <glib.h>
26
27 #include "lib/bluetooth.h"
28 #include "lib/hci.h"
29
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"
37
38 struct hciemu_client {
39         struct bthost *host;
40         struct btdev *dev;
41         guint start_source;
42         guint host_source;
43         guint source;
44         int sock[2];
45 };
46
47 struct hciemu {
48         int ref_count;
49         enum btdev_type btdev_type;
50         struct vhci *vhci;
51         struct queue *clients;
52         struct queue *post_command_hooks;
53         char bdaddr_str[18];
54
55         hciemu_debug_func_t debug_callback;
56         hciemu_destroy_func_t debug_destroy;
57         void *debug_data;
58
59         unsigned int flush_id;
60 };
61
62 struct hciemu_command_hook {
63         hciemu_command_func_t function;
64         void *user_data;
65 };
66
67 static void destroy_command_hook(void *data)
68 {
69         struct hciemu_command_hook *hook = data;
70
71         free(hook);
72 }
73
74 struct run_data {
75         uint16_t opcode;
76         const void *data;
77         uint8_t len;
78 };
79
80 static void run_command_hook(void *data, void *user_data)
81 {
82         struct hciemu_command_hook *hook = data;
83         struct run_data *run_data = user_data;
84
85         if (hook->function)
86                 hook->function(run_data->opcode, run_data->data,
87                                         run_data->len, hook->user_data);
88 }
89
90 static void central_command_callback(uint16_t opcode,
91                                 const void *data, uint8_t len,
92                                 btdev_callback callback, void *user_data)
93 {
94         struct hciemu *hciemu = user_data;
95         struct run_data run_data = { .opcode = opcode,
96                                                 .data = data, .len = len };
97
98         btdev_command_default(callback);
99
100         queue_foreach(hciemu->post_command_hooks, run_command_hook, &run_data);
101 }
102
103 static void client_command_callback(uint16_t opcode,
104                                 const void *data, uint8_t len,
105                                 btdev_callback callback, void *user_data)
106 {
107         btdev_command_default(callback);
108 }
109
110 static void writev_callback(const struct iovec *iov, int iovlen,
111                                                                 void *user_data)
112 {
113         GIOChannel *channel = user_data;
114         ssize_t written;
115         int fd;
116
117         fd = g_io_channel_unix_get_fd(channel);
118
119         written = writev(fd, iov, iovlen);
120         if (written < 0)
121                 return;
122 }
123
124 static gboolean receive_bthost(GIOChannel *channel, GIOCondition condition,
125                                                         gpointer user_data)
126 {
127         struct bthost *bthost = user_data;
128         unsigned char buf[4096];
129         ssize_t len;
130         int fd;
131
132         if (condition & (G_IO_NVAL | G_IO_ERR | G_IO_HUP))
133                 return FALSE;
134
135         fd = g_io_channel_unix_get_fd(channel);
136
137         len = read(fd, buf, sizeof(buf));
138         if (len < 0)
139                 return FALSE;
140
141         bthost_receive_h4(bthost, buf, len);
142
143         return TRUE;
144 }
145
146 static guint create_source_bthost(int fd, struct bthost *bthost)
147 {
148         GIOChannel *channel;
149         guint source;
150
151         channel = g_io_channel_unix_new(fd);
152
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);
156
157         bthost_set_send_handler(bthost, writev_callback, channel);
158
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);
162
163         g_io_channel_unref(channel);
164
165         return source;
166 }
167
168 static gboolean receive_btdev(GIOChannel *channel, GIOCondition condition,
169                                                         gpointer user_data)
170 {
171         struct btdev *btdev = user_data;
172         unsigned char buf[4096];
173         ssize_t len;
174         int fd;
175
176         if (condition & (G_IO_NVAL | G_IO_ERR | G_IO_HUP))
177                 return FALSE;
178
179         fd = g_io_channel_unix_get_fd(channel);
180
181         len = read(fd, buf, sizeof(buf));
182         if (len < 0) {
183                 if (errno == EAGAIN || errno == EINTR)
184                         return TRUE;
185
186                 return FALSE;
187         }
188
189         if (len < 1)
190                 return FALSE;
191
192         switch (buf[0]) {
193         case BT_H4_CMD_PKT:
194         case BT_H4_ACL_PKT:
195         case BT_H4_SCO_PKT:
196         case BT_H4_ISO_PKT:
197                 btdev_receive_h4(btdev, buf, len);
198                 break;
199         }
200
201         return TRUE;
202 }
203
204 static guint create_source_btdev(int fd, struct btdev *btdev)
205 {
206         GIOChannel *channel;
207         guint source;
208
209         channel = g_io_channel_unix_new(fd);
210
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);
214
215         btdev_set_send_handler(btdev, writev_callback, channel);
216
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);
220
221         g_io_channel_unref(channel);
222
223         return source;
224 }
225
226 static bool create_vhci(struct hciemu *hciemu)
227 {
228         struct vhci *vhci;
229
230         vhci = vhci_open(hciemu->btdev_type);
231         if (!vhci)
232                 return false;
233
234         btdev_set_command_handler(vhci_get_btdev(vhci),
235                                         central_command_callback, hciemu);
236         hciemu->vhci = vhci;
237
238         return true;
239 }
240
241 struct vhci *hciemu_get_vhci(struct hciemu *hciemu)
242 {
243         if (!hciemu)
244                 return NULL;
245
246         return hciemu->vhci;
247 }
248
249 struct hciemu_client *hciemu_get_client(struct hciemu *hciemu, int num)
250 {
251         const struct queue_entry *entry;
252
253         if (!hciemu)
254                 return NULL;
255
256         for (entry = queue_get_entries(hciemu->clients); entry;
257                                         entry = entry->next, num--) {
258                 if (!num)
259                         return entry->data;
260         }
261
262         return NULL;
263 }
264
265 struct bthost *hciemu_client_host(struct hciemu_client *client)
266 {
267         if (!client)
268                 return NULL;
269
270         return client->host;
271 }
272
273 struct bthost *hciemu_client_get_host(struct hciemu *hciemu)
274 {
275         struct hciemu_client *client;
276
277         if (!hciemu)
278                 return NULL;
279
280         client = hciemu_get_client(hciemu, 0);
281
282         return hciemu_client_host(client);
283 }
284
285 static gboolean start_host(gpointer user_data)
286 {
287         struct hciemu_client *client = user_data;
288
289         client->start_source = 0;
290
291         bthost_start(client->host);
292
293         return FALSE;
294 }
295
296 static void hciemu_client_destroy(void *data)
297 {
298         struct hciemu_client *client = data;
299
300         if (client->start_source)
301                 g_source_remove(client->start_source);
302
303         g_source_remove(client->host_source);
304         g_source_remove(client->source);
305
306         bthost_destroy(client->host);
307         btdev_destroy(client->dev);
308
309         free(client);
310 }
311
312 static struct hciemu_client *hciemu_client_new(struct hciemu *hciemu,
313                                                         uint8_t id)
314 {
315         struct hciemu_client *client;
316         int sv[2];
317
318         client = new0(struct hciemu_client, 1);
319         if (!client)
320                 return NULL;
321
322         client->dev = btdev_create(hciemu->btdev_type, id++);
323         if (!client->dev) {
324                 free(client);
325                 return NULL;
326         }
327
328         client->host = bthost_create();
329         if (!client->host) {
330                 btdev_destroy(client->dev);
331                 free(client);
332                 return NULL;
333         }
334
335         btdev_set_command_handler(client->dev, client_command_callback, client);
336
337         if (socketpair(AF_UNIX, SOCK_SEQPACKET | SOCK_NONBLOCK | SOCK_CLOEXEC,
338                                                                 0, sv) < 0) {
339                 bthost_destroy(client->host);
340                 btdev_destroy(client->dev);
341                 return NULL;
342         }
343
344         client->sock[0] = sv[0];
345         client->sock[1] = sv[1];
346
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);
350
351         return client;
352 }
353
354 struct hciemu *hciemu_new_num(enum hciemu_type type, uint8_t num)
355 {
356
357         struct hciemu *hciemu;
358         int i;
359
360         if (!num)
361                 return NULL;
362
363         hciemu = new0(struct hciemu, 1);
364         if (!hciemu)
365                 return NULL;
366
367         switch (type) {
368         case HCIEMU_TYPE_BREDRLE:
369                 hciemu->btdev_type = BTDEV_TYPE_BREDRLE;
370                 break;
371         case HCIEMU_TYPE_BREDR:
372                 hciemu->btdev_type = BTDEV_TYPE_BREDR;
373                 break;
374         case HCIEMU_TYPE_LE:
375                 hciemu->btdev_type = BTDEV_TYPE_LE;
376                 break;
377         case HCIEMU_TYPE_LEGACY:
378                 hciemu->btdev_type = BTDEV_TYPE_BREDR20;
379                 break;
380         case HCIEMU_TYPE_BREDRLE50:
381                 hciemu->btdev_type = BTDEV_TYPE_BREDRLE50;
382                 break;
383         case HCIEMU_TYPE_BREDRLE52:
384                 hciemu->btdev_type = BTDEV_TYPE_BREDRLE52;
385                 break;
386         default:
387                 return NULL;
388         }
389
390         hciemu->post_command_hooks = queue_new();
391         if (!hciemu->post_command_hooks) {
392                 free(hciemu);
393                 return NULL;
394         }
395
396         if (!create_vhci(hciemu)) {
397                 queue_destroy(hciemu->post_command_hooks, NULL);
398                 free(hciemu);
399                 return NULL;
400         }
401
402         hciemu->clients = queue_new();
403
404         for (i = 0; i < num; i++) {
405                 struct hciemu_client *client = hciemu_client_new(hciemu, i);
406
407                 if (!client) {
408                         queue_destroy(hciemu->clients, hciemu_client_destroy);
409                         break;
410                 }
411
412                 queue_push_tail(hciemu->clients, client);
413         }
414
415         return hciemu_ref(hciemu);
416 }
417
418 struct hciemu *hciemu_new(enum hciemu_type type)
419 {
420         return hciemu_new_num(type, 1);
421 }
422
423 struct hciemu *hciemu_ref(struct hciemu *hciemu)
424 {
425         if (!hciemu)
426                 return NULL;
427
428         __sync_fetch_and_add(&hciemu->ref_count, 1);
429
430         return hciemu;
431 }
432
433 void hciemu_unref(struct hciemu *hciemu)
434 {
435         if (!hciemu)
436                 return;
437
438         if (__sync_sub_and_fetch(&hciemu->ref_count, 1))
439                 return;
440
441         queue_destroy(hciemu->post_command_hooks, destroy_command_hook);
442         queue_destroy(hciemu->clients, hciemu_client_destroy);
443
444         if (hciemu->flush_id)
445                 g_source_remove(hciemu->flush_id);
446
447         vhci_close(hciemu->vhci);
448
449         free(hciemu);
450 }
451
452 static void bthost_print(const char *str, void *user_data)
453 {
454         struct hciemu *hciemu = user_data;
455
456         util_debug(hciemu->debug_callback, hciemu->debug_data,
457                                         "bthost: %s", str);
458 }
459
460 static void vhci_debug(const char *str, void *user_data)
461 {
462         struct hciemu *hciemu = user_data;
463
464         util_debug(hciemu->debug_callback, hciemu->debug_data,
465                                         "vhci: %s", str);
466 }
467
468 static void btdev_client_debug(const char *str, void *user_data)
469 {
470         struct hciemu *hciemu = user_data;
471
472         util_debug(hciemu->debug_callback, hciemu->debug_data,
473                                         "btdev: %s", str);
474 }
475
476 static void hciemu_client_set_debug(void *data, void *user_data)
477 {
478         struct hciemu_client *client = data;
479         struct hciemu *hciemu = user_data;
480
481         btdev_set_debug(client->dev, btdev_client_debug, hciemu, NULL);
482         bthost_set_debug(client->host, bthost_print, hciemu, NULL);
483 }
484
485 bool hciemu_set_debug(struct hciemu *hciemu, hciemu_debug_func_t callback,
486                         void *user_data, hciemu_destroy_func_t destroy)
487 {
488         if (!hciemu)
489                 return false;
490
491         if (hciemu->debug_destroy)
492                 hciemu->debug_destroy(hciemu->debug_data);
493
494         hciemu->debug_callback = callback;
495         hciemu->debug_destroy = destroy;
496         hciemu->debug_data = user_data;
497
498         vhci_set_debug(hciemu->vhci, vhci_debug, hciemu, NULL);
499
500         queue_foreach(hciemu->clients, hciemu_client_set_debug, hciemu);
501
502         return true;
503 }
504
505 const char *hciemu_get_address(struct hciemu *hciemu)
506 {
507         const uint8_t *addr;
508         struct btdev *dev;
509
510         if (!hciemu || !hciemu->vhci)
511                 return NULL;
512
513         dev = vhci_get_btdev(hciemu->vhci);
514         if (!dev)
515                 return NULL;
516
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;
521 }
522
523 uint8_t *hciemu_get_features(struct hciemu *hciemu)
524 {
525         struct btdev *dev;
526
527         if (!hciemu || !hciemu->vhci)
528                 return NULL;
529
530         dev = vhci_get_btdev(hciemu->vhci);
531         if (!dev)
532                 return NULL;
533
534         return btdev_get_features(dev);
535 }
536
537 const uint8_t *hciemu_get_central_bdaddr(struct hciemu *hciemu)
538 {
539         struct btdev *dev;
540
541         if (!hciemu || !hciemu->vhci)
542                 return NULL;
543
544         dev = vhci_get_btdev(hciemu->vhci);
545         if (!dev)
546                 return NULL;
547
548         return btdev_get_bdaddr(dev);
549 }
550
551 const uint8_t *hciemu_client_bdaddr(struct hciemu_client *client)
552 {
553         if (!client)
554                 return NULL;
555
556         return btdev_get_bdaddr(client->dev);
557 }
558
559 const uint8_t *hciemu_get_client_bdaddr(struct hciemu *hciemu)
560 {
561         struct hciemu_client *client;
562
563         if (!hciemu)
564                 return NULL;
565
566         client = hciemu_get_client(hciemu, 0);
567
568         return hciemu_client_bdaddr(client);
569 }
570
571 uint8_t hciemu_get_central_scan_enable(struct hciemu *hciemu)
572 {
573         struct btdev *dev;
574
575         if (!hciemu || !hciemu->vhci)
576                 return 0;
577
578         dev = vhci_get_btdev(hciemu->vhci);
579         if (!dev)
580                 return 0;
581
582         return btdev_get_scan_enable(dev);
583 }
584
585 uint8_t hciemu_get_central_le_scan_enable(struct hciemu *hciemu)
586 {
587         struct btdev *dev;
588
589         if (!hciemu || !hciemu->vhci)
590                 return 0;
591
592         dev = vhci_get_btdev(hciemu->vhci);
593         if (!dev)
594                 return 0;
595
596         return btdev_get_le_scan_enable(dev);
597 }
598
599 void hciemu_set_central_le_states(struct hciemu *hciemu,
600                                                 const uint8_t *le_states)
601 {
602         struct btdev *dev;
603
604         if (!hciemu || !hciemu->vhci)
605                 return;
606
607         dev = vhci_get_btdev(hciemu->vhci);
608         if (!dev)
609                 return;
610
611         btdev_set_le_states(dev, le_states);
612 }
613
614 void hciemu_set_central_le_al_len(struct hciemu *hciemu, uint8_t len)
615 {
616         struct btdev *dev;
617
618         if (!hciemu || !hciemu->vhci)
619                 return;
620
621         dev = vhci_get_btdev(hciemu->vhci);
622         if (!dev)
623                 return;
624
625         btdev_set_al_len(dev, len);
626 }
627
628 void hciemu_set_central_le_rl_len(struct hciemu *hciemu, uint8_t len)
629 {
630         struct btdev *dev;
631
632         if (!hciemu || !hciemu->vhci)
633                 return;
634
635         dev = vhci_get_btdev(hciemu->vhci);
636         if (!dev)
637                 return;
638
639         btdev_set_rl_len(dev, len);
640 }
641
642 const uint8_t *hciemu_get_central_adv_addr(struct hciemu *hciemu,
643                                                                 uint8_t handle)
644 {
645         struct btdev *dev;
646
647         if (!hciemu || !hciemu->vhci)
648                 return NULL;
649
650         dev = vhci_get_btdev(hciemu->vhci);
651         if (!dev)
652                 return NULL;
653
654         return btdev_get_adv_addr(dev, handle);
655 }
656
657 bool hciemu_add_central_post_command_hook(struct hciemu *hciemu,
658                         hciemu_command_func_t function, void *user_data)
659 {
660         struct hciemu_command_hook *hook;
661
662         if (!hciemu)
663                 return false;
664
665         hook = new0(struct hciemu_command_hook, 1);
666         if (!hook)
667                 return false;
668
669         hook->function = function;
670         hook->user_data = user_data;
671
672         if (!queue_push_tail(hciemu->post_command_hooks, hook)) {
673                 free(hook);
674                 return false;
675         }
676
677         return true;
678 }
679
680 bool hciemu_clear_central_post_command_hooks(struct hciemu *hciemu)
681 {
682         if (!hciemu)
683                 return false;
684
685         queue_remove_all(hciemu->post_command_hooks,
686                                         NULL, NULL, destroy_command_hook);
687         return true;
688 }
689
690 int hciemu_add_hook(struct hciemu *hciemu, enum hciemu_hook_type type,
691                                 uint16_t opcode, hciemu_hook_func_t function,
692                                 void *user_data)
693 {
694         enum btdev_hook_type hook_type;
695         struct btdev *dev;
696
697         if (!hciemu || !hciemu->vhci)
698                 return -1;
699
700         dev = vhci_get_btdev(hciemu->vhci);
701         if (!dev)
702                 return 0;
703
704         switch (type) {
705         case HCIEMU_HOOK_PRE_CMD:
706                 hook_type = BTDEV_HOOK_PRE_CMD;
707                 break;
708         case HCIEMU_HOOK_POST_CMD:
709                 hook_type = BTDEV_HOOK_POST_CMD;
710                 break;
711         case HCIEMU_HOOK_PRE_EVT:
712                 hook_type = BTDEV_HOOK_PRE_EVT;
713                 break;
714         case HCIEMU_HOOK_POST_EVT:
715                 hook_type = BTDEV_HOOK_POST_EVT;
716                 break;
717         default:
718                 return -1;
719         }
720
721         return btdev_add_hook(dev, hook_type, opcode, function, user_data);
722 }
723
724 bool hciemu_del_hook(struct hciemu *hciemu, enum hciemu_hook_type type,
725                                                                 uint16_t opcode)
726 {
727         enum btdev_hook_type hook_type;
728         struct btdev *dev;
729
730         if (!hciemu || !hciemu->vhci)
731                 return false;
732
733         dev = vhci_get_btdev(hciemu->vhci);
734         if (!dev)
735                 return false;
736
737         switch (type) {
738         case HCIEMU_HOOK_PRE_CMD:
739                 hook_type = BTDEV_HOOK_PRE_CMD;
740                 break;
741         case HCIEMU_HOOK_POST_CMD:
742                 hook_type = BTDEV_HOOK_POST_CMD;
743                 break;
744         case HCIEMU_HOOK_PRE_EVT:
745                 hook_type = BTDEV_HOOK_PRE_EVT;
746                 break;
747         case HCIEMU_HOOK_POST_EVT:
748                 hook_type = BTDEV_HOOK_POST_EVT;
749                 break;
750         default:
751                 return false;
752         }
753
754         return btdev_del_hook(dev, hook_type, opcode);
755 }
756
757 static bool client_is_pending(const void *data, const void *match_data)
758 {
759         struct hciemu_client *client = (struct hciemu_client *)data;
760         int used, i;
761
762         if (!client->source || !client->host_source)
763                 return false;
764
765         for (i = 0; i < 2; ++i) {
766                 if (!ioctl(client->sock[i], TIOCOUTQ, &used) && used > 0)
767                         return true;
768                 if (!ioctl(client->sock[i], TIOCINQ, &used) && used > 0)
769                         return true;
770         }
771
772         return false;
773 }
774
775 static gboolean flush_client_events(gpointer user_data)
776 {
777         struct hciemu *hciemu = user_data;
778
779         if (queue_find(hciemu->clients, client_is_pending, NULL))
780                 return TRUE;
781
782         hciemu->flush_id = 0;
783
784         util_debug(hciemu->debug_callback, hciemu->debug_data, "vhci: resume");
785         if (hciemu->vhci)
786                 vhci_pause_input(hciemu->vhci, false);
787
788         return FALSE;
789 }
790
791 void hciemu_flush_client_events(struct hciemu *hciemu)
792 {
793         if (hciemu->flush_id || !hciemu->vhci)
794                 return;
795
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);
799 }