emulator/hciemu.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2012-2014  Intel Corporation. All rights reserved.
   6  *
   7  *
   8  *  This library is free software; you can redistribute it and/or
   9  *  modify it under the terms of the GNU Lesser General Public
  10  *  License as published by the Free Software Foundation; either
  11  *  version 2.1 of the License, or (at your option) any later version.
  12  *
  13  *  This library is distributed in the hope that it will be useful,
  14  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16  *  Lesser General Public License for more details.
  17  *
  18  *  You should have received a copy of the GNU Lesser General Public
  19  *  License along with this library; if not, write to the Free Software
  20  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  21  *
  22  */
  23
  24 #ifdef HAVE_CONFIG_H
  25 #include <config.h>
  26 #endif
  27
  28 #include <stdio.h>
  29 #include <fcntl.h>
  30 #include <unistd.h>
  31 #include <stdlib.h>
  32 #include <string.h>
  33 #include <stdbool.h>
  34 #include <errno.h>
  35 #include <sys/socket.h>
  36
  37 #include <glib.h>
  38
  39 #include "lib/bluetooth.h"
  40 #include "lib/hci.h"
  41
  42 #include "monitor/bt.h"
  43 #include "emulator/btdev.h"
  44 #include "emulator/bthost.h"
  45 #include "src/shared/util.h"
  46 #include "src/shared/queue.h"
  47 #include "emulator/hciemu.h"
  48
  49 struct hciemu {
  50         int ref_count;
  51         enum btdev_type btdev_type;
  52         struct bthost *host_stack;
  53         struct btdev *master_dev;
  54         struct btdev *client_dev;
  55         guint host_source;
  56         guint master_source;
  57         guint client_source;
  58         struct queue *post_command_hooks;
  59         char bdaddr_str[18];
  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 master_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                 btdev_receive_h4(btdev, buf, len);
 197                 break;
 198         }
 199
 200         return TRUE;
 201 }
 202
 203 static guint create_source_btdev(int fd, struct btdev *btdev)
 204 {
 205         GIOChannel *channel;
 206         guint source;
 207
 208         channel = g_io_channel_unix_new(fd);
 209
 210         g_io_channel_set_close_on_unref(channel, TRUE);
 211         g_io_channel_set_encoding(channel, NULL, NULL);
 212         g_io_channel_set_buffered(channel, FALSE);
 213
 214         btdev_set_send_handler(btdev, writev_callback, channel);
 215
 216         source = g_io_add_watch_full(channel, G_PRIORITY_DEFAULT,
 217                                 G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
 218                                 receive_btdev, btdev, NULL);
 219
 220         g_io_channel_unref(channel);
 221
 222         return source;
 223 }
 224
 225 static bool create_vhci(struct hciemu *hciemu)
 226 {
 227         struct btdev *btdev;
 228         uint8_t create_req[2];
 229         ssize_t written;
 230         int fd;
 231
 232         btdev = btdev_create(hciemu->btdev_type, 0x00);
 233         if (!btdev)
 234                 return false;
 235
 236         btdev_set_command_handler(btdev, master_command_callback, hciemu);
 237
 238         fd = open("/dev/vhci", O_RDWR | O_NONBLOCK | O_CLOEXEC);
 239         if (fd < 0) {
 240                 perror("Opening /dev/vhci failed");
 241                 btdev_destroy(btdev);
 242                 return false;
 243         }
 244
 245         create_req[0] = HCI_VENDOR_PKT;
 246         create_req[1] = HCI_PRIMARY;
 247
 248         written = write(fd, create_req, sizeof(create_req));
 249         if (written < 0) {
 250                 close(fd);
 251                 btdev_destroy(btdev);
 252                 return false;
 253         }
 254
 255         hciemu->master_dev = btdev;
 256
 257         hciemu->master_source = create_source_btdev(fd, btdev);
 258
 259         return true;
 260 }
 261
 262 struct bthost *hciemu_client_get_host(struct hciemu *hciemu)
 263 {
 264         if (!hciemu)
 265                 return NULL;
 266
 267         return hciemu->host_stack;
 268 }
 269
 270 static bool create_stack(struct hciemu *hciemu)
 271 {
 272         struct btdev *btdev;
 273         struct bthost *bthost;
 274         int sv[2];
 275
 276         btdev = btdev_create(hciemu->btdev_type, 0x00);
 277         if (!btdev)
 278                 return false;
 279
 280         bthost = bthost_create();
 281         if (!bthost) {
 282                 btdev_destroy(btdev);
 283                 return false;
 284         }
 285
 286         btdev_set_command_handler(btdev, client_command_callback, hciemu);
 287
 288         if (socketpair(AF_UNIX, SOCK_SEQPACKET | SOCK_NONBLOCK | SOCK_CLOEXEC,
 289                                                                 0, sv) < 0) {
 290                 bthost_destroy(bthost);
 291                 btdev_destroy(btdev);
 292                 return false;
 293         }
 294
 295         hciemu->client_dev = btdev;
 296         hciemu->host_stack = bthost;
 297
 298         hciemu->client_source = create_source_btdev(sv[0], btdev);
 299         hciemu->host_source = create_source_bthost(sv[1], bthost);
 300
 301         return true;
 302 }
 303
 304 static gboolean start_stack(gpointer user_data)
 305 {
 306         struct hciemu *hciemu = user_data;
 307
 308         bthost_start(hciemu->host_stack);
 309
 310         return FALSE;
 311 }
 312
 313 struct hciemu *hciemu_new(enum hciemu_type type)
 314 {
 315         struct hciemu *hciemu;
 316
 317         hciemu = new0(struct hciemu, 1);
 318         if (!hciemu)
 319                 return NULL;
 320
 321         switch (type) {
 322         case HCIEMU_TYPE_BREDRLE:
 323                 hciemu->btdev_type = BTDEV_TYPE_BREDRLE;
 324                 break;
 325         case HCIEMU_TYPE_BREDR:
 326                 hciemu->btdev_type = BTDEV_TYPE_BREDR;
 327                 break;
 328         case HCIEMU_TYPE_LE:
 329                 hciemu->btdev_type = BTDEV_TYPE_LE;
 330                 break;
 331         case HCIEMU_TYPE_LEGACY:
 332                 hciemu->btdev_type = BTDEV_TYPE_BREDR20;
 333                 break;
 334         default:
 335                 return NULL;
 336         }
 337
 338         hciemu->post_command_hooks = queue_new();
 339         if (!hciemu->post_command_hooks) {
 340                 free(hciemu);
 341                 return NULL;
 342         }
 343
 344         if (!create_vhci(hciemu)) {
 345                 queue_destroy(hciemu->post_command_hooks, NULL);
 346                 free(hciemu);
 347                 return NULL;
 348         }
 349
 350         if (!create_stack(hciemu)) {
 351                 g_source_remove(hciemu->master_source);
 352                 btdev_destroy(hciemu->master_dev);
 353                 queue_destroy(hciemu->post_command_hooks, NULL);
 354                 free(hciemu);
 355                 return NULL;
 356         }
 357
 358         g_idle_add(start_stack, hciemu);
 359
 360         return hciemu_ref(hciemu);
 361 }
 362
 363 struct hciemu *hciemu_ref(struct hciemu *hciemu)
 364 {
 365         if (!hciemu)
 366                 return NULL;
 367
 368         __sync_fetch_and_add(&hciemu->ref_count, 1);
 369
 370         return hciemu;
 371 }
 372
 373 void hciemu_unref(struct hciemu *hciemu)
 374 {
 375         if (!hciemu)
 376                 return;
 377
 378         if (__sync_sub_and_fetch(&hciemu->ref_count, 1))
 379                 return;
 380
 381         queue_destroy(hciemu->post_command_hooks, destroy_command_hook);
 382
 383         g_source_remove(hciemu->host_source);
 384         g_source_remove(hciemu->client_source);
 385         g_source_remove(hciemu->master_source);
 386
 387         bthost_destroy(hciemu->host_stack);
 388         btdev_destroy(hciemu->client_dev);
 389         btdev_destroy(hciemu->master_dev);
 390
 391         free(hciemu);
 392 }
 393
 394 const char *hciemu_get_address(struct hciemu *hciemu)
 395 {
 396         const uint8_t *addr;
 397
 398         if (!hciemu || !hciemu->master_dev)
 399                 return NULL;
 400
 401         addr = btdev_get_bdaddr(hciemu->master_dev);
 402         sprintf(hciemu->bdaddr_str, "%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X",
 403                         addr[5], addr[4], addr[3], addr[2], addr[1], addr[0]);
 404         return hciemu->bdaddr_str;
 405 }
 406
 407 uint8_t *hciemu_get_features(struct hciemu *hciemu)
 408 {
 409         if (!hciemu || !hciemu->master_dev)
 410                 return NULL;
 411
 412         return btdev_get_features(hciemu->master_dev);
 413 }
 414
 415 const uint8_t *hciemu_get_master_bdaddr(struct hciemu *hciemu)
 416 {
 417         if (!hciemu || !hciemu->master_dev)
 418                 return NULL;
 419
 420         return btdev_get_bdaddr(hciemu->master_dev);
 421 }
 422
 423 const uint8_t *hciemu_get_client_bdaddr(struct hciemu *hciemu)
 424 {
 425         if (!hciemu || !hciemu->client_dev)
 426                 return NULL;
 427
 428         return btdev_get_bdaddr(hciemu->client_dev);
 429 }
 430
 431 uint8_t hciemu_get_master_scan_enable(struct hciemu *hciemu)
 432 {
 433         if (!hciemu || !hciemu->master_dev)
 434                 return 0;
 435
 436         return btdev_get_scan_enable(hciemu->master_dev);
 437 }
 438
 439 uint8_t hciemu_get_master_le_scan_enable(struct hciemu *hciemu)
 440 {
 441         if (!hciemu || !hciemu->master_dev)
 442                 return 0;
 443
 444         return btdev_get_le_scan_enable(hciemu->master_dev);
 445 }
 446
 447 void hciemu_set_master_le_states(struct hciemu *hciemu, const uint8_t *le_states)
 448 {
 449         if (!hciemu || !hciemu->master_dev)
 450                 return;
 451
 452         btdev_set_le_states(hciemu->master_dev, le_states);
 453 }
 454
 455 bool hciemu_add_master_post_command_hook(struct hciemu *hciemu,
 456                         hciemu_command_func_t function, void *user_data)
 457 {
 458         struct hciemu_command_hook *hook;
 459
 460         if (!hciemu)
 461                 return false;
 462
 463         hook = new0(struct hciemu_command_hook, 1);
 464         if (!hook)
 465                 return false;
 466
 467         hook->function = function;
 468         hook->user_data = user_data;
 469
 470         if (!queue_push_tail(hciemu->post_command_hooks, hook)) {
 471                 free(hook);
 472                 return false;
 473         }
 474
 475         return true;
 476 }
 477
 478 bool hciemu_clear_master_post_command_hooks(struct hciemu *hciemu)
 479 {
 480         if (!hciemu)
 481                 return false;
 482
 483         queue_remove_all(hciemu->post_command_hooks,
 484                                         NULL, NULL, destroy_command_hook);
 485         return true;
 486 }
 487
 488 int hciemu_add_hook(struct hciemu *hciemu, enum hciemu_hook_type type,
 489                                 uint16_t opcode, hciemu_hook_func_t function,
 490                                 void *user_data)
 491 {
 492         enum btdev_hook_type hook_type;
 493
 494         if (!hciemu)
 495                 return -1;
 496
 497         switch (type) {
 498         case HCIEMU_HOOK_PRE_CMD:
 499                 hook_type = BTDEV_HOOK_PRE_CMD;
 500                 break;
 501         case HCIEMU_HOOK_POST_CMD:
 502                 hook_type = BTDEV_HOOK_POST_CMD;
 503                 break;
 504         case HCIEMU_HOOK_PRE_EVT:
 505                 hook_type = BTDEV_HOOK_PRE_EVT;
 506                 break;
 507         case HCIEMU_HOOK_POST_EVT:
 508                 hook_type = BTDEV_HOOK_POST_EVT;
 509                 break;
 510         default:
 511                 return -1;
 512         }
 513
 514         return btdev_add_hook(hciemu->master_dev, hook_type, opcode, function,
 515                                                                 user_data);
 516 }
 517
 518 bool hciemu_del_hook(struct hciemu *hciemu, enum hciemu_hook_type type,
 519                                                                 uint16_t opcode)
 520 {
 521         enum btdev_hook_type hook_type;
 522
 523         if (!hciemu)
 524                 return false;
 525
 526         switch (type) {
 527         case HCIEMU_HOOK_PRE_CMD:
 528                 hook_type = BTDEV_HOOK_PRE_CMD;
 529                 break;
 530         case HCIEMU_HOOK_POST_CMD:
 531                 hook_type = BTDEV_HOOK_POST_CMD;
 532                 break;
 533         case HCIEMU_HOOK_PRE_EVT:
 534                 hook_type = BTDEV_HOOK_PRE_EVT;
 535                 break;
 536         case HCIEMU_HOOK_POST_EVT:
 537                 hook_type = BTDEV_HOOK_POST_EVT;
 538                 break;
 539         default:
 540                 return false;
 541         }
 542
 543         return btdev_del_hook(hciemu->master_dev, hook_type, opcode);
 544 }