src/shared/hci.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 <unistd.h>
  30 #include <string.h>
  31 #include <sys/socket.h>
  32 #include <sys/uio.h>
  33
  34 #include "monitor/bt.h"
  35 #include "src/shared/mainloop.h"
  36 #include "src/shared/io.h"
  37 #include "src/shared/util.h"
  38 #include "src/shared/queue.h"
  39 #include "src/shared/hci.h"
  40
  41 #define BTPROTO_HCI     1
  42 struct sockaddr_hci {
  43         sa_family_t     hci_family;
  44         unsigned short  hci_dev;
  45         unsigned short  hci_channel;
  46 };
  47 #define HCI_CHANNEL_RAW         0
  48 #define HCI_CHANNEL_USER        1
  49
  50 #define SOL_HCI         0
  51 #define HCI_FILTER      2
  52 struct hci_filter {
  53         uint32_t type_mask;
  54         uint32_t event_mask[2];
  55         uint16_t opcode;
  56 };
  57
  58 struct bt_hci {
  59         int ref_count;
  60         struct io *io;
  61         bool is_stream;
  62         bool writer_active;
  63         uint8_t num_cmds;
  64         unsigned int next_cmd_id;
  65         unsigned int next_evt_id;
  66         struct queue *cmd_queue;
  67         struct queue *rsp_queue;
  68         struct queue *evt_list;
  69 };
  70
  71 struct cmd {
  72         unsigned int id;
  73         uint16_t opcode;
  74         void *data;
  75         uint8_t size;
  76         bt_hci_callback_func_t callback;
  77         bt_hci_destroy_func_t destroy;
  78         void *user_data;
  79 };
  80
  81 struct evt {
  82         unsigned int id;
  83         uint8_t event;
  84         bt_hci_callback_func_t callback;
  85         bt_hci_destroy_func_t destroy;
  86         void *user_data;
  87 };
  88
  89 static void cmd_free(void *data)
  90 {
  91         struct cmd *cmd = data;
  92
  93         if (cmd->destroy)
  94                 cmd->destroy(cmd->user_data);
  95
  96         free(cmd->data);
  97         free(cmd);
  98 }
  99
 100 static void evt_free(void *data)
 101 {
 102         struct evt *evt = data;
 103
 104         if (evt->destroy)
 105                 evt->destroy(evt->user_data);
 106
 107         free(evt);
 108 }
 109
 110 static void send_command(struct bt_hci *hci, uint16_t opcode,
 111                                                 void *data, uint8_t size)
 112 {
 113         uint8_t type = BT_H4_CMD_PKT;
 114         struct bt_hci_cmd_hdr hdr;
 115         struct iovec iov[3];
 116         int iovcnt;
 117
 118         if (hci->num_cmds < 1)
 119                 return;
 120
 121         hdr.opcode = cpu_to_le16(opcode);
 122         hdr.plen = size;
 123
 124         iov[0].iov_base = &type;
 125         iov[0].iov_len  = 1;
 126         iov[1].iov_base = &hdr;
 127         iov[1].iov_len  = sizeof(hdr);
 128
 129         if (size > 0) {
 130                 iov[2].iov_base = data;
 131                 iov[2].iov_len  = size;
 132                 iovcnt = 3;
 133         } else
 134                 iovcnt = 2;
 135
 136         if (io_send(hci->io, iov, iovcnt) < 0)
 137                 return;
 138
 139         hci->num_cmds--;
 140 }
 141
 142 static bool io_write_callback(struct io *io, void *user_data)
 143 {
 144         struct bt_hci *hci = user_data;
 145         struct cmd *cmd;
 146
 147         cmd = queue_pop_head(hci->cmd_queue);
 148         if (cmd) {
 149                 send_command(hci, cmd->opcode, cmd->data, cmd->size);
 150                 queue_push_tail(hci->rsp_queue, cmd);
 151         }
 152
 153         hci->writer_active = false;
 154
 155         return false;
 156 }
 157
 158 static void wakeup_writer(struct bt_hci *hci)
 159 {
 160         if (hci->writer_active)
 161                 return;
 162
 163         if (hci->num_cmds < 1)
 164                 return;
 165
 166         if (queue_isempty(hci->cmd_queue))
 167                 return;
 168
 169         if (!io_set_write_handler(hci->io, io_write_callback, hci, NULL))
 170                 return;
 171
 172         hci->writer_active = true;
 173 }
 174
 175 static bool match_cmd_opcode(const void *a, const void *b)
 176 {
 177         const struct cmd *cmd = a;
 178         uint16_t opcode = PTR_TO_UINT(b);
 179
 180         return cmd->opcode == opcode;
 181 }
 182
 183 static void process_response(struct bt_hci *hci, uint16_t opcode,
 184                                         const void *data, size_t size)
 185 {
 186         struct cmd *cmd;
 187
 188         if (opcode == BT_HCI_CMD_NOP)
 189                 goto done;
 190
 191         cmd = queue_remove_if(hci->rsp_queue, match_cmd_opcode,
 192                                                 UINT_TO_PTR(opcode));
 193         if (!cmd)
 194                 return;
 195
 196         if (cmd->callback)
 197                 cmd->callback(data, size, cmd->user_data);
 198
 199         cmd_free(cmd);
 200
 201 done:
 202         wakeup_writer(hci);
 203 }
 204
 205 static void process_notify(void *data, void *user_data)
 206 {
 207         struct bt_hci_evt_hdr *hdr = user_data;
 208         struct evt *evt = data;
 209
 210         if (evt->event == hdr->evt)
 211                 evt->callback(user_data + sizeof(struct bt_hci_evt_hdr),
 212                                                 hdr->plen, evt->user_data);
 213 }
 214
 215 static void process_event(struct bt_hci *hci, const void *data, size_t size)
 216 {
 217         const struct bt_hci_evt_hdr *hdr = data;
 218         const struct bt_hci_evt_cmd_complete *cc;
 219         const struct bt_hci_evt_cmd_status *cs;
 220
 221         if (size < sizeof(struct bt_hci_evt_hdr))
 222                 return;
 223
 224         data += sizeof(struct bt_hci_evt_hdr);
 225         size -= sizeof(struct bt_hci_evt_hdr);
 226
 227         if (hdr->plen != size)
 228                 return;
 229
 230         switch (hdr->evt) {
 231         case BT_HCI_EVT_CMD_COMPLETE:
 232                 if (size < sizeof(*cc))
 233                         return;
 234                 cc = data;
 235                 hci->num_cmds = cc->ncmd;
 236                 process_response(hci, le16_to_cpu(cc->opcode),
 237                                                 data + sizeof(*cc),
 238                                                 size - sizeof(*cc));
 239                 break;
 240
 241         case BT_HCI_EVT_CMD_STATUS:
 242                 if (size < sizeof(*cs))
 243                         return;
 244                 cs = data;
 245                 hci->num_cmds = cs->ncmd;
 246                 process_response(hci, le16_to_cpu(cs->opcode), &cs->status, 1);
 247                 break;
 248
 249         default:
 250                 queue_foreach(hci->evt_list, process_notify, (void *) hdr);
 251                 break;
 252         }
 253 }
 254
 255 static bool io_read_callback(struct io *io, void *user_data)
 256 {
 257         struct bt_hci *hci = user_data;
 258         uint8_t buf[512];
 259         ssize_t len;
 260         int fd;
 261
 262         fd = io_get_fd(hci->io);
 263         if (fd < 0)
 264                 return false;
 265
 266         if (hci->is_stream)
 267                 return false;
 268
 269         len = read(fd, buf, sizeof(buf));
 270         if (len < 0)
 271                 return false;
 272
 273         if (len < 1)
 274                 return true;
 275
 276         switch (buf[0]) {
 277         case BT_H4_EVT_PKT:
 278                 process_event(hci, buf + 1, len - 1);
 279                 break;
 280         }
 281
 282         return true;
 283 }
 284
 285 static struct bt_hci *create_hci(int fd)
 286 {
 287         struct bt_hci *hci;
 288
 289         if (fd < 0)
 290                 return NULL;
 291
 292         hci = new0(struct bt_hci, 1);
 293         if (!hci)
 294                 return NULL;
 295
 296         hci->io = io_new(fd);
 297         if (!hci->io) {
 298                 free(hci);
 299                 return NULL;
 300         }
 301
 302         hci->is_stream = true;
 303         hci->writer_active = false;
 304         hci->num_cmds = 1;
 305         hci->next_cmd_id = 1;
 306         hci->next_evt_id = 1;
 307
 308         hci->cmd_queue = queue_new();
 309         if (!hci->cmd_queue) {
 310                 io_destroy(hci->io);
 311                 free(hci);
 312                 return NULL;
 313         }
 314
 315         hci->rsp_queue = queue_new();
 316         if (!hci->rsp_queue) {
 317                 queue_destroy(hci->cmd_queue, NULL);
 318                 io_destroy(hci->io);
 319                 free(hci);
 320                 return NULL;
 321         }
 322
 323         hci->evt_list = queue_new();
 324         if (!hci->evt_list) {
 325                 queue_destroy(hci->rsp_queue, NULL);
 326                 queue_destroy(hci->cmd_queue, NULL);
 327                 io_destroy(hci->io);
 328                 free(hci);
 329                 return NULL;
 330         }
 331
 332         if (!io_set_read_handler(hci->io, io_read_callback, hci, NULL)) {
 333                 queue_destroy(hci->evt_list, NULL);
 334                 queue_destroy(hci->rsp_queue, NULL);
 335                 queue_destroy(hci->cmd_queue, NULL);
 336                 io_destroy(hci->io);
 337                 free(hci);
 338                 return NULL;
 339         }
 340
 341         return bt_hci_ref(hci);
 342 }
 343
 344 struct bt_hci *bt_hci_new(int fd)
 345 {
 346         struct bt_hci *hci;
 347
 348         hci = create_hci(fd);
 349         if (!hci)
 350                 return NULL;
 351
 352         return hci;
 353 }
 354
 355 static int create_socket(uint16_t index, uint16_t channel)
 356 {
 357         struct sockaddr_hci addr;
 358         int fd;
 359
 360         fd = socket(PF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC | SOCK_NONBLOCK,
 361                                                                 BTPROTO_HCI);
 362         if (fd < 0)
 363                 return -1;
 364
 365         memset(&addr, 0, sizeof(addr));
 366         addr.hci_family = AF_BLUETOOTH;
 367         addr.hci_dev = index;
 368         addr.hci_channel = channel;
 369
 370         if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 371                 close(fd);
 372                 return -1;
 373         }
 374
 375         return fd;
 376 }
 377
 378 struct bt_hci *bt_hci_new_user_channel(uint16_t index)
 379 {
 380         struct bt_hci *hci;
 381         int fd;
 382
 383         fd = create_socket(index, HCI_CHANNEL_USER);
 384         if (fd < 0)
 385                 return NULL;
 386
 387         hci = create_hci(fd);
 388         if (!hci) {
 389                 close(fd);
 390                 return NULL;
 391         }
 392
 393         hci->is_stream = false;
 394
 395         bt_hci_set_close_on_unref(hci, true);
 396
 397         return hci;
 398 }
 399
 400 struct bt_hci *bt_hci_new_raw_device(uint16_t index)
 401 {
 402         struct bt_hci *hci;
 403         struct hci_filter flt;
 404         int fd;
 405
 406         fd = create_socket(index, HCI_CHANNEL_RAW);
 407         if (fd < 0)
 408                 return NULL;
 409
 410         memset(&flt, 0, sizeof(flt));
 411         flt.type_mask = 1 << BT_H4_EVT_PKT;
 412         flt.event_mask[0] = 0xffffffff;
 413         flt.event_mask[1] = 0xffffffff;
 414
 415         if (setsockopt(fd, SOL_HCI, HCI_FILTER, &flt, sizeof(flt)) < 0) {
 416                 close(fd);
 417                 return NULL;
 418         }
 419
 420         hci = create_hci(fd);
 421         if (!hci) {
 422                 close(fd);
 423                 return NULL;
 424         }
 425
 426         hci->is_stream = false;
 427
 428         bt_hci_set_close_on_unref(hci, true);
 429
 430         return hci;
 431 }
 432
 433 struct bt_hci *bt_hci_ref(struct bt_hci *hci)
 434 {
 435         if (!hci)
 436                 return NULL;
 437
 438         __sync_fetch_and_add(&hci->ref_count, 1);
 439
 440         return hci;
 441 }
 442
 443 void bt_hci_unref(struct bt_hci *hci)
 444 {
 445         if (!hci)
 446                 return;
 447
 448         if (__sync_sub_and_fetch(&hci->ref_count, 1))
 449                 return;
 450
 451         queue_destroy(hci->evt_list, evt_free);
 452         queue_destroy(hci->cmd_queue, cmd_free);
 453         queue_destroy(hci->rsp_queue, cmd_free);
 454
 455         io_destroy(hci->io);
 456
 457         free(hci);
 458 }
 459
 460 bool bt_hci_set_close_on_unref(struct bt_hci *hci, bool do_close)
 461 {
 462         if (!hci)
 463                 return false;
 464
 465         return io_set_close_on_destroy(hci->io, do_close);
 466 }
 467
 468 unsigned int bt_hci_send(struct bt_hci *hci, uint16_t opcode,
 469                                 const void *data, uint8_t size,
 470                                 bt_hci_callback_func_t callback,
 471                                 void *user_data, bt_hci_destroy_func_t destroy)
 472 {
 473         struct cmd *cmd;
 474
 475         if (!hci)
 476                 return 0;
 477
 478         cmd = new0(struct cmd, 1);
 479         if (!cmd)
 480                 return 0;
 481
 482         cmd->opcode = opcode;
 483         cmd->size = size;
 484
 485         if (cmd->size > 0) {
 486                 cmd->data = malloc(cmd->size);
 487                 if (!cmd->data) {
 488                         free(cmd);
 489                         return 0;
 490                 }
 491
 492                 memcpy(cmd->data, data, cmd->size);
 493         }
 494
 495         if (hci->next_cmd_id < 1)
 496                 hci->next_cmd_id = 1;
 497
 498         cmd->id = hci->next_cmd_id++;
 499
 500         cmd->callback = callback;
 501         cmd->destroy = destroy;
 502         cmd->user_data = user_data;
 503
 504         if (!queue_push_tail(hci->cmd_queue, cmd)) {
 505                 free(cmd->data);
 506                 free(cmd);
 507                 return 0;
 508         }
 509
 510         wakeup_writer(hci);
 511
 512         return cmd->id;
 513 }
 514
 515 static bool match_cmd_id(const void *a, const void *b)
 516 {
 517         const struct cmd *cmd = a;
 518         unsigned int id = PTR_TO_UINT(b);
 519
 520         return cmd->id == id;
 521 }
 522
 523 bool bt_hci_cancel(struct bt_hci *hci, unsigned int id)
 524 {
 525         struct cmd *cmd;
 526
 527         if (!hci || !id)
 528                 return false;
 529
 530         cmd = queue_remove_if(hci->cmd_queue, match_cmd_id, UINT_TO_PTR(id));
 531         if (!cmd) {
 532                 cmd = queue_remove_if(hci->rsp_queue, match_cmd_id,
 533                                                         UINT_TO_PTR(id));
 534                 if (!cmd)
 535                         return false;
 536         }
 537
 538         cmd_free(cmd);
 539
 540         wakeup_writer(hci);
 541
 542         return true;
 543 }
 544
 545 bool bt_hci_flush(struct bt_hci *hci)
 546 {
 547         if (!hci)
 548                 return false;
 549
 550         if (hci->writer_active) {
 551                 io_set_write_handler(hci->io, NULL, NULL, NULL);
 552                 hci->writer_active = false;
 553         }
 554
 555         queue_remove_all(hci->cmd_queue, NULL, NULL, cmd_free);
 556         queue_remove_all(hci->rsp_queue, NULL, NULL, cmd_free);
 557
 558         return true;
 559 }
 560
 561 unsigned int bt_hci_register(struct bt_hci *hci, uint8_t event,
 562                                 bt_hci_callback_func_t callback,
 563                                 void *user_data, bt_hci_destroy_func_t destroy)
 564 {
 565         struct evt *evt;
 566
 567         if (!hci)
 568                 return 0;
 569
 570         evt = new0(struct evt, 1);
 571         if (!evt)
 572                 return 0;
 573
 574         evt->event = event;
 575
 576         if (hci->next_evt_id < 1)
 577                 hci->next_evt_id = 1;
 578
 579         evt->id = hci->next_evt_id++;
 580
 581         evt->callback = callback;
 582         evt->destroy = destroy;
 583         evt->user_data = user_data;
 584
 585         if (!queue_push_tail(hci->evt_list, evt)) {
 586                 free(evt);
 587                 return 0;
 588         }
 589
 590         return evt->id;
 591 }
 592
 593 static bool match_evt_id(const void *a, const void *b)
 594 {
 595         const struct evt *evt = a;
 596         unsigned int id = PTR_TO_UINT(b);
 597
 598         return evt->id == id;
 599 }
 600
 601 bool bt_hci_unregister(struct bt_hci *hci, unsigned int id)
 602 {
 603         struct evt *evt;
 604
 605         if (!hci || !id)
 606                 return false;
 607
 608         evt = queue_remove_if(hci->evt_list, match_evt_id, UINT_TO_PTR(id));
 609         if (!evt)
 610                 return false;
 611
 612         evt_free(evt);
 613
 614         return true;
 615 }