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         default:
 331                 return NULL;
 332         }
 333
 334         hciemu->post_command_hooks = queue_new();
 335         if (!hciemu->post_command_hooks) {
 336                 free(hciemu);
 337                 return NULL;
 338         }
 339
 340         if (!create_vhci(hciemu)) {
 341                 queue_destroy(hciemu->post_command_hooks, NULL);
 342                 free(hciemu);
 343                 return NULL;
 344         }
 345
 346         if (!create_stack(hciemu)) {
 347                 g_source_remove(hciemu->master_source);
 348                 btdev_destroy(hciemu->master_dev);
 349                 queue_destroy(hciemu->post_command_hooks, NULL);
 350                 free(hciemu);
 351                 return NULL;
 352         }
 353
 354         g_idle_add(start_stack, hciemu);
 355
 356         return hciemu_ref(hciemu);
 357 }
 358
 359 struct hciemu *hciemu_ref(struct hciemu *hciemu)
 360 {
 361         if (!hciemu)
 362                 return NULL;
 363
 364         __sync_fetch_and_add(&hciemu->ref_count, 1);
 365
 366         return hciemu;
 367 }
 368
 369 void hciemu_unref(struct hciemu *hciemu)
 370 {
 371         if (!hciemu)
 372                 return;
 373
 374         if (__sync_sub_and_fetch(&hciemu->ref_count, 1))
 375                 return;
 376
 377         queue_destroy(hciemu->post_command_hooks, destroy_command_hook);
 378
 379         g_source_remove(hciemu->host_source);
 380         g_source_remove(hciemu->client_source);
 381         g_source_remove(hciemu->master_source);
 382
 383         bthost_destroy(hciemu->host_stack);
 384         btdev_destroy(hciemu->client_dev);
 385         btdev_destroy(hciemu->master_dev);
 386
 387         free(hciemu);
 388 }
 389
 390 const char *hciemu_get_address(struct hciemu *hciemu)
 391 {
 392         const uint8_t *addr;
 393
 394         if (!hciemu || !hciemu->master_dev)
 395                 return NULL;
 396
 397         addr = btdev_get_bdaddr(hciemu->master_dev);
 398         sprintf(hciemu->bdaddr_str, "%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X",
 399                         addr[5], addr[4], addr[3], addr[2], addr[1], addr[0]);
 400         return hciemu->bdaddr_str;
 401 }
 402
 403 uint8_t *hciemu_get_features(struct hciemu *hciemu)
 404 {
 405         if (!hciemu || !hciemu->master_dev)
 406                 return NULL;
 407
 408         return btdev_get_features(hciemu->master_dev);
 409 }
 410
 411 const uint8_t *hciemu_get_master_bdaddr(struct hciemu *hciemu)
 412 {
 413         if (!hciemu || !hciemu->master_dev)
 414                 return NULL;
 415
 416         return btdev_get_bdaddr(hciemu->master_dev);
 417 }
 418
 419 const uint8_t *hciemu_get_client_bdaddr(struct hciemu *hciemu)
 420 {
 421         if (!hciemu || !hciemu->client_dev)
 422                 return NULL;
 423
 424         return btdev_get_bdaddr(hciemu->client_dev);
 425 }
 426
 427 bool hciemu_add_master_post_command_hook(struct hciemu *hciemu,
 428                         hciemu_command_func_t function, void *user_data)
 429 {
 430         struct hciemu_command_hook *hook;
 431
 432         if (!hciemu)
 433                 return false;
 434
 435         hook = new0(struct hciemu_command_hook, 1);
 436         if (!hook)
 437                 return false;
 438
 439         hook->function = function;
 440         hook->user_data = user_data;
 441
 442         if (!queue_push_tail(hciemu->post_command_hooks, hook)) {
 443                 free(hook);
 444                 return false;
 445         }
 446
 447         return true;
 448 }
 449
 450 int hciemu_add_hook(struct hciemu *hciemu, enum hciemu_hook_type type,
 451                                 uint16_t opcode, hciemu_hook_func_t function,
 452                                 void *user_data)
 453 {
 454         enum btdev_hook_type hook_type;
 455
 456         if (!hciemu)
 457                 return -1;
 458
 459         switch (type) {
 460         case HCIEMU_HOOK_PRE_CMD:
 461                 hook_type = BTDEV_HOOK_PRE_CMD;
 462                 break;
 463         case HCIEMU_HOOK_POST_CMD:
 464                 hook_type = BTDEV_HOOK_POST_CMD;
 465                 break;
 466         case HCIEMU_HOOK_PRE_EVT:
 467                 hook_type = BTDEV_HOOK_PRE_EVT;
 468                 break;
 469         case HCIEMU_HOOK_POST_EVT:
 470                 hook_type = BTDEV_HOOK_POST_EVT;
 471                 break;
 472         default:
 473                 return -1;
 474         }
 475
 476         return btdev_add_hook(hciemu->master_dev, hook_type, opcode, function,
 477                                                                 user_data);
 478 }
 479
 480 bool hciemu_del_hook(struct hciemu *hciemu, enum hciemu_hook_type type,
 481                                                                 uint16_t opcode)
 482 {
 483         enum btdev_hook_type hook_type;
 484
 485         if (!hciemu)
 486                 return false;
 487
 488         switch (type) {
 489         case HCIEMU_HOOK_PRE_CMD:
 490                 hook_type = BTDEV_HOOK_PRE_CMD;
 491                 break;
 492         case HCIEMU_HOOK_POST_CMD:
 493                 hook_type = BTDEV_HOOK_POST_CMD;
 494                 break;
 495         case HCIEMU_HOOK_PRE_EVT:
 496                 hook_type = BTDEV_HOOK_PRE_EVT;
 497                 break;
 498         case HCIEMU_HOOK_POST_EVT:
 499                 hook_type = BTDEV_HOOK_POST_EVT;
 500                 break;
 501         default:
 502                 return false;
 503         }
 504
 505         return btdev_del_hook(hciemu->master_dev, hook_type, opcode);
 506 }