qemu/hw/bt/hci.c

   1 /*
   2  * QEMU Bluetooth HCI logic.
   3  *
   4  * Copyright (C) 2007 OpenMoko, Inc.
   5  * Copyright (C) 2008 Andrzej Zaborowski  <balrog@zabor.org>
   6  *
   7  * This program is free software; you can redistribute it and/or
   8  * modify it under the terms of the GNU General Public License as
   9  * published by the Free Software Foundation; either version 2 of
  10  * the License, or (at your option) any later version.
  11  *
  12  * This program is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License
  18  * along with this program; if not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include "qemu-common.h"
  22 #include "qemu/timer.h"
  23 #include "hw/usb.h"
  24 #include "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                    muldiv64(period << 7, get_ticks_per_sec(), 100));
 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                    muldiv64(interval * 625, get_ticks_per_sec(), 1000000));
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     if (hci->device.lmp_name)
1155         g_free((void *) hci->device.lmp_name);
1156     hci->device.lmp_name = NULL;
1157     hci->device.class[0] = 0x00;
1158     hci->device.class[1] = 0x00;
1159     hci->device.class[2] = 0x00;
1160     hci->voice_setting = 0x0000;
1161     hci->conn_accept_tout = 0x1f40;
1162     hci->lm.inquiry_mode = 0x00;
1163
1164     hci->psb_handle = 0x000;
1165     hci->asb_handle = 0x000;
1166
1167     /* XXX: timer_del(sl->acl_mode_timer); for all links */
1168     timer_del(hci->lm.inquiry_done);
1169     timer_del(hci->lm.inquiry_next);
1170     timer_del(hci->conn_accept_timer);
1171 }
1172
1173 static void bt_hci_read_local_version_rp(struct bt_hci_s *hci)
1174 {
1175     read_local_version_rp lv = {
1176         .status         = HCI_SUCCESS,
1177         .hci_ver        = 0x03,
1178         .hci_rev        = cpu_to_le16(0xa607),
1179         .lmp_ver        = 0x03,
1180         .manufacturer   = cpu_to_le16(0xa000),
1181         .lmp_subver     = cpu_to_le16(0xa607),
1182     };
1183
1184     bt_hci_event_complete(hci, &lv, READ_LOCAL_VERSION_RP_SIZE);
1185 }
1186
1187 static void bt_hci_read_local_commands_rp(struct bt_hci_s *hci)
1188 {
1189     read_local_commands_rp lc = {
1190         .status         = HCI_SUCCESS,
1191         .commands       = {
1192             /* Keep updated! */
1193             /* Also, keep in sync with hci->device.lmp_caps in bt_new_hci */
1194             0xbf, 0x80, 0xf9, 0x03, 0xb2, 0xc0, 0x03, 0xc3,
1195             0x00, 0x0f, 0x80, 0x00, 0xc0, 0x00, 0xe8, 0x13,
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             0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
1202         },
1203     };
1204
1205     bt_hci_event_complete(hci, &lc, READ_LOCAL_COMMANDS_RP_SIZE);
1206 }
1207
1208 static void bt_hci_read_local_features_rp(struct bt_hci_s *hci)
1209 {
1210     read_local_features_rp lf = {
1211         .status         = HCI_SUCCESS,
1212         .features       = {
1213             (hci->device.lmp_caps >>  0) & 0xff,
1214             (hci->device.lmp_caps >>  8) & 0xff,
1215             (hci->device.lmp_caps >> 16) & 0xff,
1216             (hci->device.lmp_caps >> 24) & 0xff,
1217             (hci->device.lmp_caps >> 32) & 0xff,
1218             (hci->device.lmp_caps >> 40) & 0xff,
1219             (hci->device.lmp_caps >> 48) & 0xff,
1220             (hci->device.lmp_caps >> 56) & 0xff,
1221         },
1222     };
1223
1224     bt_hci_event_complete(hci, &lf, READ_LOCAL_FEATURES_RP_SIZE);
1225 }
1226
1227 static void bt_hci_read_local_ext_features_rp(struct bt_hci_s *hci, int page)
1228 {
1229     read_local_ext_features_rp lef = {
1230         .status         = HCI_SUCCESS,
1231         .page_num       = page,
1232         .max_page_num   = 0x00,
1233         .features       = {
1234             /* Keep updated! */
1235             0x5f, 0x35, 0x85, 0x7e, 0x9b, 0x19, 0x00, 0x80,
1236         },
1237     };
1238     if (page)
1239         memset(lef.features, 0, sizeof(lef.features));
1240
1241     bt_hci_event_complete(hci, &lef, READ_LOCAL_EXT_FEATURES_RP_SIZE);
1242 }
1243
1244 static void bt_hci_read_buffer_size_rp(struct bt_hci_s *hci)
1245 {
1246     read_buffer_size_rp bs = {
1247         /* This can be made configurable, for one standard USB dongle HCI
1248          * the four values are cpu_to_le16(0x0180), 0x40,
1249          * cpu_to_le16(0x0008), cpu_to_le16(0x0008).  */
1250         .status         = HCI_SUCCESS,
1251         .acl_mtu        = cpu_to_le16(0x0200),
1252         .sco_mtu        = 0,
1253         .acl_max_pkt    = cpu_to_le16(0x0001),
1254         .sco_max_pkt    = cpu_to_le16(0x0000),
1255     };
1256
1257     bt_hci_event_complete(hci, &bs, READ_BUFFER_SIZE_RP_SIZE);
1258 }
1259
1260 /* Deprecated in V2.0 (page 661) */
1261 static void bt_hci_read_country_code_rp(struct bt_hci_s *hci)
1262 {
1263     read_country_code_rp cc ={
1264         .status         = HCI_SUCCESS,
1265         .country_code   = 0x00, /* North America & Europe^1 and Japan */
1266     };
1267
1268     bt_hci_event_complete(hci, &cc, READ_COUNTRY_CODE_RP_SIZE);
1269
1270     /* ^1. Except France, sorry */
1271 }
1272
1273 static void bt_hci_read_bd_addr_rp(struct bt_hci_s *hci)
1274 {
1275     read_bd_addr_rp ba = {
1276         .status = HCI_SUCCESS,
1277         .bdaddr = BAINIT(&hci->device.bd_addr),
1278     };
1279
1280     bt_hci_event_complete(hci, &ba, READ_BD_ADDR_RP_SIZE);
1281 }
1282
1283 static int bt_hci_link_quality_rp(struct bt_hci_s *hci, uint16_t handle)
1284 {
1285     read_link_quality_rp lq = {
1286         .status         = HCI_SUCCESS,
1287         .handle         = HNDL(handle),
1288         .link_quality   = 0xff,
1289     };
1290
1291     if (bt_hci_handle_bad(hci, handle))
1292         lq.status = HCI_NO_CONNECTION;
1293
1294     bt_hci_event_complete(hci, &lq, READ_LINK_QUALITY_RP_SIZE);
1295     return 0;
1296 }
1297
1298 /* Generate a Command Complete event with only the Status parameter */
1299 static inline void bt_hci_event_complete_status(struct bt_hci_s *hci,
1300                 uint8_t status)
1301 {
1302     bt_hci_event_complete(hci, &status, 1);
1303 }
1304
1305 static inline void bt_hci_event_complete_conn_cancel(struct bt_hci_s *hci,
1306                 uint8_t status, bdaddr_t *bd_addr)
1307 {
1308     create_conn_cancel_rp params = {
1309         .status = status,
1310         .bdaddr = BAINIT(bd_addr),
1311     };
1312
1313     bt_hci_event_complete(hci, &params, CREATE_CONN_CANCEL_RP_SIZE);
1314 }
1315
1316 static inline void bt_hci_event_auth_complete(struct bt_hci_s *hci,
1317                 uint16_t handle)
1318 {
1319     evt_auth_complete params = {
1320         .status = HCI_SUCCESS,
1321         .handle = HNDL(handle),
1322     };
1323
1324     bt_hci_event(hci, EVT_AUTH_COMPLETE, &params, EVT_AUTH_COMPLETE_SIZE);
1325 }
1326
1327 static inline void bt_hci_event_encrypt_change(struct bt_hci_s *hci,
1328                 uint16_t handle, uint8_t mode)
1329 {
1330     evt_encrypt_change params = {
1331         .status         = HCI_SUCCESS,
1332         .handle         = HNDL(handle),
1333         .encrypt        = mode,
1334     };
1335
1336     bt_hci_event(hci, EVT_ENCRYPT_CHANGE, &params, EVT_ENCRYPT_CHANGE_SIZE);
1337 }
1338
1339 static inline void bt_hci_event_complete_name_cancel(struct bt_hci_s *hci,
1340                 bdaddr_t *bd_addr)
1341 {
1342     remote_name_req_cancel_rp params = {
1343         .status = HCI_INVALID_PARAMETERS,
1344         .bdaddr = BAINIT(bd_addr),
1345     };
1346
1347     bt_hci_event_complete(hci, &params, REMOTE_NAME_REQ_CANCEL_RP_SIZE);
1348 }
1349
1350 static inline void bt_hci_event_read_remote_ext_features(struct bt_hci_s *hci,
1351                 uint16_t handle)
1352 {
1353     evt_read_remote_ext_features_complete params = {
1354         .status = HCI_UNSUPPORTED_FEATURE,
1355         .handle = HNDL(handle),
1356         /* Rest uninitialised */
1357     };
1358
1359     bt_hci_event(hci, EVT_READ_REMOTE_EXT_FEATURES_COMPLETE,
1360                     &params, EVT_READ_REMOTE_EXT_FEATURES_COMPLETE_SIZE);
1361 }
1362
1363 static inline void bt_hci_event_complete_lmp_handle(struct bt_hci_s *hci,
1364                 uint16_t handle)
1365 {
1366     read_lmp_handle_rp params = {
1367         .status         = HCI_NO_CONNECTION,
1368         .handle         = HNDL(handle),
1369         .reserved       = 0,
1370         /* Rest uninitialised */
1371     };
1372
1373     bt_hci_event_complete(hci, &params, READ_LMP_HANDLE_RP_SIZE);
1374 }
1375
1376 static inline void bt_hci_event_complete_role_discovery(struct bt_hci_s *hci,
1377                 int status, uint16_t handle, int master)
1378 {
1379     role_discovery_rp params = {
1380         .status         = status,
1381         .handle         = HNDL(handle),
1382         .role           = master ? 0x00 : 0x01,
1383     };
1384
1385     bt_hci_event_complete(hci, &params, ROLE_DISCOVERY_RP_SIZE);
1386 }
1387
1388 static inline void bt_hci_event_complete_flush(struct bt_hci_s *hci,
1389                 int status, uint16_t handle)
1390 {
1391     flush_rp params = {
1392         .status         = status,
1393         .handle         = HNDL(handle),
1394     };
1395
1396     bt_hci_event_complete(hci, &params, FLUSH_RP_SIZE);
1397 }
1398
1399 static inline void bt_hci_event_complete_read_local_name(struct bt_hci_s *hci)
1400 {
1401     read_local_name_rp params;
1402     params.status = HCI_SUCCESS;
1403     memset(params.name, 0, sizeof(params.name));
1404     if (hci->device.lmp_name)
1405         pstrcpy(params.name, sizeof(params.name), hci->device.lmp_name);
1406
1407     bt_hci_event_complete(hci, &params, READ_LOCAL_NAME_RP_SIZE);
1408 }
1409
1410 static inline void bt_hci_event_complete_read_conn_accept_timeout(
1411                 struct bt_hci_s *hci)
1412 {
1413     read_conn_accept_timeout_rp params = {
1414         .status         = HCI_SUCCESS,
1415         .timeout        = cpu_to_le16(hci->conn_accept_tout),
1416     };
1417
1418     bt_hci_event_complete(hci, &params, READ_CONN_ACCEPT_TIMEOUT_RP_SIZE);
1419 }
1420
1421 static inline void bt_hci_event_complete_read_scan_enable(struct bt_hci_s *hci)
1422 {
1423     read_scan_enable_rp params = {
1424         .status = HCI_SUCCESS,
1425         .enable =
1426                 (hci->device.inquiry_scan ? SCAN_INQUIRY : 0) |
1427                 (hci->device.page_scan ? SCAN_PAGE : 0),
1428     };
1429
1430     bt_hci_event_complete(hci, &params, READ_SCAN_ENABLE_RP_SIZE);
1431 }
1432
1433 static inline void bt_hci_event_complete_read_local_class(struct bt_hci_s *hci)
1434 {
1435     read_class_of_dev_rp params;
1436
1437     params.status = HCI_SUCCESS;
1438     memcpy(params.dev_class, hci->device.class, sizeof(params.dev_class));
1439
1440     bt_hci_event_complete(hci, &params, READ_CLASS_OF_DEV_RP_SIZE);
1441 }
1442
1443 static inline void bt_hci_event_complete_voice_setting(struct bt_hci_s *hci)
1444 {
1445     read_voice_setting_rp params = {
1446         .status         = HCI_SUCCESS,
1447         .voice_setting  = hci->voice_setting,   /* Note: no swapping */
1448     };
1449
1450     bt_hci_event_complete(hci, &params, READ_VOICE_SETTING_RP_SIZE);
1451 }
1452
1453 static inline void bt_hci_event_complete_read_inquiry_mode(
1454                 struct bt_hci_s *hci)
1455 {
1456     read_inquiry_mode_rp params = {
1457         .status         = HCI_SUCCESS,
1458         .mode           = hci->lm.inquiry_mode,
1459     };
1460
1461     bt_hci_event_complete(hci, &params, READ_INQUIRY_MODE_RP_SIZE);
1462 }
1463
1464 static inline void bt_hci_event_num_comp_pkts(struct bt_hci_s *hci,
1465                 uint16_t handle, int packets)
1466 {
1467     uint16_t buf[EVT_NUM_COMP_PKTS_SIZE(1) / 2 + 1];
1468     evt_num_comp_pkts *params = (void *) ((uint8_t *) buf + 1);
1469
1470     params->num_hndl                    = 1;
1471     params->connection->handle          = HNDL(handle);
1472     params->connection->num_packets     = cpu_to_le16(packets);
1473
1474     bt_hci_event(hci, EVT_NUM_COMP_PKTS, params, EVT_NUM_COMP_PKTS_SIZE(1));
1475 }
1476
1477 static void bt_submit_hci(struct HCIInfo *info,
1478                 const uint8_t *data, int length)
1479 {
1480     struct bt_hci_s *hci = hci_from_info(info);
1481     uint16_t cmd;
1482     int paramlen, i;
1483
1484     if (length < HCI_COMMAND_HDR_SIZE)
1485         goto short_hci;
1486
1487     memcpy(&hci->last_cmd, data, 2);
1488
1489     cmd = (data[1] << 8) | data[0];
1490     paramlen = data[2];
1491     if (cmd_opcode_ogf(cmd) == 0 || cmd_opcode_ocf(cmd) == 0)   /* NOP */
1492         return;
1493
1494     data += HCI_COMMAND_HDR_SIZE;
1495     length -= HCI_COMMAND_HDR_SIZE;
1496
1497     if (paramlen > length)
1498         return;
1499
1500 #define PARAM(cmd, param)       (((cmd##_cp *) data)->param)
1501 #define PARAM16(cmd, param)     le16_to_cpup(&PARAM(cmd, param))
1502 #define PARAMHANDLE(cmd)        HNDL(PARAM(cmd, handle))
1503 #define LENGTH_CHECK(cmd)       if (length < sizeof(cmd##_cp)) goto short_hci
1504     /* Note: the supported commands bitmask in bt_hci_read_local_commands_rp
1505      * needs to be updated every time a command is implemented here!  */
1506     switch (cmd) {
1507     case cmd_opcode_pack(OGF_LINK_CTL, OCF_INQUIRY):
1508         LENGTH_CHECK(inquiry);
1509
1510         if (PARAM(inquiry, length) < 1) {
1511             bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS);
1512             break;
1513         }
1514
1515         hci->lm.inquire = 1;
1516         hci->lm.periodic = 0;
1517         hci->lm.responses_left = PARAM(inquiry, num_rsp) ?: INT_MAX;
1518         hci->lm.responses = 0;
1519         bt_hci_event_status(hci, HCI_SUCCESS);
1520         bt_hci_inquiry_start(hci, PARAM(inquiry, length));
1521         break;
1522
1523     case cmd_opcode_pack(OGF_LINK_CTL, OCF_INQUIRY_CANCEL):
1524         if (!hci->lm.inquire || hci->lm.periodic) {
1525             fprintf(stderr, "%s: Inquiry Cancel should only be issued after "
1526                             "the Inquiry command has been issued, a Command "
1527                             "Status event has been received for the Inquiry "
1528                             "command, and before the Inquiry Complete event "
1529                             "occurs", __FUNCTION__);
1530             bt_hci_event_complete_status(hci, HCI_COMMAND_DISALLOWED);
1531             break;
1532         }
1533
1534         hci->lm.inquire = 0;
1535         timer_del(hci->lm.inquiry_done);
1536         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1537         break;
1538
1539     case cmd_opcode_pack(OGF_LINK_CTL, OCF_PERIODIC_INQUIRY):
1540         LENGTH_CHECK(periodic_inquiry);
1541
1542         if (!(PARAM(periodic_inquiry, length) <
1543                                 PARAM16(periodic_inquiry, min_period) &&
1544                                 PARAM16(periodic_inquiry, min_period) <
1545                                 PARAM16(periodic_inquiry, max_period)) ||
1546                         PARAM(periodic_inquiry, length) < 1 ||
1547                         PARAM16(periodic_inquiry, min_period) < 2 ||
1548                         PARAM16(periodic_inquiry, max_period) < 3) {
1549             bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS);
1550             break;
1551         }
1552
1553         hci->lm.inquire = 1;
1554         hci->lm.periodic = 1;
1555         hci->lm.responses_left = PARAM(periodic_inquiry, num_rsp);
1556         hci->lm.responses = 0;
1557         hci->lm.inquiry_period = PARAM16(periodic_inquiry, max_period);
1558         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1559         bt_hci_inquiry_start(hci, PARAM(periodic_inquiry, length));
1560         break;
1561
1562     case cmd_opcode_pack(OGF_LINK_CTL, OCF_EXIT_PERIODIC_INQUIRY):
1563         if (!hci->lm.inquire || !hci->lm.periodic) {
1564             fprintf(stderr, "%s: Inquiry Cancel should only be issued after "
1565                             "the Inquiry command has been issued, a Command "
1566                             "Status event has been received for the Inquiry "
1567                             "command, and before the Inquiry Complete event "
1568                             "occurs", __FUNCTION__);
1569             bt_hci_event_complete_status(hci, HCI_COMMAND_DISALLOWED);
1570             break;
1571         }
1572         hci->lm.inquire = 0;
1573         timer_del(hci->lm.inquiry_done);
1574         timer_del(hci->lm.inquiry_next);
1575         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1576         break;
1577
1578     case cmd_opcode_pack(OGF_LINK_CTL, OCF_CREATE_CONN):
1579         LENGTH_CHECK(create_conn);
1580
1581         if (hci->lm.connecting >= HCI_HANDLES_MAX) {
1582             bt_hci_event_status(hci, HCI_REJECTED_LIMITED_RESOURCES);
1583             break;
1584         }
1585         bt_hci_event_status(hci, HCI_SUCCESS);
1586
1587         if (bt_hci_connect(hci, &PARAM(create_conn, bdaddr)))
1588             bt_hci_connection_reject_event(hci, &PARAM(create_conn, bdaddr));
1589         break;
1590
1591     case cmd_opcode_pack(OGF_LINK_CTL, OCF_DISCONNECT):
1592         LENGTH_CHECK(disconnect);
1593
1594         if (bt_hci_handle_bad(hci, PARAMHANDLE(disconnect))) {
1595             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1596             break;
1597         }
1598
1599         bt_hci_event_status(hci, HCI_SUCCESS);
1600         bt_hci_disconnect(hci, PARAMHANDLE(disconnect),
1601                         PARAM(disconnect, reason));
1602         break;
1603
1604     case cmd_opcode_pack(OGF_LINK_CTL, OCF_CREATE_CONN_CANCEL):
1605         LENGTH_CHECK(create_conn_cancel);
1606
1607         if (bt_hci_lmp_connection_ready(hci,
1608                                 &PARAM(create_conn_cancel, bdaddr))) {
1609             for (i = 0; i < HCI_HANDLES_MAX; i ++)
1610                 if (bt_hci_role_master(hci, i) && hci->lm.handle[i].link &&
1611                                 !bacmp(&hci->lm.handle[i].link->slave->bd_addr,
1612                                         &PARAM(create_conn_cancel, bdaddr)))
1613                    break;
1614
1615             bt_hci_event_complete_conn_cancel(hci, i < HCI_HANDLES_MAX ?
1616                             HCI_ACL_CONNECTION_EXISTS : HCI_NO_CONNECTION,
1617                             &PARAM(create_conn_cancel, bdaddr));
1618         } else
1619             bt_hci_event_complete_conn_cancel(hci, HCI_SUCCESS,
1620                             &PARAM(create_conn_cancel, bdaddr));
1621         break;
1622
1623     case cmd_opcode_pack(OGF_LINK_CTL, OCF_ACCEPT_CONN_REQ):
1624         LENGTH_CHECK(accept_conn_req);
1625
1626         if (!hci->conn_req_host ||
1627                         bacmp(&PARAM(accept_conn_req, bdaddr),
1628                                 &hci->conn_req_host->bd_addr)) {
1629             bt_hci_event_status(hci, HCI_INVALID_PARAMETERS);
1630             break;
1631         }
1632
1633         bt_hci_event_status(hci, HCI_SUCCESS);
1634         bt_hci_connection_accept(hci, hci->conn_req_host);
1635         hci->conn_req_host = NULL;
1636         break;
1637
1638     case cmd_opcode_pack(OGF_LINK_CTL, OCF_REJECT_CONN_REQ):
1639         LENGTH_CHECK(reject_conn_req);
1640
1641         if (!hci->conn_req_host ||
1642                         bacmp(&PARAM(reject_conn_req, bdaddr),
1643                                 &hci->conn_req_host->bd_addr)) {
1644             bt_hci_event_status(hci, HCI_INVALID_PARAMETERS);
1645             break;
1646         }
1647
1648         bt_hci_event_status(hci, HCI_SUCCESS);
1649         bt_hci_connection_reject(hci, hci->conn_req_host,
1650                         PARAM(reject_conn_req, reason));
1651         bt_hci_connection_reject_event(hci, &hci->conn_req_host->bd_addr);
1652         hci->conn_req_host = NULL;
1653         break;
1654
1655     case cmd_opcode_pack(OGF_LINK_CTL, OCF_AUTH_REQUESTED):
1656         LENGTH_CHECK(auth_requested);
1657
1658         if (bt_hci_handle_bad(hci, PARAMHANDLE(auth_requested)))
1659             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1660         else {
1661             bt_hci_event_status(hci, HCI_SUCCESS);
1662             bt_hci_event_auth_complete(hci, PARAMHANDLE(auth_requested));
1663         }
1664         break;
1665
1666     case cmd_opcode_pack(OGF_LINK_CTL, OCF_SET_CONN_ENCRYPT):
1667         LENGTH_CHECK(set_conn_encrypt);
1668
1669         if (bt_hci_handle_bad(hci, PARAMHANDLE(set_conn_encrypt)))
1670             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1671         else {
1672             bt_hci_event_status(hci, HCI_SUCCESS);
1673             bt_hci_event_encrypt_change(hci,
1674                             PARAMHANDLE(set_conn_encrypt),
1675                             PARAM(set_conn_encrypt, encrypt));
1676         }
1677         break;
1678
1679     case cmd_opcode_pack(OGF_LINK_CTL, OCF_REMOTE_NAME_REQ):
1680         LENGTH_CHECK(remote_name_req);
1681
1682         if (bt_hci_name_req(hci, &PARAM(remote_name_req, bdaddr)))
1683             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1684         break;
1685
1686     case cmd_opcode_pack(OGF_LINK_CTL, OCF_REMOTE_NAME_REQ_CANCEL):
1687         LENGTH_CHECK(remote_name_req_cancel);
1688
1689         bt_hci_event_complete_name_cancel(hci,
1690                         &PARAM(remote_name_req_cancel, bdaddr));
1691         break;
1692
1693     case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_REMOTE_FEATURES):
1694         LENGTH_CHECK(read_remote_features);
1695
1696         if (bt_hci_features_req(hci, PARAMHANDLE(read_remote_features)))
1697             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1698         break;
1699
1700     case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_REMOTE_EXT_FEATURES):
1701         LENGTH_CHECK(read_remote_ext_features);
1702
1703         if (bt_hci_handle_bad(hci, PARAMHANDLE(read_remote_ext_features)))
1704             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1705         else {
1706             bt_hci_event_status(hci, HCI_SUCCESS);
1707             bt_hci_event_read_remote_ext_features(hci,
1708                             PARAMHANDLE(read_remote_ext_features));
1709         }
1710         break;
1711
1712     case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_REMOTE_VERSION):
1713         LENGTH_CHECK(read_remote_version);
1714
1715         if (bt_hci_version_req(hci, PARAMHANDLE(read_remote_version)))
1716             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1717         break;
1718
1719     case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_CLOCK_OFFSET):
1720         LENGTH_CHECK(read_clock_offset);
1721
1722         if (bt_hci_clkoffset_req(hci, PARAMHANDLE(read_clock_offset)))
1723             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1724         break;
1725
1726     case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_LMP_HANDLE):
1727         LENGTH_CHECK(read_lmp_handle);
1728
1729         /* TODO: */
1730         bt_hci_event_complete_lmp_handle(hci, PARAMHANDLE(read_lmp_handle));
1731         break;
1732
1733     case cmd_opcode_pack(OGF_LINK_POLICY, OCF_HOLD_MODE):
1734         LENGTH_CHECK(hold_mode);
1735
1736         if (PARAM16(hold_mode, min_interval) >
1737                         PARAM16(hold_mode, max_interval) ||
1738                         PARAM16(hold_mode, min_interval) < 0x0002 ||
1739                         PARAM16(hold_mode, max_interval) > 0xff00 ||
1740                         (PARAM16(hold_mode, min_interval) & 1) ||
1741                         (PARAM16(hold_mode, max_interval) & 1)) {
1742             bt_hci_event_status(hci, HCI_INVALID_PARAMETERS);
1743             break;
1744         }
1745
1746         if (bt_hci_mode_change(hci, PARAMHANDLE(hold_mode),
1747                                 PARAM16(hold_mode, max_interval),
1748                                 acl_hold))
1749             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1750         break;
1751
1752     case cmd_opcode_pack(OGF_LINK_POLICY, OCF_PARK_MODE):
1753         LENGTH_CHECK(park_mode);
1754
1755         if (PARAM16(park_mode, min_interval) >
1756                         PARAM16(park_mode, max_interval) ||
1757                         PARAM16(park_mode, min_interval) < 0x000e ||
1758                         (PARAM16(park_mode, min_interval) & 1) ||
1759                         (PARAM16(park_mode, max_interval) & 1)) {
1760             bt_hci_event_status(hci, HCI_INVALID_PARAMETERS);
1761             break;
1762         }
1763
1764         if (bt_hci_mode_change(hci, PARAMHANDLE(park_mode),
1765                                 PARAM16(park_mode, max_interval),
1766                                 acl_parked))
1767             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1768         break;
1769
1770     case cmd_opcode_pack(OGF_LINK_POLICY, OCF_EXIT_PARK_MODE):
1771         LENGTH_CHECK(exit_park_mode);
1772
1773         if (bt_hci_mode_cancel(hci, PARAMHANDLE(exit_park_mode),
1774                                 acl_parked))
1775             bt_hci_event_status(hci, HCI_NO_CONNECTION);
1776         break;
1777
1778     case cmd_opcode_pack(OGF_LINK_POLICY, OCF_ROLE_DISCOVERY):
1779         LENGTH_CHECK(role_discovery);
1780
1781         if (bt_hci_handle_bad(hci, PARAMHANDLE(role_discovery)))
1782             bt_hci_event_complete_role_discovery(hci,
1783                             HCI_NO_CONNECTION, PARAMHANDLE(role_discovery), 0);
1784         else
1785             bt_hci_event_complete_role_discovery(hci,
1786                             HCI_SUCCESS, PARAMHANDLE(role_discovery),
1787                             bt_hci_role_master(hci,
1788                                     PARAMHANDLE(role_discovery)));
1789         break;
1790
1791     case cmd_opcode_pack(OGF_HOST_CTL, OCF_SET_EVENT_MASK):
1792         LENGTH_CHECK(set_event_mask);
1793
1794         memcpy(hci->event_mask, PARAM(set_event_mask, mask), 8);
1795         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1796         break;
1797
1798     case cmd_opcode_pack(OGF_HOST_CTL, OCF_RESET):
1799         bt_hci_reset(hci);
1800         bt_hci_event_status(hci, HCI_SUCCESS);
1801         break;
1802
1803     case cmd_opcode_pack(OGF_HOST_CTL, OCF_SET_EVENT_FLT):
1804         if (length >= 1 && PARAM(set_event_flt, flt_type) == FLT_CLEAR_ALL)
1805             /* No length check */;
1806         else
1807             LENGTH_CHECK(set_event_flt);
1808
1809         /* Filters are not implemented */
1810         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1811         break;
1812
1813     case cmd_opcode_pack(OGF_HOST_CTL, OCF_FLUSH):
1814         LENGTH_CHECK(flush);
1815
1816         if (bt_hci_handle_bad(hci, PARAMHANDLE(flush)))
1817             bt_hci_event_complete_flush(hci,
1818                             HCI_NO_CONNECTION, PARAMHANDLE(flush));
1819         else {
1820             /* TODO: ordering? */
1821             bt_hci_event(hci, EVT_FLUSH_OCCURRED,
1822                             &PARAM(flush, handle),
1823                             EVT_FLUSH_OCCURRED_SIZE);
1824             bt_hci_event_complete_flush(hci,
1825                             HCI_SUCCESS, PARAMHANDLE(flush));
1826         }
1827         break;
1828
1829     case cmd_opcode_pack(OGF_HOST_CTL, OCF_CHANGE_LOCAL_NAME):
1830         LENGTH_CHECK(change_local_name);
1831
1832         if (hci->device.lmp_name)
1833             g_free((void *) hci->device.lmp_name);
1834         hci->device.lmp_name = g_strndup(PARAM(change_local_name, name),
1835                         sizeof(PARAM(change_local_name, name)));
1836         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1837         break;
1838
1839     case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_LOCAL_NAME):
1840         bt_hci_event_complete_read_local_name(hci);
1841         break;
1842
1843     case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_CONN_ACCEPT_TIMEOUT):
1844         bt_hci_event_complete_read_conn_accept_timeout(hci);
1845         break;
1846
1847     case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_CONN_ACCEPT_TIMEOUT):
1848         /* TODO */
1849         LENGTH_CHECK(write_conn_accept_timeout);
1850
1851         if (PARAM16(write_conn_accept_timeout, timeout) < 0x0001 ||
1852                         PARAM16(write_conn_accept_timeout, timeout) > 0xb540) {
1853             bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS);
1854             break;
1855         }
1856
1857         hci->conn_accept_tout = PARAM16(write_conn_accept_timeout, timeout);
1858         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1859         break;
1860
1861     case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_SCAN_ENABLE):
1862         bt_hci_event_complete_read_scan_enable(hci);
1863         break;
1864
1865     case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_SCAN_ENABLE):
1866         LENGTH_CHECK(write_scan_enable);
1867
1868         /* TODO: check that the remaining bits are all 0 */
1869         hci->device.inquiry_scan =
1870                 !!(PARAM(write_scan_enable, scan_enable) & SCAN_INQUIRY);
1871         hci->device.page_scan =
1872                 !!(PARAM(write_scan_enable, scan_enable) & SCAN_PAGE);
1873         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1874         break;
1875
1876     case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_CLASS_OF_DEV):
1877         bt_hci_event_complete_read_local_class(hci);
1878         break;
1879
1880     case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_CLASS_OF_DEV):
1881         LENGTH_CHECK(write_class_of_dev);
1882
1883         memcpy(hci->device.class, PARAM(write_class_of_dev, dev_class),
1884                         sizeof(PARAM(write_class_of_dev, dev_class)));
1885         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1886         break;
1887
1888     case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_VOICE_SETTING):
1889         bt_hci_event_complete_voice_setting(hci);
1890         break;
1891
1892     case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_VOICE_SETTING):
1893         LENGTH_CHECK(write_voice_setting);
1894
1895         hci->voice_setting = PARAM(write_voice_setting, voice_setting);
1896         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1897         break;
1898
1899     case cmd_opcode_pack(OGF_HOST_CTL, OCF_HOST_NUMBER_OF_COMPLETED_PACKETS):
1900         if (length < data[0] * 2 + 1)
1901             goto short_hci;
1902
1903         for (i = 0; i < data[0]; i ++)
1904             if (bt_hci_handle_bad(hci,
1905                                     data[i * 2 + 1] | (data[i * 2 + 2] << 8)))
1906                 bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS);
1907         break;
1908
1909     case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_INQUIRY_MODE):
1910         /* Only if (local_features[3] & 0x40) && (local_commands[12] & 0x40)
1911          * else
1912          *     goto unknown_command */
1913         bt_hci_event_complete_read_inquiry_mode(hci);
1914         break;
1915
1916     case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_INQUIRY_MODE):
1917         /* Only if (local_features[3] & 0x40) && (local_commands[12] & 0x80)
1918          * else
1919          *     goto unknown_command */
1920         LENGTH_CHECK(write_inquiry_mode);
1921
1922         if (PARAM(write_inquiry_mode, mode) > 0x01) {
1923             bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS);
1924             break;
1925         }
1926
1927         hci->lm.inquiry_mode = PARAM(write_inquiry_mode, mode);
1928         bt_hci_event_complete_status(hci, HCI_SUCCESS);
1929         break;
1930
1931     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_VERSION):
1932         bt_hci_read_local_version_rp(hci);
1933         break;
1934
1935     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_COMMANDS):
1936         bt_hci_read_local_commands_rp(hci);
1937         break;
1938
1939     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_FEATURES):
1940         bt_hci_read_local_features_rp(hci);
1941         break;
1942
1943     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_EXT_FEATURES):
1944         LENGTH_CHECK(read_local_ext_features);
1945
1946         bt_hci_read_local_ext_features_rp(hci,
1947                         PARAM(read_local_ext_features, page_num));
1948         break;
1949
1950     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_BUFFER_SIZE):
1951         bt_hci_read_buffer_size_rp(hci);
1952         break;
1953
1954     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_COUNTRY_CODE):
1955         bt_hci_read_country_code_rp(hci);
1956         break;
1957
1958     case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_BD_ADDR):
1959         bt_hci_read_bd_addr_rp(hci);
1960         break;
1961
1962     case cmd_opcode_pack(OGF_STATUS_PARAM, OCF_READ_LINK_QUALITY):
1963         LENGTH_CHECK(read_link_quality);
1964
1965         bt_hci_link_quality_rp(hci, PARAMHANDLE(read_link_quality));
1966         break;
1967
1968     default:
1969         bt_hci_event_status(hci, HCI_UNKNOWN_COMMAND);
1970         break;
1971
1972     short_hci:
1973         fprintf(stderr, "%s: HCI packet too short (%iB)\n",
1974                         __FUNCTION__, length);
1975         bt_hci_event_status(hci, HCI_INVALID_PARAMETERS);
1976         break;
1977     }
1978 }
1979
1980 /* We could perform fragmentation here, we can't do "recombination" because
1981  * at this layer the length of the payload is not know ahead, so we only
1982  * know that a packet contained the last fragment of the SDU when the next
1983  * SDU starts.  */
1984 static inline void bt_hci_lmp_acl_data(struct bt_hci_s *hci, uint16_t handle,
1985                 const uint8_t *data, int start, int len)
1986 {
1987     struct hci_acl_hdr *pkt = (void *) hci->acl_buf;
1988
1989     /* TODO: packet flags */
1990     /* TODO: avoid memcpy'ing */
1991
1992     if (len + HCI_ACL_HDR_SIZE > sizeof(hci->acl_buf)) {
1993         fprintf(stderr, "%s: can't take ACL packets %i bytes long\n",
1994                         __FUNCTION__, len);
1995         return;
1996     }
1997     memcpy(hci->acl_buf + HCI_ACL_HDR_SIZE, data, len);
1998
1999     pkt->handle = cpu_to_le16(
2000                     acl_handle_pack(handle, start ? ACL_START : ACL_CONT));
2001     pkt->dlen = cpu_to_le16(len);
2002     hci->info.acl_recv(hci->info.opaque,
2003                     hci->acl_buf, len + HCI_ACL_HDR_SIZE);
2004 }
2005
2006 static void bt_hci_lmp_acl_data_slave(struct bt_link_s *btlink,
2007                 const uint8_t *data, int start, int len)
2008 {
2009     struct bt_hci_link_s *link = (struct bt_hci_link_s *) btlink;
2010
2011     bt_hci_lmp_acl_data(hci_from_device(btlink->slave),
2012                     link->handle, data, start, len);
2013 }
2014
2015 static void bt_hci_lmp_acl_data_host(struct bt_link_s *link,
2016                 const uint8_t *data, int start, int len)
2017 {
2018     bt_hci_lmp_acl_data(hci_from_device(link->host),
2019                     link->handle, data, start, len);
2020 }
2021
2022 static void bt_submit_acl(struct HCIInfo *info,
2023                 const uint8_t *data, int length)
2024 {
2025     struct bt_hci_s *hci = hci_from_info(info);
2026     uint16_t handle;
2027     int datalen, flags;
2028     struct bt_link_s *link;
2029
2030     if (length < HCI_ACL_HDR_SIZE) {
2031         fprintf(stderr, "%s: ACL packet too short (%iB)\n",
2032                         __FUNCTION__, length);
2033         return;
2034     }
2035
2036     handle = acl_handle((data[1] << 8) | data[0]);
2037     flags = acl_flags((data[1] << 8) | data[0]);
2038     datalen = (data[3] << 8) | data[2];
2039     data += HCI_ACL_HDR_SIZE;
2040     length -= HCI_ACL_HDR_SIZE;
2041
2042     if (bt_hci_handle_bad(hci, handle)) {
2043         fprintf(stderr, "%s: invalid ACL handle %03x\n",
2044                         __FUNCTION__, handle);
2045         /* TODO: signal an error */
2046         return;
2047     }
2048     handle &= ~HCI_HANDLE_OFFSET;
2049
2050     if (datalen > length) {
2051         fprintf(stderr, "%s: ACL packet too short (%iB < %iB)\n",
2052                         __FUNCTION__, length, datalen);
2053         return;
2054     }
2055
2056     link = hci->lm.handle[handle].link;
2057
2058     if ((flags & ~3) == ACL_ACTIVE_BCAST) {
2059         if (!hci->asb_handle)
2060             hci->asb_handle = handle;
2061         else if (handle != hci->asb_handle) {
2062             fprintf(stderr, "%s: Bad handle %03x in Active Slave Broadcast\n",
2063                             __FUNCTION__, handle);
2064             /* TODO: signal an error */
2065             return;
2066         }
2067
2068         /* TODO */
2069     }
2070
2071     if ((flags & ~3) == ACL_PICO_BCAST) {
2072         if (!hci->psb_handle)
2073             hci->psb_handle = handle;
2074         else if (handle != hci->psb_handle) {
2075             fprintf(stderr, "%s: Bad handle %03x in Parked Slave Broadcast\n",
2076                             __FUNCTION__, handle);
2077             /* TODO: signal an error */
2078             return;
2079         }
2080
2081         /* TODO */
2082     }
2083
2084     /* TODO: increase counter and send EVT_NUM_COMP_PKTS */
2085     bt_hci_event_num_comp_pkts(hci, handle | HCI_HANDLE_OFFSET, 1);
2086
2087     /* Do this last as it can trigger further events even in this HCI */
2088     hci->lm.handle[handle].lmp_acl_data(link, data,
2089                     (flags & 3) == ACL_START, length);
2090 }
2091
2092 static void bt_submit_sco(struct HCIInfo *info,
2093                 const uint8_t *data, int length)
2094 {
2095     struct bt_hci_s *hci = hci_from_info(info);
2096     uint16_t handle;
2097     int datalen;
2098
2099     if (length < 3)
2100         return;
2101
2102     handle = acl_handle((data[1] << 8) | data[0]);
2103     datalen = data[2];
2104     length -= 3;
2105
2106     if (bt_hci_handle_bad(hci, handle)) {
2107         fprintf(stderr, "%s: invalid SCO handle %03x\n",
2108                         __FUNCTION__, handle);
2109         return;
2110     }
2111
2112     if (datalen > length) {
2113         fprintf(stderr, "%s: SCO packet too short (%iB < %iB)\n",
2114                         __FUNCTION__, length, datalen);
2115         return;
2116     }
2117
2118     /* TODO */
2119
2120     /* TODO: increase counter and send EVT_NUM_COMP_PKTS if synchronous
2121      * Flow Control is enabled.
2122      * (See Read/Write_Synchronous_Flow_Control_Enable on page 513 and
2123      * page 514.)  */
2124 }
2125
2126 static uint8_t *bt_hci_evt_packet(void *opaque)
2127 {
2128     /* TODO: allocate a packet from upper layer */
2129     struct bt_hci_s *s = opaque;
2130
2131     return s->evt_buf;
2132 }
2133
2134 static void bt_hci_evt_submit(void *opaque, int len)
2135 {
2136     /* TODO: notify upper layer */
2137     struct bt_hci_s *s = opaque;
2138
2139     s->info.evt_recv(s->info.opaque, s->evt_buf, len);
2140 }
2141
2142 static int bt_hci_bdaddr_set(struct HCIInfo *info, const uint8_t *bd_addr)
2143 {
2144     struct bt_hci_s *hci = hci_from_info(info);
2145
2146     bacpy(&hci->device.bd_addr, (const bdaddr_t *) bd_addr);
2147     return 0;
2148 }
2149
2150 static void bt_hci_done(struct HCIInfo *info);
2151 static void bt_hci_destroy(struct bt_device_s *dev)
2152 {
2153     struct bt_hci_s *hci = hci_from_device(dev);
2154
2155     bt_hci_done(&hci->info);
2156 }
2157
2158 struct HCIInfo *bt_new_hci(struct bt_scatternet_s *net)
2159 {
2160     struct bt_hci_s *s = g_malloc0(sizeof(struct bt_hci_s));
2161
2162     s->lm.inquiry_done = timer_new_ns(QEMU_CLOCK_VIRTUAL, bt_hci_inquiry_done, s);
2163     s->lm.inquiry_next = timer_new_ns(QEMU_CLOCK_VIRTUAL, bt_hci_inquiry_next, s);
2164     s->conn_accept_timer =
2165             timer_new_ns(QEMU_CLOCK_VIRTUAL, bt_hci_conn_accept_timeout, s);
2166
2167     s->evt_packet = bt_hci_evt_packet;
2168     s->evt_submit = bt_hci_evt_submit;
2169     s->opaque = s;
2170
2171     bt_device_init(&s->device, net);
2172     s->device.lmp_connection_request = bt_hci_lmp_connection_request;
2173     s->device.lmp_connection_complete = bt_hci_lmp_connection_complete;
2174     s->device.lmp_disconnect_master = bt_hci_lmp_disconnect_host;
2175     s->device.lmp_disconnect_slave = bt_hci_lmp_disconnect_slave;
2176     s->device.lmp_acl_data = bt_hci_lmp_acl_data_slave;
2177     s->device.lmp_acl_resp = bt_hci_lmp_acl_data_host;
2178     s->device.lmp_mode_change = bt_hci_lmp_mode_change_slave;
2179
2180     /* Keep updated! */
2181     /* Also keep in sync with supported commands bitmask in
2182      * bt_hci_read_local_commands_rp */
2183     s->device.lmp_caps = 0x8000199b7e85355fll;
2184
2185     bt_hci_reset(s);
2186
2187     s->info.cmd_send = bt_submit_hci;
2188     s->info.sco_send = bt_submit_sco;
2189     s->info.acl_send = bt_submit_acl;
2190     s->info.bdaddr_set = bt_hci_bdaddr_set;
2191
2192     s->device.handle_destroy = bt_hci_destroy;
2193
2194     return &s->info;
2195 }
2196
2197 struct HCIInfo *hci_init(const char *str)
2198 {
2199     char *endp;
2200     struct bt_scatternet_s *vlan = 0;
2201
2202     if (!strcmp(str, "null"))
2203         /* null */
2204         return &null_hci;
2205     else if (!strncmp(str, "host", 4) && (str[4] == '\0' || str[4] == ':'))
2206         /* host[:hciN] */
2207         return bt_host_hci(str[4] ? str + 5 : "hci0");
2208     else if (!strncmp(str, "hci", 3)) {
2209         /* hci[,vlan=n] */
2210         if (str[3]) {
2211             if (!strncmp(str + 3, ",vlan=", 6)) {
2212                 vlan = qemu_find_bt_vlan(strtol(str + 9, &endp, 0));
2213                 if (*endp)
2214                     vlan = 0;
2215             }
2216         } else
2217             vlan = qemu_find_bt_vlan(0);
2218         if (vlan)
2219            return bt_new_hci(vlan);
2220     }
2221
2222     fprintf(stderr, "qemu: Unknown bluetooth HCI `%s'.\n", str);
2223
2224     return 0;
2225 }
2226
2227 static void bt_hci_done(struct HCIInfo *info)
2228 {
2229     struct bt_hci_s *hci = hci_from_info(info);
2230     int handle;
2231
2232     bt_device_done(&hci->device);
2233
2234     if (hci->device.lmp_name)
2235         g_free((void *) hci->device.lmp_name);
2236
2237     /* Be gentle and send DISCONNECT to all connected peers and those
2238      * currently waiting for us to accept or reject a connection request.
2239      * This frees the links.  */
2240     if (hci->conn_req_host) {
2241         bt_hci_connection_reject(hci,
2242                                  hci->conn_req_host, HCI_OE_POWER_OFF);
2243         return;
2244     }
2245
2246     for (handle = HCI_HANDLE_OFFSET;
2247                     handle < (HCI_HANDLE_OFFSET | HCI_HANDLES_MAX); handle ++)
2248         if (!bt_hci_handle_bad(hci, handle))
2249             bt_hci_disconnect(hci, handle, HCI_OE_POWER_OFF);
2250
2251     /* TODO: this is not enough actually, there may be slaves from whom
2252      * we have requested a connection who will soon (or not) respond with
2253      * an accept or a reject, so we should also check if hci->lm.connecting
2254      * is non-zero and if so, avoid freeing the hci but otherwise disappear
2255      * from all qemu social life (e.g. stop scanning and request to be
2256      * removed from s->device.net) and arrange for
2257      * s->device.lmp_connection_complete to free the remaining bits once
2258      * hci->lm.awaiting_bdaddr[] is empty.  */
2259
2260     timer_free(hci->lm.inquiry_done);
2261     timer_free(hci->lm.inquiry_next);
2262     timer_free(hci->conn_accept_timer);
2263
2264     g_free(hci);
2265 }