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_BREDR;
 247         written = write(fd, create_req, sizeof(create_req));
 248         if (written < 0) {
 249                 close(fd);
 250                 btdev_destroy(btdev);
 251                 return false;
 252         }
 253
 254         hciemu->master_dev = btdev;
 255
 256         hciemu->master_source = create_source_btdev(fd, btdev);
 257
 258         return true;
 259 }
 260
 261 struct bthost *hciemu_client_get_host(struct hciemu *hciemu)
 262 {
 263         if (!hciemu)
 264                 return NULL;
 265
 266         return hciemu->host_stack;
 267 }
 268
 269 static bool create_stack(struct hciemu *hciemu)
 270 {
 271         struct btdev *btdev;
 272         struct bthost *bthost;
 273         int sv[2];
 274
 275         btdev = btdev_create(hciemu->btdev_type, 0x00);
 276         if (!btdev)
 277                 return false;
 278
 279         bthost = bthost_create();
 280         if (!bthost) {
 281                 btdev_destroy(btdev);
 282                 return false;
 283         }
 284
 285         btdev_set_command_handler(btdev, client_command_callback, hciemu);
 286
 287         if (socketpair(AF_UNIX, SOCK_SEQPACKET | SOCK_NONBLOCK | SOCK_CLOEXEC,
 288                                                                 0, sv) < 0) {
 289                 bthost_destroy(bthost);
 290                 btdev_destroy(btdev);
 291                 return false;
 292         }
 293
 294         hciemu->client_dev = btdev;
 295         hciemu->host_stack = bthost;
 296
 297         hciemu->client_source = create_source_btdev(sv[0], btdev);
 298         hciemu->host_source = create_source_bthost(sv[1], bthost);
 299
 300         return true;
 301 }
 302
 303 static gboolean start_stack(gpointer user_data)
 304 {
 305         struct hciemu *hciemu = user_data;
 306
 307         bthost_start(hciemu->host_stack);
 308
 309         return FALSE;
 310 }
 311
 312 struct hciemu *hciemu_new(enum hciemu_type type)
 313 {
 314         struct hciemu *hciemu;
 315
 316         hciemu = new0(struct hciemu, 1);
 317         if (!hciemu)
 318                 return NULL;
 319
 320         switch (type) {
 321         case HCIEMU_TYPE_BREDRLE:
 322                 hciemu->btdev_type = BTDEV_TYPE_BREDRLE;
 323                 break;
 324         case HCIEMU_TYPE_BREDR:
 325                 hciemu->btdev_type = BTDEV_TYPE_BREDR;
 326                 break;
 327         case HCIEMU_TYPE_LE:
 328                 hciemu->btdev_type = BTDEV_TYPE_LE;
 329                 break;
 330         case HCIEMU_TYPE_LEGACY:
 331                 hciemu->btdev_type = BTDEV_TYPE_BREDR20;
 332                 break;
 333         default:
 334                 return NULL;
 335         }
 336
 337         hciemu->post_command_hooks = queue_new();
 338         if (!hciemu->post_command_hooks) {
 339                 free(hciemu);
 340                 return NULL;
 341         }
 342
 343         if (!create_vhci(hciemu)) {
 344                 queue_destroy(hciemu->post_command_hooks, NULL);
 345                 free(hciemu);
 346                 return NULL;
 347         }
 348
 349         if (!create_stack(hciemu)) {
 350                 g_source_remove(hciemu->master_source);
 351                 btdev_destroy(hciemu->master_dev);
 352                 queue_destroy(hciemu->post_command_hooks, NULL);
 353                 free(hciemu);
 354                 return NULL;
 355         }
 356
 357         g_idle_add(start_stack, hciemu);
 358
 359         return hciemu_ref(hciemu);
 360 }
 361
 362 struct hciemu *hciemu_ref(struct hciemu *hciemu)
 363 {
 364         if (!hciemu)
 365                 return NULL;
 366
 367         __sync_fetch_and_add(&hciemu->ref_count, 1);
 368
 369         return hciemu;
 370 }
 371
 372 void hciemu_unref(struct hciemu *hciemu)
 373 {
 374         if (!hciemu)
 375                 return;
 376
 377         if (__sync_sub_and_fetch(&hciemu->ref_count, 1))
 378                 return;
 379
 380         queue_destroy(hciemu->post_command_hooks, destroy_command_hook);
 381
 382         g_source_remove(hciemu->host_source);
 383         g_source_remove(hciemu->client_source);
 384         g_source_remove(hciemu->master_source);
 385
 386         bthost_destroy(hciemu->host_stack);
 387         btdev_destroy(hciemu->client_dev);
 388         btdev_destroy(hciemu->master_dev);
 389
 390         free(hciemu);
 391 }
 392
 393 const char *hciemu_get_address(struct hciemu *hciemu)
 394 {
 395         const uint8_t *addr;
 396
 397         if (!hciemu || !hciemu->master_dev)
 398                 return NULL;
 399
 400         addr = btdev_get_bdaddr(hciemu->master_dev);
 401         sprintf(hciemu->bdaddr_str, "%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X",
 402                         addr[5], addr[4], addr[3], addr[2], addr[1], addr[0]);
 403         return hciemu->bdaddr_str;
 404 }
 405
 406 uint8_t *hciemu_get_features(struct hciemu *hciemu)
 407 {
 408         if (!hciemu || !hciemu->master_dev)
 409                 return NULL;
 410
 411         return btdev_get_features(hciemu->master_dev);
 412 }
 413
 414 const uint8_t *hciemu_get_master_bdaddr(struct hciemu *hciemu)
 415 {
 416         if (!hciemu || !hciemu->master_dev)
 417                 return NULL;
 418
 419         return btdev_get_bdaddr(hciemu->master_dev);
 420 }
 421
 422 const uint8_t *hciemu_get_client_bdaddr(struct hciemu *hciemu)
 423 {
 424         if (!hciemu || !hciemu->client_dev)
 425                 return NULL;
 426
 427         return btdev_get_bdaddr(hciemu->client_dev);
 428 }
 429
 430 uint8_t hciemu_get_master_scan_enable(struct hciemu *hciemu)
 431 {
 432         if (!hciemu || !hciemu->master_dev)
 433                 return 0;
 434
 435         return btdev_get_scan_enable(hciemu->master_dev);
 436 }
 437
 438 uint8_t hciemu_get_master_le_scan_enable(struct hciemu *hciemu)
 439 {
 440         if (!hciemu || !hciemu->master_dev)
 441                 return 0;
 442
 443         return btdev_get_le_scan_enable(hciemu->master_dev);
 444 }
 445
 446 bool hciemu_add_master_post_command_hook(struct hciemu *hciemu,
 447                         hciemu_command_func_t function, void *user_data)
 448 {
 449         struct hciemu_command_hook *hook;
 450
 451         if (!hciemu)
 452                 return false;
 453
 454         hook = new0(struct hciemu_command_hook, 1);
 455         if (!hook)
 456                 return false;
 457
 458         hook->function = function;
 459         hook->user_data = user_data;
 460
 461         if (!queue_push_tail(hciemu->post_command_hooks, hook)) {
 462                 free(hook);
 463                 return false;
 464         }
 465
 466         return true;
 467 }
 468
 469 int hciemu_add_hook(struct hciemu *hciemu, enum hciemu_hook_type type,
 470                                 uint16_t opcode, hciemu_hook_func_t function,
 471                                 void *user_data)
 472 {
 473         enum btdev_hook_type hook_type;
 474
 475         if (!hciemu)
 476                 return -1;
 477
 478         switch (type) {
 479         case HCIEMU_HOOK_PRE_CMD:
 480                 hook_type = BTDEV_HOOK_PRE_CMD;
 481                 break;
 482         case HCIEMU_HOOK_POST_CMD:
 483                 hook_type = BTDEV_HOOK_POST_CMD;
 484                 break;
 485         case HCIEMU_HOOK_PRE_EVT:
 486                 hook_type = BTDEV_HOOK_PRE_EVT;
 487                 break;
 488         case HCIEMU_HOOK_POST_EVT:
 489                 hook_type = BTDEV_HOOK_POST_EVT;
 490                 break;
 491         default:
 492                 return -1;
 493         }
 494
 495         return btdev_add_hook(hciemu->master_dev, hook_type, opcode, function,
 496                                                                 user_data);
 497 }
 498
 499 bool hciemu_del_hook(struct hciemu *hciemu, enum hciemu_hook_type type,
 500                                                                 uint16_t opcode)
 501 {
 502         enum btdev_hook_type hook_type;
 503
 504         if (!hciemu)
 505                 return false;
 506
 507         switch (type) {
 508         case HCIEMU_HOOK_PRE_CMD:
 509                 hook_type = BTDEV_HOOK_PRE_CMD;
 510                 break;
 511         case HCIEMU_HOOK_POST_CMD:
 512                 hook_type = BTDEV_HOOK_POST_CMD;
 513                 break;
 514         case HCIEMU_HOOK_PRE_EVT:
 515                 hook_type = BTDEV_HOOK_PRE_EVT;
 516                 break;
 517         case HCIEMU_HOOK_POST_EVT:
 518                 hook_type = BTDEV_HOOK_POST_EVT;
 519                 break;
 520         default:
 521                 return false;
 522         }
 523
 524         return btdev_del_hook(hciemu->master_dev, hook_type, opcode);
 525 }