src/shared/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 <bluetooth/bluetooth.h>
  40 #include <bluetooth/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 "src/shared/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 write_callback(const void *data, uint16_t len, void *user_data)
 111 {
 112         GIOChannel *channel = user_data;
 113         ssize_t written;
 114         int fd;
 115
 116         fd = g_io_channel_unix_get_fd(channel);
 117
 118         written = write(fd, data, len);
 119         if (written < 0)
 120                 return;
 121 }
 122
 123 static gboolean receive_bthost(GIOChannel *channel, GIOCondition condition,
 124                                                         gpointer user_data)
 125 {
 126         struct bthost *bthost = user_data;
 127         unsigned char buf[4096];
 128         ssize_t len;
 129         int fd;
 130
 131         if (condition & (G_IO_NVAL | G_IO_ERR | G_IO_HUP))
 132                 return FALSE;
 133
 134         fd = g_io_channel_unix_get_fd(channel);
 135
 136         len = read(fd, buf, sizeof(buf));
 137         if (len < 0)
 138                 return FALSE;
 139
 140         bthost_receive_h4(bthost, buf, len);
 141
 142         return TRUE;
 143 }
 144
 145 static guint create_source_bthost(int fd, struct bthost *bthost)
 146 {
 147         GIOChannel *channel;
 148         guint source;
 149
 150         channel = g_io_channel_unix_new(fd);
 151
 152         g_io_channel_set_close_on_unref(channel, TRUE);
 153         g_io_channel_set_encoding(channel, NULL, NULL);
 154         g_io_channel_set_buffered(channel, FALSE);
 155
 156         bthost_set_send_handler(bthost, write_callback, channel);
 157
 158         source = g_io_add_watch_full(channel, G_PRIORITY_DEFAULT,
 159                                 G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
 160                                 receive_bthost, bthost, NULL);
 161
 162         g_io_channel_unref(channel);
 163
 164         return source;
 165 }
 166
 167 static gboolean receive_btdev(GIOChannel *channel, GIOCondition condition,
 168                                                         gpointer user_data)
 169 {
 170         struct btdev *btdev = user_data;
 171         unsigned char buf[4096];
 172         ssize_t len;
 173         int fd;
 174
 175         if (condition & (G_IO_NVAL | G_IO_ERR | G_IO_HUP))
 176                 return FALSE;
 177
 178         fd = g_io_channel_unix_get_fd(channel);
 179
 180         len = read(fd, buf, sizeof(buf));
 181         if (len < 1)
 182                 return FALSE;
 183
 184         switch (buf[0]) {
 185         case BT_H4_CMD_PKT:
 186         case BT_H4_ACL_PKT:
 187         case BT_H4_SCO_PKT:
 188                 btdev_receive_h4(btdev, buf, len);
 189                 break;
 190         }
 191
 192         return TRUE;
 193 }
 194
 195 static guint create_source_btdev(int fd, struct btdev *btdev)
 196 {
 197         GIOChannel *channel;
 198         guint source;
 199
 200         channel = g_io_channel_unix_new(fd);
 201
 202         g_io_channel_set_close_on_unref(channel, TRUE);
 203         g_io_channel_set_encoding(channel, NULL, NULL);
 204         g_io_channel_set_buffered(channel, FALSE);
 205
 206         btdev_set_send_handler(btdev, write_callback, channel);
 207
 208         source = g_io_add_watch_full(channel, G_PRIORITY_DEFAULT,
 209                                 G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
 210                                 receive_btdev, btdev, NULL);
 211
 212         g_io_channel_unref(channel);
 213
 214         return source;
 215 }
 216
 217 static bool create_vhci(struct hciemu *hciemu)
 218 {
 219         struct btdev *btdev;
 220         uint8_t create_req[2];
 221         ssize_t written;
 222         int fd;
 223
 224         btdev = btdev_create(hciemu->btdev_type, 0x00);
 225         if (!btdev)
 226                 return false;
 227
 228         btdev_set_command_handler(btdev, master_command_callback, hciemu);
 229
 230         fd = open("/dev/vhci", O_RDWR | O_NONBLOCK | O_CLOEXEC);
 231         if (fd < 0) {
 232                 perror("Opening /dev/vhci failed");
 233                 btdev_destroy(btdev);
 234                 return false;
 235         }
 236
 237         create_req[0] = HCI_VENDOR_PKT;
 238         create_req[1] = HCI_BREDR;
 239         written = write(fd, create_req, sizeof(create_req));
 240         if (written < 0) {
 241                 close(fd);
 242                 btdev_destroy(btdev);
 243                 return false;
 244         }
 245
 246         hciemu->master_dev = btdev;
 247
 248         hciemu->master_source = create_source_btdev(fd, btdev);
 249
 250         return true;
 251 }
 252
 253 struct bthost *hciemu_client_get_host(struct hciemu *hciemu)
 254 {
 255         if (!hciemu)
 256                 return NULL;
 257
 258         return hciemu->host_stack;
 259 }
 260
 261 static bool create_stack(struct hciemu *hciemu)
 262 {
 263         struct btdev *btdev;
 264         struct bthost *bthost;
 265         int sv[2];
 266
 267         btdev = btdev_create(hciemu->btdev_type, 0x00);
 268         if (!btdev)
 269                 return false;
 270
 271         bthost = bthost_create();
 272         if (!bthost) {
 273                 btdev_destroy(btdev);
 274                 return false;
 275         }
 276
 277         btdev_set_command_handler(btdev, client_command_callback, hciemu);
 278
 279         if (socketpair(AF_UNIX, SOCK_SEQPACKET | SOCK_NONBLOCK | SOCK_CLOEXEC,
 280                                                                 0, sv) < 0) {
 281                 bthost_destroy(bthost);
 282                 btdev_destroy(btdev);
 283                 return false;
 284         }
 285
 286         hciemu->client_dev = btdev;
 287         hciemu->host_stack = bthost;
 288
 289         hciemu->client_source = create_source_btdev(sv[0], btdev);
 290         hciemu->host_source = create_source_bthost(sv[1], bthost);
 291
 292         return true;
 293 }
 294
 295 static gboolean start_stack(gpointer user_data)
 296 {
 297         struct hciemu *hciemu = user_data;
 298
 299         bthost_start(hciemu->host_stack);
 300
 301         return FALSE;
 302 }
 303
 304 struct hciemu *hciemu_new(enum hciemu_type type)
 305 {
 306         struct hciemu *hciemu;
 307
 308         hciemu = new0(struct hciemu, 1);
 309         if (!hciemu)
 310                 return NULL;
 311
 312         switch (type) {
 313         case HCIEMU_TYPE_BREDRLE:
 314                 hciemu->btdev_type = BTDEV_TYPE_BREDRLE;
 315                 break;
 316         case HCIEMU_TYPE_BREDR:
 317                 hciemu->btdev_type = BTDEV_TYPE_BREDR;
 318                 break;
 319         case HCIEMU_TYPE_LE:
 320                 hciemu->btdev_type = BTDEV_TYPE_LE;
 321                 break;
 322         default:
 323                 return NULL;
 324         }
 325
 326         hciemu->post_command_hooks = queue_new();
 327         if (!hciemu->post_command_hooks) {
 328                 free(hciemu);
 329                 return NULL;
 330         }
 331
 332         if (!create_vhci(hciemu)) {
 333                 queue_destroy(hciemu->post_command_hooks, NULL);
 334                 free(hciemu);
 335                 return NULL;
 336         }
 337
 338         if (!create_stack(hciemu)) {
 339                 g_source_remove(hciemu->master_source);
 340                 btdev_destroy(hciemu->master_dev);
 341                 queue_destroy(hciemu->post_command_hooks, NULL);
 342                 free(hciemu);
 343                 return NULL;
 344         }
 345
 346         g_idle_add(start_stack, hciemu);
 347
 348         return hciemu_ref(hciemu);
 349 }
 350
 351 struct hciemu *hciemu_ref(struct hciemu *hciemu)
 352 {
 353         if (!hciemu)
 354                 return NULL;
 355
 356         __sync_fetch_and_add(&hciemu->ref_count, 1);
 357
 358         return hciemu;
 359 }
 360
 361 void hciemu_unref(struct hciemu *hciemu)
 362 {
 363         if (!hciemu)
 364                 return;
 365
 366         if (__sync_sub_and_fetch(&hciemu->ref_count, 1))
 367                 return;
 368
 369         queue_destroy(hciemu->post_command_hooks, destroy_command_hook);
 370
 371         bthost_stop(hciemu->host_stack);
 372
 373         g_source_remove(hciemu->host_source);
 374         g_source_remove(hciemu->client_source);
 375         g_source_remove(hciemu->master_source);
 376
 377         bthost_destroy(hciemu->host_stack);
 378         btdev_destroy(hciemu->client_dev);
 379         btdev_destroy(hciemu->master_dev);
 380
 381         free(hciemu);
 382 }
 383
 384 const char *hciemu_get_address(struct hciemu *hciemu)
 385 {
 386         const uint8_t *addr;
 387
 388         if (!hciemu || !hciemu->master_dev)
 389                 return NULL;
 390
 391         addr = btdev_get_bdaddr(hciemu->master_dev);
 392         sprintf(hciemu->bdaddr_str, "%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X",
 393                         addr[5], addr[4], addr[3], addr[2], addr[1], addr[0]);
 394         return hciemu->bdaddr_str;
 395 }
 396
 397 uint8_t *hciemu_get_features(struct hciemu *hciemu)
 398 {
 399         if (!hciemu || !hciemu->master_dev)
 400                 return NULL;
 401
 402         return btdev_get_features(hciemu->master_dev);
 403 }
 404
 405 const uint8_t *hciemu_get_master_bdaddr(struct hciemu *hciemu)
 406 {
 407         if (!hciemu || !hciemu->master_dev)
 408                 return NULL;
 409
 410         return btdev_get_bdaddr(hciemu->master_dev);
 411 }
 412
 413 const uint8_t *hciemu_get_client_bdaddr(struct hciemu *hciemu)
 414 {
 415         if (!hciemu || !hciemu->client_dev)
 416                 return NULL;
 417
 418         return btdev_get_bdaddr(hciemu->client_dev);
 419 }
 420
 421 bool hciemu_add_master_post_command_hook(struct hciemu *hciemu,
 422                         hciemu_command_func_t function, void *user_data)
 423 {
 424         struct hciemu_command_hook *hook;
 425
 426         if (!hciemu)
 427                 return false;
 428
 429         hook = new0(struct hciemu_command_hook, 1);
 430         if (!hook)
 431                 return false;
 432
 433         hook->function = function;
 434         hook->user_data = user_data;
 435
 436         if (!queue_push_tail(hciemu->post_command_hooks, hook)) {
 437                 free(hook);
 438                 return false;
 439         }
 440
 441         return true;
 442 }
 443
 444 int hciemu_add_hook(struct hciemu *hciemu, enum hciemu_hook_type type,
 445                                 uint16_t opcode, hciemu_hook_func_t function,
 446                                 void *user_data)
 447 {
 448         enum btdev_hook_type hook_type;
 449
 450         if (!hciemu)
 451                 return -1;
 452
 453         switch (type) {
 454         case HCIEMU_HOOK_PRE_CMD:
 455                 hook_type = BTDEV_HOOK_PRE_CMD;
 456                 break;
 457         case HCIEMU_HOOK_POST_CMD:
 458                 hook_type = BTDEV_HOOK_POST_CMD;
 459                 break;
 460         case HCIEMU_HOOK_PRE_EVT:
 461                 hook_type = BTDEV_HOOK_PRE_EVT;
 462                 break;
 463         case HCIEMU_HOOK_POST_EVT:
 464                 hook_type = BTDEV_HOOK_POST_EVT;
 465                 break;
 466         default:
 467                 return -1;
 468         }
 469
 470         return btdev_add_hook(hciemu->master_dev, hook_type, opcode, function,
 471                                                                 user_data);
 472 }
 473
 474 bool hciemu_del_hook(struct hciemu *hciemu, enum hciemu_hook_type type,
 475                                                                 uint16_t opcode)
 476 {
 477         enum btdev_hook_type hook_type;
 478
 479         if (!hciemu)
 480                 return false;
 481
 482         switch (type) {
 483         case HCIEMU_HOOK_PRE_CMD:
 484                 hook_type = BTDEV_HOOK_PRE_CMD;
 485                 break;
 486         case HCIEMU_HOOK_POST_CMD:
 487                 hook_type = BTDEV_HOOK_POST_CMD;
 488                 break;
 489         case HCIEMU_HOOK_PRE_EVT:
 490                 hook_type = BTDEV_HOOK_PRE_EVT;
 491                 break;
 492         case HCIEMU_HOOK_POST_EVT:
 493                 hook_type = BTDEV_HOOK_POST_EVT;
 494                 break;
 495         default:
 496                 return false;
 497         }
 498
 499         return btdev_del_hook(hciemu->master_dev, hook_type, opcode);
 500 }