hw/bt/hci.c

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