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