git.maemo.org Git - qemu/blob - hw/bt-hci.c

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