hw/bt-hci.c

   1 /*
   2  * QEMU Bluetooth HCI logic.
   3  *
   4  * Copyright (C) 2007 OpenMoko, Inc.
   5  * Copyright (C) 2008 Andrzej Zaborowski  <[email protected]>
   6  *
   7  * This program is free software; you can redistribute it and/or
   8  * modify it under the terms of the GNU General Public License as
   9  * published by the Free Software Foundation; either version 2 of
  10  * the License, or (at your option) any later version.
  11  *
  12  * This program is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License
  18  * along with this program; if not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include "qemu-common.h"
  22 #include "qemu-timer.h"
  23 #include "usb.h"
  24 #include "net.h"
  25 #include "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     return;
 790 }
 791
 792 static void bt_hci_conn_accept_timeout(void *opaque)
 793 {
 794     struct bt_hci_s *hci = (struct bt_hci_s *) opaque;
 795
 796     if (!hci->conn_req_host)
 797         /* Already accepted or rejected.  If the other end cancelled the
 798          * connection request then we still have to reject or accept it
 799          * and then we'll get a disconnect.  */
 800         return;
 801
 802     /* TODO */
 803 }
 804
 805 /* Remove from the list of devices which we wanted to connect to and
 806  * are awaiting a response from.  If the callback sees a response from
 807  * a device which is not on the list it will assume it's a connection
 808  * that's been cancelled by the host in the meantime and immediately
 809  * try to detach the link and send a Connection Complete.  */
 810 static int bt_hci_lmp_connection_ready(struct bt_hci_s *hci,
 811                 bdaddr_t *bdaddr)
 812 {
 813     int i;
 814
 815     for (i = 0; i < hci->lm.connecting; i ++)
 816         if (!bacmp(&hci->lm.awaiting_bdaddr[i], bdaddr)) {
 817             if (i < -- hci->lm.connecting)
 818                 bacpy(&hci->lm.awaiting_bdaddr[i],
 819                                 &hci->lm.awaiting_bdaddr[hci->lm.connecting]);
 820             return 0;
 821         }
 822
 823     return 1;
 824 }
 825
 826 static void bt_hci_lmp_connection_complete(struct bt_link_s *link)
 827 {
 828     struct bt_hci_s *hci = hci_from_device(link->host);
 829     evt_conn_complete params;
 830     uint16_t handle;
 831     uint8_t status = HCI_SUCCESS;
 832     int tries = HCI_HANDLES_MAX;
 833
 834     if (bt_hci_lmp_connection_ready(hci, &link->slave->bd_addr)) {
 835         if (!hci->device.reject_reason)
 836             link->slave->lmp_disconnect_slave(link);
 837         handle = 0;
 838         status = HCI_NO_CONNECTION;
 839         goto complete;
 840     }
 841
 842     if (hci->device.reject_reason) {
 843         handle = 0;
 844         status = hci->device.reject_reason;
 845         goto complete;
 846     }
 847
 848     /* Make a connection handle */
 849     do {
 850         while (hci->lm.handle[++ hci->lm.last_handle].link && -- tries)
 851             hci->lm.last_handle &= HCI_HANDLES_MAX - 1;
 852         handle = hci->lm.last_handle | HCI_HANDLE_OFFSET;
 853     } while ((handle == hci->asb_handle || handle == hci->psb_handle) &&
 854             tries);
 855
 856     if (!tries) {
 857         link->slave->lmp_disconnect_slave(link);
 858         status = HCI_NO_CONNECTION;
 859         goto complete;
 860     }
 861
 862     /* Link established */
 863     link->handle = handle;
 864     bt_hci_lmp_link_establish(hci, link, 1);
 865
 866 complete:
 867     params.status       = status;
 868     params.handle       = HNDL(handle);
 869     params.link_type    = ACL_LINK;
 870     bacpy(&params.bdaddr, &link->slave->bd_addr);
 871     params.encr_mode    = 0x00;         /* Encryption not required */
 872     bt_hci_event(hci, EVT_CONN_COMPLETE, &params, EVT_CONN_COMPLETE_SIZE);
 873 }
 874
 875 static void bt_hci_disconnect(struct bt_hci_s *hci,
 876                 uint16_t handle, int reason)
 877 {
 878     struct bt_link_s *btlink =
 879             hci->lm.handle[handle & ~HCI_HANDLE_OFFSET].link;
 880     struct bt_hci_link_s *link;
 881     evt_disconn_complete params;
 882
 883     if (bt_hci_role_master(hci, handle)) {
 884         btlink->slave->reject_reason = reason;
 885         btlink->slave->lmp_disconnect_slave(btlink);
 886         /* The link pointer is invalid from now on */
 887
 888         goto complete;
 889     }
 890
 891     btlink->host->reject_reason = reason;
 892     btlink->host->lmp_disconnect_master(btlink);
 893
 894     /* We are the slave, we get to clean this burden */
 895     link = (struct bt_hci_link_s *) btlink;
 896     g_free(link);
 897
 898 complete:
 899     bt_hci_lmp_link_teardown(hci, handle);
 900
 901     params.status       = HCI_SUCCESS;
 902     params.handle       = HNDL(handle);
 903     params.reason       = HCI_CONNECTION_TERMINATED;
 904     bt_hci_event(hci, EVT_DISCONN_COMPLETE,
 905                     &params, EVT_DISCONN_COMPLETE_SIZE);
 906 }
 907
 908 /* TODO: use only one function */
 909 static void bt_hci_lmp_disconnect_host(struct bt_link_s *link)
 910 {
 911     struct bt_hci_s *hci = hci_from_device(link->host);
 912     uint16_t handle = link->handle;
 913     evt_disconn_complete params;
 914
 915     bt_hci_lmp_link_teardown(hci, handle);
 916
 917     params.status       = HCI_SUCCESS;
 918     params.handle       = HNDL(handle);
 919     params.reason       = hci->device.reject_reason;
 920     bt_hci_event(hci, EVT_DISCONN_COMPLETE,
 921                     &params, EVT_DISCONN_COMPLETE_SIZE);
 922 }
 923
 924 static void bt_hci_lmp_disconnect_slave(struct bt_link_s *btlink)
 925 {
 926     struct bt_hci_link_s *link = (struct bt_hci_link_s *) btlink;
 927     struct bt_hci_s *hci = hci_from_device(btlink->slave);
 928     uint16_t handle = link->handle;
 929     evt_disconn_complete params;
 930
 931     g_free(link);
 932
 933     bt_hci_lmp_link_teardown(hci, handle);
 934
 935     params.status       = HCI_SUCCESS;
 936     params.handle       = HNDL(handle);
 937     params.reason       = hci->device.reject_reason;
 938     bt_hci_event(hci, EVT_DISCONN_COMPLETE,
 939                     &params, EVT_DISCONN_COMPLETE_SIZE);
 940 }
 941
 942 static int bt_hci_name_req(struct bt_hci_s *hci, bdaddr_t *bdaddr)
 943 {
 944     struct bt_device_s *slave;
 945     evt_remote_name_req_complete params;
 946
 947     for (slave = hci->device.net->slave; slave; slave = slave->next)
 948         if (slave->page_scan && !bacmp(&slave->bd_addr, bdaddr))
 949             break;
 950     if (!slave)
 951         return -ENODEV;
 952
 953     bt_hci_event_status(hci, HCI_SUCCESS);
 954
 955     params.status       = HCI_SUCCESS;
 956     bacpy(&params.bdaddr, &slave->bd_addr);
 957     pstrcpy(params.name, sizeof(params.name), slave->lmp_name ?: "");
 958     bt_hci_event(hci, EVT_REMOTE_NAME_REQ_COMPLETE,
 959                     &params, EVT_REMOTE_NAME_REQ_COMPLETE_SIZE);
 960
 961     return 0;
 962 }
 963
 964 static int bt_hci_features_req(struct bt_hci_s *hci, uint16_t handle)
 965 {
 966     struct bt_device_s *slave;
 967     evt_read_remote_features_complete params;
 968
 969     if (bt_hci_handle_bad(hci, handle))
 970         return -ENODEV;
 971
 972     slave = bt_hci_remote_dev(hci, handle);
 973
 974     bt_hci_event_status(hci, HCI_SUCCESS);
 975
 976     params.status       = HCI_SUCCESS;
 977     params.handle       = HNDL(handle);
 978     params.features[0]  = (slave->lmp_caps >>  0) & 0xff;
 979     params.features[1]  = (slave->lmp_caps >>  8) & 0xff;
 980     params.features[2]  = (slave->lmp_caps >> 16) & 0xff;
 981     params.features[3]  = (slave->lmp_caps >> 24) & 0xff;
 982     params.features[4]  = (slave->lmp_caps >> 32) & 0xff;
 983     params.features[5]  = (slave->lmp_caps >> 40) & 0xff;
 984     params.features[6]  = (slave->lmp_caps >> 48) & 0xff;
 985     params.features[7]  = (slave->lmp_caps >> 56) & 0xff;
 986     bt_hci_event(hci, EVT_READ_REMOTE_FEATURES_COMPLETE,
 987                     &params, EVT_READ_REMOTE_FEATURES_COMPLETE_SIZE);
 988
 989     return 0;
 990 }
 991
 992 static int bt_hci_version_req(struct bt_hci_s *hci, uint16_t handle)
 993 {
 994     evt_read_remote_version_complete params;
 995
 996     if (bt_hci_handle_bad(hci, handle))
 997         return -ENODEV;
 998
 999     bt_hci_remote_dev(hci, handle);
1000
1001     bt_hci_event_status(hci, HCI_SUCCESS);
1002
1003     params.status       = HCI_SUCCESS;
1004     params.handle       = HNDL(handle);
1005     params.lmp_ver      = 0x03;
1006     params.manufacturer = cpu_to_le16(0xa000);
1007     params.lmp_subver   = cpu_to_le16(0xa607);
1008     bt_hci_event(hci, EVT_READ_REMOTE_VERSION_COMPLETE,
1009                     &params, EVT_READ_REMOTE_VERSION_COMPLETE_SIZE);
1010
1011     return 0;
1012 }
1013
1014 static int bt_hci_clkoffset_req(struct bt_hci_s *hci, uint16_t handle)
1015 {
1016     struct bt_device_s *slave;
1017     evt_read_clock_offset_complete params;
1018
1019     if (bt_hci_handle_bad(hci, handle))
1020         return -ENODEV;
1021
1022     slave = bt_hci_remote_dev(hci, handle);
1023
1024     bt_hci_event_status(hci, HCI_SUCCESS);
1025
1026     params.status       = HCI_SUCCESS;
1027     params.handle       = HNDL(handle);
1028     /* TODO: return the clkoff *differenece* */
1029     params.clock_offset = slave->clkoff;        /* Note: no swapping */
1030     bt_hci_event(hci, EVT_READ_CLOCK_OFFSET_COMPLETE,
1031                     &params, EVT_READ_CLOCK_OFFSET_COMPLETE_SIZE);
1032
1033     return 0;
1034 }
1035
1036 static void bt_hci_event_mode(struct bt_hci_s *hci, struct bt_link_s *link,
1037                 uint16_t handle)
1038 {
1039     evt_mode_change params = {
1040         .status         = HCI_SUCCESS,
1041         .handle         = HNDL(handle),
1042         .mode           = link->acl_mode,
1043         .interval       = cpu_to_le16(link->acl_interval),
1044     };
1045
1046     bt_hci_event(hci, EVT_MODE_CHANGE, &params, EVT_MODE_CHANGE_SIZE);
1047 }
1048
1049 static void bt_hci_lmp_mode_change_master(struct bt_hci_s *hci,
1050                 struct bt_link_s *link, int mode, uint16_t interval)
1051 {
1052     link->acl_mode = mode;
1053     link->acl_interval = interval;
1054
1055     bt_hci_event_mode(hci, link, link->handle);
1056
1057     link->slave->lmp_mode_change(link);
1058 }
1059
1060 static void bt_hci_lmp_mode_change_slave(struct bt_link_s *btlink)
1061 {
1062     struct bt_hci_link_s *link = (struct bt_hci_link_s *) btlink;
1063     struct bt_hci_s *hci = hci_from_device(btlink->slave);
1064
1065     bt_hci_event_mode(hci, btlink, link->handle);
1066 }
1067
1068 static int bt_hci_mode_change(struct bt_hci_s *hci, uint16_t handle,
1069                 int interval, int mode)
1070 {
1071     struct bt_hci_master_link_s *link;
1072
1073     if (bt_hci_handle_bad(hci, handle) || !bt_hci_role_master(hci, handle))
1074         return -ENODEV;
1075
1076     link = &hci->lm.handle[handle & ~HCI_HANDLE_OFFSET];
1077     if (link->link->acl_mode != acl_active) {
1078         bt_hci_event_status(hci, HCI_COMMAND_DISALLOWED);
1079         return 0;
1080     }
1081
1082     bt_hci_event_status(hci, HCI_SUCCESS);
1083
1084     qemu_mod_timer(link->acl_mode_timer, qemu_get_clock_ns(vm_clock) +
1085                    muldiv64(interval * 625, get_ticks_per_sec(), 1000000));
1086     bt_hci_lmp_mode_change_master(hci, link->link, mode, interval);
1087
1088     return 0;
1089 }
1090
1091 static int bt_hci_mode_cancel(struct bt_hci_s *hci, uint16_t handle, int mode)
1092 {
1093     struct bt_hci_master_link_s *link;
1094
1095     if (bt_hci_handle_bad(hci, handle) || !bt_hci_role_master(hci, handle))
1096         return -ENODEV;
1097
1098     link = &hci->lm.handle[handle & ~HCI_HANDLE_OFFSET];
1099     if (link->link->acl_mode != mode) {
1100         bt_hci_event_status(hci, HCI_COMMAND_DISALLOWED);
1101
1102         return 0;
1103     }
1104
1105     bt_hci_event_status(hci, HCI_SUCCESS);
1106
1107     qemu_del_timer(link->acl_mode_timer);
1108     bt_hci_lmp_mode_change_master(hci, link->link, acl_active, 0);
1109
1110     return 0;
1111 }
1112
1113 static void bt_hci_mode_tick(void *opaque)
1114 {
1115     struct bt_link_s *link = opaque;
1116     struct bt_hci_s *hci = hci_from_device(link->host);
1117
1118     bt_hci_lmp_mode_change_master(hci, link, acl_active, 0);
1119 }
1120
1121 static void bt_hci_reset(struct bt_hci_s *hci)
1122 {
1123     hci->acl_len = 0;
1124     hci->last_cmd = 0;
1125     hci->lm.connecting = 0;
1126
1127     hci->event_mask[0] = 0xff;
1128     hci->event_mask[1] = 0xff;
1129     hci->event_mask[2] = 0xff;
1130     hci->event_mask[3] = 0xff;
1131     hci->event_mask[4] = 0xff;
1132     hci->event_mask[5] = 0x1f;
1133     hci->event_mask[6] = 0x00;
1134     hci->event_mask[7] = 0x00;
1135     hci->device.inquiry_scan = 0;
1136     hci->device.page_scan = 0;
1137     if (hci->device.lmp_name)
1138         g_free((void *) hci->device.lmp_name);
1139     hci->device.lmp_name = NULL;
1140     hci->device.class[0] = 0x00;
1141     hci->device.class[1] = 0x00;
1142     hci->device.class[2] = 0x00;
1143     hci->voice_setting = 0x0000;
1144     hci->conn_accept_tout = 0x1f40;
1145     hci->lm.inquiry_mode = 0x00;
1146
1147     hci->psb_handle = 0x000;
1148     hci->asb_handle = 0x000;
1149
1150     /* XXX: qemu_del_timer(sl->acl_mode_timer); for all links */
1151     qemu_del_timer(hci->lm.inquiry_done);
1152     qemu_del_timer(hci->lm.inquiry_next);
1153     qemu_del_timer(hci->conn_accept_timer);
1154 }
1155
1156 static void bt_hci_read_local_version_rp(struct bt_hci_s *hci)
1157 {
1158     read_local_version_rp lv = {
1159         .status         = HCI_SUCCESS,
1160         .hci_ver        = 0x03,
1161         .hci_rev        = cpu_to_le16(0xa607),
1162         .lmp_ver        = 0x03,
1163         .manufacturer   = cpu_to_le16(0xa000),
1164         .lmp_subver     = cpu_to_le16(0xa607),
1165     };
1166
1167     bt_hci_event_complete(hci, &lv, READ_LOCAL_VERSION_RP_SIZE);
1168 }
1169
1170 static void bt_hci_read_local_commands_rp(struct bt_hci_s *hci)
1171 {
1172     read_local_commands_rp lc = {
1173         .status         = HCI_SUCCESS,
1174         .commands       = {
1175             /* Keep updated! */
1176             /* Also, keep in sync with hci->device.lmp_caps in bt_new_hci */
1177             0xbf, 0x80, 0xf9, 0x03, 0xb2, 0xc0, 0x03, 0xc3,
1178             0x00, 0x0f, 0x80, 0x00, 0xc0, 0x00, 0xe8, 0x13,
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             0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
1185         },
1186     };
1187
1188     bt_hci_event_complete(hci, &lc, READ_LOCAL_COMMANDS_RP_SIZE);
1189 }
1190
1191 static void bt_hci_read_local_features_rp(struct bt_hci_s *hci)
1192 {
1193     read_local_features_rp lf = {
1194         .status         = HCI_SUCCESS,
1195         .features       = {
1196             (hci->device.lmp_caps >>  0) & 0xff,
1197             (hci->device.lmp_caps >>  8) & 0xff,
1198             (hci->device.lmp_caps >> 16) & 0xff,
1199             (hci->device.lmp_caps >> 24) & 0xff,
1200             (hci->device.lmp_caps >> 32) & 0xff,
1201             (hci->device.lmp_caps >> 40) & 0xff,
1202             (hci->device.lmp_caps >> 48) & 0xff,
1203             (hci->device.lmp_caps >> 56) & 0xff,
1204         },
1205     };
1206
1207     bt_hci_event_complete(hci, &lf, READ_LOCAL_FEATURES_RP_SIZE);
1208 }
1209
1210 static void bt_hci_read_local_ext_features_rp(struct bt_hci_s *hci, int page)
1211 {
1212     read_local_ext_features_rp lef = {
1213         .status         = HCI_SUCCESS,
1214         .page_num       = page,
1215         .max_page_num   = 0x00,
1216         .features       = {
1217             /* Keep updated! */
1218             0x5f, 0x35, 0x85, 0x7e, 0x9b, 0x19, 0x00, 0x80,
1219         },
1220     };
1221     if (page)
1222         memset(lef.features, 0, sizeof(lef.features));
1223
1224     bt_hci_event_complete(hci, &lef, READ_LOCAL_EXT_FEATURES_RP_SIZE);
1225 }
1226
1227 static void bt_hci_read_buffer_size_rp(struct bt_hci_s *hci)
1228 {
1229     read_buffer_size_rp bs = {
1230         /* This can be made configurable, for one standard USB dongle HCI
1231          * the four values are cpu_to_le16(0x0180), 0x40,
1232          * cpu_to_le16(0x0008), cpu_to_le16(0x0008).  */
1233         .status         = HCI_SUCCESS,
1234         .acl_mtu        = cpu_to_le16(0x0200),
1235         .sco_mtu        = 0,
1236         .acl_max_pkt    = cpu_to_le16(0x0001),
1237         .sco_max_pkt    = cpu_to_le16(0x0000),
1238     };
1239
1240     bt_hci_event_complete(hci, &bs, READ_BUFFER_SIZE_RP_SIZE);
1241 }
1242
1243 /* Deprecated in V2.0 (page 661) */
1244 static void bt_hci_read_country_code_rp(struct bt_hci_s *hci)
1245 {
1246     read_country_code_rp cc ={
1247         .status         = HCI_SUCCESS,
1248         .country_code   = 0x00, /* North America & Europe^1 and Japan */
1249     };
1250
1251     bt_hci_event_complete(hci, &cc, READ_COUNTRY_CODE_RP_SIZE);
1252
1253     /* ^1. Except France, sorry */
1254 }
1255
1256 static void bt_hci_read_bd_addr_rp(struct bt_hci_s *hci)
1257 {
1258     read_bd_addr_rp ba = {
1259         .status = HCI_SUCCESS,
1260         .bdaddr = BAINIT(&hci->device.bd_addr),
1261     };
1262
1263     bt_hci_event_complete(hci, &ba, READ_BD_ADDR_RP_SIZE);
1264 }
1265
1266 static int bt_hci_link_quality_rp(struct bt_hci_s *hci, uint16_t handle)
1267 {
1268     read_link_quality_rp lq = {
1269         .status         = HCI_SUCCESS,
1270         .handle         = HNDL(handle),
1271         .link_quality   = 0xff,
1272     };
1273
1274     if (bt_hci_handle_bad(hci, handle))
1275         lq.status = HCI_NO_CONNECTION;
1276
1277     bt_hci_event_complete(hci, &lq, READ_LINK_QUALITY_RP_SIZE);
1278     return 0;
1279 }
1280
1281 /* Generate a Command Complete event with only the Status parameter */
1282 static inline void bt_hci_event_complete_status(struct bt_hci_s *hci,
1283                 uint8_t status)
1284 {
1285     bt_hci_event_complete(hci, &status, 1);
1286 }
1287
1288 static inline void bt_hci_event_complete_conn_cancel(struct bt_hci_s *hci,
1289                 uint8_t status, bdaddr_t *bd_addr)
1290 {
1291     create_conn_cancel_rp params = {
1292         .status = status,
1293         .bdaddr = BAINIT(bd_addr),
1294     };
1295
1296     bt_hci_event_complete(hci, &params, CREATE_CONN_CANCEL_RP_SIZE);
1297 }
1298
1299 static inline void bt_hci_event_auth_complete(struct bt_hci_s *hci,
1300                 uint16_t handle)
1301 {
1302     evt_auth_complete params = {
1303         .status = HCI_SUCCESS,
1304         .handle = HNDL(handle),
1305     };
1306
1307     bt_hci_event(hci, EVT_AUTH_COMPLETE, &params, EVT_AUTH_COMPLETE_SIZE);
1308 }
1309
1310 static inline void bt_hci_event_encrypt_change(struct bt_hci_s *hci,
1311                 uint16_t handle, uint8_t mode)
1312 {
1313     evt_encrypt_change params = {
1314         .status         = HCI_SUCCESS,
1315         .handle         = HNDL(handle),
1316         .encrypt        = mode,
1317     };
1318
1319     bt_hci_event(hci, EVT_ENCRYPT_CHANGE, &params, EVT_ENCRYPT_CHANGE_SIZE);
1320 }
1321
1322 static inline void bt_hci_event_complete_name_cancel(struct bt_hci_s *hci,
1323                 bdaddr_t *bd_addr)
1324 {
1325     remote_name_req_cancel_rp params = {
1326         .status = HCI_INVALID_PARAMETERS,
1327         .bdaddr = BAINIT(bd_addr),
1328     };
1329
1330     bt_hci_event_complete(hci, &params, REMOTE_NAME_REQ_CANCEL_RP_SIZE);
1331 }
1332
1333 static inline void bt_hci_event_read_remote_ext_features(struct bt_hci_s *hci,
1334                 uint16_t handle)
1335 {
1336     evt_read_remote_ext_features_complete params = {
1337         .status = HCI_UNSUPPORTED_FEATURE,
1338         .handle = HNDL(handle),
1339         /* Rest uninitialised */
1340     };
1341
1342     bt_hci_event(hci, EVT_READ_REMOTE_EXT_FEATURES_COMPLETE,
1343                     &params, EVT_READ_REMOTE_EXT_FEATURES_COMPLETE_SIZE);
1344 }
1345
1346 static inline void bt_hci_event_complete_lmp_handle(struct bt_hci_s *hci,
1347                 uint16_t handle)
1348 {
1349     read_lmp_handle_rp params = {
1350         .status         = HCI_NO_CONNECTION,
1351         .handle         = HNDL(handle),
1352         .reserved       = 0,
1353         /* Rest uninitialised */
1354     };
1355
1356     bt_hci_event_complete(hci, &params, READ_LMP_HANDLE_RP_SIZE);
1357 }
1358
1359 static inline void bt_hci_event_complete_role_discovery(struct bt_hci_s *hci,
1360                 int status, uint16_t handle, int master)
1361 {
1362     role_discovery_rp params = {
1363         .status         = status,
1364         .handle         = HNDL(handle),
1365         .role           = master ? 0x00 : 0x01,
1366     };
1367
1368     bt_hci_event_complete(hci, &params, ROLE_DISCOVERY_RP_SIZE);
1369 }
1370
1371 static inline void bt_hci_event_complete_flush(struct bt_hci_s *hci,
1372                 int status, uint16_t handle)
1373 {
1374     flush_rp params = {
1375         .status         = status,
1376         .handle         = HNDL(handle),
1377     };
1378
1379     bt_hci_event_complete(hci, &params, FLUSH_RP_SIZE);
1380 }
1381
1382 static inline void bt_hci_event_complete_read_local_name(struct bt_hci_s *hci)
1383 {
1384     read_local_name_rp params;
1385     params.status = HCI_SUCCESS;
1386     memset(params.name, 0, sizeof(params.name));
1387     if (hci->device.lmp_name)
1388         pstrcpy(params.name, sizeof(params.name), hci->device.lmp_name);
1389
1390     bt_hci_event_complete(hci, &params, READ_LOCAL_NAME_RP_SIZE);
1391 }
1392
1393 static inline void bt_hci_event_complete_read_conn_accept_timeout(
1394                 struct bt_hci_s *hci)
1395 {
1396     read_conn_accept_timeout_rp params = {
1397         .status         = HCI_SUCCESS,
1398         .timeout        = cpu_to_le16(hci->conn_accept_tout),
1399     };
1400
1401     bt_hci_event_complete(hci, &params, READ_CONN_ACCEPT_TIMEOUT_RP_SIZE);
1402 }
1403
1404 static inline void bt_hci_event_complete_read_scan_enable(struct bt_hci_s *hci)
1405 {
1406     read_scan_enable_rp params = {
1407         .status = HCI_SUCCESS,
1408         .enable =
1409                 (hci->device.inquiry_scan ? SCAN_INQUIRY : 0) |
1410                 (hci->device.page_scan ? SCAN_PAGE : 0),
1411     };
1412
1413     bt_hci_event_complete(hci, &params, READ_SCAN_ENABLE_RP_SIZE);
1414 }
1415
1416 static inline void bt_hci_event_complete_read_local_class(struct bt_hci_s *hci)
1417 {
1418     read_class_of_dev_rp params;
1419
1420     params.status = HCI_SUCCESS;
1421     memcpy(params.dev_class, hci->device.class, sizeof(params.dev_class));
1422
1423     bt_hci_event_complete(hci, &params, READ_CLASS_OF_DEV_RP_SIZE);
1424 }
1425
1426 static inline void bt_hci_event_complete_voice_setting(struct bt_hci_s *hci)
1427 {
1428     read_voice_setting_rp params = {
1429         .status         = HCI_SUCCESS,
1430         .voice_setting  = hci->voice_setting,   /* Note: no swapping */
1431     };
1432
1433     bt_hci_event_complete(hci, &params, READ_VOICE_SETTING_RP_SIZE);
1434 }
1435
1436 static inline void bt_hci_event_complete_read_inquiry_mode(
1437                 struct bt_hci_s *hci)
1438 {
1439     read_inquiry_mode_rp params = {
1440         .status         = HCI_SUCCESS,
1441         .mode           = hci->lm.inquiry_mode,
1442     };
1443
1444     bt_hci_event_complete(hci, &params, READ_INQUIRY_MODE_RP_SIZE);
1445 }
1446
1447 static inline void bt_hci_event_num_comp_pkts(struct bt_hci_s *hci,
1448                 uint16_t handle, int packets)
1449 {
1450     uint16_t buf[EVT_NUM_COMP_PKTS_SIZE(1) / 2 + 1];
1451     evt_num_comp_pkts *params = (void *) ((uint8_t *) buf + 1);
1452
1453     params->num_hndl                    = 1;
1454     params->connection->handle          = HNDL(handle);
1455     params->connection->num_packets     = cpu_to_le16(packets);
1456
1457     bt_hci_event(hci, EVT_NUM_COMP_PKTS, params, EVT_NUM_COMP_PKTS_SIZE(1));
1458 }
1459
1460 static void bt_submit_hci(struct HCIInfo *info,
1461                 const uint8_t *data, int length)
1462 {
1463     struct bt_hci_s *hci = hci_from_info(info);
1464     uint16_t cmd;
1465     int paramlen, i;
1466
1467     if (length < HCI_COMMAND_HDR_SIZE)
1468         goto short_hci;
1469
1470     memcpy(&hci->last_cmd, data, 2);
1471
1472     cmd = (data[1] << 8) | data[0];
1473     paramlen = data[2];
1474     if (cmd_opcode_ogf(cmd) == 0 || cmd_opcode_ocf(cmd) == 0)   /* NOP */
1475         return;
1476
1477     data += HCI_COMMAND_HDR_SIZE;
1478     length -= HCI_COMMAND_HDR_SIZE;
1479
1480     if (paramlen > length)
1481         return;
1482
1483 #define PARAM(cmd, param)       (((cmd##_cp *) data)->param)
1484 #define PARAM16(cmd, param)     le16_to_cpup(&PARAM(cmd, param))
1485 #define PARAMHANDLE(cmd)        HNDL(PARAM(cmd, handle))
1486 #define LENGTH_CHECK(cmd)       if (length < sizeof(cmd##_cp)) goto short_hci
1487     /* Note: the supported commands bitmask in bt_hci_read_local_commands_rp
1488      * needs to be updated every time a command is implemented here!  */
1489     switch (cmd) {
1490     case cmd_opcode_pack(OGF_LINK_CTL, OCF_INQUIRY):
1491         LENGTH_CHECK(inquiry);
1492
1493         if (PARAM(inquiry, length) < 1) {
1494             bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS);
1495             break;
1496         }
1497
1498         hci->lm.inquire = 1;
1499         hci->lm.periodic = 0;
1500         hci->lm.responses_left = PARAM(inquiry, num_rsp) ?: INT_MAX;
1501         hci->lm.responses = 0;
1502         bt_hci_event_status(hci, HCI_SUCCESS);
1503         bt_hci_inquiry_start(hci, PARAM(inquiry, length));
1504         break;
1505
1506     case cmd_opcode_pack(OGF_LINK_CTL, OCF_INQUIRY_CANCEL):
1507         if (!hci->lm.inquire || hci->lm.periodic) {
1508             fprintf(stderr, "%s: Inquiry Cancel should only be issued after "
1509                             "the Inquiry command has been issued, a Command "
1510                             "Status event has been received for the Inquiry "
1511                             "command, and before the Inquiry Complete event "
1512                             "occurs", __FUNCTION__);
1513             bt_hci_event_complete_status(hci, HCI_COMMAND_DISALLOWED);
1514             break;
1515         }
1516
1517         hci->lm.inquire = 0;
1518         qemu_del_timer(hci->lm.inquiry_done);
1519         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1520         break;
1521
1522     case cmd_opcode_pack(OGF_LINK_CTL, OCF_PERIODIC_INQUIRY):
1523         LENGTH_CHECK(periodic_inquiry);
1524
1525         if (!(PARAM(periodic_inquiry, length) <
1526                                 PARAM16(periodic_inquiry, min_period) &&
1527                                 PARAM16(periodic_inquiry, min_period) <
1528                                 PARAM16(periodic_inquiry, max_period)) ||
1529                         PARAM(periodic_inquiry, length) < 1 ||
1530                         PARAM16(periodic_inquiry, min_period) < 2 ||
1531                         PARAM16(periodic_inquiry, max_period) < 3) {
1532             bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS);
1533             break;
1534         }
1535
1536         hci->lm.inquire = 1;
1537         hci->lm.periodic = 1;
1538         hci->lm.responses_left = PARAM(periodic_inquiry, num_rsp);
1539         hci->lm.responses = 0;
1540         hci->lm.inquiry_period = PARAM16(periodic_inquiry, max_period);
1541         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1542         bt_hci_inquiry_start(hci, PARAM(periodic_inquiry, length));
1543         break;
1544
1545     case cmd_opcode_pack(OGF_LINK_CTL, OCF_EXIT_PERIODIC_INQUIRY):
1546         if (!hci->lm.inquire || !hci->lm.periodic) {
1547             fprintf(stderr, "%s: Inquiry Cancel should only be issued after "
1548                             "the Inquiry command has been issued, a Command "
1549                             "Status event has been received for the Inquiry "
1550                             "command, and before the Inquiry Complete event "
1551                             "occurs", __FUNCTION__);
1552             bt_hci_event_complete_status(hci, HCI_COMMAND_DISALLOWED);
1553             break;
1554         }
1555         hci->lm.inquire = 0;
1556         qemu_del_timer(hci->lm.inquiry_done);
1557         qemu_del_timer(hci->lm.inquiry_next);
1558         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1559         break;
1560
1561     case cmd_opcode_pack(OGF_LINK_CTL, OCF_CREATE_CONN):
1562         LENGTH_CHECK(create_conn);
1563
1564         if (hci->lm.connecting >= HCI_HANDLES_MAX) {
1565             bt_hci_event_status(hci, HCI_REJECTED_LIMITED_RESOURCES);
1566             break;
1567         }
1568         bt_hci_event_status(hci, HCI_SUCCESS);
1569
1570         if (bt_hci_connect(hci, &PARAM(create_conn, bdaddr)))
1571             bt_hci_connection_reject_event(hci, &PARAM(create_conn, bdaddr));
1572         break;
1573
1574     case cmd_opcode_pack(OGF_LINK_CTL, OCF_DISCONNECT):
1575         LENGTH_CHECK(disconnect);
1576
1577         if (bt_hci_handle_bad(hci, PARAMHANDLE(disconnect))) {
1578             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1579             break;
1580         }
1581
1582         bt_hci_event_status(hci, HCI_SUCCESS);
1583         bt_hci_disconnect(hci, PARAMHANDLE(disconnect),
1584                         PARAM(disconnect, reason));
1585         break;
1586
1587     case cmd_opcode_pack(OGF_LINK_CTL, OCF_CREATE_CONN_CANCEL):
1588         LENGTH_CHECK(create_conn_cancel);
1589
1590         if (bt_hci_lmp_connection_ready(hci,
1591                                 &PARAM(create_conn_cancel, bdaddr))) {
1592             for (i = 0; i < HCI_HANDLES_MAX; i ++)
1593                 if (bt_hci_role_master(hci, i) && hci->lm.handle[i].link &&
1594                                 !bacmp(&hci->lm.handle[i].link->slave->bd_addr,
1595                                         &PARAM(create_conn_cancel, bdaddr)))
1596                    break;
1597
1598             bt_hci_event_complete_conn_cancel(hci, i < HCI_HANDLES_MAX ?
1599                             HCI_ACL_CONNECTION_EXISTS : HCI_NO_CONNECTION,
1600                             &PARAM(create_conn_cancel, bdaddr));
1601         } else
1602             bt_hci_event_complete_conn_cancel(hci, HCI_SUCCESS,
1603                             &PARAM(create_conn_cancel, bdaddr));
1604         break;
1605
1606     case cmd_opcode_pack(OGF_LINK_CTL, OCF_ACCEPT_CONN_REQ):
1607         LENGTH_CHECK(accept_conn_req);
1608
1609         if (!hci->conn_req_host ||
1610                         bacmp(&PARAM(accept_conn_req, bdaddr),
1611                                 &hci->conn_req_host->bd_addr)) {
1612             bt_hci_event_status(hci, HCI_INVALID_PARAMETERS);
1613             break;
1614         }
1615
1616         bt_hci_event_status(hci, HCI_SUCCESS);
1617         bt_hci_connection_accept(hci, hci->conn_req_host);
1618         hci->conn_req_host = NULL;
1619         break;
1620
1621     case cmd_opcode_pack(OGF_LINK_CTL, OCF_REJECT_CONN_REQ):
1622         LENGTH_CHECK(reject_conn_req);
1623
1624         if (!hci->conn_req_host ||
1625                         bacmp(&PARAM(reject_conn_req, bdaddr),
1626                                 &hci->conn_req_host->bd_addr)) {
1627             bt_hci_event_status(hci, HCI_INVALID_PARAMETERS);
1628             break;
1629         }
1630
1631         bt_hci_event_status(hci, HCI_SUCCESS);
1632         bt_hci_connection_reject(hci, hci->conn_req_host,
1633                         PARAM(reject_conn_req, reason));
1634         bt_hci_connection_reject_event(hci, &hci->conn_req_host->bd_addr);
1635         hci->conn_req_host = NULL;
1636         break;
1637
1638     case cmd_opcode_pack(OGF_LINK_CTL, OCF_AUTH_REQUESTED):
1639         LENGTH_CHECK(auth_requested);
1640
1641         if (bt_hci_handle_bad(hci, PARAMHANDLE(auth_requested)))
1642             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1643         else {
1644             bt_hci_event_status(hci, HCI_SUCCESS);
1645             bt_hci_event_auth_complete(hci, PARAMHANDLE(auth_requested));
1646         }
1647         break;
1648
1649     case cmd_opcode_pack(OGF_LINK_CTL, OCF_SET_CONN_ENCRYPT):
1650         LENGTH_CHECK(set_conn_encrypt);
1651
1652         if (bt_hci_handle_bad(hci, PARAMHANDLE(set_conn_encrypt)))
1653             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1654         else {
1655             bt_hci_event_status(hci, HCI_SUCCESS);
1656             bt_hci_event_encrypt_change(hci,
1657                             PARAMHANDLE(set_conn_encrypt),
1658                             PARAM(set_conn_encrypt, encrypt));
1659         }
1660         break;
1661
1662     case cmd_opcode_pack(OGF_LINK_CTL, OCF_REMOTE_NAME_REQ):
1663         LENGTH_CHECK(remote_name_req);
1664
1665         if (bt_hci_name_req(hci, &PARAM(remote_name_req, bdaddr)))
1666             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1667         break;
1668
1669     case cmd_opcode_pack(OGF_LINK_CTL, OCF_REMOTE_NAME_REQ_CANCEL):
1670         LENGTH_CHECK(remote_name_req_cancel);
1671
1672         bt_hci_event_complete_name_cancel(hci,
1673                         &PARAM(remote_name_req_cancel, bdaddr));
1674         break;
1675
1676     case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_REMOTE_FEATURES):
1677         LENGTH_CHECK(read_remote_features);
1678
1679         if (bt_hci_features_req(hci, PARAMHANDLE(read_remote_features)))
1680             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1681         break;
1682
1683     case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_REMOTE_EXT_FEATURES):
1684         LENGTH_CHECK(read_remote_ext_features);
1685
1686         if (bt_hci_handle_bad(hci, PARAMHANDLE(read_remote_ext_features)))
1687             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1688         else {
1689             bt_hci_event_status(hci, HCI_SUCCESS);
1690             bt_hci_event_read_remote_ext_features(hci,
1691                             PARAMHANDLE(read_remote_ext_features));
1692         }
1693         break;
1694
1695     case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_REMOTE_VERSION):
1696         LENGTH_CHECK(read_remote_version);
1697
1698         if (bt_hci_version_req(hci, PARAMHANDLE(read_remote_version)))
1699             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1700         break;
1701
1702     case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_CLOCK_OFFSET):
1703         LENGTH_CHECK(read_clock_offset);
1704
1705         if (bt_hci_clkoffset_req(hci, PARAMHANDLE(read_clock_offset)))
1706             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1707         break;
1708
1709     case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_LMP_HANDLE):
1710         LENGTH_CHECK(read_lmp_handle);
1711
1712         /* TODO: */
1713         bt_hci_event_complete_lmp_handle(hci, PARAMHANDLE(read_lmp_handle));
1714         break;
1715
1716     case cmd_opcode_pack(OGF_LINK_POLICY, OCF_HOLD_MODE):
1717         LENGTH_CHECK(hold_mode);
1718
1719         if (PARAM16(hold_mode, min_interval) >
1720                         PARAM16(hold_mode, max_interval) ||
1721                         PARAM16(hold_mode, min_interval) < 0x0002 ||
1722                         PARAM16(hold_mode, max_interval) > 0xff00 ||
1723                         (PARAM16(hold_mode, min_interval) & 1) ||
1724                         (PARAM16(hold_mode, max_interval) & 1)) {
1725             bt_hci_event_status(hci, HCI_INVALID_PARAMETERS);
1726             break;
1727         }
1728
1729         if (bt_hci_mode_change(hci, PARAMHANDLE(hold_mode),
1730                                 PARAM16(hold_mode, max_interval),
1731                                 acl_hold))
1732             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1733         break;
1734
1735     case cmd_opcode_pack(OGF_LINK_POLICY, OCF_PARK_MODE):
1736         LENGTH_CHECK(park_mode);
1737
1738         if (PARAM16(park_mode, min_interval) >
1739                         PARAM16(park_mode, max_interval) ||
1740                         PARAM16(park_mode, min_interval) < 0x000e ||
1741                         (PARAM16(park_mode, min_interval) & 1) ||
1742                         (PARAM16(park_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(park_mode),
1748                                 PARAM16(park_mode, max_interval),
1749                                 acl_parked))
1750             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1751         break;
1752
1753     case cmd_opcode_pack(OGF_LINK_POLICY, OCF_EXIT_PARK_MODE):
1754         LENGTH_CHECK(exit_park_mode);
1755
1756         if (bt_hci_mode_cancel(hci, PARAMHANDLE(exit_park_mode),
1757                                 acl_parked))
1758             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1759         break;
1760
1761     case cmd_opcode_pack(OGF_LINK_POLICY, OCF_ROLE_DISCOVERY):
1762         LENGTH_CHECK(role_discovery);
1763
1764         if (bt_hci_handle_bad(hci, PARAMHANDLE(role_discovery)))
1765             bt_hci_event_complete_role_discovery(hci,
1766                             HCI_NO_CONNECTION, PARAMHANDLE(role_discovery), 0);
1767         else
1768             bt_hci_event_complete_role_discovery(hci,
1769                             HCI_SUCCESS, PARAMHANDLE(role_discovery),
1770                             bt_hci_role_master(hci,
1771                                     PARAMHANDLE(role_discovery)));
1772         break;
1773
1774     case cmd_opcode_pack(OGF_HOST_CTL, OCF_SET_EVENT_MASK):
1775         LENGTH_CHECK(set_event_mask);
1776
1777         memcpy(hci->event_mask, PARAM(set_event_mask, mask), 8);
1778         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1779         break;
1780
1781     case cmd_opcode_pack(OGF_HOST_CTL, OCF_RESET):
1782         bt_hci_reset(hci);
1783         bt_hci_event_status(hci, HCI_SUCCESS);
1784         break;
1785
1786     case cmd_opcode_pack(OGF_HOST_CTL, OCF_SET_EVENT_FLT):
1787         if (length >= 1 && PARAM(set_event_flt, flt_type) == FLT_CLEAR_ALL)
1788             /* No length check */;
1789         else
1790             LENGTH_CHECK(set_event_flt);
1791
1792         /* Filters are not implemented */
1793         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1794         break;
1795
1796     case cmd_opcode_pack(OGF_HOST_CTL, OCF_FLUSH):
1797         LENGTH_CHECK(flush);
1798
1799         if (bt_hci_handle_bad(hci, PARAMHANDLE(flush)))
1800             bt_hci_event_complete_flush(hci,
1801                             HCI_NO_CONNECTION, PARAMHANDLE(flush));
1802         else {
1803             /* TODO: ordering? */
1804             bt_hci_event(hci, EVT_FLUSH_OCCURRED,
1805                             &PARAM(flush, handle),
1806                             EVT_FLUSH_OCCURRED_SIZE);
1807             bt_hci_event_complete_flush(hci,
1808                             HCI_SUCCESS, PARAMHANDLE(flush));
1809         }
1810         break;
1811
1812     case cmd_opcode_pack(OGF_HOST_CTL, OCF_CHANGE_LOCAL_NAME):
1813         LENGTH_CHECK(change_local_name);
1814
1815         if (hci->device.lmp_name)
1816             g_free((void *) hci->device.lmp_name);
1817         hci->device.lmp_name = g_strndup(PARAM(change_local_name, name),
1818                         sizeof(PARAM(change_local_name, name)));
1819         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1820         break;
1821
1822     case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_LOCAL_NAME):
1823         bt_hci_event_complete_read_local_name(hci);
1824         break;
1825
1826     case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_CONN_ACCEPT_TIMEOUT):
1827         bt_hci_event_complete_read_conn_accept_timeout(hci);
1828         break;
1829
1830     case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_CONN_ACCEPT_TIMEOUT):
1831         /* TODO */
1832         LENGTH_CHECK(write_conn_accept_timeout);
1833
1834         if (PARAM16(write_conn_accept_timeout, timeout) < 0x0001 ||
1835                         PARAM16(write_conn_accept_timeout, timeout) > 0xb540) {
1836             bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS);
1837             break;
1838         }
1839
1840         hci->conn_accept_tout = PARAM16(write_conn_accept_timeout, timeout);
1841         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1842         break;
1843
1844     case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_SCAN_ENABLE):
1845         bt_hci_event_complete_read_scan_enable(hci);
1846         break;
1847
1848     case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_SCAN_ENABLE):
1849         LENGTH_CHECK(write_scan_enable);
1850
1851         /* TODO: check that the remaining bits are all 0 */
1852         hci->device.inquiry_scan =
1853                 !!(PARAM(write_scan_enable, scan_enable) & SCAN_INQUIRY);
1854         hci->device.page_scan =
1855                 !!(PARAM(write_scan_enable, scan_enable) & SCAN_PAGE);
1856         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1857         break;
1858
1859     case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_CLASS_OF_DEV):
1860         bt_hci_event_complete_read_local_class(hci);
1861         break;
1862
1863     case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_CLASS_OF_DEV):
1864         LENGTH_CHECK(write_class_of_dev);
1865
1866         memcpy(hci->device.class, PARAM(write_class_of_dev, dev_class),
1867                         sizeof(PARAM(write_class_of_dev, dev_class)));
1868         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1869         break;
1870
1871     case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_VOICE_SETTING):
1872         bt_hci_event_complete_voice_setting(hci);
1873         break;
1874
1875     case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_VOICE_SETTING):
1876         LENGTH_CHECK(write_voice_setting);
1877
1878         hci->voice_setting = PARAM(write_voice_setting, voice_setting);
1879         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1880         break;
1881
1882     case cmd_opcode_pack(OGF_HOST_CTL, OCF_HOST_NUMBER_OF_COMPLETED_PACKETS):
1883         if (length < data[0] * 2 + 1)
1884             goto short_hci;
1885
1886         for (i = 0; i < data[0]; i ++)
1887             if (bt_hci_handle_bad(hci,
1888                                     data[i * 2 + 1] | (data[i * 2 + 2] << 8)))
1889                 bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS);
1890         break;
1891
1892     case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_INQUIRY_MODE):
1893         /* Only if (local_features[3] & 0x40) && (local_commands[12] & 0x40)
1894          * else
1895          *     goto unknown_command */
1896         bt_hci_event_complete_read_inquiry_mode(hci);
1897         break;
1898
1899     case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_INQUIRY_MODE):
1900         /* Only if (local_features[3] & 0x40) && (local_commands[12] & 0x80)
1901          * else
1902          *     goto unknown_command */
1903         LENGTH_CHECK(write_inquiry_mode);
1904
1905         if (PARAM(write_inquiry_mode, mode) > 0x01) {
1906             bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS);
1907             break;
1908         }
1909
1910         hci->lm.inquiry_mode = PARAM(write_inquiry_mode, mode);
1911         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1912         break;
1913
1914     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_VERSION):
1915         bt_hci_read_local_version_rp(hci);
1916         break;
1917
1918     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_COMMANDS):
1919         bt_hci_read_local_commands_rp(hci);
1920         break;
1921
1922     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_FEATURES):
1923         bt_hci_read_local_features_rp(hci);
1924         break;
1925
1926     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_EXT_FEATURES):
1927         LENGTH_CHECK(read_local_ext_features);
1928
1929         bt_hci_read_local_ext_features_rp(hci,
1930                         PARAM(read_local_ext_features, page_num));
1931         break;
1932
1933     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_BUFFER_SIZE):
1934         bt_hci_read_buffer_size_rp(hci);
1935         break;
1936
1937     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_COUNTRY_CODE):
1938         bt_hci_read_country_code_rp(hci);
1939         break;
1940
1941     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_BD_ADDR):
1942         bt_hci_read_bd_addr_rp(hci);
1943         break;
1944
1945     case cmd_opcode_pack(OGF_STATUS_PARAM, OCF_READ_LINK_QUALITY):
1946         LENGTH_CHECK(read_link_quality);
1947
1948         bt_hci_link_quality_rp(hci, PARAMHANDLE(read_link_quality));
1949         break;
1950
1951     default:
1952         bt_hci_event_status(hci, HCI_UNKNOWN_COMMAND);
1953         break;
1954
1955     short_hci:
1956         fprintf(stderr, "%s: HCI packet too short (%iB)\n",
1957                         __FUNCTION__, length);
1958         bt_hci_event_status(hci, HCI_INVALID_PARAMETERS);
1959         break;
1960     }
1961 }
1962
1963 /* We could perform fragmentation here, we can't do "recombination" because
1964  * at this layer the length of the payload is not know ahead, so we only
1965  * know that a packet contained the last fragment of the SDU when the next
1966  * SDU starts.  */
1967 static inline void bt_hci_lmp_acl_data(struct bt_hci_s *hci, uint16_t handle,
1968                 const uint8_t *data, int start, int len)
1969 {
1970     struct hci_acl_hdr *pkt = (void *) hci->acl_buf;
1971
1972     /* TODO: packet flags */
1973     /* TODO: avoid memcpy'ing */
1974
1975     if (len + HCI_ACL_HDR_SIZE > sizeof(hci->acl_buf)) {
1976         fprintf(stderr, "%s: can't take ACL packets %i bytes long\n",
1977                         __FUNCTION__, len);
1978         return;
1979     }
1980     memcpy(hci->acl_buf + HCI_ACL_HDR_SIZE, data, len);
1981
1982     pkt->handle = cpu_to_le16(
1983                     acl_handle_pack(handle, start ? ACL_START : ACL_CONT));
1984     pkt->dlen = cpu_to_le16(len);
1985     hci->info.acl_recv(hci->info.opaque,
1986                     hci->acl_buf, len + HCI_ACL_HDR_SIZE);
1987 }
1988
1989 static void bt_hci_lmp_acl_data_slave(struct bt_link_s *btlink,
1990                 const uint8_t *data, int start, int len)
1991 {
1992     struct bt_hci_link_s *link = (struct bt_hci_link_s *) btlink;
1993
1994     bt_hci_lmp_acl_data(hci_from_device(btlink->slave),
1995                     link->handle, data, start, len);
1996 }
1997
1998 static void bt_hci_lmp_acl_data_host(struct bt_link_s *link,
1999                 const uint8_t *data, int start, int len)
2000 {
2001     bt_hci_lmp_acl_data(hci_from_device(link->host),
2002                     link->handle, data, start, len);
2003 }
2004
2005 static void bt_submit_acl(struct HCIInfo *info,
2006                 const uint8_t *data, int length)
2007 {
2008     struct bt_hci_s *hci = hci_from_info(info);
2009     uint16_t handle;
2010     int datalen, flags;
2011     struct bt_link_s *link;
2012
2013     if (length < HCI_ACL_HDR_SIZE) {
2014         fprintf(stderr, "%s: ACL packet too short (%iB)\n",
2015                         __FUNCTION__, length);
2016         return;
2017     }
2018
2019     handle = acl_handle((data[1] << 8) | data[0]);
2020     flags = acl_flags((data[1] << 8) | data[0]);
2021     datalen = (data[3] << 8) | data[2];
2022     data += HCI_ACL_HDR_SIZE;
2023     length -= HCI_ACL_HDR_SIZE;
2024
2025     if (bt_hci_handle_bad(hci, handle)) {
2026         fprintf(stderr, "%s: invalid ACL handle %03x\n",
2027                         __FUNCTION__, handle);
2028         /* TODO: signal an error */
2029         return;
2030     }
2031     handle &= ~HCI_HANDLE_OFFSET;
2032
2033     if (datalen > length) {
2034         fprintf(stderr, "%s: ACL packet too short (%iB < %iB)\n",
2035                         __FUNCTION__, length, datalen);
2036         return;
2037     }
2038
2039     link = hci->lm.handle[handle].link;
2040
2041     if ((flags & ~3) == ACL_ACTIVE_BCAST) {
2042         if (!hci->asb_handle)
2043             hci->asb_handle = handle;
2044         else if (handle != hci->asb_handle) {
2045             fprintf(stderr, "%s: Bad handle %03x in Active Slave Broadcast\n",
2046                             __FUNCTION__, handle);
2047             /* TODO: signal an error */
2048             return;
2049         }
2050
2051         /* TODO */
2052     }
2053
2054     if ((flags & ~3) == ACL_PICO_BCAST) {
2055         if (!hci->psb_handle)
2056             hci->psb_handle = handle;
2057         else if (handle != hci->psb_handle) {
2058             fprintf(stderr, "%s: Bad handle %03x in Parked Slave Broadcast\n",
2059                             __FUNCTION__, handle);
2060             /* TODO: signal an error */
2061             return;
2062         }
2063
2064         /* TODO */
2065     }
2066
2067     /* TODO: increase counter and send EVT_NUM_COMP_PKTS */
2068     bt_hci_event_num_comp_pkts(hci, handle | HCI_HANDLE_OFFSET, 1);
2069
2070     /* Do this last as it can trigger further events even in this HCI */
2071     hci->lm.handle[handle].lmp_acl_data(link, data,
2072                     (flags & 3) == ACL_START, length);
2073 }
2074
2075 static void bt_submit_sco(struct HCIInfo *info,
2076                 const uint8_t *data, int length)
2077 {
2078     struct bt_hci_s *hci = hci_from_info(info);
2079     uint16_t handle;
2080     int datalen;
2081
2082     if (length < 3)
2083         return;
2084
2085     handle = acl_handle((data[1] << 8) | data[0]);
2086     datalen = data[2];
2087     length -= 3;
2088
2089     if (bt_hci_handle_bad(hci, handle)) {
2090         fprintf(stderr, "%s: invalid SCO handle %03x\n",
2091                         __FUNCTION__, handle);
2092         return;
2093     }
2094
2095     if (datalen > length) {
2096         fprintf(stderr, "%s: SCO packet too short (%iB < %iB)\n",
2097                         __FUNCTION__, length, datalen);
2098         return;
2099     }
2100
2101     /* TODO */
2102
2103     /* TODO: increase counter and send EVT_NUM_COMP_PKTS if synchronous
2104      * Flow Control is enabled.
2105      * (See Read/Write_Synchronous_Flow_Control_Enable on page 513 and
2106      * page 514.)  */
2107 }
2108
2109 static uint8_t *bt_hci_evt_packet(void *opaque)
2110 {
2111     /* TODO: allocate a packet from upper layer */
2112     struct bt_hci_s *s = opaque;
2113
2114     return s->evt_buf;
2115 }
2116
2117 static void bt_hci_evt_submit(void *opaque, int len)
2118 {
2119     /* TODO: notify upper layer */
2120     struct bt_hci_s *s = opaque;
2121
2122     s->info.evt_recv(s->info.opaque, s->evt_buf, len);
2123 }
2124
2125 static int bt_hci_bdaddr_set(struct HCIInfo *info, const uint8_t *bd_addr)
2126 {
2127     struct bt_hci_s *hci = hci_from_info(info);
2128
2129     bacpy(&hci->device.bd_addr, (const bdaddr_t *) bd_addr);
2130     return 0;
2131 }
2132
2133 static void bt_hci_done(struct HCIInfo *info);
2134 static void bt_hci_destroy(struct bt_device_s *dev)
2135 {
2136     struct bt_hci_s *hci = hci_from_device(dev);
2137
2138     bt_hci_done(&hci->info);
2139 }
2140
2141 struct HCIInfo *bt_new_hci(struct bt_scatternet_s *net)
2142 {
2143     struct bt_hci_s *s = g_malloc0(sizeof(struct bt_hci_s));
2144
2145     s->lm.inquiry_done = qemu_new_timer_ns(vm_clock, bt_hci_inquiry_done, s);
2146     s->lm.inquiry_next = qemu_new_timer_ns(vm_clock, bt_hci_inquiry_next, s);
2147     s->conn_accept_timer =
2148             qemu_new_timer_ns(vm_clock, bt_hci_conn_accept_timeout, s);
2149
2150     s->evt_packet = bt_hci_evt_packet;
2151     s->evt_submit = bt_hci_evt_submit;
2152     s->opaque = s;
2153
2154     bt_device_init(&s->device, net);
2155     s->device.lmp_connection_request = bt_hci_lmp_connection_request;
2156     s->device.lmp_connection_complete = bt_hci_lmp_connection_complete;
2157     s->device.lmp_disconnect_master = bt_hci_lmp_disconnect_host;
2158     s->device.lmp_disconnect_slave = bt_hci_lmp_disconnect_slave;
2159     s->device.lmp_acl_data = bt_hci_lmp_acl_data_slave;
2160     s->device.lmp_acl_resp = bt_hci_lmp_acl_data_host;
2161     s->device.lmp_mode_change = bt_hci_lmp_mode_change_slave;
2162
2163     /* Keep updated! */
2164     /* Also keep in sync with supported commands bitmask in
2165      * bt_hci_read_local_commands_rp */
2166     s->device.lmp_caps = 0x8000199b7e85355fll;
2167
2168     bt_hci_reset(s);
2169
2170     s->info.cmd_send = bt_submit_hci;
2171     s->info.sco_send = bt_submit_sco;
2172     s->info.acl_send = bt_submit_acl;
2173     s->info.bdaddr_set = bt_hci_bdaddr_set;
2174
2175     s->device.handle_destroy = bt_hci_destroy;
2176
2177     return &s->info;
2178 }
2179
2180 static void bt_hci_done(struct HCIInfo *info)
2181 {
2182     struct bt_hci_s *hci = hci_from_info(info);
2183     int handle;
2184
2185     bt_device_done(&hci->device);
2186
2187     if (hci->device.lmp_name)
2188         g_free((void *) hci->device.lmp_name);
2189
2190     /* Be gentle and send DISCONNECT to all connected peers and those
2191      * currently waiting for us to accept or reject a connection request.
2192      * This frees the links.  */
2193     if (hci->conn_req_host) {
2194         bt_hci_connection_reject(hci,
2195                                  hci->conn_req_host, HCI_OE_POWER_OFF);
2196         return;
2197     }
2198
2199     for (handle = HCI_HANDLE_OFFSET;
2200                     handle < (HCI_HANDLE_OFFSET | HCI_HANDLES_MAX); handle ++)
2201         if (!bt_hci_handle_bad(hci, handle))
2202             bt_hci_disconnect(hci, handle, HCI_OE_POWER_OFF);
2203
2204     /* TODO: this is not enough actually, there may be slaves from whom
2205      * we have requested a connection who will soon (or not) respond with
2206      * an accept or a reject, so we should also check if hci->lm.connecting
2207      * is non-zero and if so, avoid freeing the hci but otherwise disappear
2208      * from all qemu social life (e.g. stop scanning and request to be
2209      * removed from s->device.net) and arrange for
2210      * s->device.lmp_connection_complete to free the remaining bits once
2211      * hci->lm.awaiting_bdaddr[] is empty.  */
2212
2213     qemu_free_timer(hci->lm.inquiry_done);
2214     qemu_free_timer(hci->lm.inquiry_next);
2215     qemu_free_timer(hci->conn_accept_timer);
2216
2217     g_free(hci);
2218 }