hw/bt/hci.c

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