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