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