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