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