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 "qemu/error-report.h"
  23 #include "qapi/error.h"
  24 #include "qemu-common.h"
  25 #include "qemu/timer.h"
  26 #include "hw/usb.h"
  27 #include "sysemu/bt.h"
  28 #include "hw/bt.h"
  29 #include "qapi/qmp/qerror.h"
  30 #include "sysemu/replay.h"
  31 #include "qemu/cutils.h"
  32
  33 struct bt_hci_s {
  34     uint8_t *(*evt_packet)(void *opaque);
  35     void (*evt_submit)(void *opaque, int len);
  36     void *opaque;
  37     uint8_t evt_buf[256];
  38
  39     uint8_t acl_buf[4096];
  40     int acl_len;
  41
  42     uint16_t asb_handle;
  43     uint16_t psb_handle;
  44
  45     int last_cmd;       /* Note: Always little-endian */
  46
  47     struct bt_device_s *conn_req_host;
  48
  49     struct {
  50         int inquire;
  51         int periodic;
  52         int responses_left;
  53         int responses;
  54         QEMUTimer *inquiry_done;
  55         QEMUTimer *inquiry_next;
  56         int inquiry_length;
  57         int inquiry_period;
  58         int inquiry_mode;
  59
  60 #define HCI_HANDLE_OFFSET       0x20
  61 #define HCI_HANDLES_MAX         0x10
  62         struct bt_hci_master_link_s {
  63             struct bt_link_s *link;
  64             void (*lmp_acl_data)(struct bt_link_s *link,
  65                             const uint8_t *data, int start, int len);
  66             QEMUTimer *acl_mode_timer;
  67         } handle[HCI_HANDLES_MAX];
  68         uint32_t role_bmp;
  69         int last_handle;
  70         int connecting;
  71         bdaddr_t awaiting_bdaddr[HCI_HANDLES_MAX];
  72     } lm;
  73
  74     uint8_t event_mask[8];
  75     uint16_t voice_setting;     /* Notw: Always little-endian */
  76     uint16_t conn_accept_tout;
  77     QEMUTimer *conn_accept_timer;
  78
  79     struct HCIInfo info;
  80     struct bt_device_s device;
  81
  82     Error *replay_blocker;
  83 };
  84
  85 #define DEFAULT_RSSI_DBM        20
  86
  87 #define hci_from_info(ptr)      container_of((ptr), struct bt_hci_s, info)
  88 #define hci_from_device(ptr)    container_of((ptr), struct bt_hci_s, device)
  89
  90 struct bt_hci_link_s {
  91     struct bt_link_s btlink;
  92     uint16_t handle;    /* Local */
  93 };
  94
  95 /* LMP layer emulation */
  96 #if 0
  97 static void bt_submit_lmp(struct bt_device_s *bt, int length, uint8_t *data)
  98 {
  99     int resp, resplen, error, op, tr;
 100     uint8_t respdata[17];
 101
 102     if (length < 1)
 103         return;
 104
 105     tr = *data & 1;
 106     op = *(data ++) >> 1;
 107     resp = LMP_ACCEPTED;
 108     resplen = 2;
 109     respdata[1] = op;
 110     error = 0;
 111     length --;
 112
 113     if (op >= 0x7c) {   /* Extended opcode */
 114         op |= *(data ++) << 8;
 115         resp = LMP_ACCEPTED_EXT;
 116         resplen = 4;
 117         respdata[0] = op >> 8;
 118         respdata[1] = op & 0xff;
 119         length --;
 120     }
 121
 122     switch (op) {
 123     case LMP_ACCEPTED:
 124         /* data[0]      Op code
 125          */
 126         if (length < 1) {
 127             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 128             goto not_accepted;
 129         }
 130         resp = 0;
 131         break;
 132
 133     case LMP_ACCEPTED_EXT:
 134         /* data[0]      Escape op code
 135          * data[1]      Extended op code
 136          */
 137         if (length < 2) {
 138             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 139             goto not_accepted;
 140         }
 141         resp = 0;
 142         break;
 143
 144     case LMP_NOT_ACCEPTED:
 145         /* data[0]      Op code
 146          * data[1]      Error code
 147          */
 148         if (length < 2) {
 149             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 150             goto not_accepted;
 151         }
 152         resp = 0;
 153         break;
 154
 155     case LMP_NOT_ACCEPTED_EXT:
 156         /* data[0]      Op code
 157          * data[1]      Extended op code
 158          * data[2]      Error code
 159          */
 160         if (length < 3) {
 161             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 162             goto not_accepted;
 163         }
 164         resp = 0;
 165         break;
 166
 167     case LMP_HOST_CONNECTION_REQ:
 168         break;
 169
 170     case LMP_SETUP_COMPLETE:
 171         resp = LMP_SETUP_COMPLETE;
 172         resplen = 1;
 173         bt->setup = 1;
 174         break;
 175
 176     case LMP_DETACH:
 177         /* data[0]      Error code
 178          */
 179         if (length < 1) {
 180             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 181             goto not_accepted;
 182         }
 183         bt->setup = 0;
 184         resp = 0;
 185         break;
 186
 187     case LMP_SUPERVISION_TIMEOUT:
 188         /* data[0,1]    Supervision timeout
 189          */
 190         if (length < 2) {
 191             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 192             goto not_accepted;
 193         }
 194         resp = 0;
 195         break;
 196
 197     case LMP_QUALITY_OF_SERVICE:
 198         resp = 0;
 199         /* Fall through */
 200     case LMP_QOS_REQ:
 201         /* data[0,1]    Poll interval
 202          * data[2]      N(BC)
 203          */
 204         if (length < 3) {
 205             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 206             goto not_accepted;
 207         }
 208         break;
 209
 210     case LMP_MAX_SLOT:
 211         resp = 0;
 212         /* Fall through */
 213     case LMP_MAX_SLOT_REQ:
 214         /* data[0]      Max slots
 215          */
 216         if (length < 1) {
 217             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 218             goto not_accepted;
 219         }
 220         break;
 221
 222     case LMP_AU_RAND:
 223     case LMP_IN_RAND:
 224     case LMP_COMB_KEY:
 225         /* data[0-15]   Random number
 226          */
 227         if (length < 16) {
 228             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 229             goto not_accepted;
 230         }
 231         if (op == LMP_AU_RAND) {
 232             if (bt->key_present) {
 233                 resp = LMP_SRES;
 234                 resplen = 5;
 235                 /* XXX: [Part H] Section 6.1 on page 801 */
 236             } else {
 237                 error = HCI_PIN_OR_KEY_MISSING;
 238                 goto not_accepted;
 239             }
 240         } else if (op == LMP_IN_RAND) {
 241             error = HCI_PAIRING_NOT_ALLOWED;
 242             goto not_accepted;
 243         } else {
 244             /* XXX: [Part H] Section 3.2 on page 779 */
 245             resp = LMP_UNIT_KEY;
 246             resplen = 17;
 247             memcpy(respdata + 1, bt->key, 16);
 248
 249             error = HCI_UNIT_LINK_KEY_USED;
 250             goto not_accepted;
 251         }
 252         break;
 253
 254     case LMP_UNIT_KEY:
 255         /* data[0-15]   Key
 256          */
 257         if (length < 16) {
 258             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 259             goto not_accepted;
 260         }
 261         memcpy(bt->key, data, 16);
 262         bt->key_present = 1;
 263         break;
 264
 265     case LMP_SRES:
 266         /* data[0-3]    Authentication response
 267          */
 268         if (length < 4) {
 269             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 270             goto not_accepted;
 271         }
 272         break;
 273
 274     case LMP_CLKOFFSET_REQ:
 275         resp = LMP_CLKOFFSET_RES;
 276         resplen = 3;
 277         respdata[1] = 0x33;
 278         respdata[2] = 0x33;
 279         break;
 280
 281     case LMP_CLKOFFSET_RES:
 282         /* data[0,1]    Clock offset
 283          * (Slave to master only)
 284          */
 285         if (length < 2) {
 286             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 287             goto not_accepted;
 288         }
 289         break;
 290
 291     case LMP_VERSION_REQ:
 292     case LMP_VERSION_RES:
 293         /* data[0]      VersNr
 294          * data[1,2]    CompId
 295          * data[3,4]    SubVersNr
 296          */
 297         if (length < 5) {
 298             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 299             goto not_accepted;
 300         }
 301         if (op == LMP_VERSION_REQ) {
 302             resp = LMP_VERSION_RES;
 303             resplen = 6;
 304             respdata[1] = 0x20;
 305             respdata[2] = 0xff;
 306             respdata[3] = 0xff;
 307             respdata[4] = 0xff;
 308             respdata[5] = 0xff;
 309         } else
 310             resp = 0;
 311         break;
 312
 313     case LMP_FEATURES_REQ:
 314     case LMP_FEATURES_RES:
 315         /* data[0-7]    Features
 316          */
 317         if (length < 8) {
 318             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 319             goto not_accepted;
 320         }
 321         if (op == LMP_FEATURES_REQ) {
 322             resp = LMP_FEATURES_RES;
 323             resplen = 9;
 324             respdata[1] = (bt->lmp_caps >> 0) & 0xff;
 325             respdata[2] = (bt->lmp_caps >> 8) & 0xff;
 326             respdata[3] = (bt->lmp_caps >> 16) & 0xff;
 327             respdata[4] = (bt->lmp_caps >> 24) & 0xff;
 328             respdata[5] = (bt->lmp_caps >> 32) & 0xff;
 329             respdata[6] = (bt->lmp_caps >> 40) & 0xff;
 330             respdata[7] = (bt->lmp_caps >> 48) & 0xff;
 331             respdata[8] = (bt->lmp_caps >> 56) & 0xff;
 332         } else
 333             resp = 0;
 334         break;
 335
 336     case LMP_NAME_REQ:
 337         /* data[0]      Name offset
 338          */
 339         if (length < 1) {
 340             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 341             goto not_accepted;
 342         }
 343         resp = LMP_NAME_RES;
 344         resplen = 17;
 345         respdata[1] = data[0];
 346         respdata[2] = strlen(bt->lmp_name);
 347         memset(respdata + 3, 0x00, 14);
 348         if (respdata[2] > respdata[1])
 349             memcpy(respdata + 3, bt->lmp_name + respdata[1],
 350                             respdata[2] - respdata[1]);
 351         break;
 352
 353     case LMP_NAME_RES:
 354         /* data[0]      Name offset
 355          * data[1]      Name length
 356          * data[2-15]   Name fragment
 357          */
 358         if (length < 16) {
 359             error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
 360             goto not_accepted;
 361         }
 362         resp = 0;
 363         break;
 364
 365     default:
 366         error = HCI_UNKNOWN_LMP_PDU;
 367         /* Fall through */
 368     not_accepted:
 369         if (op >> 8) {
 370             resp = LMP_NOT_ACCEPTED_EXT;
 371             resplen = 5;
 372             respdata[0] = op >> 8;
 373             respdata[1] = op & 0xff;
 374             respdata[2] = error;
 375         } else {
 376             resp = LMP_NOT_ACCEPTED;
 377             resplen = 3;
 378             respdata[0] = op & 0xff;
 379             respdata[1] = error;
 380         }
 381     }
 382
 383     if (resp == 0)
 384         return;
 385
 386     if (resp >> 8) {
 387         respdata[0] = resp >> 8;
 388         respdata[1] = resp & 0xff;
 389     } else
 390         respdata[0] = resp & 0xff;
 391
 392     respdata[0] <<= 1;
 393     respdata[0] |= tr;
 394 }
 395
 396 static void bt_submit_raw_acl(struct bt_piconet_s *net, int length, uint8_t *data)
 397 {
 398     struct bt_device_s *slave;
 399     if (length < 1)
 400         return;
 401
 402     slave = 0;
 403 #if 0
 404     slave = net->slave;
 405 #endif
 406
 407     switch (data[0] & 3) {
 408     case LLID_ACLC:
 409         bt_submit_lmp(slave, length - 1, data + 1);
 410         break;
 411     case LLID_ACLU_START:
 412 #if 0
 413         bt_sumbit_l2cap(slave, length - 1, data + 1, (data[0] >> 2) & 1);
 414         breka;
 415 #endif
 416     default:
 417     case LLID_ACLU_CONT:
 418         break;
 419     }
 420 }
 421 #endif
 422
 423 /* HCI layer emulation */
 424
 425 /* Note: we could ignore endianness because unswapped handles will still
 426  * be valid as connection identifiers for the guest - they don't have to
 427  * be continuously allocated.  We do it though, to preserve similar
 428  * behaviour between hosts.  Some things, like the BD_ADDR cannot be
 429  * preserved though (for example if a real hci is used).  */
 430 #define HNDL(raw) cpu_to_le16(raw)
 431
 432 static const uint8_t bt_event_reserved_mask[8] = {
 433     0xff, 0x9f, 0xfb, 0xff, 0x07, 0x18, 0x00, 0x00,
 434 };
 435
 436
 437 static void null_hci_send(struct HCIInfo *hci, const uint8_t *data, int len)
 438 {
 439 }
 440
 441 static int null_hci_addr_set(struct HCIInfo *hci, const uint8_t *bd_addr)
 442 {
 443     return -ENOTSUP;
 444 }
 445
 446 struct HCIInfo null_hci = {
 447     .cmd_send = null_hci_send,
 448     .sco_send = null_hci_send,
 449     .acl_send = null_hci_send,
 450     .bdaddr_set = null_hci_addr_set,
 451 };
 452
 453
 454 static inline uint8_t *bt_hci_event_start(struct bt_hci_s *hci,
 455                 int evt, int len)
 456 {
 457     uint8_t *packet, mask;
 458     int mask_byte;
 459
 460     if (len > 255) {
 461         error_report("%s: HCI event params too long (%ib)", __func__, 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         error_report("%s: bad inquiry mode %02x", __func__,
 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", __func__);
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", __func__);
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         error_report("%s: HCI packet too short (%iB)", __func__, length);
1975         bt_hci_event_status(hci, HCI_INVALID_PARAMETERS);
1976         break;
1977     }
1978 }
1979
1980 /* We could perform fragmentation here, we can't do "recombination" because
1981  * at this layer the length of the payload is not know ahead, so we only
1982  * know that a packet contained the last fragment of the SDU when the next
1983  * SDU starts.  */
1984 static inline void bt_hci_lmp_acl_data(struct bt_hci_s *hci, uint16_t handle,
1985                 const uint8_t *data, int start, int len)
1986 {
1987     struct hci_acl_hdr *pkt = (void *) hci->acl_buf;
1988
1989     /* TODO: packet flags */
1990     /* TODO: avoid memcpy'ing */
1991
1992     if (len + HCI_ACL_HDR_SIZE > sizeof(hci->acl_buf)) {
1993         error_report("%s: can't take ACL packets %i bytes long",
1994                      __func__, len);
1995         return;
1996     }
1997     memcpy(hci->acl_buf + HCI_ACL_HDR_SIZE, data, len);
1998
1999     pkt->handle = cpu_to_le16(
2000                     acl_handle_pack(handle, start ? ACL_START : ACL_CONT));
2001     pkt->dlen = cpu_to_le16(len);
2002     hci->info.acl_recv(hci->info.opaque,
2003                     hci->acl_buf, len + HCI_ACL_HDR_SIZE);
2004 }
2005
2006 static void bt_hci_lmp_acl_data_slave(struct bt_link_s *btlink,
2007                 const uint8_t *data, int start, int len)
2008 {
2009     struct bt_hci_link_s *link = (struct bt_hci_link_s *) btlink;
2010
2011     bt_hci_lmp_acl_data(hci_from_device(btlink->slave),
2012                     link->handle, data, start, len);
2013 }
2014
2015 static void bt_hci_lmp_acl_data_host(struct bt_link_s *link,
2016                 const uint8_t *data, int start, int len)
2017 {
2018     bt_hci_lmp_acl_data(hci_from_device(link->host),
2019                     link->handle, data, start, len);
2020 }
2021
2022 static void bt_submit_acl(struct HCIInfo *info,
2023                 const uint8_t *data, int length)
2024 {
2025     struct bt_hci_s *hci = hci_from_info(info);
2026     uint16_t handle;
2027     int datalen, flags;
2028     struct bt_link_s *link;
2029
2030     if (length < HCI_ACL_HDR_SIZE) {
2031         error_report("%s: ACL packet too short (%iB)", __func__, length);
2032         return;
2033     }
2034
2035     handle = acl_handle((data[1] << 8) | data[0]);
2036     flags = acl_flags((data[1] << 8) | data[0]);
2037     datalen = (data[3] << 8) | data[2];
2038     data += HCI_ACL_HDR_SIZE;
2039     length -= HCI_ACL_HDR_SIZE;
2040
2041     if (bt_hci_handle_bad(hci, handle)) {
2042         error_report("%s: invalid ACL handle %03x", __func__, handle);
2043         /* TODO: signal an error */
2044         return;
2045     }
2046     handle &= ~HCI_HANDLE_OFFSET;
2047
2048     if (datalen > length) {
2049         error_report("%s: ACL packet too short (%iB < %iB)",
2050                      __func__, length, datalen);
2051         return;
2052     }
2053
2054     link = hci->lm.handle[handle].link;
2055
2056     if ((flags & ~3) == ACL_ACTIVE_BCAST) {
2057         if (!hci->asb_handle)
2058             hci->asb_handle = handle;
2059         else if (handle != hci->asb_handle) {
2060             error_report("%s: Bad handle %03x in Active Slave Broadcast",
2061                          __func__, handle);
2062             /* TODO: signal an error */
2063             return;
2064         }
2065
2066         /* TODO */
2067     }
2068
2069     if ((flags & ~3) == ACL_PICO_BCAST) {
2070         if (!hci->psb_handle)
2071             hci->psb_handle = handle;
2072         else if (handle != hci->psb_handle) {
2073             error_report("%s: Bad handle %03x in Parked Slave Broadcast",
2074                             __func__, handle);
2075             /* TODO: signal an error */
2076             return;
2077         }
2078
2079         /* TODO */
2080     }
2081
2082     /* TODO: increase counter and send EVT_NUM_COMP_PKTS */
2083     bt_hci_event_num_comp_pkts(hci, handle | HCI_HANDLE_OFFSET, 1);
2084
2085     /* Do this last as it can trigger further events even in this HCI */
2086     hci->lm.handle[handle].lmp_acl_data(link, data,
2087                     (flags & 3) == ACL_START, length);
2088 }
2089
2090 static void bt_submit_sco(struct HCIInfo *info,
2091                 const uint8_t *data, int length)
2092 {
2093     struct bt_hci_s *hci = hci_from_info(info);
2094     uint16_t handle;
2095     int datalen;
2096
2097     if (length < 3)
2098         return;
2099
2100     handle = acl_handle((data[1] << 8) | data[0]);
2101     datalen = data[2];
2102     length -= 3;
2103
2104     if (bt_hci_handle_bad(hci, handle)) {
2105         error_report("%s: invalid SCO handle %03x", __func__, handle);
2106         return;
2107     }
2108
2109     if (datalen > length) {
2110         error_report("%s: SCO packet too short (%iB < %iB)",
2111                      __func__, length, datalen);
2112         return;
2113     }
2114
2115     /* TODO */
2116
2117     /* TODO: increase counter and send EVT_NUM_COMP_PKTS if synchronous
2118      * Flow Control is enabled.
2119      * (See Read/Write_Synchronous_Flow_Control_Enable on page 513 and
2120      * page 514.)  */
2121 }
2122
2123 static uint8_t *bt_hci_evt_packet(void *opaque)
2124 {
2125     /* TODO: allocate a packet from upper layer */
2126     struct bt_hci_s *s = opaque;
2127
2128     return s->evt_buf;
2129 }
2130
2131 static void bt_hci_evt_submit(void *opaque, int len)
2132 {
2133     /* TODO: notify upper layer */
2134     struct bt_hci_s *s = opaque;
2135
2136     s->info.evt_recv(s->info.opaque, s->evt_buf, len);
2137 }
2138
2139 static int bt_hci_bdaddr_set(struct HCIInfo *info, const uint8_t *bd_addr)
2140 {
2141     struct bt_hci_s *hci = hci_from_info(info);
2142
2143     bacpy(&hci->device.bd_addr, (const bdaddr_t *) bd_addr);
2144     return 0;
2145 }
2146
2147 static void bt_hci_done(struct HCIInfo *info);
2148 static void bt_hci_destroy(struct bt_device_s *dev)
2149 {
2150     struct bt_hci_s *hci = hci_from_device(dev);
2151
2152     bt_hci_done(&hci->info);
2153 }
2154
2155 struct HCIInfo *bt_new_hci(struct bt_scatternet_s *net)
2156 {
2157     struct bt_hci_s *s = g_malloc0(sizeof(struct bt_hci_s));
2158
2159     s->lm.inquiry_done = timer_new_ns(QEMU_CLOCK_VIRTUAL, bt_hci_inquiry_done, s);
2160     s->lm.inquiry_next = timer_new_ns(QEMU_CLOCK_VIRTUAL, bt_hci_inquiry_next, s);
2161     s->conn_accept_timer =
2162             timer_new_ns(QEMU_CLOCK_VIRTUAL, bt_hci_conn_accept_timeout, s);
2163
2164     s->evt_packet = bt_hci_evt_packet;
2165     s->evt_submit = bt_hci_evt_submit;
2166     s->opaque = s;
2167
2168     bt_device_init(&s->device, net);
2169     s->device.lmp_connection_request = bt_hci_lmp_connection_request;
2170     s->device.lmp_connection_complete = bt_hci_lmp_connection_complete;
2171     s->device.lmp_disconnect_master = bt_hci_lmp_disconnect_host;
2172     s->device.lmp_disconnect_slave = bt_hci_lmp_disconnect_slave;
2173     s->device.lmp_acl_data = bt_hci_lmp_acl_data_slave;
2174     s->device.lmp_acl_resp = bt_hci_lmp_acl_data_host;
2175     s->device.lmp_mode_change = bt_hci_lmp_mode_change_slave;
2176
2177     /* Keep updated! */
2178     /* Also keep in sync with supported commands bitmask in
2179      * bt_hci_read_local_commands_rp */
2180     s->device.lmp_caps = 0x8000199b7e85355fll;
2181
2182     bt_hci_reset(s);
2183
2184     s->info.cmd_send = bt_submit_hci;
2185     s->info.sco_send = bt_submit_sco;
2186     s->info.acl_send = bt_submit_acl;
2187     s->info.bdaddr_set = bt_hci_bdaddr_set;
2188
2189     s->device.handle_destroy = bt_hci_destroy;
2190
2191     error_setg(&s->replay_blocker, QERR_REPLAY_NOT_SUPPORTED, "-bt hci");
2192     replay_add_blocker(s->replay_blocker);
2193
2194     return &s->info;
2195 }
2196
2197 struct HCIInfo *hci_init(const char *str)
2198 {
2199     char *endp;
2200     struct bt_scatternet_s *vlan = 0;
2201
2202     if (!strcmp(str, "null"))
2203         /* null */
2204         return &null_hci;
2205     else if (!strncmp(str, "host", 4) && (str[4] == '\0' || str[4] == ':'))
2206         /* host[:hciN] */
2207         return bt_host_hci(str[4] ? str + 5 : "hci0");
2208     else if (!strncmp(str, "hci", 3)) {
2209         /* hci[,vlan=n] */
2210         if (str[3]) {
2211             if (!strncmp(str + 3, ",vlan=", 6)) {
2212                 vlan = qemu_find_bt_vlan(strtol(str + 9, &endp, 0));
2213                 if (*endp)
2214                     vlan = 0;
2215             }
2216         } else
2217             vlan = qemu_find_bt_vlan(0);
2218         if (vlan)
2219            return bt_new_hci(vlan);
2220     }
2221
2222     error_report("Unknown bluetooth HCI `%s'.", str);
2223
2224     return 0;
2225 }
2226
2227 static void bt_hci_done(struct HCIInfo *info)
2228 {
2229     struct bt_hci_s *hci = hci_from_info(info);
2230     int handle;
2231
2232     bt_device_done(&hci->device);
2233
2234     g_free((void *) hci->device.lmp_name);
2235
2236     /* Be gentle and send DISCONNECT to all connected peers and those
2237      * currently waiting for us to accept or reject a connection request.
2238      * This frees the links.  */
2239     if (hci->conn_req_host) {
2240         bt_hci_connection_reject(hci,
2241                                  hci->conn_req_host, HCI_OE_POWER_OFF);
2242         return;
2243     }
2244
2245     for (handle = HCI_HANDLE_OFFSET;
2246                     handle < (HCI_HANDLE_OFFSET | HCI_HANDLES_MAX); handle ++)
2247         if (!bt_hci_handle_bad(hci, handle))
2248             bt_hci_disconnect(hci, handle, HCI_OE_POWER_OFF);
2249
2250     /* TODO: this is not enough actually, there may be slaves from whom
2251      * we have requested a connection who will soon (or not) respond with
2252      * an accept or a reject, so we should also check if hci->lm.connecting
2253      * is non-zero and if so, avoid freeing the hci but otherwise disappear
2254      * from all qemu social life (e.g. stop scanning and request to be
2255      * removed from s->device.net) and arrange for
2256      * s->device.lmp_connection_complete to free the remaining bits once
2257      * hci->lm.awaiting_bdaddr[] is empty.  */
2258
2259     timer_free(hci->lm.inquiry_done);
2260     timer_free(hci->lm.inquiry_next);
2261     timer_free(hci->conn_accept_timer);
2262
2263     g_free(hci);
2264 }