src/shared/hciemu.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2012  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 <sys/socket.h>
  34
  35 #include <glib.h>
  36
  37 #include "monitor/bt.h"
  38 #include "emulator/btdev.h"
  39 #include "emulator/bthost.h"
  40
  41 #include "hciemu.h"
  42
  43 struct hciemu {
  44         int ref_count;
  45         enum btdev_type btdev_type;
  46         struct bthost *host_stack;
  47         struct btdev *master_dev;
  48         struct btdev *client_dev;
  49         guint host_source;
  50         guint master_source;
  51         guint client_source;
  52         GList *post_command_hooks;
  53         char bdaddr_str[18];
  54 };
  55
  56 struct hciemu_command_hook {
  57         hciemu_command_func_t function;
  58         void *user_data;
  59 };
  60
  61 static void destroy_command_hook(gpointer data, gpointer user_data)
  62 {
  63         struct hciemu_command_hook *hook = data;
  64
  65         g_free(hook);
  66 }
  67
  68 static void master_command_callback(uint16_t opcode,
  69                                 const void *data, uint8_t len,
  70                                 btdev_callback callback, void *user_data)
  71 {
  72         struct hciemu *hciemu = user_data;
  73         GList *list;
  74
  75         btdev_command_default(callback);
  76
  77         for (list = g_list_first(hciemu->post_command_hooks); list;
  78                                                 list = g_list_next(list)) {
  79                 struct hciemu_command_hook *hook = list->data;
  80
  81                 if (hook->function)
  82                         hook->function(opcode, data, len, hook->user_data);
  83         }
  84 }
  85
  86 static void client_command_callback(uint16_t opcode,
  87                                 const void *data, uint8_t len,
  88                                 btdev_callback callback, void *user_data)
  89 {
  90         btdev_command_default(callback);
  91 }
  92
  93 static void write_callback(const void *data, uint16_t len, void *user_data)
  94 {
  95         GIOChannel *channel = user_data;
  96         ssize_t written;
  97         int fd;
  98
  99         fd = g_io_channel_unix_get_fd(channel);
 100
 101         written = write(fd, data, len);
 102         if (written < 0)
 103                 return;
 104 }
 105
 106 static gboolean receive_bthost(GIOChannel *channel, GIOCondition condition,
 107                                                         gpointer user_data)
 108 {
 109         struct bthost *bthost = user_data;
 110         unsigned char buf[4096];
 111         ssize_t len;
 112         int fd;
 113
 114         if (condition & (G_IO_NVAL | G_IO_ERR | G_IO_HUP))
 115                 return FALSE;
 116
 117         fd = g_io_channel_unix_get_fd(channel);
 118
 119         len = read(fd, buf, sizeof(buf));
 120         if (len < 0)
 121                 return FALSE;
 122
 123         bthost_receive_h4(bthost, buf, len);
 124
 125         return TRUE;
 126 }
 127
 128 static guint create_source_bthost(int fd, struct bthost *bthost)
 129 {
 130         GIOChannel *channel;
 131         guint source;
 132
 133         channel = g_io_channel_unix_new(fd);
 134
 135         g_io_channel_set_close_on_unref(channel, TRUE);
 136         g_io_channel_set_encoding(channel, NULL, NULL);
 137         g_io_channel_set_buffered(channel, FALSE);
 138
 139         bthost_set_send_handler(bthost, write_callback, channel);
 140
 141         source = g_io_add_watch_full(channel, G_PRIORITY_DEFAULT,
 142                                 G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
 143                                 receive_bthost, bthost, NULL);
 144
 145         g_io_channel_unref(channel);
 146
 147         return source;
 148 }
 149
 150 static gboolean receive_btdev(GIOChannel *channel, GIOCondition condition,
 151                                                         gpointer user_data)
 152 {
 153         struct btdev *btdev = user_data;
 154         unsigned char buf[4096];
 155         ssize_t len;
 156         int fd;
 157
 158         if (condition & (G_IO_NVAL | G_IO_ERR | G_IO_HUP))
 159                 return FALSE;
 160
 161         fd = g_io_channel_unix_get_fd(channel);
 162
 163         len = read(fd, buf, sizeof(buf));
 164         if (len < 0)
 165                 return FALSE;
 166
 167         btdev_receive_h4(btdev, buf, len);
 168
 169         return TRUE;
 170 }
 171
 172 static guint create_source_btdev(int fd, struct btdev *btdev)
 173 {
 174         GIOChannel *channel;
 175         guint source;
 176
 177         channel = g_io_channel_unix_new(fd);
 178
 179         g_io_channel_set_close_on_unref(channel, TRUE);
 180         g_io_channel_set_encoding(channel, NULL, NULL);
 181         g_io_channel_set_buffered(channel, FALSE);
 182
 183         btdev_set_send_handler(btdev, write_callback, channel);
 184
 185         source = g_io_add_watch_full(channel, G_PRIORITY_DEFAULT,
 186                                 G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
 187                                 receive_btdev, btdev, NULL);
 188
 189         g_io_channel_unref(channel);
 190
 191         return source;
 192 }
 193
 194 static bool create_vhci(struct hciemu *hciemu)
 195 {
 196         struct btdev *btdev;
 197         int fd;
 198
 199         btdev = btdev_create(hciemu->btdev_type, 0x00);
 200         if (!btdev)
 201                 return false;
 202
 203         btdev_set_command_handler(btdev, master_command_callback, hciemu);
 204
 205         fd = open("/dev/vhci", O_RDWR | O_NONBLOCK | O_CLOEXEC);
 206         if (fd < 0) {
 207                 btdev_destroy(btdev);
 208                 return false;
 209         }
 210
 211         hciemu->master_dev = btdev;
 212
 213         hciemu->master_source = create_source_btdev(fd, btdev);
 214
 215         return true;
 216 }
 217
 218 static bool create_stack(struct hciemu *hciemu)
 219 {
 220         struct btdev *btdev;
 221         struct bthost *bthost;
 222         int sv[2];
 223
 224         btdev = btdev_create(hciemu->btdev_type, 0x00);
 225         if (!btdev)
 226                 return false;
 227
 228         bthost = bthost_create();
 229         if (!bthost) {
 230                 btdev_destroy(btdev);
 231                 return false;
 232         }
 233
 234         btdev_set_command_handler(btdev, client_command_callback, hciemu);
 235
 236         if (socketpair(AF_UNIX, SOCK_SEQPACKET | SOCK_NONBLOCK | SOCK_CLOEXEC,
 237                                                                 0, sv) < 0) {
 238                 bthost_destroy(bthost);
 239                 btdev_destroy(btdev);
 240                 return false;
 241         }
 242
 243         hciemu->client_dev = btdev;
 244         hciemu->host_stack = bthost;
 245
 246         hciemu->client_source = create_source_btdev(sv[0], btdev);
 247         hciemu->host_source = create_source_bthost(sv[1], bthost);
 248
 249         return true;
 250 }
 251
 252 static gboolean start_stack(gpointer user_data)
 253 {
 254         struct hciemu *hciemu = user_data;
 255
 256         bthost_start(hciemu->host_stack);
 257
 258         return FALSE;
 259 }
 260
 261 struct hciemu *hciemu_new(enum hciemu_type type)
 262 {
 263         struct hciemu *hciemu;
 264
 265         hciemu = g_try_new0(struct hciemu, 1);
 266         if (!hciemu)
 267                 return NULL;
 268
 269         switch (type) {
 270         case HCIEMU_TYPE_BREDRLE:
 271                 hciemu->btdev_type = BTDEV_TYPE_BREDRLE;
 272                 break;
 273         case HCIEMU_TYPE_BREDR:
 274                 hciemu->btdev_type = BTDEV_TYPE_BREDR;
 275                 break;
 276         case HCIEMU_TYPE_LE:
 277                 hciemu->btdev_type = BTDEV_TYPE_LE;
 278                 break;
 279         default:
 280                 return NULL;
 281         }
 282
 283         if (!create_vhci(hciemu)) {
 284                 g_free(hciemu);
 285                 return NULL;
 286         }
 287
 288         if (!create_stack(hciemu)) {
 289                 g_source_remove(hciemu->master_source);
 290                 btdev_destroy(hciemu->master_dev);
 291                 g_free(hciemu);
 292                 return NULL;
 293         }
 294
 295         g_idle_add(start_stack, hciemu);
 296
 297         return hciemu_ref(hciemu);
 298 }
 299
 300 struct hciemu *hciemu_ref(struct hciemu *hciemu)
 301 {
 302         if (!hciemu)
 303                 return NULL;
 304
 305         __sync_fetch_and_add(&hciemu->ref_count, 1);
 306
 307         return hciemu;
 308 }
 309
 310 void hciemu_unref(struct hciemu *hciemu)
 311 {
 312         if (!hciemu)
 313                 return;
 314
 315         if (__sync_sub_and_fetch(&hciemu->ref_count, 1) > 0)
 316                 return;
 317
 318         g_list_foreach(hciemu->post_command_hooks, destroy_command_hook, NULL);
 319         g_list_free(hciemu->post_command_hooks);
 320
 321         bthost_stop(hciemu->host_stack);
 322
 323         g_source_remove(hciemu->host_source);
 324         g_source_remove(hciemu->client_source);
 325         g_source_remove(hciemu->master_source);
 326
 327         bthost_destroy(hciemu->host_stack);
 328         btdev_destroy(hciemu->client_dev);
 329         btdev_destroy(hciemu->master_dev);
 330
 331         g_free(hciemu);
 332 }
 333
 334 const char *hciemu_get_address(struct hciemu *hciemu)
 335 {
 336         const uint8_t *addr;
 337
 338         if (!hciemu || !hciemu->master_dev)
 339                 return NULL;
 340
 341         addr = btdev_get_bdaddr(hciemu->master_dev);
 342         sprintf(hciemu->bdaddr_str, "%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X",
 343                         addr[5], addr[4], addr[3], addr[2], addr[1], addr[0]);
 344         return hciemu->bdaddr_str;
 345 }
 346
 347 bool hciemu_add_master_post_command_hook(struct hciemu *hciemu,
 348                         hciemu_command_func_t function, void *user_data)
 349 {
 350         struct hciemu_command_hook *hook;
 351
 352         if (!hciemu)
 353                 return false;
 354
 355         hook = g_try_new0(struct hciemu_command_hook, 1);
 356         if (!hook)
 357                 return false;
 358
 359         hook->function = function;
 360         hook->user_data = user_data;
 361
 362         hciemu->post_command_hooks = g_list_append(hciemu->post_command_hooks,
 363                                                                         hook);
 364
 365         return true;
 366 }