hw/bt-hci.c

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