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