hw/bt/hci.c

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