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         btdev_receive_h4(btdev, buf, len);
 193
 194         return TRUE;
 195 }
 196
 197 static guint create_source_btdev(int fd, struct btdev *btdev)
 198 {
 199         GIOChannel *channel;
 200         guint source;
 201
 202         channel = g_io_channel_unix_new(fd);
 203
 204         g_io_channel_set_close_on_unref(channel, TRUE);
 205         g_io_channel_set_encoding(channel, NULL, NULL);
 206         g_io_channel_set_buffered(channel, FALSE);
 207
 208         btdev_set_send_handler(btdev, writev_callback, channel);
 209
 210         source = g_io_add_watch_full(channel, G_PRIORITY_DEFAULT,
 211                                 G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
 212                                 receive_btdev, btdev, NULL);
 213
 214         g_io_channel_unref(channel);
 215
 216         return source;
 217 }
 218
 219 static bool create_vhci(struct hciemu *hciemu)
 220 {
 221         struct vhci *vhci;
 222
 223         vhci = vhci_open(hciemu->btdev_type);
 224         if (!vhci)
 225                 return false;
 226
 227         btdev_set_command_handler(vhci_get_btdev(vhci),
 228                                         central_command_callback, hciemu);
 229         hciemu->vhci = vhci;
 230
 231         return true;
 232 }
 233
 234 struct vhci *hciemu_get_vhci(struct hciemu *hciemu)
 235 {
 236         if (!hciemu)
 237                 return NULL;
 238
 239         return hciemu->vhci;
 240 }
 241
 242 struct hciemu_client *hciemu_get_client(struct hciemu *hciemu, int num)
 243 {
 244         const struct queue_entry *entry;
 245
 246         if (!hciemu)
 247                 return NULL;
 248
 249         for (entry = queue_get_entries(hciemu->clients); entry;
 250                                         entry = entry->next, num--) {
 251                 if (!num)
 252                         return entry->data;
 253         }
 254
 255         return NULL;
 256 }
 257
 258 struct bthost *hciemu_client_host(struct hciemu_client *client)
 259 {
 260         if (!client)
 261                 return NULL;
 262
 263         return client->host;
 264 }
 265
 266 struct bthost *hciemu_client_get_host(struct hciemu *hciemu)
 267 {
 268         struct hciemu_client *client;
 269
 270         if (!hciemu)
 271                 return NULL;
 272
 273         client = hciemu_get_client(hciemu, 0);
 274
 275         return hciemu_client_host(client);
 276 }
 277
 278 static gboolean start_host(gpointer user_data)
 279 {
 280         struct hciemu_client *client = user_data;
 281
 282         client->start_source = 0;
 283
 284         bthost_start(client->host);
 285
 286         return FALSE;
 287 }
 288
 289 static void hciemu_client_destroy(void *data)
 290 {
 291         struct hciemu_client *client = data;
 292
 293         if (client->start_source)
 294                 g_source_remove(client->start_source);
 295
 296         g_source_remove(client->host_source);
 297         g_source_remove(client->source);
 298
 299         bthost_destroy(client->host);
 300         btdev_destroy(client->dev);
 301
 302         free(client);
 303 }
 304
 305 static struct hciemu_client *hciemu_client_new(struct hciemu *hciemu,
 306                                                         uint8_t id)
 307 {
 308         struct hciemu_client *client;
 309         int sv[2];
 310
 311         client = new0(struct hciemu_client, 1);
 312         if (!client)
 313                 return NULL;
 314
 315         client->dev = btdev_create(hciemu->btdev_type, id++);
 316         if (!client->dev) {
 317                 free(client);
 318                 return NULL;
 319         }
 320
 321         client->host = bthost_create();
 322         if (!client->host) {
 323                 btdev_destroy(client->dev);
 324                 free(client);
 325                 return NULL;
 326         }
 327
 328         btdev_set_command_handler(client->dev, client_command_callback, client);
 329
 330         if (socketpair(AF_UNIX, SOCK_SEQPACKET | SOCK_NONBLOCK | SOCK_CLOEXEC,
 331                                                                 0, sv) < 0) {
 332                 bthost_destroy(client->host);
 333                 btdev_destroy(client->dev);
 334                 return NULL;
 335         }
 336
 337         client->sock[0] = sv[0];
 338         client->sock[1] = sv[1];
 339
 340         client->source = create_source_btdev(sv[0], client->dev);
 341         client->host_source = create_source_bthost(sv[1], client->host);
 342         client->start_source = g_idle_add(start_host, client);
 343
 344         return client;
 345 }
 346
 347 struct hciemu *hciemu_new_num(enum hciemu_type type, uint8_t num)
 348 {
 349
 350         struct hciemu *hciemu;
 351         int i;
 352
 353         if (!num)
 354                 return NULL;
 355
 356         hciemu = new0(struct hciemu, 1);
 357         if (!hciemu)
 358                 return NULL;
 359
 360         switch (type) {
 361         case HCIEMU_TYPE_BREDRLE:
 362                 hciemu->btdev_type = BTDEV_TYPE_BREDRLE;
 363                 break;
 364         case HCIEMU_TYPE_BREDR:
 365                 hciemu->btdev_type = BTDEV_TYPE_BREDR;
 366                 break;
 367         case HCIEMU_TYPE_LE:
 368                 hciemu->btdev_type = BTDEV_TYPE_LE;
 369                 break;
 370         case HCIEMU_TYPE_LEGACY:
 371                 hciemu->btdev_type = BTDEV_TYPE_BREDR20;
 372                 break;
 373         case HCIEMU_TYPE_BREDRLE50:
 374                 hciemu->btdev_type = BTDEV_TYPE_BREDRLE50;
 375                 break;
 376         case HCIEMU_TYPE_BREDRLE52:
 377                 hciemu->btdev_type = BTDEV_TYPE_BREDRLE52;
 378                 break;
 379         default:
 380                 return NULL;
 381         }
 382
 383         hciemu->post_command_hooks = queue_new();
 384         if (!hciemu->post_command_hooks) {
 385                 free(hciemu);
 386                 return NULL;
 387         }
 388
 389         if (!create_vhci(hciemu)) {
 390                 queue_destroy(hciemu->post_command_hooks, NULL);
 391                 free(hciemu);
 392                 return NULL;
 393         }
 394
 395         hciemu->clients = queue_new();
 396
 397         for (i = 0; i < num; i++) {
 398                 struct hciemu_client *client = hciemu_client_new(hciemu, i);
 399
 400                 if (!client) {
 401                         queue_destroy(hciemu->clients, hciemu_client_destroy);
 402                         break;
 403                 }
 404
 405                 queue_push_tail(hciemu->clients, client);
 406         }
 407
 408         return hciemu_ref(hciemu);
 409 }
 410
 411 struct hciemu *hciemu_new(enum hciemu_type type)
 412 {
 413         return hciemu_new_num(type, 1);
 414 }
 415
 416 struct hciemu *hciemu_ref(struct hciemu *hciemu)
 417 {
 418         if (!hciemu)
 419                 return NULL;
 420
 421         __sync_fetch_and_add(&hciemu->ref_count, 1);
 422
 423         return hciemu;
 424 }
 425
 426 void hciemu_unref(struct hciemu *hciemu)
 427 {
 428         if (!hciemu)
 429                 return;
 430
 431         if (__sync_sub_and_fetch(&hciemu->ref_count, 1))
 432                 return;
 433
 434         queue_destroy(hciemu->post_command_hooks, destroy_command_hook);
 435         queue_destroy(hciemu->clients, hciemu_client_destroy);
 436
 437         if (hciemu->flush_id)
 438                 g_source_remove(hciemu->flush_id);
 439
 440         vhci_close(hciemu->vhci);
 441
 442         free(hciemu);
 443 }
 444
 445 static void bthost_print(const char *str, void *user_data)
 446 {
 447         struct hciemu *hciemu = user_data;
 448
 449         util_debug(hciemu->debug_callback, hciemu->debug_data,
 450                                         "bthost: %s", str);
 451 }
 452
 453 static void vhci_debug(const char *str, void *user_data)
 454 {
 455         struct hciemu *hciemu = user_data;
 456
 457         util_debug(hciemu->debug_callback, hciemu->debug_data,
 458                                         "vhci: %s", str);
 459 }
 460
 461 static void btdev_client_debug(const char *str, void *user_data)
 462 {
 463         struct hciemu *hciemu = user_data;
 464
 465         util_debug(hciemu->debug_callback, hciemu->debug_data,
 466                                         "btdev: %s", str);
 467 }
 468
 469 static void hciemu_client_set_debug(void *data, void *user_data)
 470 {
 471         struct hciemu_client *client = data;
 472         struct hciemu *hciemu = user_data;
 473
 474         btdev_set_debug(client->dev, btdev_client_debug, hciemu, NULL);
 475         bthost_set_debug(client->host, bthost_print, hciemu, NULL);
 476 }
 477
 478 bool hciemu_set_debug(struct hciemu *hciemu, hciemu_debug_func_t callback,
 479                         void *user_data, hciemu_destroy_func_t destroy)
 480 {
 481         if (!hciemu)
 482                 return false;
 483
 484         if (hciemu->debug_destroy)
 485                 hciemu->debug_destroy(hciemu->debug_data);
 486
 487         hciemu->debug_callback = callback;
 488         hciemu->debug_destroy = destroy;
 489         hciemu->debug_data = user_data;
 490
 491         vhci_set_debug(hciemu->vhci, vhci_debug, hciemu, NULL);
 492
 493         queue_foreach(hciemu->clients, hciemu_client_set_debug, hciemu);
 494
 495         return true;
 496 }
 497
 498 const char *hciemu_get_address(struct hciemu *hciemu)
 499 {
 500         const uint8_t *addr;
 501         struct btdev *dev;
 502
 503         if (!hciemu || !hciemu->vhci)
 504                 return NULL;
 505
 506         dev = vhci_get_btdev(hciemu->vhci);
 507         if (!dev)
 508                 return NULL;
 509
 510         addr = btdev_get_bdaddr(dev);
 511         sprintf(hciemu->bdaddr_str, "%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X",
 512                         addr[5], addr[4], addr[3], addr[2], addr[1], addr[0]);
 513         return hciemu->bdaddr_str;
 514 }
 515
 516 uint8_t *hciemu_get_features(struct hciemu *hciemu)
 517 {
 518         struct btdev *dev;
 519
 520         if (!hciemu || !hciemu->vhci)
 521                 return NULL;
 522
 523         dev = vhci_get_btdev(hciemu->vhci);
 524         if (!dev)
 525                 return NULL;
 526
 527         return btdev_get_features(dev);
 528 }
 529
 530 const uint8_t *hciemu_get_central_bdaddr(struct hciemu *hciemu)
 531 {
 532         struct btdev *dev;
 533
 534         if (!hciemu || !hciemu->vhci)
 535                 return NULL;
 536
 537         dev = vhci_get_btdev(hciemu->vhci);
 538         if (!dev)
 539                 return NULL;
 540
 541         return btdev_get_bdaddr(dev);
 542 }
 543
 544 const uint8_t *hciemu_client_bdaddr(struct hciemu_client *client)
 545 {
 546         if (!client)
 547                 return NULL;
 548
 549         return btdev_get_bdaddr(client->dev);
 550 }
 551
 552 const uint8_t *hciemu_get_client_bdaddr(struct hciemu *hciemu)
 553 {
 554         struct hciemu_client *client;
 555
 556         if (!hciemu)
 557                 return NULL;
 558
 559         client = hciemu_get_client(hciemu, 0);
 560
 561         return hciemu_client_bdaddr(client);
 562 }
 563
 564 uint8_t hciemu_get_central_scan_enable(struct hciemu *hciemu)
 565 {
 566         struct btdev *dev;
 567
 568         if (!hciemu || !hciemu->vhci)
 569                 return 0;
 570
 571         dev = vhci_get_btdev(hciemu->vhci);
 572         if (!dev)
 573                 return 0;
 574
 575         return btdev_get_scan_enable(dev);
 576 }
 577
 578 uint8_t hciemu_get_central_le_scan_enable(struct hciemu *hciemu)
 579 {
 580         struct btdev *dev;
 581
 582         if (!hciemu || !hciemu->vhci)
 583                 return 0;
 584
 585         dev = vhci_get_btdev(hciemu->vhci);
 586         if (!dev)
 587                 return 0;
 588
 589         return btdev_get_le_scan_enable(dev);
 590 }
 591
 592 void hciemu_set_central_le_states(struct hciemu *hciemu,
 593                                                 const uint8_t *le_states)
 594 {
 595         struct btdev *dev;
 596
 597         if (!hciemu || !hciemu->vhci)
 598                 return;
 599
 600         dev = vhci_get_btdev(hciemu->vhci);
 601         if (!dev)
 602                 return;
 603
 604         btdev_set_le_states(dev, le_states);
 605 }
 606
 607 void hciemu_set_central_le_al_len(struct hciemu *hciemu, uint8_t len)
 608 {
 609         struct btdev *dev;
 610
 611         if (!hciemu || !hciemu->vhci)
 612                 return;
 613
 614         dev = vhci_get_btdev(hciemu->vhci);
 615         if (!dev)
 616                 return;
 617
 618         btdev_set_al_len(dev, len);
 619 }
 620
 621 void hciemu_set_central_le_rl_len(struct hciemu *hciemu, uint8_t len)
 622 {
 623         struct btdev *dev;
 624
 625         if (!hciemu || !hciemu->vhci)
 626                 return;
 627
 628         dev = vhci_get_btdev(hciemu->vhci);
 629         if (!dev)
 630                 return;
 631
 632         btdev_set_rl_len(dev, len);
 633 }
 634
 635 const uint8_t *hciemu_get_central_adv_addr(struct hciemu *hciemu,
 636                                                                 uint8_t handle)
 637 {
 638         struct btdev *dev;
 639
 640         if (!hciemu || !hciemu->vhci)
 641                 return NULL;
 642
 643         dev = vhci_get_btdev(hciemu->vhci);
 644         if (!dev)
 645                 return NULL;
 646
 647         return btdev_get_adv_addr(dev, handle);
 648 }
 649
 650 bool hciemu_add_central_post_command_hook(struct hciemu *hciemu,
 651                         hciemu_command_func_t function, void *user_data)
 652 {
 653         struct hciemu_command_hook *hook;
 654
 655         if (!hciemu)
 656                 return false;
 657
 658         hook = new0(struct hciemu_command_hook, 1);
 659         if (!hook)
 660                 return false;
 661
 662         hook->function = function;
 663         hook->user_data = user_data;
 664
 665         if (!queue_push_tail(hciemu->post_command_hooks, hook)) {
 666                 free(hook);
 667                 return false;
 668         }
 669
 670         return true;
 671 }
 672
 673 bool hciemu_clear_central_post_command_hooks(struct hciemu *hciemu)
 674 {
 675         if (!hciemu)
 676                 return false;
 677
 678         queue_remove_all(hciemu->post_command_hooks,
 679                                         NULL, NULL, destroy_command_hook);
 680         return true;
 681 }
 682
 683 int hciemu_add_hook(struct hciemu *hciemu, enum hciemu_hook_type type,
 684                                 uint16_t opcode, hciemu_hook_func_t function,
 685                                 void *user_data)
 686 {
 687         enum btdev_hook_type hook_type;
 688         struct btdev *dev;
 689
 690         if (!hciemu || !hciemu->vhci)
 691                 return -1;
 692
 693         dev = vhci_get_btdev(hciemu->vhci);
 694         if (!dev)
 695                 return 0;
 696
 697         switch (type) {
 698         case HCIEMU_HOOK_PRE_CMD:
 699                 hook_type = BTDEV_HOOK_PRE_CMD;
 700                 break;
 701         case HCIEMU_HOOK_POST_CMD:
 702                 hook_type = BTDEV_HOOK_POST_CMD;
 703                 break;
 704         case HCIEMU_HOOK_PRE_EVT:
 705                 hook_type = BTDEV_HOOK_PRE_EVT;
 706                 break;
 707         case HCIEMU_HOOK_POST_EVT:
 708                 hook_type = BTDEV_HOOK_POST_EVT;
 709                 break;
 710         default:
 711                 return -1;
 712         }
 713
 714         return btdev_add_hook(dev, hook_type, opcode, function, user_data);
 715 }
 716
 717 bool hciemu_del_hook(struct hciemu *hciemu, enum hciemu_hook_type type,
 718                                                                 uint16_t opcode)
 719 {
 720         enum btdev_hook_type hook_type;
 721         struct btdev *dev;
 722
 723         if (!hciemu || !hciemu->vhci)
 724                 return false;
 725
 726         dev = vhci_get_btdev(hciemu->vhci);
 727         if (!dev)
 728                 return false;
 729
 730         switch (type) {
 731         case HCIEMU_HOOK_PRE_CMD:
 732                 hook_type = BTDEV_HOOK_PRE_CMD;
 733                 break;
 734         case HCIEMU_HOOK_POST_CMD:
 735                 hook_type = BTDEV_HOOK_POST_CMD;
 736                 break;
 737         case HCIEMU_HOOK_PRE_EVT:
 738                 hook_type = BTDEV_HOOK_PRE_EVT;
 739                 break;
 740         case HCIEMU_HOOK_POST_EVT:
 741                 hook_type = BTDEV_HOOK_POST_EVT;
 742                 break;
 743         default:
 744                 return false;
 745         }
 746
 747         return btdev_del_hook(dev, hook_type, opcode);
 748 }
 749
 750 static bool client_is_pending(const void *data, const void *match_data)
 751 {
 752         struct hciemu_client *client = (struct hciemu_client *)data;
 753         int used, i;
 754
 755         if (!client->source || !client->host_source)
 756                 return false;
 757
 758         for (i = 0; i < 2; ++i) {
 759                 if (!ioctl(client->sock[i], TIOCOUTQ, &used) && used > 0)
 760                         return true;
 761                 if (!ioctl(client->sock[i], TIOCINQ, &used) && used > 0)
 762                         return true;
 763         }
 764
 765         return false;
 766 }
 767
 768 static gboolean flush_client_events(gpointer user_data)
 769 {
 770         struct hciemu *hciemu = user_data;
 771
 772         if (queue_find(hciemu->clients, client_is_pending, NULL))
 773                 return TRUE;
 774
 775         hciemu->flush_id = 0;
 776
 777         util_debug(hciemu->debug_callback, hciemu->debug_data, "vhci: resume");
 778         if (hciemu->vhci)
 779                 vhci_pause_input(hciemu->vhci, false);
 780
 781         return FALSE;
 782 }
 783
 784 void hciemu_flush_client_events(struct hciemu *hciemu)
 785 {
 786         if (hciemu->flush_id || !hciemu->vhci)
 787                 return;
 788
 789         util_debug(hciemu->debug_callback, hciemu->debug_data, "vhci: pause");
 790         vhci_pause_input(hciemu->vhci, true);
 791         hciemu->flush_id = g_idle_add(flush_client_events, hciemu);
 792 }