qemu/hw/bt/hci.c

   1 /*
   2  * QEMU Bluetooth HCI logic.
   3  *
   4  * Copyright (C) 2007 OpenMoko, Inc.
   5  * Copyright (C) 2008 Andrzej Zaborowski  <balrog@zabor.org>
   6  *
   7  * This program is free software; you can redistribute it and/or
   8  * modify it under the terms of the GNU General Public License as
   9  * published by the Free Software Foundation; either version 2 of
  10  * the License, or (at your option) any later version.
  11  *
  12  * This program is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License
  18  * along with this program; if not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include "qemu/osdep.h"
  22 #include "qapi/error.h"
  23 #include "qemu-common.h"
  24 #include "qemu/timer.h"
  25 #include "hw/usb.h"
  26 #include "sysemu/bt.h"
  27 #include "hw/bt.h"
  28 #include "qapi/qmp/qerror.h"
  29 #include "sysemu/replay.h"
  30 #include "qemu/cutils.h"
  31
  32 struct bt_hci_s {
  33     uint8_t *(*evt_packet)(void *opaque);
  34     void (*evt_submit)(void *opaque, int len);
  35     void *opaque;
  36     uint8_t evt_buf[256];
  37
  38     uint8_t acl_buf[4096];
  39     int acl_len;
  40
  41     uint16_t asb_handle;
  42     uint16_t psb_handle;
  43
  44     int last_cmd;       /* Note: Always little-endian */
  45
  46     struct bt_device_s *conn_req_host;
  47
  48     struct {
  49         int inquire;
  50         int periodic;
  51         int responses_left;
  52         int responses;
  53         QEMUTimer *inquiry_done;
  54         QEMUTimer *inquiry_next;
  55         int inquiry_length;
  56         int inquiry_period;
  57         int inquiry_mode;
  58
  59 #define HCI_HANDLE_OFFSET       0x20
  60 #define HCI_HANDLES_MAX         0x10
  61         struct bt_hci_master_link_s {
  62             struct bt_link_s *link;
  63             void (*lmp_acl_data)(struct bt_link_s *link,
  64                             const uint8_t *data, int start, int len);
  65             QEMUTimer *acl_mode_timer;
  66         } handle[HCI_HANDLES_MAX];
  67         uint32_t role_bmp;
  68         int last_handle;
  69         int connecting;
  70         bdaddr_t awaiting_bdaddr[HCI_HANDLES_MAX];
  71     } lm;
  72
  73     uint8_t event_mask[8];
  74     uint16_t voice_setting;     /* Notw: Always little-endian */
  75     uint16_t conn_accept_tout;
  76     QEMUTimer *conn_accept_timer;
  77
  78     struct HCIInfo info;
  79     struct bt_device_s device;
  80
  81     Error *replay_blocker;
  82 };
  83
  84 #define DEFAULT_RSSI_DBM        20
  85
  86 #define hci_from_info(ptr)      container_of((ptr), struct bt_hci_s, info)
  87 #define hci_from_device(ptr)    container_of((ptr), struct bt_hci_s, device)
  88
  89 struct bt_hci_link_s {
  90     struct bt_link_s btlink;
  91     uint16_t handle;    /* Local */
  92 };
  93
  94 /* LMP layer emulation */
  95 #if 0
  96 static void bt_submit_lmp(struct bt_device_s *bt, int length, uint8_t *data)
  97 {
  98     int resp, resplen, error, op, tr;
  99     uint8_t respdata[17];
 100
 101     if (length < 1)
 102         return;
 103
 104     tr = *data & 1;
 105     op = *(data ++) >> 1;
 106     resp = LMP_ACCEPTED;
 107     resplen = 2;
 108     respdata[1] = op;
 109     error = 0;
 110     length --;
 111
 112     if (op >= 0x7c) {   /* Extended opcode */
 113         op |= *(data ++) << 8;
 114         resp = LMP_ACCEPTED_EXT;
 115         resplen = 4;
 116         respdata[0] = op >> 8;
 117         respdata[1] = op & 0xff;
 118         length --;
 119     }
 120
 121     switch (op) {
 122     case LMP_ACCEPTED:
 123         /* data[0]      Op code
 124          */
 125         if (length < 1) {
 126             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 127             goto not_accepted;
 128         }
 129         resp = 0;
 130         break;
 131
 132     case LMP_ACCEPTED_EXT:
 133         /* data[0]      Escape op code
 134          * data[1]      Extended op code
 135          */
 136         if (length < 2) {
 137             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 138             goto not_accepted;
 139         }
 140         resp = 0;
 141         break;
 142
 143     case LMP_NOT_ACCEPTED:
 144         /* data[0]      Op code
 145          * data[1]      Error code
 146          */
 147         if (length < 2) {
 148             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 149             goto not_accepted;
 150         }
 151         resp = 0;
 152         break;
 153
 154     case LMP_NOT_ACCEPTED_EXT:
 155         /* data[0]      Op code
 156          * data[1]      Extended op code
 157          * data[2]      Error code
 158          */
 159         if (length < 3) {
 160             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 161             goto not_accepted;
 162         }
 163         resp = 0;
 164         break;
 165
 166     case LMP_HOST_CONNECTION_REQ:
 167         break;
 168
 169     case LMP_SETUP_COMPLETE:
 170         resp = LMP_SETUP_COMPLETE;
 171         resplen = 1;
 172         bt->setup = 1;
 173         break;
 174
 175     case LMP_DETACH:
 176         /* data[0]      Error code
 177          */
 178         if (length < 1) {
 179             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 180             goto not_accepted;
 181         }
 182         bt->setup = 0;
 183         resp = 0;
 184         break;
 185
 186     case LMP_SUPERVISION_TIMEOUT:
 187         /* data[0,1]    Supervision timeout
 188          */
 189         if (length < 2) {
 190             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 191             goto not_accepted;
 192         }
 193         resp = 0;
 194         break;
 195
 196     case LMP_QUALITY_OF_SERVICE:
 197         resp = 0;
 198         /* Fall through */
 199     case LMP_QOS_REQ:
 200         /* data[0,1]    Poll interval
 201          * data[2]      N(BC)
 202          */
 203         if (length < 3) {
 204             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 205             goto not_accepted;
 206         }
 207         break;
 208
 209     case LMP_MAX_SLOT:
 210         resp = 0;
 211         /* Fall through */
 212     case LMP_MAX_SLOT_REQ:
 213         /* data[0]      Max slots
 214          */
 215         if (length < 1) {
 216             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 217             goto not_accepted;
 218         }
 219         break;
 220
 221     case LMP_AU_RAND:
 222     case LMP_IN_RAND:
 223     case LMP_COMB_KEY:
 224         /* data[0-15]   Random number
 225          */
 226         if (length < 16) {
 227             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 228             goto not_accepted;
 229         }
 230         if (op == LMP_AU_RAND) {
 231             if (bt->key_present) {
 232                 resp = LMP_SRES;
 233                 resplen = 5;
 234                 /* XXX: [Part H] Section 6.1 on page 801 */
 235             } else {
 236                 error = HCI_PIN_OR_KEY_MISSING;
 237                 goto not_accepted;
 238             }
 239         } else if (op == LMP_IN_RAND) {
 240             error = HCI_PAIRING_NOT_ALLOWED;
 241             goto not_accepted;
 242         } else {
 243             /* XXX: [Part H] Section 3.2 on page 779 */
 244             resp = LMP_UNIT_KEY;
 245             resplen = 17;
 246             memcpy(respdata + 1, bt->key, 16);
 247
 248             error = HCI_UNIT_LINK_KEY_USED;
 249             goto not_accepted;
 250         }
 251         break;
 252
 253     case LMP_UNIT_KEY:
 254         /* data[0-15]   Key
 255          */
 256         if (length < 16) {
 257             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 258             goto not_accepted;
 259         }
 260         memcpy(bt->key, data, 16);
 261         bt->key_present = 1;
 262         break;
 263
 264     case LMP_SRES:
 265         /* data[0-3]    Authentication response
 266          */
 267         if (length < 4) {
 268             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 269             goto not_accepted;
 270         }
 271         break;
 272
 273     case LMP_CLKOFFSET_REQ:
 274         resp = LMP_CLKOFFSET_RES;
 275         resplen = 3;
 276         respdata[1] = 0x33;
 277         respdata[2] = 0x33;
 278         break;
 279
 280     case LMP_CLKOFFSET_RES:
 281         /* data[0,1]    Clock offset
 282          * (Slave to master only)
 283          */
 284         if (length < 2) {
 285             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 286             goto not_accepted;
 287         }
 288         break;
 289
 290     case LMP_VERSION_REQ:
 291     case LMP_VERSION_RES:
 292         /* data[0]      VersNr
 293          * data[1,2]    CompId
 294          * data[3,4]    SubVersNr
 295          */
 296         if (length < 5) {
 297             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 298             goto not_accepted;
 299         }
 300         if (op == LMP_VERSION_REQ) {
 301             resp = LMP_VERSION_RES;
 302             resplen = 6;
 303             respdata[1] = 0x20;
 304             respdata[2] = 0xff;
 305             respdata[3] = 0xff;
 306             respdata[4] = 0xff;
 307             respdata[5] = 0xff;
 308         } else
 309             resp = 0;
 310         break;
 311
 312     case LMP_FEATURES_REQ:
 313     case LMP_FEATURES_RES:
 314         /* data[0-7]    Features
 315          */
 316         if (length < 8) {
 317             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 318             goto not_accepted;
 319         }
 320         if (op == LMP_FEATURES_REQ) {
 321             resp = LMP_FEATURES_RES;
 322             resplen = 9;
 323             respdata[1] = (bt->lmp_caps >> 0) & 0xff;
 324             respdata[2] = (bt->lmp_caps >> 8) & 0xff;
 325             respdata[3] = (bt->lmp_caps >> 16) & 0xff;
 326             respdata[4] = (bt->lmp_caps >> 24) & 0xff;
 327             respdata[5] = (bt->lmp_caps >> 32) & 0xff;
 328             respdata[6] = (bt->lmp_caps >> 40) & 0xff;
 329             respdata[7] = (bt->lmp_caps >> 48) & 0xff;
 330             respdata[8] = (bt->lmp_caps >> 56) & 0xff;
 331         } else
 332             resp = 0;
 333         break;
 334
 335     case LMP_NAME_REQ:
 336         /* data[0]      Name offset
 337          */
 338         if (length < 1) {
 339             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 340             goto not_accepted;
 341         }
 342         resp = LMP_NAME_RES;
 343         resplen = 17;
 344         respdata[1] = data[0];
 345         respdata[2] = strlen(bt->lmp_name);
 346         memset(respdata + 3, 0x00, 14);
 347         if (respdata[2] > respdata[1])
 348             memcpy(respdata + 3, bt->lmp_name + respdata[1],
 349                             respdata[2] - respdata[1]);
 350         break;
 351
 352     case LMP_NAME_RES:
 353         /* data[0]      Name offset
 354          * data[1]      Name length
 355          * data[2-15]   Name fragment
 356          */
 357         if (length < 16) {
 358             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 359             goto not_accepted;
 360         }
 361         resp = 0;
 362         break;
 363
 364     default:
 365         error = HCI_UNKNOWN_LMP_PDU;
 366         /* Fall through */
 367     not_accepted:
 368         if (op >> 8) {
 369             resp = LMP_NOT_ACCEPTED_EXT;
 370             resplen = 5;
 371             respdata[0] = op >> 8;
 372             respdata[1] = op & 0xff;
 373             respdata[2] = error;
 374         } else {
 375             resp = LMP_NOT_ACCEPTED;
 376             resplen = 3;
 377             respdata[0] = op & 0xff;
 378             respdata[1] = error;
 379         }
 380     }
 381
 382     if (resp == 0)
 383         return;
 384
 385     if (resp >> 8) {
 386         respdata[0] = resp >> 8;
 387         respdata[1] = resp & 0xff;
 388     } else
 389         respdata[0] = resp & 0xff;
 390
 391     respdata[0] <<= 1;
 392     respdata[0] |= tr;
 393 }
 394
 395 static void bt_submit_raw_acl(struct bt_piconet_s *net, int length, uint8_t *data)
 396 {
 397     struct bt_device_s *slave;
 398     if (length < 1)
 399         return;
 400
 401     slave = 0;
 402 #if 0
 403     slave = net->slave;
 404 #endif
 405
 406     switch (data[0] & 3) {
 407     case LLID_ACLC:
 408         bt_submit_lmp(slave, length - 1, data + 1);
 409         break;
 410     case LLID_ACLU_START:
 411 #if 0
 412         bt_sumbit_l2cap(slave, length - 1, data + 1, (data[0] >> 2) & 1);
 413         breka;
 414 #endif
 415     default:
 416     case LLID_ACLU_CONT:
 417         break;
 418     }
 419 }
 420 #endif
 421
 422 /* HCI layer emulation */
 423
 424 /* Note: we could ignore endiannes because unswapped handles will still
 425  * be valid as connection identifiers for the guest - they don't have to
 426  * be continuously allocated.  We do it though, to preserve similar
 427  * behaviour between hosts.  Some things, like the BD_ADDR cannot be
 428  * preserved though (for example if a real hci is used).  */
 429 #ifdef HOST_WORDS_BIGENDIAN
 430 # define HNDL(raw)      bswap16(raw)
 431 #else
 432 # define HNDL(raw)      (raw)
 433 #endif
 434
 435 static const uint8_t bt_event_reserved_mask[8] = {
 436     0xff, 0x9f, 0xfb, 0xff, 0x07, 0x18, 0x00, 0x00,
 437 };
 438
 439
 440 static void null_hci_send(struct HCIInfo *hci, const uint8_t *data, int len)
 441 {
 442 }
 443
 444 static int null_hci_addr_set(struct HCIInfo *hci, const uint8_t *bd_addr)
 445 {
 446     return -ENOTSUP;
 447 }
 448
 449 struct HCIInfo null_hci = {
 450     .cmd_send = null_hci_send,
 451     .sco_send = null_hci_send,
 452     .acl_send = null_hci_send,
 453     .bdaddr_set = null_hci_addr_set,
 454 };
 455
 456
 457 static inline uint8_t *bt_hci_event_start(struct bt_hci_s *hci,
 458                 int evt, int len)
 459 {
 460     uint8_t *packet, mask;
 461     int mask_byte;
 462
 463     if (len > 255) {
 464         fprintf(stderr, "%s: HCI event params too long (%ib)\n",
 465                         __FUNCTION__, len);
 466         exit(-1);
 467     }
 468
 469     mask_byte = (evt - 1) >> 3;
 470     mask = 1 << ((evt - 1) & 3);
 471     if (mask & bt_event_reserved_mask[mask_byte] & ~hci->event_mask[mask_byte])
 472         return NULL;
 473
 474     packet = hci->evt_packet(hci->opaque);
 475     packet[0] = evt;
 476     packet[1] = len;
 477
 478     return &packet[2];
 479 }
 480
 481 static inline void bt_hci_event(struct bt_hci_s *hci, int evt,
 482                 void *params, int len)
 483 {
 484     uint8_t *packet = bt_hci_event_start(hci, evt, len);
 485
 486     if (!packet)
 487         return;
 488
 489     if (len)
 490         memcpy(packet, params, len);
 491
 492     hci->evt_submit(hci->opaque, len + 2);
 493 }
 494
 495 static inline void bt_hci_event_status(struct bt_hci_s *hci, int status)
 496 {
 497     evt_cmd_status params = {
 498         .status = status,
 499         .ncmd   = 1,
 500         .opcode = hci->last_cmd,
 501     };
 502
 503     bt_hci_event(hci, EVT_CMD_STATUS, &params, EVT_CMD_STATUS_SIZE);
 504 }
 505
 506 static inline void bt_hci_event_complete(struct bt_hci_s *hci,
 507                 void *ret, int len)
 508 {
 509     uint8_t *packet = bt_hci_event_start(hci, EVT_CMD_COMPLETE,
 510                     len + EVT_CMD_COMPLETE_SIZE);
 511     evt_cmd_complete *params = (evt_cmd_complete *) packet;
 512
 513     if (!packet)
 514         return;
 515
 516     params->ncmd        = 1;
 517     params->opcode      = hci->last_cmd;
 518     if (len)
 519         memcpy(&packet[EVT_CMD_COMPLETE_SIZE], ret, len);
 520
 521     hci->evt_submit(hci->opaque, len + EVT_CMD_COMPLETE_SIZE + 2);
 522 }
 523
 524 static void bt_hci_inquiry_done(void *opaque)
 525 {
 526     struct bt_hci_s *hci = (struct bt_hci_s *) opaque;
 527     uint8_t status = HCI_SUCCESS;
 528
 529     if (!hci->lm.periodic)
 530         hci->lm.inquire = 0;
 531
 532     /* The specification is inconsistent about this one.  Page 565 reads
 533      * "The event parameters of Inquiry Complete event will have a summary
 534      * of the result from the Inquiry process, which reports the number of
 535      * nearby Bluetooth devices that responded [so hci->responses].", but
 536      * Event Parameters (see page 729) has only Status.  */
 537     bt_hci_event(hci, EVT_INQUIRY_COMPLETE, &status, 1);
 538 }
 539
 540 static void bt_hci_inquiry_result_standard(struct bt_hci_s *hci,
 541                 struct bt_device_s *slave)
 542 {
 543     inquiry_info params = {
 544         .num_responses          = 1,
 545         .bdaddr                 = BAINIT(&slave->bd_addr),
 546         .pscan_rep_mode         = 0x00, /* R0 */
 547         .pscan_period_mode      = 0x00, /* P0 - deprecated */
 548         .pscan_mode             = 0x00, /* Standard scan - deprecated */
 549         .dev_class[0]           = slave->class[0],
 550         .dev_class[1]           = slave->class[1],
 551         .dev_class[2]           = slave->class[2],
 552         /* TODO: return the clkoff *differenece* */
 553         .clock_offset           = slave->clkoff,        /* Note: no swapping */
 554     };
 555
 556     bt_hci_event(hci, EVT_INQUIRY_RESULT, &params, INQUIRY_INFO_SIZE);
 557 }
 558
 559 static void bt_hci_inquiry_result_with_rssi(struct bt_hci_s *hci,
 560                 struct bt_device_s *slave)
 561 {
 562     inquiry_info_with_rssi params = {
 563         .num_responses          = 1,
 564         .bdaddr                 = BAINIT(&slave->bd_addr),
 565         .pscan_rep_mode         = 0x00, /* R0 */
 566         .pscan_period_mode      = 0x00, /* P0 - deprecated */
 567         .dev_class[0]           = slave->class[0],
 568         .dev_class[1]           = slave->class[1],
 569         .dev_class[2]           = slave->class[2],
 570         /* TODO: return the clkoff *differenece* */
 571         .clock_offset           = slave->clkoff,        /* Note: no swapping */
 572         .rssi                   = DEFAULT_RSSI_DBM,
 573     };
 574
 575     bt_hci_event(hci, EVT_INQUIRY_RESULT_WITH_RSSI,
 576                     &params, INQUIRY_INFO_WITH_RSSI_SIZE);
 577 }
 578
 579 static void bt_hci_inquiry_result(struct bt_hci_s *hci,
 580                 struct bt_device_s *slave)
 581 {
 582     if (!slave->inquiry_scan || !hci->lm.responses_left)
 583         return;
 584
 585     hci->lm.responses_left --;
 586     hci->lm.responses ++;
 587
 588     switch (hci->lm.inquiry_mode) {
 589     case 0x00:
 590         bt_hci_inquiry_result_standard(hci, slave);
 591         return;
 592     case 0x01:
 593         bt_hci_inquiry_result_with_rssi(hci, slave);
 594         return;
 595     default:
 596         fprintf(stderr, "%s: bad inquiry mode %02x\n", __FUNCTION__,
 597                         hci->lm.inquiry_mode);
 598         exit(-1);
 599     }
 600 }
 601
 602 static void bt_hci_mod_timer_1280ms(QEMUTimer *timer, int period)
 603 {
 604     timer_mod(timer, qemu_clock_get_ns(QEMU_CLOCK_VIRTUAL) +
 605                      (uint64_t)(period << 7) * 10000000);
 606 }
 607
 608 static void bt_hci_inquiry_start(struct bt_hci_s *hci, int length)
 609 {
 610     struct bt_device_s *slave;
 611
 612     hci->lm.inquiry_length = length;
 613     for (slave = hci->device.net->slave; slave; slave = slave->next)
 614         /* Don't uncover ourselves.  */
 615         if (slave != &hci->device)
 616             bt_hci_inquiry_result(hci, slave);
 617
 618     /* TODO: register for a callback on a new device's addition to the
 619      * scatternet so that if it's added before inquiry_length expires,
 620      * an Inquiry Result is generated immediately.  Alternatively re-loop
 621      * through the devices on the inquiry_length expiration and report
 622      * devices not seen before.  */
 623     if (hci->lm.responses_left)
 624         bt_hci_mod_timer_1280ms(hci->lm.inquiry_done, hci->lm.inquiry_length);
 625     else
 626         bt_hci_inquiry_done(hci);
 627
 628     if (hci->lm.periodic)
 629         bt_hci_mod_timer_1280ms(hci->lm.inquiry_next, hci->lm.inquiry_period);
 630 }
 631
 632 static void bt_hci_inquiry_next(void *opaque)
 633 {
 634     struct bt_hci_s *hci = (struct bt_hci_s *) opaque;
 635
 636     hci->lm.responses_left += hci->lm.responses;
 637     hci->lm.responses = 0;
 638     bt_hci_inquiry_start(hci,  hci->lm.inquiry_length);
 639 }
 640
 641 static inline int bt_hci_handle_bad(struct bt_hci_s *hci, uint16_t handle)
 642 {
 643     return !(handle & HCI_HANDLE_OFFSET) ||
 644             handle >= (HCI_HANDLE_OFFSET | HCI_HANDLES_MAX) ||
 645             !hci->lm.handle[handle & ~HCI_HANDLE_OFFSET].link;
 646 }
 647
 648 static inline int bt_hci_role_master(struct bt_hci_s *hci, uint16_t handle)
 649 {
 650     return !!(hci->lm.role_bmp & (1 << (handle & ~HCI_HANDLE_OFFSET)));
 651 }
 652
 653 static inline struct bt_device_s *bt_hci_remote_dev(struct bt_hci_s *hci,
 654                 uint16_t handle)
 655 {
 656     struct bt_link_s *link = hci->lm.handle[handle & ~HCI_HANDLE_OFFSET].link;
 657
 658     return bt_hci_role_master(hci, handle) ? link->slave : link->host;
 659 }
 660
 661 static void bt_hci_mode_tick(void *opaque);
 662 static void bt_hci_lmp_link_establish(struct bt_hci_s *hci,
 663                 struct bt_link_s *link, int master)
 664 {
 665     hci->lm.handle[hci->lm.last_handle].link = link;
 666
 667     if (master) {
 668         /* We are the master side of an ACL link */
 669         hci->lm.role_bmp |= 1 << hci->lm.last_handle;
 670
 671         hci->lm.handle[hci->lm.last_handle].lmp_acl_data =
 672                 link->slave->lmp_acl_data;
 673     } else {
 674         /* We are the slave side of an ACL link */
 675         hci->lm.role_bmp &= ~(1 << hci->lm.last_handle);
 676
 677         hci->lm.handle[hci->lm.last_handle].lmp_acl_data =
 678                 link->host->lmp_acl_resp;
 679     }
 680
 681     /* Mode */
 682     if (master) {
 683         link->acl_mode = acl_active;
 684         hci->lm.handle[hci->lm.last_handle].acl_mode_timer =
 685                 timer_new_ns(QEMU_CLOCK_VIRTUAL, bt_hci_mode_tick, link);
 686     }
 687 }
 688
 689 static void bt_hci_lmp_link_teardown(struct bt_hci_s *hci, uint16_t handle)
 690 {
 691     handle &= ~HCI_HANDLE_OFFSET;
 692     hci->lm.handle[handle].link = NULL;
 693
 694     if (bt_hci_role_master(hci, handle)) {
 695         timer_del(hci->lm.handle[handle].acl_mode_timer);
 696         timer_free(hci->lm.handle[handle].acl_mode_timer);
 697     }
 698 }
 699
 700 static int bt_hci_connect(struct bt_hci_s *hci, bdaddr_t *bdaddr)
 701 {
 702     struct bt_device_s *slave;
 703     struct bt_link_s link;
 704
 705     for (slave = hci->device.net->slave; slave; slave = slave->next)
 706         if (slave->page_scan && !bacmp(&slave->bd_addr, bdaddr))
 707             break;
 708     if (!slave || slave == &hci->device)
 709         return -ENODEV;
 710
 711     bacpy(&hci->lm.awaiting_bdaddr[hci->lm.connecting ++], &slave->bd_addr);
 712
 713     link.slave = slave;
 714     link.host = &hci->device;
 715     link.slave->lmp_connection_request(&link);  /* Always last */
 716
 717     return 0;
 718 }
 719
 720 static void bt_hci_connection_reject(struct bt_hci_s *hci,
 721                 struct bt_device_s *host, uint8_t because)
 722 {
 723     struct bt_link_s link = {
 724         .slave  = &hci->device,
 725         .host   = host,
 726         /* Rest uninitialised */
 727     };
 728
 729     host->reject_reason = because;
 730     host->lmp_connection_complete(&link);
 731 }
 732
 733 static void bt_hci_connection_reject_event(struct bt_hci_s *hci,
 734                 bdaddr_t *bdaddr)
 735 {
 736     evt_conn_complete params;
 737
 738     params.status       = HCI_NO_CONNECTION;
 739     params.handle       = 0;
 740     bacpy(&params.bdaddr, bdaddr);
 741     params.link_type    = ACL_LINK;
 742     params.encr_mode    = 0x00;         /* Encryption not required */
 743     bt_hci_event(hci, EVT_CONN_COMPLETE, &params, EVT_CONN_COMPLETE_SIZE);
 744 }
 745
 746 static void bt_hci_connection_accept(struct bt_hci_s *hci,
 747                 struct bt_device_s *host)
 748 {
 749     struct bt_hci_link_s *link = g_malloc0(sizeof(struct bt_hci_link_s));
 750     evt_conn_complete params;
 751     uint16_t handle;
 752     uint8_t status = HCI_SUCCESS;
 753     int tries = HCI_HANDLES_MAX;
 754
 755     /* Make a connection handle */
 756     do {
 757         while (hci->lm.handle[++ hci->lm.last_handle].link && -- tries)
 758             hci->lm.last_handle &= HCI_HANDLES_MAX - 1;
 759         handle = hci->lm.last_handle | HCI_HANDLE_OFFSET;
 760     } while ((handle == hci->asb_handle || handle == hci->psb_handle) &&
 761             tries);
 762
 763     if (!tries) {
 764         g_free(link);
 765         bt_hci_connection_reject(hci, host, HCI_REJECTED_LIMITED_RESOURCES);
 766         status = HCI_NO_CONNECTION;
 767         goto complete;
 768     }
 769
 770     link->btlink.slave  = &hci->device;
 771     link->btlink.host   = host;
 772     link->handle = handle;
 773
 774     /* Link established */
 775     bt_hci_lmp_link_establish(hci, &link->btlink, 0);
 776
 777 complete:
 778     params.status       = status;
 779     params.handle       = HNDL(handle);
 780     bacpy(&params.bdaddr, &host->bd_addr);
 781     params.link_type    = ACL_LINK;
 782     params.encr_mode    = 0x00;         /* Encryption not required */
 783     bt_hci_event(hci, EVT_CONN_COMPLETE, &params, EVT_CONN_COMPLETE_SIZE);
 784
 785     /* Neets to be done at the very end because it can trigger a (nested)
 786      * disconnected, in case the other and had cancelled the request
 787      * locally.  */
 788     if (status == HCI_SUCCESS) {
 789         host->reject_reason = 0;
 790         host->lmp_connection_complete(&link->btlink);
 791     }
 792 }
 793
 794 static void bt_hci_lmp_connection_request(struct bt_link_s *link)
 795 {
 796     struct bt_hci_s *hci = hci_from_device(link->slave);
 797     evt_conn_request params;
 798
 799     if (hci->conn_req_host) {
 800         bt_hci_connection_reject(hci, link->host,
 801                                  HCI_REJECTED_LIMITED_RESOURCES);
 802         return;
 803     }
 804     hci->conn_req_host = link->host;
 805     /* TODO: if masked and auto-accept, then auto-accept,
 806      * if masked and not auto-accept, then auto-reject */
 807     /* TODO: kick the hci->conn_accept_timer, timeout after
 808      * hci->conn_accept_tout * 0.625 msec */
 809
 810     bacpy(&params.bdaddr, &link->host->bd_addr);
 811     memcpy(&params.dev_class, &link->host->class, sizeof(params.dev_class));
 812     params.link_type    = ACL_LINK;
 813     bt_hci_event(hci, EVT_CONN_REQUEST, &params, EVT_CONN_REQUEST_SIZE);
 814 }
 815
 816 static void bt_hci_conn_accept_timeout(void *opaque)
 817 {
 818     struct bt_hci_s *hci = (struct bt_hci_s *) opaque;
 819
 820     if (!hci->conn_req_host)
 821         /* Already accepted or rejected.  If the other end cancelled the
 822          * connection request then we still have to reject or accept it
 823          * and then we'll get a disconnect.  */
 824         return;
 825
 826     /* TODO */
 827 }
 828
 829 /* Remove from the list of devices which we wanted to connect to and
 830  * are awaiting a response from.  If the callback sees a response from
 831  * a device which is not on the list it will assume it's a connection
 832  * that's been cancelled by the host in the meantime and immediately
 833  * try to detach the link and send a Connection Complete.  */
 834 static int bt_hci_lmp_connection_ready(struct bt_hci_s *hci,
 835                 bdaddr_t *bdaddr)
 836 {
 837     int i;
 838
 839     for (i = 0; i < hci->lm.connecting; i ++)
 840         if (!bacmp(&hci->lm.awaiting_bdaddr[i], bdaddr)) {
 841             if (i < -- hci->lm.connecting)
 842                 bacpy(&hci->lm.awaiting_bdaddr[i],
 843                                 &hci->lm.awaiting_bdaddr[hci->lm.connecting]);
 844             return 0;
 845         }
 846
 847     return 1;
 848 }
 849
 850 static void bt_hci_lmp_connection_complete(struct bt_link_s *link)
 851 {
 852     struct bt_hci_s *hci = hci_from_device(link->host);
 853     evt_conn_complete params;
 854     uint16_t handle;
 855     uint8_t status = HCI_SUCCESS;
 856     int tries = HCI_HANDLES_MAX;
 857
 858     if (bt_hci_lmp_connection_ready(hci, &link->slave->bd_addr)) {
 859         if (!hci->device.reject_reason)
 860             link->slave->lmp_disconnect_slave(link);
 861         handle = 0;
 862         status = HCI_NO_CONNECTION;
 863         goto complete;
 864     }
 865
 866     if (hci->device.reject_reason) {
 867         handle = 0;
 868         status = hci->device.reject_reason;
 869         goto complete;
 870     }
 871
 872     /* Make a connection handle */
 873     do {
 874         while (hci->lm.handle[++ hci->lm.last_handle].link && -- tries)
 875             hci->lm.last_handle &= HCI_HANDLES_MAX - 1;
 876         handle = hci->lm.last_handle | HCI_HANDLE_OFFSET;
 877     } while ((handle == hci->asb_handle || handle == hci->psb_handle) &&
 878             tries);
 879
 880     if (!tries) {
 881         link->slave->lmp_disconnect_slave(link);
 882         status = HCI_NO_CONNECTION;
 883         goto complete;
 884     }
 885
 886     /* Link established */
 887     link->handle = handle;
 888     bt_hci_lmp_link_establish(hci, link, 1);
 889
 890 complete:
 891     params.status       = status;
 892     params.handle       = HNDL(handle);
 893     params.link_type    = ACL_LINK;
 894     bacpy(&params.bdaddr, &link->slave->bd_addr);
 895     params.encr_mode    = 0x00;         /* Encryption not required */
 896     bt_hci_event(hci, EVT_CONN_COMPLETE, &params, EVT_CONN_COMPLETE_SIZE);
 897 }
 898
 899 static void bt_hci_disconnect(struct bt_hci_s *hci,
 900                 uint16_t handle, int reason)
 901 {
 902     struct bt_link_s *btlink =
 903             hci->lm.handle[handle & ~HCI_HANDLE_OFFSET].link;
 904     struct bt_hci_link_s *link;
 905     evt_disconn_complete params;
 906
 907     if (bt_hci_role_master(hci, handle)) {
 908         btlink->slave->reject_reason = reason;
 909         btlink->slave->lmp_disconnect_slave(btlink);
 910         /* The link pointer is invalid from now on */
 911
 912         goto complete;
 913     }
 914
 915     btlink->host->reject_reason = reason;
 916     btlink->host->lmp_disconnect_master(btlink);
 917
 918     /* We are the slave, we get to clean this burden */
 919     link = (struct bt_hci_link_s *) btlink;
 920     g_free(link);
 921
 922 complete:
 923     bt_hci_lmp_link_teardown(hci, handle);
 924
 925     params.status       = HCI_SUCCESS;
 926     params.handle       = HNDL(handle);
 927     params.reason       = HCI_CONNECTION_TERMINATED;
 928     bt_hci_event(hci, EVT_DISCONN_COMPLETE,
 929                     &params, EVT_DISCONN_COMPLETE_SIZE);
 930 }
 931
 932 /* TODO: use only one function */
 933 static void bt_hci_lmp_disconnect_host(struct bt_link_s *link)
 934 {
 935     struct bt_hci_s *hci = hci_from_device(link->host);
 936     uint16_t handle = link->handle;
 937     evt_disconn_complete params;
 938
 939     bt_hci_lmp_link_teardown(hci, handle);
 940
 941     params.status       = HCI_SUCCESS;
 942     params.handle       = HNDL(handle);
 943     params.reason       = hci->device.reject_reason;
 944     bt_hci_event(hci, EVT_DISCONN_COMPLETE,
 945                     &params, EVT_DISCONN_COMPLETE_SIZE);
 946 }
 947
 948 static void bt_hci_lmp_disconnect_slave(struct bt_link_s *btlink)
 949 {
 950     struct bt_hci_link_s *link = (struct bt_hci_link_s *) btlink;
 951     struct bt_hci_s *hci = hci_from_device(btlink->slave);
 952     uint16_t handle = link->handle;
 953     evt_disconn_complete params;
 954
 955     g_free(link);
 956
 957     bt_hci_lmp_link_teardown(hci, handle);
 958
 959     params.status       = HCI_SUCCESS;
 960     params.handle       = HNDL(handle);
 961     params.reason       = hci->device.reject_reason;
 962     bt_hci_event(hci, EVT_DISCONN_COMPLETE,
 963                     &params, EVT_DISCONN_COMPLETE_SIZE);
 964 }
 965
 966 static int bt_hci_name_req(struct bt_hci_s *hci, bdaddr_t *bdaddr)
 967 {
 968     struct bt_device_s *slave;
 969     evt_remote_name_req_complete params;
 970
 971     for (slave = hci->device.net->slave; slave; slave = slave->next)
 972         if (slave->page_scan && !bacmp(&slave->bd_addr, bdaddr))
 973             break;
 974     if (!slave)
 975         return -ENODEV;
 976
 977     bt_hci_event_status(hci, HCI_SUCCESS);
 978
 979     params.status       = HCI_SUCCESS;
 980     bacpy(&params.bdaddr, &slave->bd_addr);
 981     pstrcpy(params.name, sizeof(params.name), slave->lmp_name ?: "");
 982     bt_hci_event(hci, EVT_REMOTE_NAME_REQ_COMPLETE,
 983                     &params, EVT_REMOTE_NAME_REQ_COMPLETE_SIZE);
 984
 985     return 0;
 986 }
 987
 988 static int bt_hci_features_req(struct bt_hci_s *hci, uint16_t handle)
 989 {
 990     struct bt_device_s *slave;
 991     evt_read_remote_features_complete params;
 992
 993     if (bt_hci_handle_bad(hci, handle))
 994         return -ENODEV;
 995
 996     slave = bt_hci_remote_dev(hci, handle);
 997
 998     bt_hci_event_status(hci, HCI_SUCCESS);
 999
1000     params.status       = HCI_SUCCESS;
1001     params.handle       = HNDL(handle);
1002     params.features[0]  = (slave->lmp_caps >>  0) & 0xff;
1003     params.features[1]  = (slave->lmp_caps >>  8) & 0xff;
1004     params.features[2]  = (slave->lmp_caps >> 16) & 0xff;
1005     params.features[3]  = (slave->lmp_caps >> 24) & 0xff;
1006     params.features[4]  = (slave->lmp_caps >> 32) & 0xff;
1007     params.features[5]  = (slave->lmp_caps >> 40) & 0xff;
1008     params.features[6]  = (slave->lmp_caps >> 48) & 0xff;
1009     params.features[7]  = (slave->lmp_caps >> 56) & 0xff;
1010     bt_hci_event(hci, EVT_READ_REMOTE_FEATURES_COMPLETE,
1011                     &params, EVT_READ_REMOTE_FEATURES_COMPLETE_SIZE);
1012
1013     return 0;
1014 }
1015
1016 static int bt_hci_version_req(struct bt_hci_s *hci, uint16_t handle)
1017 {
1018     evt_read_remote_version_complete params;
1019
1020     if (bt_hci_handle_bad(hci, handle))
1021         return -ENODEV;
1022
1023     bt_hci_remote_dev(hci, handle);
1024
1025     bt_hci_event_status(hci, HCI_SUCCESS);
1026
1027     params.status       = HCI_SUCCESS;
1028     params.handle       = HNDL(handle);
1029     params.lmp_ver      = 0x03;
1030     params.manufacturer = cpu_to_le16(0xa000);
1031     params.lmp_subver   = cpu_to_le16(0xa607);
1032     bt_hci_event(hci, EVT_READ_REMOTE_VERSION_COMPLETE,
1033                     &params, EVT_READ_REMOTE_VERSION_COMPLETE_SIZE);
1034
1035     return 0;
1036 }
1037
1038 static int bt_hci_clkoffset_req(struct bt_hci_s *hci, uint16_t handle)
1039 {
1040     struct bt_device_s *slave;
1041     evt_read_clock_offset_complete params;
1042
1043     if (bt_hci_handle_bad(hci, handle))
1044         return -ENODEV;
1045
1046     slave = bt_hci_remote_dev(hci, handle);
1047
1048     bt_hci_event_status(hci, HCI_SUCCESS);
1049
1050     params.status       = HCI_SUCCESS;
1051     params.handle       = HNDL(handle);
1052     /* TODO: return the clkoff *differenece* */
1053     params.clock_offset = slave->clkoff;        /* Note: no swapping */
1054     bt_hci_event(hci, EVT_READ_CLOCK_OFFSET_COMPLETE,
1055                     &params, EVT_READ_CLOCK_OFFSET_COMPLETE_SIZE);
1056
1057     return 0;
1058 }
1059
1060 static void bt_hci_event_mode(struct bt_hci_s *hci, struct bt_link_s *link,
1061                 uint16_t handle)
1062 {
1063     evt_mode_change params = {
1064         .status         = HCI_SUCCESS,
1065         .handle         = HNDL(handle),
1066         .mode           = link->acl_mode,
1067         .interval       = cpu_to_le16(link->acl_interval),
1068     };
1069
1070     bt_hci_event(hci, EVT_MODE_CHANGE, &params, EVT_MODE_CHANGE_SIZE);
1071 }
1072
1073 static void bt_hci_lmp_mode_change_master(struct bt_hci_s *hci,
1074                 struct bt_link_s *link, int mode, uint16_t interval)
1075 {
1076     link->acl_mode = mode;
1077     link->acl_interval = interval;
1078
1079     bt_hci_event_mode(hci, link, link->handle);
1080
1081     link->slave->lmp_mode_change(link);
1082 }
1083
1084 static void bt_hci_lmp_mode_change_slave(struct bt_link_s *btlink)
1085 {
1086     struct bt_hci_link_s *link = (struct bt_hci_link_s *) btlink;
1087     struct bt_hci_s *hci = hci_from_device(btlink->slave);
1088
1089     bt_hci_event_mode(hci, btlink, link->handle);
1090 }
1091
1092 static int bt_hci_mode_change(struct bt_hci_s *hci, uint16_t handle,
1093                 int interval, int mode)
1094 {
1095     struct bt_hci_master_link_s *link;
1096
1097     if (bt_hci_handle_bad(hci, handle) || !bt_hci_role_master(hci, handle))
1098         return -ENODEV;
1099
1100     link = &hci->lm.handle[handle & ~HCI_HANDLE_OFFSET];
1101     if (link->link->acl_mode != acl_active) {
1102         bt_hci_event_status(hci, HCI_COMMAND_DISALLOWED);
1103         return 0;
1104     }
1105
1106     bt_hci_event_status(hci, HCI_SUCCESS);
1107
1108     timer_mod(link->acl_mode_timer, qemu_clock_get_ns(QEMU_CLOCK_VIRTUAL) +
1109                                     ((uint64_t)interval * 625) * 1000);
1110     bt_hci_lmp_mode_change_master(hci, link->link, mode, interval);
1111
1112     return 0;
1113 }
1114
1115 static int bt_hci_mode_cancel(struct bt_hci_s *hci, uint16_t handle, int mode)
1116 {
1117     struct bt_hci_master_link_s *link;
1118
1119     if (bt_hci_handle_bad(hci, handle) || !bt_hci_role_master(hci, handle))
1120         return -ENODEV;
1121
1122     link = &hci->lm.handle[handle & ~HCI_HANDLE_OFFSET];
1123     if (link->link->acl_mode != mode) {
1124         bt_hci_event_status(hci, HCI_COMMAND_DISALLOWED);
1125
1126         return 0;
1127     }
1128
1129     bt_hci_event_status(hci, HCI_SUCCESS);
1130
1131     timer_del(link->acl_mode_timer);
1132     bt_hci_lmp_mode_change_master(hci, link->link, acl_active, 0);
1133
1134     return 0;
1135 }
1136
1137 static void bt_hci_mode_tick(void *opaque)
1138 {
1139     struct bt_link_s *link = opaque;
1140     struct bt_hci_s *hci = hci_from_device(link->host);
1141
1142     bt_hci_lmp_mode_change_master(hci, link, acl_active, 0);
1143 }
1144
1145 static void bt_hci_reset(struct bt_hci_s *hci)
1146 {
1147     hci->acl_len = 0;
1148     hci->last_cmd = 0;
1149     hci->lm.connecting = 0;
1150
1151     hci->event_mask[0] = 0xff;
1152     hci->event_mask[1] = 0xff;
1153     hci->event_mask[2] = 0xff;
1154     hci->event_mask[3] = 0xff;
1155     hci->event_mask[4] = 0xff;
1156     hci->event_mask[5] = 0x1f;
1157     hci->event_mask[6] = 0x00;
1158     hci->event_mask[7] = 0x00;
1159     hci->device.inquiry_scan = 0;
1160     hci->device.page_scan = 0;
1161     g_free((void *) hci->device.lmp_name);
1162     hci->device.lmp_name = NULL;
1163     hci->device.class[0] = 0x00;
1164     hci->device.class[1] = 0x00;
1165     hci->device.class[2] = 0x00;
1166     hci->voice_setting = 0x0000;
1167     hci->conn_accept_tout = 0x1f40;
1168     hci->lm.inquiry_mode = 0x00;
1169
1170     hci->psb_handle = 0x000;
1171     hci->asb_handle = 0x000;
1172
1173     /* XXX: timer_del(sl->acl_mode_timer); for all links */
1174     timer_del(hci->lm.inquiry_done);
1175     timer_del(hci->lm.inquiry_next);
1176     timer_del(hci->conn_accept_timer);
1177 }
1178
1179 static void bt_hci_read_local_version_rp(struct bt_hci_s *hci)
1180 {
1181     read_local_version_rp lv = {
1182         .status         = HCI_SUCCESS,
1183         .hci_ver        = 0x03,
1184         .hci_rev        = cpu_to_le16(0xa607),
1185         .lmp_ver        = 0x03,
1186         .manufacturer   = cpu_to_le16(0xa000),
1187         .lmp_subver     = cpu_to_le16(0xa607),
1188     };
1189
1190     bt_hci_event_complete(hci, &lv, READ_LOCAL_VERSION_RP_SIZE);
1191 }
1192
1193 static void bt_hci_read_local_commands_rp(struct bt_hci_s *hci)
1194 {
1195     read_local_commands_rp lc = {
1196         .status         = HCI_SUCCESS,
1197         .commands       = {
1198             /* Keep updated! */
1199             /* Also, keep in sync with hci->device.lmp_caps in bt_new_hci */
1200             0xbf, 0x80, 0xf9, 0x03, 0xb2, 0xc0, 0x03, 0xc3,
1201             0x00, 0x0f, 0x80, 0x00, 0xc0, 0x00, 0xe8, 0x13,
1202             0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
1203             0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
1204             0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
1205             0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
1206             0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
1207             0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
1208         },
1209     };
1210
1211     bt_hci_event_complete(hci, &lc, READ_LOCAL_COMMANDS_RP_SIZE);
1212 }
1213
1214 static void bt_hci_read_local_features_rp(struct bt_hci_s *hci)
1215 {
1216     read_local_features_rp lf = {
1217         .status         = HCI_SUCCESS,
1218         .features       = {
1219             (hci->device.lmp_caps >>  0) & 0xff,
1220             (hci->device.lmp_caps >>  8) & 0xff,
1221             (hci->device.lmp_caps >> 16) & 0xff,
1222             (hci->device.lmp_caps >> 24) & 0xff,
1223             (hci->device.lmp_caps >> 32) & 0xff,
1224             (hci->device.lmp_caps >> 40) & 0xff,
1225             (hci->device.lmp_caps >> 48) & 0xff,
1226             (hci->device.lmp_caps >> 56) & 0xff,
1227         },
1228     };
1229
1230     bt_hci_event_complete(hci, &lf, READ_LOCAL_FEATURES_RP_SIZE);
1231 }
1232
1233 static void bt_hci_read_local_ext_features_rp(struct bt_hci_s *hci, int page)
1234 {
1235     read_local_ext_features_rp lef = {
1236         .status         = HCI_SUCCESS,
1237         .page_num       = page,
1238         .max_page_num   = 0x00,
1239         .features       = {
1240             /* Keep updated! */
1241             0x5f, 0x35, 0x85, 0x7e, 0x9b, 0x19, 0x00, 0x80,
1242         },
1243     };
1244     if (page)
1245         memset(lef.features, 0, sizeof(lef.features));
1246
1247     bt_hci_event_complete(hci, &lef, READ_LOCAL_EXT_FEATURES_RP_SIZE);
1248 }
1249
1250 static void bt_hci_read_buffer_size_rp(struct bt_hci_s *hci)
1251 {
1252     read_buffer_size_rp bs = {
1253         /* This can be made configurable, for one standard USB dongle HCI
1254          * the four values are cpu_to_le16(0x0180), 0x40,
1255          * cpu_to_le16(0x0008), cpu_to_le16(0x0008).  */
1256         .status         = HCI_SUCCESS,
1257         .acl_mtu        = cpu_to_le16(0x0200),
1258         .sco_mtu        = 0,
1259         .acl_max_pkt    = cpu_to_le16(0x0001),
1260         .sco_max_pkt    = cpu_to_le16(0x0000),
1261     };
1262
1263     bt_hci_event_complete(hci, &bs, READ_BUFFER_SIZE_RP_SIZE);
1264 }
1265
1266 /* Deprecated in V2.0 (page 661) */
1267 static void bt_hci_read_country_code_rp(struct bt_hci_s *hci)
1268 {
1269     read_country_code_rp cc ={
1270         .status         = HCI_SUCCESS,
1271         .country_code   = 0x00, /* North America & Europe^1 and Japan */
1272     };
1273
1274     bt_hci_event_complete(hci, &cc, READ_COUNTRY_CODE_RP_SIZE);
1275
1276     /* ^1. Except France, sorry */
1277 }
1278
1279 static void bt_hci_read_bd_addr_rp(struct bt_hci_s *hci)
1280 {
1281     read_bd_addr_rp ba = {
1282         .status = HCI_SUCCESS,
1283         .bdaddr = BAINIT(&hci->device.bd_addr),
1284     };
1285
1286     bt_hci_event_complete(hci, &ba, READ_BD_ADDR_RP_SIZE);
1287 }
1288
1289 static int bt_hci_link_quality_rp(struct bt_hci_s *hci, uint16_t handle)
1290 {
1291     read_link_quality_rp lq = {
1292         .status         = HCI_SUCCESS,
1293         .handle         = HNDL(handle),
1294         .link_quality   = 0xff,
1295     };
1296
1297     if (bt_hci_handle_bad(hci, handle))
1298         lq.status = HCI_NO_CONNECTION;
1299
1300     bt_hci_event_complete(hci, &lq, READ_LINK_QUALITY_RP_SIZE);
1301     return 0;
1302 }
1303
1304 /* Generate a Command Complete event with only the Status parameter */
1305 static inline void bt_hci_event_complete_status(struct bt_hci_s *hci,
1306                 uint8_t status)
1307 {
1308     bt_hci_event_complete(hci, &status, 1);
1309 }
1310
1311 static inline void bt_hci_event_complete_conn_cancel(struct bt_hci_s *hci,
1312                 uint8_t status, bdaddr_t *bd_addr)
1313 {
1314     create_conn_cancel_rp params = {
1315         .status = status,
1316         .bdaddr = BAINIT(bd_addr),
1317     };
1318
1319     bt_hci_event_complete(hci, &params, CREATE_CONN_CANCEL_RP_SIZE);
1320 }
1321
1322 static inline void bt_hci_event_auth_complete(struct bt_hci_s *hci,
1323                 uint16_t handle)
1324 {
1325     evt_auth_complete params = {
1326         .status = HCI_SUCCESS,
1327         .handle = HNDL(handle),
1328     };
1329
1330     bt_hci_event(hci, EVT_AUTH_COMPLETE, &params, EVT_AUTH_COMPLETE_SIZE);
1331 }
1332
1333 static inline void bt_hci_event_encrypt_change(struct bt_hci_s *hci,
1334                 uint16_t handle, uint8_t mode)
1335 {
1336     evt_encrypt_change params = {
1337         .status         = HCI_SUCCESS,
1338         .handle         = HNDL(handle),
1339         .encrypt        = mode,
1340     };
1341
1342     bt_hci_event(hci, EVT_ENCRYPT_CHANGE, &params, EVT_ENCRYPT_CHANGE_SIZE);
1343 }
1344
1345 static inline void bt_hci_event_complete_name_cancel(struct bt_hci_s *hci,
1346                 bdaddr_t *bd_addr)
1347 {
1348     remote_name_req_cancel_rp params = {
1349         .status = HCI_INVALID_PARAMETERS,
1350         .bdaddr = BAINIT(bd_addr),
1351     };
1352
1353     bt_hci_event_complete(hci, &params, REMOTE_NAME_REQ_CANCEL_RP_SIZE);
1354 }
1355
1356 static inline void bt_hci_event_read_remote_ext_features(struct bt_hci_s *hci,
1357                 uint16_t handle)
1358 {
1359     evt_read_remote_ext_features_complete params = {
1360         .status = HCI_UNSUPPORTED_FEATURE,
1361         .handle = HNDL(handle),
1362         /* Rest uninitialised */
1363     };
1364
1365     bt_hci_event(hci, EVT_READ_REMOTE_EXT_FEATURES_COMPLETE,
1366                     &params, EVT_READ_REMOTE_EXT_FEATURES_COMPLETE_SIZE);
1367 }
1368
1369 static inline void bt_hci_event_complete_lmp_handle(struct bt_hci_s *hci,
1370                 uint16_t handle)
1371 {
1372     read_lmp_handle_rp params = {
1373         .status         = HCI_NO_CONNECTION,
1374         .handle         = HNDL(handle),
1375         .reserved       = 0,
1376         /* Rest uninitialised */
1377     };
1378
1379     bt_hci_event_complete(hci, &params, READ_LMP_HANDLE_RP_SIZE);
1380 }
1381
1382 static inline void bt_hci_event_complete_role_discovery(struct bt_hci_s *hci,
1383                 int status, uint16_t handle, int master)
1384 {
1385     role_discovery_rp params = {
1386         .status         = status,
1387         .handle         = HNDL(handle),
1388         .role           = master ? 0x00 : 0x01,
1389     };
1390
1391     bt_hci_event_complete(hci, &params, ROLE_DISCOVERY_RP_SIZE);
1392 }
1393
1394 static inline void bt_hci_event_complete_flush(struct bt_hci_s *hci,
1395                 int status, uint16_t handle)
1396 {
1397     flush_rp params = {
1398         .status         = status,
1399         .handle         = HNDL(handle),
1400     };
1401
1402     bt_hci_event_complete(hci, &params, FLUSH_RP_SIZE);
1403 }
1404
1405 static inline void bt_hci_event_complete_read_local_name(struct bt_hci_s *hci)
1406 {
1407     read_local_name_rp params;
1408     params.status = HCI_SUCCESS;
1409     memset(params.name, 0, sizeof(params.name));
1410     if (hci->device.lmp_name)
1411         pstrcpy(params.name, sizeof(params.name), hci->device.lmp_name);
1412
1413     bt_hci_event_complete(hci, &params, READ_LOCAL_NAME_RP_SIZE);
1414 }
1415
1416 static inline void bt_hci_event_complete_read_conn_accept_timeout(
1417                 struct bt_hci_s *hci)
1418 {
1419     read_conn_accept_timeout_rp params = {
1420         .status         = HCI_SUCCESS,
1421         .timeout        = cpu_to_le16(hci->conn_accept_tout),
1422     };
1423
1424     bt_hci_event_complete(hci, &params, READ_CONN_ACCEPT_TIMEOUT_RP_SIZE);
1425 }
1426
1427 static inline void bt_hci_event_complete_read_scan_enable(struct bt_hci_s *hci)
1428 {
1429     read_scan_enable_rp params = {
1430         .status = HCI_SUCCESS,
1431         .enable =
1432                 (hci->device.inquiry_scan ? SCAN_INQUIRY : 0) |
1433                 (hci->device.page_scan ? SCAN_PAGE : 0),
1434     };
1435
1436     bt_hci_event_complete(hci, &params, READ_SCAN_ENABLE_RP_SIZE);
1437 }
1438
1439 static inline void bt_hci_event_complete_read_local_class(struct bt_hci_s *hci)
1440 {
1441     read_class_of_dev_rp params;
1442
1443     params.status = HCI_SUCCESS;
1444     memcpy(params.dev_class, hci->device.class, sizeof(params.dev_class));
1445
1446     bt_hci_event_complete(hci, &params, READ_CLASS_OF_DEV_RP_SIZE);
1447 }
1448
1449 static inline void bt_hci_event_complete_voice_setting(struct bt_hci_s *hci)
1450 {
1451     read_voice_setting_rp params = {
1452         .status         = HCI_SUCCESS,
1453         .voice_setting  = hci->voice_setting,   /* Note: no swapping */
1454     };
1455
1456     bt_hci_event_complete(hci, &params, READ_VOICE_SETTING_RP_SIZE);
1457 }
1458
1459 static inline void bt_hci_event_complete_read_inquiry_mode(
1460                 struct bt_hci_s *hci)
1461 {
1462     read_inquiry_mode_rp params = {
1463         .status         = HCI_SUCCESS,
1464         .mode           = hci->lm.inquiry_mode,
1465     };
1466
1467     bt_hci_event_complete(hci, &params, READ_INQUIRY_MODE_RP_SIZE);
1468 }
1469
1470 static inline void bt_hci_event_num_comp_pkts(struct bt_hci_s *hci,
1471                 uint16_t handle, int packets)
1472 {
1473     uint16_t buf[EVT_NUM_COMP_PKTS_SIZE(1) / 2 + 1];
1474     evt_num_comp_pkts *params = (void *) ((uint8_t *) buf + 1);
1475
1476     params->num_hndl                    = 1;
1477     params->connection->handle          = HNDL(handle);
1478     params->connection->num_packets     = cpu_to_le16(packets);
1479
1480     bt_hci_event(hci, EVT_NUM_COMP_PKTS, params, EVT_NUM_COMP_PKTS_SIZE(1));
1481 }
1482
1483 static void bt_submit_hci(struct HCIInfo *info,
1484                 const uint8_t *data, int length)
1485 {
1486     struct bt_hci_s *hci = hci_from_info(info);
1487     uint16_t cmd;
1488     int paramlen, i;
1489
1490     if (length < HCI_COMMAND_HDR_SIZE)
1491         goto short_hci;
1492
1493     memcpy(&hci->last_cmd, data, 2);
1494
1495     cmd = (data[1] << 8) | data[0];
1496     paramlen = data[2];
1497     if (cmd_opcode_ogf(cmd) == 0 || cmd_opcode_ocf(cmd) == 0)   /* NOP */
1498         return;
1499
1500     data += HCI_COMMAND_HDR_SIZE;
1501     length -= HCI_COMMAND_HDR_SIZE;
1502
1503     if (paramlen > length)
1504         return;
1505
1506 #define PARAM(cmd, param)       (((cmd##_cp *) data)->param)
1507 #define PARAM16(cmd, param)     le16_to_cpup(&PARAM(cmd, param))
1508 #define PARAMHANDLE(cmd)        HNDL(PARAM(cmd, handle))
1509 #define LENGTH_CHECK(cmd)       if (length < sizeof(cmd##_cp)) goto short_hci
1510     /* Note: the supported commands bitmask in bt_hci_read_local_commands_rp
1511      * needs to be updated every time a command is implemented here!  */
1512     switch (cmd) {
1513     case cmd_opcode_pack(OGF_LINK_CTL, OCF_INQUIRY):
1514         LENGTH_CHECK(inquiry);
1515
1516         if (PARAM(inquiry, length) < 1) {
1517             bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS);
1518             break;
1519         }
1520
1521         hci->lm.inquire = 1;
1522         hci->lm.periodic = 0;
1523         hci->lm.responses_left = PARAM(inquiry, num_rsp) ?: INT_MAX;
1524         hci->lm.responses = 0;
1525         bt_hci_event_status(hci, HCI_SUCCESS);
1526         bt_hci_inquiry_start(hci, PARAM(inquiry, length));
1527         break;
1528
1529     case cmd_opcode_pack(OGF_LINK_CTL, OCF_INQUIRY_CANCEL):
1530         if (!hci->lm.inquire || hci->lm.periodic) {
1531             fprintf(stderr, "%s: Inquiry Cancel should only be issued after "
1532                             "the Inquiry command has been issued, a Command "
1533                             "Status event has been received for the Inquiry "
1534                             "command, and before the Inquiry Complete event "
1535                             "occurs", __FUNCTION__);
1536             bt_hci_event_complete_status(hci, HCI_COMMAND_DISALLOWED);
1537             break;
1538         }
1539
1540         hci->lm.inquire = 0;
1541         timer_del(hci->lm.inquiry_done);
1542         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1543         break;
1544
1545     case cmd_opcode_pack(OGF_LINK_CTL, OCF_PERIODIC_INQUIRY):
1546         LENGTH_CHECK(periodic_inquiry);
1547
1548         if (!(PARAM(periodic_inquiry, length) <
1549                                 PARAM16(periodic_inquiry, min_period) &&
1550                                 PARAM16(periodic_inquiry, min_period) <
1551                                 PARAM16(periodic_inquiry, max_period)) ||
1552                         PARAM(periodic_inquiry, length) < 1 ||
1553                         PARAM16(periodic_inquiry, min_period) < 2 ||
1554                         PARAM16(periodic_inquiry, max_period) < 3) {
1555             bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS);
1556             break;
1557         }
1558
1559         hci->lm.inquire = 1;
1560         hci->lm.periodic = 1;
1561         hci->lm.responses_left = PARAM(periodic_inquiry, num_rsp);
1562         hci->lm.responses = 0;
1563         hci->lm.inquiry_period = PARAM16(periodic_inquiry, max_period);
1564         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1565         bt_hci_inquiry_start(hci, PARAM(periodic_inquiry, length));
1566         break;
1567
1568     case cmd_opcode_pack(OGF_LINK_CTL, OCF_EXIT_PERIODIC_INQUIRY):
1569         if (!hci->lm.inquire || !hci->lm.periodic) {
1570             fprintf(stderr, "%s: Inquiry Cancel should only be issued after "
1571                             "the Inquiry command has been issued, a Command "
1572                             "Status event has been received for the Inquiry "
1573                             "command, and before the Inquiry Complete event "
1574                             "occurs", __FUNCTION__);
1575             bt_hci_event_complete_status(hci, HCI_COMMAND_DISALLOWED);
1576             break;
1577         }
1578         hci->lm.inquire = 0;
1579         timer_del(hci->lm.inquiry_done);
1580         timer_del(hci->lm.inquiry_next);
1581         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1582         break;
1583
1584     case cmd_opcode_pack(OGF_LINK_CTL, OCF_CREATE_CONN):
1585         LENGTH_CHECK(create_conn);
1586
1587         if (hci->lm.connecting >= HCI_HANDLES_MAX) {
1588             bt_hci_event_status(hci, HCI_REJECTED_LIMITED_RESOURCES);
1589             break;
1590         }
1591         bt_hci_event_status(hci, HCI_SUCCESS);
1592
1593         if (bt_hci_connect(hci, &PARAM(create_conn, bdaddr)))
1594             bt_hci_connection_reject_event(hci, &PARAM(create_conn, bdaddr));
1595         break;
1596
1597     case cmd_opcode_pack(OGF_LINK_CTL, OCF_DISCONNECT):
1598         LENGTH_CHECK(disconnect);
1599
1600         if (bt_hci_handle_bad(hci, PARAMHANDLE(disconnect))) {
1601             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1602             break;
1603         }
1604
1605         bt_hci_event_status(hci, HCI_SUCCESS);
1606         bt_hci_disconnect(hci, PARAMHANDLE(disconnect),
1607                         PARAM(disconnect, reason));
1608         break;
1609
1610     case cmd_opcode_pack(OGF_LINK_CTL, OCF_CREATE_CONN_CANCEL):
1611         LENGTH_CHECK(create_conn_cancel);
1612
1613         if (bt_hci_lmp_connection_ready(hci,
1614                                 &PARAM(create_conn_cancel, bdaddr))) {
1615             for (i = 0; i < HCI_HANDLES_MAX; i ++)
1616                 if (bt_hci_role_master(hci, i) && hci->lm.handle[i].link &&
1617                                 !bacmp(&hci->lm.handle[i].link->slave->bd_addr,
1618                                         &PARAM(create_conn_cancel, bdaddr)))
1619                    break;
1620
1621             bt_hci_event_complete_conn_cancel(hci, i < HCI_HANDLES_MAX ?
1622                             HCI_ACL_CONNECTION_EXISTS : HCI_NO_CONNECTION,
1623                             &PARAM(create_conn_cancel, bdaddr));
1624         } else
1625             bt_hci_event_complete_conn_cancel(hci, HCI_SUCCESS,
1626                             &PARAM(create_conn_cancel, bdaddr));
1627         break;
1628
1629     case cmd_opcode_pack(OGF_LINK_CTL, OCF_ACCEPT_CONN_REQ):
1630         LENGTH_CHECK(accept_conn_req);
1631
1632         if (!hci->conn_req_host ||
1633                         bacmp(&PARAM(accept_conn_req, bdaddr),
1634                                 &hci->conn_req_host->bd_addr)) {
1635             bt_hci_event_status(hci, HCI_INVALID_PARAMETERS);
1636             break;
1637         }
1638
1639         bt_hci_event_status(hci, HCI_SUCCESS);
1640         bt_hci_connection_accept(hci, hci->conn_req_host);
1641         hci->conn_req_host = NULL;
1642         break;
1643
1644     case cmd_opcode_pack(OGF_LINK_CTL, OCF_REJECT_CONN_REQ):
1645         LENGTH_CHECK(reject_conn_req);
1646
1647         if (!hci->conn_req_host ||
1648                         bacmp(&PARAM(reject_conn_req, bdaddr),
1649                                 &hci->conn_req_host->bd_addr)) {
1650             bt_hci_event_status(hci, HCI_INVALID_PARAMETERS);
1651             break;
1652         }
1653
1654         bt_hci_event_status(hci, HCI_SUCCESS);
1655         bt_hci_connection_reject(hci, hci->conn_req_host,
1656                         PARAM(reject_conn_req, reason));
1657         bt_hci_connection_reject_event(hci, &hci->conn_req_host->bd_addr);
1658         hci->conn_req_host = NULL;
1659         break;
1660
1661     case cmd_opcode_pack(OGF_LINK_CTL, OCF_AUTH_REQUESTED):
1662         LENGTH_CHECK(auth_requested);
1663
1664         if (bt_hci_handle_bad(hci, PARAMHANDLE(auth_requested)))
1665             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1666         else {
1667             bt_hci_event_status(hci, HCI_SUCCESS);
1668             bt_hci_event_auth_complete(hci, PARAMHANDLE(auth_requested));
1669         }
1670         break;
1671
1672     case cmd_opcode_pack(OGF_LINK_CTL, OCF_SET_CONN_ENCRYPT):
1673         LENGTH_CHECK(set_conn_encrypt);
1674
1675         if (bt_hci_handle_bad(hci, PARAMHANDLE(set_conn_encrypt)))
1676             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1677         else {
1678             bt_hci_event_status(hci, HCI_SUCCESS);
1679             bt_hci_event_encrypt_change(hci,
1680                             PARAMHANDLE(set_conn_encrypt),
1681                             PARAM(set_conn_encrypt, encrypt));
1682         }
1683         break;
1684
1685     case cmd_opcode_pack(OGF_LINK_CTL, OCF_REMOTE_NAME_REQ):
1686         LENGTH_CHECK(remote_name_req);
1687
1688         if (bt_hci_name_req(hci, &PARAM(remote_name_req, bdaddr)))
1689             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1690         break;
1691
1692     case cmd_opcode_pack(OGF_LINK_CTL, OCF_REMOTE_NAME_REQ_CANCEL):
1693         LENGTH_CHECK(remote_name_req_cancel);
1694
1695         bt_hci_event_complete_name_cancel(hci,
1696                         &PARAM(remote_name_req_cancel, bdaddr));
1697         break;
1698
1699     case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_REMOTE_FEATURES):
1700         LENGTH_CHECK(read_remote_features);
1701
1702         if (bt_hci_features_req(hci, PARAMHANDLE(read_remote_features)))
1703             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1704         break;
1705
1706     case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_REMOTE_EXT_FEATURES):
1707         LENGTH_CHECK(read_remote_ext_features);
1708
1709         if (bt_hci_handle_bad(hci, PARAMHANDLE(read_remote_ext_features)))
1710             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1711         else {
1712             bt_hci_event_status(hci, HCI_SUCCESS);
1713             bt_hci_event_read_remote_ext_features(hci,
1714                             PARAMHANDLE(read_remote_ext_features));
1715         }
1716         break;
1717
1718     case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_REMOTE_VERSION):
1719         LENGTH_CHECK(read_remote_version);
1720
1721         if (bt_hci_version_req(hci, PARAMHANDLE(read_remote_version)))
1722             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1723         break;
1724
1725     case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_CLOCK_OFFSET):
1726         LENGTH_CHECK(read_clock_offset);
1727
1728         if (bt_hci_clkoffset_req(hci, PARAMHANDLE(read_clock_offset)))
1729             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1730         break;
1731
1732     case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_LMP_HANDLE):
1733         LENGTH_CHECK(read_lmp_handle);
1734
1735         /* TODO: */
1736         bt_hci_event_complete_lmp_handle(hci, PARAMHANDLE(read_lmp_handle));
1737         break;
1738
1739     case cmd_opcode_pack(OGF_LINK_POLICY, OCF_HOLD_MODE):
1740         LENGTH_CHECK(hold_mode);
1741
1742         if (PARAM16(hold_mode, min_interval) >
1743                         PARAM16(hold_mode, max_interval) ||
1744                         PARAM16(hold_mode, min_interval) < 0x0002 ||
1745                         PARAM16(hold_mode, max_interval) > 0xff00 ||
1746                         (PARAM16(hold_mode, min_interval) & 1) ||
1747                         (PARAM16(hold_mode, max_interval) & 1)) {
1748             bt_hci_event_status(hci, HCI_INVALID_PARAMETERS);
1749             break;
1750         }
1751
1752         if (bt_hci_mode_change(hci, PARAMHANDLE(hold_mode),
1753                                 PARAM16(hold_mode, max_interval),
1754                                 acl_hold))
1755             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1756         break;
1757
1758     case cmd_opcode_pack(OGF_LINK_POLICY, OCF_PARK_MODE):
1759         LENGTH_CHECK(park_mode);
1760
1761         if (PARAM16(park_mode, min_interval) >
1762                         PARAM16(park_mode, max_interval) ||
1763                         PARAM16(park_mode, min_interval) < 0x000e ||
1764                         (PARAM16(park_mode, min_interval) & 1) ||
1765                         (PARAM16(park_mode, max_interval) & 1)) {
1766             bt_hci_event_status(hci, HCI_INVALID_PARAMETERS);
1767             break;
1768         }
1769
1770         if (bt_hci_mode_change(hci, PARAMHANDLE(park_mode),
1771                                 PARAM16(park_mode, max_interval),
1772                                 acl_parked))
1773             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1774         break;
1775
1776     case cmd_opcode_pack(OGF_LINK_POLICY, OCF_EXIT_PARK_MODE):
1777         LENGTH_CHECK(exit_park_mode);
1778
1779         if (bt_hci_mode_cancel(hci, PARAMHANDLE(exit_park_mode),
1780                                 acl_parked))
1781             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1782         break;
1783
1784     case cmd_opcode_pack(OGF_LINK_POLICY, OCF_ROLE_DISCOVERY):
1785         LENGTH_CHECK(role_discovery);
1786
1787         if (bt_hci_handle_bad(hci, PARAMHANDLE(role_discovery)))
1788             bt_hci_event_complete_role_discovery(hci,
1789                             HCI_NO_CONNECTION, PARAMHANDLE(role_discovery), 0);
1790         else
1791             bt_hci_event_complete_role_discovery(hci,
1792                             HCI_SUCCESS, PARAMHANDLE(role_discovery),
1793                             bt_hci_role_master(hci,
1794                                     PARAMHANDLE(role_discovery)));
1795         break;
1796
1797     case cmd_opcode_pack(OGF_HOST_CTL, OCF_SET_EVENT_MASK):
1798         LENGTH_CHECK(set_event_mask);
1799
1800         memcpy(hci->event_mask, PARAM(set_event_mask, mask), 8);
1801         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1802         break;
1803
1804     case cmd_opcode_pack(OGF_HOST_CTL, OCF_RESET):
1805         bt_hci_reset(hci);
1806         bt_hci_event_status(hci, HCI_SUCCESS);
1807         break;
1808
1809     case cmd_opcode_pack(OGF_HOST_CTL, OCF_SET_EVENT_FLT):
1810         if (length >= 1 && PARAM(set_event_flt, flt_type) == FLT_CLEAR_ALL)
1811             /* No length check */;
1812         else
1813             LENGTH_CHECK(set_event_flt);
1814
1815         /* Filters are not implemented */
1816         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1817         break;
1818
1819     case cmd_opcode_pack(OGF_HOST_CTL, OCF_FLUSH):
1820         LENGTH_CHECK(flush);
1821
1822         if (bt_hci_handle_bad(hci, PARAMHANDLE(flush)))
1823             bt_hci_event_complete_flush(hci,
1824                             HCI_NO_CONNECTION, PARAMHANDLE(flush));
1825         else {
1826             /* TODO: ordering? */
1827             bt_hci_event(hci, EVT_FLUSH_OCCURRED,
1828                             &PARAM(flush, handle),
1829                             EVT_FLUSH_OCCURRED_SIZE);
1830             bt_hci_event_complete_flush(hci,
1831                             HCI_SUCCESS, PARAMHANDLE(flush));
1832         }
1833         break;
1834
1835     case cmd_opcode_pack(OGF_HOST_CTL, OCF_CHANGE_LOCAL_NAME):
1836         LENGTH_CHECK(change_local_name);
1837
1838         g_free((void *) hci->device.lmp_name);
1839         hci->device.lmp_name = g_strndup(PARAM(change_local_name, name),
1840                         sizeof(PARAM(change_local_name, name)));
1841         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1842         break;
1843
1844     case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_LOCAL_NAME):
1845         bt_hci_event_complete_read_local_name(hci);
1846         break;
1847
1848     case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_CONN_ACCEPT_TIMEOUT):
1849         bt_hci_event_complete_read_conn_accept_timeout(hci);
1850         break;
1851
1852     case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_CONN_ACCEPT_TIMEOUT):
1853         /* TODO */
1854         LENGTH_CHECK(write_conn_accept_timeout);
1855
1856         if (PARAM16(write_conn_accept_timeout, timeout) < 0x0001 ||
1857                         PARAM16(write_conn_accept_timeout, timeout) > 0xb540) {
1858             bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS);
1859             break;
1860         }
1861
1862         hci->conn_accept_tout = PARAM16(write_conn_accept_timeout, timeout);
1863         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1864         break;
1865
1866     case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_SCAN_ENABLE):
1867         bt_hci_event_complete_read_scan_enable(hci);
1868         break;
1869
1870     case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_SCAN_ENABLE):
1871         LENGTH_CHECK(write_scan_enable);
1872
1873         /* TODO: check that the remaining bits are all 0 */
1874         hci->device.inquiry_scan =
1875                 !!(PARAM(write_scan_enable, scan_enable) & SCAN_INQUIRY);
1876         hci->device.page_scan =
1877                 !!(PARAM(write_scan_enable, scan_enable) & SCAN_PAGE);
1878         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1879         break;
1880
1881     case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_CLASS_OF_DEV):
1882         bt_hci_event_complete_read_local_class(hci);
1883         break;
1884
1885     case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_CLASS_OF_DEV):
1886         LENGTH_CHECK(write_class_of_dev);
1887
1888         memcpy(hci->device.class, PARAM(write_class_of_dev, dev_class),
1889                         sizeof(PARAM(write_class_of_dev, dev_class)));
1890         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1891         break;
1892
1893     case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_VOICE_SETTING):
1894         bt_hci_event_complete_voice_setting(hci);
1895         break;
1896
1897     case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_VOICE_SETTING):
1898         LENGTH_CHECK(write_voice_setting);
1899
1900         hci->voice_setting = PARAM(write_voice_setting, voice_setting);
1901         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1902         break;
1903
1904     case cmd_opcode_pack(OGF_HOST_CTL, OCF_HOST_NUMBER_OF_COMPLETED_PACKETS):
1905         if (length < data[0] * 2 + 1)
1906             goto short_hci;
1907
1908         for (i = 0; i < data[0]; i ++)
1909             if (bt_hci_handle_bad(hci,
1910                                     data[i * 2 + 1] | (data[i * 2 + 2] << 8)))
1911                 bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS);
1912         break;
1913
1914     case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_INQUIRY_MODE):
1915         /* Only if (local_features[3] & 0x40) && (local_commands[12] & 0x40)
1916          * else
1917          *     goto unknown_command */
1918         bt_hci_event_complete_read_inquiry_mode(hci);
1919         break;
1920
1921     case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_INQUIRY_MODE):
1922         /* Only if (local_features[3] & 0x40) && (local_commands[12] & 0x80)
1923          * else
1924          *     goto unknown_command */
1925         LENGTH_CHECK(write_inquiry_mode);
1926
1927         if (PARAM(write_inquiry_mode, mode) > 0x01) {
1928             bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS);
1929             break;
1930         }
1931
1932         hci->lm.inquiry_mode = PARAM(write_inquiry_mode, mode);
1933         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1934         break;
1935
1936     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_VERSION):
1937         bt_hci_read_local_version_rp(hci);
1938         break;
1939
1940     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_COMMANDS):
1941         bt_hci_read_local_commands_rp(hci);
1942         break;
1943
1944     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_FEATURES):
1945         bt_hci_read_local_features_rp(hci);
1946         break;
1947
1948     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_EXT_FEATURES):
1949         LENGTH_CHECK(read_local_ext_features);
1950
1951         bt_hci_read_local_ext_features_rp(hci,
1952                         PARAM(read_local_ext_features, page_num));
1953         break;
1954
1955     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_BUFFER_SIZE):
1956         bt_hci_read_buffer_size_rp(hci);
1957         break;
1958
1959     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_COUNTRY_CODE):
1960         bt_hci_read_country_code_rp(hci);
1961         break;
1962
1963     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_BD_ADDR):
1964         bt_hci_read_bd_addr_rp(hci);
1965         break;
1966
1967     case cmd_opcode_pack(OGF_STATUS_PARAM, OCF_READ_LINK_QUALITY):
1968         LENGTH_CHECK(read_link_quality);
1969
1970         bt_hci_link_quality_rp(hci, PARAMHANDLE(read_link_quality));
1971         break;
1972
1973     default:
1974         bt_hci_event_status(hci, HCI_UNKNOWN_COMMAND);
1975         break;
1976
1977     short_hci:
1978         fprintf(stderr, "%s: HCI packet too short (%iB)\n",
1979                         __FUNCTION__, length);
1980         bt_hci_event_status(hci, HCI_INVALID_PARAMETERS);
1981         break;
1982     }
1983 }
1984
1985 /* We could perform fragmentation here, we can't do "recombination" because
1986  * at this layer the length of the payload is not know ahead, so we only
1987  * know that a packet contained the last fragment of the SDU when the next
1988  * SDU starts.  */
1989 static inline void bt_hci_lmp_acl_data(struct bt_hci_s *hci, uint16_t handle,
1990                 const uint8_t *data, int start, int len)
1991 {
1992     struct hci_acl_hdr *pkt = (void *) hci->acl_buf;
1993
1994     /* TODO: packet flags */
1995     /* TODO: avoid memcpy'ing */
1996
1997     if (len + HCI_ACL_HDR_SIZE > sizeof(hci->acl_buf)) {
1998         fprintf(stderr, "%s: can't take ACL packets %i bytes long\n",
1999                         __FUNCTION__, len);
2000         return;
2001     }
2002     memcpy(hci->acl_buf + HCI_ACL_HDR_SIZE, data, len);
2003
2004     pkt->handle = cpu_to_le16(
2005                     acl_handle_pack(handle, start ? ACL_START : ACL_CONT));
2006     pkt->dlen = cpu_to_le16(len);
2007     hci->info.acl_recv(hci->info.opaque,
2008                     hci->acl_buf, len + HCI_ACL_HDR_SIZE);
2009 }
2010
2011 static void bt_hci_lmp_acl_data_slave(struct bt_link_s *btlink,
2012                 const uint8_t *data, int start, int len)
2013 {
2014     struct bt_hci_link_s *link = (struct bt_hci_link_s *) btlink;
2015
2016     bt_hci_lmp_acl_data(hci_from_device(btlink->slave),
2017                     link->handle, data, start, len);
2018 }
2019
2020 static void bt_hci_lmp_acl_data_host(struct bt_link_s *link,
2021                 const uint8_t *data, int start, int len)
2022 {
2023     bt_hci_lmp_acl_data(hci_from_device(link->host),
2024                     link->handle, data, start, len);
2025 }
2026
2027 static void bt_submit_acl(struct HCIInfo *info,
2028                 const uint8_t *data, int length)
2029 {
2030     struct bt_hci_s *hci = hci_from_info(info);
2031     uint16_t handle;
2032     int datalen, flags;
2033     struct bt_link_s *link;
2034
2035     if (length < HCI_ACL_HDR_SIZE) {
2036         fprintf(stderr, "%s: ACL packet too short (%iB)\n",
2037                         __FUNCTION__, length);
2038         return;
2039     }
2040
2041     handle = acl_handle((data[1] << 8) | data[0]);
2042     flags = acl_flags((data[1] << 8) | data[0]);
2043     datalen = (data[3] << 8) | data[2];
2044     data += HCI_ACL_HDR_SIZE;
2045     length -= HCI_ACL_HDR_SIZE;
2046
2047     if (bt_hci_handle_bad(hci, handle)) {
2048         fprintf(stderr, "%s: invalid ACL handle %03x\n",
2049                         __FUNCTION__, handle);
2050         /* TODO: signal an error */
2051         return;
2052     }
2053     handle &= ~HCI_HANDLE_OFFSET;
2054
2055     if (datalen > length) {
2056         fprintf(stderr, "%s: ACL packet too short (%iB < %iB)\n",
2057                         __FUNCTION__, length, datalen);
2058         return;
2059     }
2060
2061     link = hci->lm.handle[handle].link;
2062
2063     if ((flags & ~3) == ACL_ACTIVE_BCAST) {
2064         if (!hci->asb_handle)
2065             hci->asb_handle = handle;
2066         else if (handle != hci->asb_handle) {
2067             fprintf(stderr, "%s: Bad handle %03x in Active Slave Broadcast\n",
2068                             __FUNCTION__, handle);
2069             /* TODO: signal an error */
2070             return;
2071         }
2072
2073         /* TODO */
2074     }
2075
2076     if ((flags & ~3) == ACL_PICO_BCAST) {
2077         if (!hci->psb_handle)
2078             hci->psb_handle = handle;
2079         else if (handle != hci->psb_handle) {
2080             fprintf(stderr, "%s: Bad handle %03x in Parked Slave Broadcast\n",
2081                             __FUNCTION__, handle);
2082             /* TODO: signal an error */
2083             return;
2084         }
2085
2086         /* TODO */
2087     }
2088
2089     /* TODO: increase counter and send EVT_NUM_COMP_PKTS */
2090     bt_hci_event_num_comp_pkts(hci, handle | HCI_HANDLE_OFFSET, 1);
2091
2092     /* Do this last as it can trigger further events even in this HCI */
2093     hci->lm.handle[handle].lmp_acl_data(link, data,
2094                     (flags & 3) == ACL_START, length);
2095 }
2096
2097 static void bt_submit_sco(struct HCIInfo *info,
2098                 const uint8_t *data, int length)
2099 {
2100     struct bt_hci_s *hci = hci_from_info(info);
2101     uint16_t handle;
2102     int datalen;
2103
2104     if (length < 3)
2105         return;
2106
2107     handle = acl_handle((data[1] << 8) | data[0]);
2108     datalen = data[2];
2109     length -= 3;
2110
2111     if (bt_hci_handle_bad(hci, handle)) {
2112         fprintf(stderr, "%s: invalid SCO handle %03x\n",
2113                         __FUNCTION__, handle);
2114         return;
2115     }
2116
2117     if (datalen > length) {
2118         fprintf(stderr, "%s: SCO packet too short (%iB < %iB)\n",
2119                         __FUNCTION__, length, datalen);
2120         return;
2121     }
2122
2123     /* TODO */
2124
2125     /* TODO: increase counter and send EVT_NUM_COMP_PKTS if synchronous
2126      * Flow Control is enabled.
2127      * (See Read/Write_Synchronous_Flow_Control_Enable on page 513 and
2128      * page 514.)  */
2129 }
2130
2131 static uint8_t *bt_hci_evt_packet(void *opaque)
2132 {
2133     /* TODO: allocate a packet from upper layer */
2134     struct bt_hci_s *s = opaque;
2135
2136     return s->evt_buf;
2137 }
2138
2139 static void bt_hci_evt_submit(void *opaque, int len)
2140 {
2141     /* TODO: notify upper layer */
2142     struct bt_hci_s *s = opaque;
2143
2144     s->info.evt_recv(s->info.opaque, s->evt_buf, len);
2145 }
2146
2147 static int bt_hci_bdaddr_set(struct HCIInfo *info, const uint8_t *bd_addr)
2148 {
2149     struct bt_hci_s *hci = hci_from_info(info);
2150
2151     bacpy(&hci->device.bd_addr, (const bdaddr_t *) bd_addr);
2152     return 0;
2153 }
2154
2155 static void bt_hci_done(struct HCIInfo *info);
2156 static void bt_hci_destroy(struct bt_device_s *dev)
2157 {
2158     struct bt_hci_s *hci = hci_from_device(dev);
2159
2160     bt_hci_done(&hci->info);
2161 }
2162
2163 struct HCIInfo *bt_new_hci(struct bt_scatternet_s *net)
2164 {
2165     struct bt_hci_s *s = g_malloc0(sizeof(struct bt_hci_s));
2166
2167     s->lm.inquiry_done = timer_new_ns(QEMU_CLOCK_VIRTUAL, bt_hci_inquiry_done, s);
2168     s->lm.inquiry_next = timer_new_ns(QEMU_CLOCK_VIRTUAL, bt_hci_inquiry_next, s);
2169     s->conn_accept_timer =
2170             timer_new_ns(QEMU_CLOCK_VIRTUAL, bt_hci_conn_accept_timeout, s);
2171
2172     s->evt_packet = bt_hci_evt_packet;
2173     s->evt_submit = bt_hci_evt_submit;
2174     s->opaque = s;
2175
2176     bt_device_init(&s->device, net);
2177     s->device.lmp_connection_request = bt_hci_lmp_connection_request;
2178     s->device.lmp_connection_complete = bt_hci_lmp_connection_complete;
2179     s->device.lmp_disconnect_master = bt_hci_lmp_disconnect_host;
2180     s->device.lmp_disconnect_slave = bt_hci_lmp_disconnect_slave;
2181     s->device.lmp_acl_data = bt_hci_lmp_acl_data_slave;
2182     s->device.lmp_acl_resp = bt_hci_lmp_acl_data_host;
2183     s->device.lmp_mode_change = bt_hci_lmp_mode_change_slave;
2184
2185     /* Keep updated! */
2186     /* Also keep in sync with supported commands bitmask in
2187      * bt_hci_read_local_commands_rp */
2188     s->device.lmp_caps = 0x8000199b7e85355fll;
2189
2190     bt_hci_reset(s);
2191
2192     s->info.cmd_send = bt_submit_hci;
2193     s->info.sco_send = bt_submit_sco;
2194     s->info.acl_send = bt_submit_acl;
2195     s->info.bdaddr_set = bt_hci_bdaddr_set;
2196
2197     s->device.handle_destroy = bt_hci_destroy;
2198
2199     error_setg(&s->replay_blocker, QERR_REPLAY_NOT_SUPPORTED, "-bt hci");
2200     replay_add_blocker(s->replay_blocker);
2201
2202     return &s->info;
2203 }
2204
2205 struct HCIInfo *hci_init(const char *str)
2206 {
2207     char *endp;
2208     struct bt_scatternet_s *vlan = 0;
2209
2210     if (!strcmp(str, "null"))
2211         /* null */
2212         return &null_hci;
2213     else if (!strncmp(str, "host", 4) && (str[4] == '\0' || str[4] == ':'))
2214         /* host[:hciN] */
2215         return bt_host_hci(str[4] ? str + 5 : "hci0");
2216     else if (!strncmp(str, "hci", 3)) {
2217         /* hci[,vlan=n] */
2218         if (str[3]) {
2219             if (!strncmp(str + 3, ",vlan=", 6)) {
2220                 vlan = qemu_find_bt_vlan(strtol(str + 9, &endp, 0));
2221                 if (*endp)
2222                     vlan = 0;
2223             }
2224         } else
2225             vlan = qemu_find_bt_vlan(0);
2226         if (vlan)
2227            return bt_new_hci(vlan);
2228     }
2229
2230     fprintf(stderr, "qemu: Unknown bluetooth HCI `%s'.\n", str);
2231
2232     return 0;
2233 }
2234
2235 static void bt_hci_done(struct HCIInfo *info)
2236 {
2237     struct bt_hci_s *hci = hci_from_info(info);
2238     int handle;
2239
2240     bt_device_done(&hci->device);
2241
2242     g_free((void *) hci->device.lmp_name);
2243
2244     /* Be gentle and send DISCONNECT to all connected peers and those
2245      * currently waiting for us to accept or reject a connection request.
2246      * This frees the links.  */
2247     if (hci->conn_req_host) {
2248         bt_hci_connection_reject(hci,
2249                                  hci->conn_req_host, HCI_OE_POWER_OFF);
2250         return;
2251     }
2252
2253     for (handle = HCI_HANDLE_OFFSET;
2254                     handle < (HCI_HANDLE_OFFSET | HCI_HANDLES_MAX); handle ++)
2255         if (!bt_hci_handle_bad(hci, handle))
2256             bt_hci_disconnect(hci, handle, HCI_OE_POWER_OFF);
2257
2258     /* TODO: this is not enough actually, there may be slaves from whom
2259      * we have requested a connection who will soon (or not) respond with
2260      * an accept or a reject, so we should also check if hci->lm.connecting
2261      * is non-zero and if so, avoid freeing the hci but otherwise disappear
2262      * from all qemu social life (e.g. stop scanning and request to be
2263      * removed from s->device.net) and arrange for
2264      * s->device.lmp_connection_complete to free the remaining bits once
2265      * hci->lm.awaiting_bdaddr[] is empty.  */
2266
2267     timer_free(hci->lm.inquiry_done);
2268     timer_free(hci->lm.inquiry_next);
2269     timer_free(hci->conn_accept_timer);
2270
2271     g_free(hci);
2272 }