wearable/emulator/btdev.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2011-2012  Intel Corporation
   6  *  Copyright (C) 2004-2010  Marcel Holtmann <marcel@holtmann.org>
   7  *
   8  *
   9  *  This program is free software; you can redistribute it and/or modify
  10  *  it under the terms of the GNU General Public License as published by
  11  *  the Free Software Foundation; either version 2 of the License, or
  12  *  (at your option) any later version.
  13  *
  14  *  This program is distributed in the hope that it will be useful,
  15  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  16  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17  *  GNU General Public License for more details.
  18  *
  19  *  You should have received a copy of the GNU General Public License
  20  *  along with this program; if not, write to the Free Software
  21  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  22  *
  23  */
  24
  25 #ifdef HAVE_CONFIG_H
  26 #include <config.h>
  27 #endif
  28
  29 #include <stdio.h>
  30 #include <ctype.h>
  31 #include <stdlib.h>
  32 #include <string.h>
  33
  34 #include "bt.h"
  35 #include "btdev.h"
  36
  37 #define le16_to_cpu(val) (val)
  38 #define cpu_to_le16(val) (val)
  39
  40 struct btdev {
  41         struct btdev *conn;
  42
  43         btdev_send_func send_handler;
  44         void *send_data;
  45
  46         uint16_t manufacturer;
  47         uint8_t  version;
  48         uint16_t revision;
  49         uint8_t  commands[64];
  50         uint8_t  features[8];
  51         uint16_t acl_mtu;
  52         uint16_t acl_max_pkt;
  53         uint8_t  country_code;
  54         uint8_t  bdaddr[6];
  55         uint8_t  le_features[8];
  56         uint8_t  le_states[8];
  57
  58         uint16_t default_link_policy;
  59         uint8_t  event_mask[8];
  60         uint8_t  event_filter;
  61         uint8_t  name[248];
  62         uint8_t  dev_class[3];
  63         uint16_t voice_setting;
  64         uint16_t conn_accept_timeout;
  65         uint16_t page_timeout;
  66         uint8_t  scan_enable;
  67         uint8_t  auth_enable;
  68         uint8_t  inquiry_mode;
  69         uint8_t  afh_assess_mode;
  70         uint8_t  ext_inquiry_fec;
  71         uint8_t  ext_inquiry_rsp[240];
  72         uint8_t  simple_pairing_mode;
  73         uint8_t  le_supported;
  74         uint8_t  le_simultaneous;
  75         uint8_t  le_event_mask[8];
  76 };
  77
  78 #define MAX_BTDEV_ENTRIES 16
  79
  80 static struct btdev *btdev_list[MAX_BTDEV_ENTRIES] = { };
  81
  82 static inline int add_btdev(struct btdev *btdev)
  83 {
  84         int i, index = -1;
  85
  86         for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
  87                 if (btdev_list[i] == NULL) {
  88                         index = i;
  89                         btdev_list[index] = btdev;
  90                         break;
  91                 }
  92         }
  93
  94         return index;
  95 }
  96
  97 static inline int del_btdev(struct btdev *btdev)
  98 {
  99         int i, index = -1;
 100
 101         for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
 102                 if (btdev_list[i] == btdev) {
 103                         index = i;
 104                         btdev_list[index] = NULL;
 105                         break;
 106                 }
 107         }
 108
 109         return index;
 110 }
 111
 112 static inline struct btdev *find_btdev_by_bdaddr(const uint8_t *bdaddr)
 113 {
 114         int i;
 115
 116         for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
 117                 if (btdev_list[i] && !memcmp(btdev_list[i]->bdaddr, bdaddr, 6))
 118                         return btdev_list[i];
 119         }
 120
 121         return NULL;
 122 }
 123
 124 static void hexdump(const unsigned char *buf, uint16_t len)
 125 {
 126         static const char hexdigits[] = "0123456789abcdef";
 127         char str[68];
 128         uint16_t i;
 129
 130         if (!len)
 131                 return;
 132
 133         for (i = 0; i < len; i++) {
 134                 str[((i % 16) * 3) + 0] = hexdigits[buf[i] >> 4];
 135                 str[((i % 16) * 3) + 1] = hexdigits[buf[i] & 0xf];
 136                 str[((i % 16) * 3) + 2] = ' ';
 137                 str[(i % 16) + 49] = isprint(buf[i]) ? buf[i] : '.';
 138
 139                 if ((i + 1) % 16 == 0) {
 140                         str[47] = ' ';
 141                         str[48] = ' ';
 142                         str[65] = '\0';
 143                         printf("%-12c%s\n", ' ', str);
 144                         str[0] = ' ';
 145                 }
 146         }
 147
 148         if (i % 16 > 0) {
 149                 uint16_t j;
 150                 for (j = (i % 16); j < 16; j++) {
 151                         str[(j * 3) + 0] = ' ';
 152                         str[(j * 3) + 1] = ' ';
 153                         str[(j * 3) + 2] = ' ';
 154                         str[j + 49] = ' ';
 155                 }
 156                 str[47] = ' ';
 157                 str[48] = ' ';
 158                 str[65] = '\0';
 159                 printf("%-12c%s\n", ' ', str);
 160         }
 161 }
 162
 163 static void get_bdaddr(uint16_t id, uint8_t *bdaddr)
 164 {
 165         bdaddr[0] = id & 0xff;
 166         bdaddr[1] = id >> 8;
 167         bdaddr[2] = 0x00;
 168         bdaddr[3] = 0x01;
 169         bdaddr[4] = 0xaa;
 170         bdaddr[5] = 0x00;
 171 }
 172
 173 struct btdev *btdev_create(uint16_t id)
 174 {
 175         struct btdev *btdev;
 176
 177         btdev = malloc(sizeof(*btdev));
 178         if (!btdev)
 179                 return NULL;
 180
 181         memset(btdev, 0, sizeof(*btdev));
 182
 183         btdev->manufacturer = 63;
 184         btdev->version = 0x06;
 185         btdev->revision = 0x0000;
 186
 187         btdev->features[0] |= 0x04;     /* Encryption */
 188         btdev->features[0] |= 0x20;     /* Role switch */
 189         btdev->features[0] |= 0x80;     /* Sniff mode */
 190         btdev->features[1] |= 0x08;     /* SCO link */
 191         btdev->features[3] |= 0x40;     /* RSSI with inquiry results */
 192         btdev->features[3] |= 0x80;     /* Extended SCO link */
 193         btdev->features[4] |= 0x08;     /* AFH capable slave */
 194         btdev->features[4] |= 0x10;     /* AFH classification slave */
 195         btdev->features[4] |= 0x40;     /* LE Supported */
 196         btdev->features[5] |= 0x02;     /* Sniff subrating */
 197         btdev->features[5] |= 0x04;     /* Pause encryption */
 198         btdev->features[5] |= 0x08;     /* AFH capable master */
 199         btdev->features[5] |= 0x10;     /* AFH classification master */
 200         btdev->features[6] |= 0x01;     /* Extended Inquiry Response */
 201         btdev->features[6] |= 0x02;     /* Simultaneous LE and BR/EDR */
 202         btdev->features[6] |= 0x08;     /* Secure Simple Pairing */
 203         btdev->features[6] |= 0x10;     /* Encapsulated PDU */
 204         btdev->features[6] |= 0x20;     /* Erroneous Data Reporting */
 205         btdev->features[6] |= 0x40;     /* Non-flushable Packet Boundary Flag */
 206         btdev->features[7] |= 0x01;     /* Link Supervision Timeout Event */
 207         btdev->features[7] |= 0x02;     /* Inquiry TX Power Level */
 208         btdev->features[7] |= 0x80;     /* Extended features */
 209
 210         btdev->acl_mtu = 192;
 211         btdev->acl_max_pkt = 1;
 212
 213         btdev->country_code = 0x00;
 214
 215         get_bdaddr(id, btdev->bdaddr);
 216
 217         add_btdev(btdev);
 218
 219         return btdev;
 220 }
 221
 222 void btdev_destroy(struct btdev *btdev)
 223 {
 224         if (!btdev)
 225                 return;
 226
 227         del_btdev(btdev);
 228
 229         free(btdev);
 230 }
 231
 232 void btdev_set_send_handler(struct btdev *btdev, btdev_send_func handler,
 233                                                         void *user_data)
 234 {
 235         if (!btdev)
 236                 return;
 237
 238         btdev->send_handler = handler;
 239         btdev->send_data = user_data;
 240 }
 241
 242 static void send_packet(struct btdev *btdev, const void *data, uint16_t len)
 243 {
 244         if (!btdev->send_handler)
 245                 return;
 246
 247         btdev->send_handler(data, len, btdev->send_data);
 248 }
 249
 250 static void send_event(struct btdev *btdev, uint8_t event,
 251                                                 const void *data, uint8_t len)
 252 {
 253         struct bt_hci_evt_hdr *hdr;
 254         uint16_t pkt_len;
 255         void *pkt_data;
 256
 257         pkt_len = 1 + sizeof(*hdr) + len;
 258
 259         pkt_data = malloc(pkt_len);
 260         if (!pkt_data)
 261                 return;
 262
 263         ((uint8_t *) pkt_data)[0] = BT_H4_EVT_PKT;
 264
 265         hdr = pkt_data + 1;
 266         hdr->evt = event;
 267         hdr->plen = len;
 268
 269         if (len > 0)
 270                 memcpy(pkt_data + 1 + sizeof(*hdr), data, len);
 271
 272         send_packet(btdev, pkt_data, pkt_len);
 273
 274         free(pkt_data);
 275 }
 276
 277 static void cmd_complete(struct btdev *btdev, uint16_t opcode,
 278                                                 const void *data, uint8_t len)
 279 {
 280         struct bt_hci_evt_hdr *hdr;
 281         struct bt_hci_evt_cmd_complete *cc;
 282         uint16_t pkt_len;
 283         void *pkt_data;
 284
 285         pkt_len = 1 + sizeof(*hdr) + sizeof(*cc) + len;
 286
 287         pkt_data = malloc(pkt_len);
 288         if (!pkt_data)
 289                 return;
 290
 291         ((uint8_t *) pkt_data)[0] = BT_H4_EVT_PKT;
 292
 293         hdr = pkt_data + 1;
 294         hdr->evt = BT_HCI_EVT_CMD_COMPLETE;
 295         hdr->plen = sizeof(*cc) + len;
 296
 297         cc = pkt_data + 1 + sizeof(*hdr);
 298         cc->ncmd = 0x01;
 299         cc->opcode = cpu_to_le16(opcode);
 300
 301         if (len > 0)
 302                 memcpy(pkt_data + 1 + sizeof(*hdr) + sizeof(*cc), data, len);
 303
 304         send_packet(btdev, pkt_data, pkt_len);
 305
 306         free(pkt_data);
 307 }
 308
 309 static void cmd_status(struct btdev *btdev, uint8_t status, uint16_t opcode)
 310 {
 311         struct bt_hci_evt_cmd_status cs;
 312
 313         cs.status = status;
 314         cs.ncmd = 0x01;
 315         cs.opcode = cpu_to_le16(opcode);
 316
 317         send_event(btdev, BT_HCI_EVT_CMD_STATUS, &cs, sizeof(cs));
 318 }
 319
 320 static void num_completed_packets(struct btdev *btdev)
 321 {
 322         if (btdev->conn) {
 323                 struct bt_hci_evt_num_completed_packets ncp;
 324
 325                 ncp.num_handles = 1;
 326                 ncp.handle = cpu_to_le16(42);
 327                 ncp.count = cpu_to_le16(1);
 328
 329                 send_event(btdev, BT_HCI_EVT_NUM_COMPLETED_PACKETS,
 330                                                         &ncp, sizeof(ncp));
 331         }
 332 }
 333
 334 static void inquiry_complete(struct btdev *btdev, uint8_t status)
 335 {
 336         struct bt_hci_evt_inquiry_complete ic;
 337         int i;
 338
 339         for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
 340                 if (!btdev_list[i] || btdev_list[i] == btdev)
 341                         continue;
 342
 343                 if (!(btdev_list[i]->scan_enable & 0x02))
 344                         continue;
 345
 346                 if (btdev->inquiry_mode == 0x02 &&
 347                                         btdev_list[i]->ext_inquiry_rsp[0]) {
 348                         struct bt_hci_evt_ext_inquiry_result ir;
 349
 350                         ir.num_resp = 0x01;
 351                         memcpy(ir.bdaddr, btdev_list[i]->bdaddr, 6);
 352                         memcpy(ir.dev_class, btdev_list[i]->dev_class, 3);
 353                         ir.rssi = -60;
 354                         memcpy(ir.data, btdev_list[i]->ext_inquiry_rsp, 240);
 355
 356                         send_event(btdev, BT_HCI_EVT_EXT_INQUIRY_RESULT,
 357                                                         &ir, sizeof(ir));
 358                         continue;
 359                 }
 360
 361                 if (btdev->inquiry_mode > 0x00) {
 362                         struct bt_hci_evt_inquiry_result_with_rssi ir;
 363
 364                         ir.num_resp = 0x01;
 365                         memcpy(ir.bdaddr, btdev_list[i]->bdaddr, 6);
 366                         memcpy(ir.dev_class, btdev_list[i]->dev_class, 3);
 367                         ir.rssi = -60;
 368
 369                         send_event(btdev, BT_HCI_EVT_INQUIRY_RESULT_WITH_RSSI,
 370                                                         &ir, sizeof(ir));
 371                 } else {
 372                         struct bt_hci_evt_inquiry_result ir;
 373
 374                         ir.num_resp = 0x01;
 375                         memcpy(ir.bdaddr, btdev_list[i]->bdaddr, 6);
 376                         memcpy(ir.dev_class, btdev_list[i]->dev_class, 3);
 377
 378                         send_event(btdev, BT_HCI_EVT_INQUIRY_RESULT,
 379                                                         &ir, sizeof(ir));
 380                 }
 381         }
 382
 383         ic.status = status;
 384
 385         send_event(btdev, BT_HCI_EVT_INQUIRY_COMPLETE, &ic, sizeof(ic));
 386 }
 387
 388 static void conn_complete(struct btdev *btdev,
 389                                         const uint8_t *bdaddr, uint8_t status)
 390 {
 391         struct bt_hci_evt_conn_complete cc;
 392
 393         if (!status) {
 394                 struct btdev *remote = find_btdev_by_bdaddr(bdaddr);
 395
 396                 btdev->conn = remote;
 397                 remote->conn = btdev;
 398
 399                 cc.status = status;
 400                 memcpy(cc.bdaddr, btdev->bdaddr, 6);
 401                 cc.encr_mode = 0x00;
 402
 403                 cc.handle = cpu_to_le16(42);
 404                 cc.link_type = 0x01;
 405
 406                 send_event(remote, BT_HCI_EVT_CONN_COMPLETE, &cc, sizeof(cc));
 407
 408                 cc.handle = cpu_to_le16(42);
 409                 cc.link_type = 0x01;
 410         } else {
 411                 cc.handle = cpu_to_le16(0x0000);
 412                 cc.link_type = 0x01;
 413         }
 414
 415         cc.status = status;
 416         memcpy(cc.bdaddr, bdaddr, 6);
 417         cc.encr_mode = 0x00;
 418
 419         send_event(btdev, BT_HCI_EVT_CONN_COMPLETE, &cc, sizeof(cc));
 420 }
 421
 422 static void conn_request(struct btdev *btdev, const uint8_t *bdaddr)
 423 {
 424         struct btdev *remote = find_btdev_by_bdaddr(bdaddr);
 425
 426         if (remote) {
 427                 if (remote->scan_enable & 0x01) {
 428                         struct bt_hci_evt_conn_request cr;
 429
 430                         memcpy(cr.bdaddr, btdev->bdaddr, 6);
 431                         memcpy(cr.dev_class, btdev->dev_class, 3);
 432                         cr.link_type = 0x01;
 433
 434                         send_event(remote, BT_HCI_EVT_CONN_REQUEST,
 435                                                         &cr, sizeof(cr));
 436                 } else
 437                         conn_complete(btdev, bdaddr, BT_HCI_ERR_PAGE_TIMEOUT);
 438         } else
 439                 conn_complete(btdev, bdaddr, BT_HCI_ERR_UNKNOWN_CONN_ID);
 440 }
 441
 442 static void disconnect_complete(struct btdev *btdev, uint16_t handle,
 443                                                         uint8_t reason)
 444 {
 445         struct bt_hci_evt_disconnect_complete dc;
 446         struct btdev *remote;
 447
 448         if (!btdev) {
 449                 dc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
 450                 dc.handle = cpu_to_le16(handle);
 451                 dc.reason = 0x00;
 452
 453                 send_event(btdev, BT_HCI_EVT_DISCONNECT_COMPLETE,
 454                                                         &dc, sizeof(dc));
 455                 return;
 456         }
 457
 458         dc.status = BT_HCI_ERR_SUCCESS;
 459         dc.handle = cpu_to_le16(handle);
 460         dc.reason = reason;
 461
 462         remote = btdev->conn;
 463
 464         btdev->conn = NULL;
 465         remote->conn = NULL;
 466
 467         send_event(btdev, BT_HCI_EVT_DISCONNECT_COMPLETE, &dc, sizeof(dc));
 468         send_event(remote, BT_HCI_EVT_DISCONNECT_COMPLETE, &dc, sizeof(dc));
 469 }
 470
 471 static void name_request_complete(struct btdev *btdev,
 472                                         const uint8_t *bdaddr, uint8_t status)
 473 {
 474         struct bt_hci_evt_remote_name_req_complete nc;
 475
 476         nc.status = status;
 477         memcpy(nc.bdaddr, bdaddr, 6);
 478         memset(nc.name, 0, 248);
 479
 480         if (!status) {
 481                 struct btdev *remote = find_btdev_by_bdaddr(bdaddr);
 482
 483                 if (remote)
 484                         memcpy(nc.name, remote->name, 248);
 485                 else
 486                         nc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
 487         }
 488
 489         send_event(btdev, BT_HCI_EVT_REMOTE_NAME_REQUEST_COMPLETE,
 490                                                         &nc, sizeof(nc));
 491 }
 492
 493 static void remote_features_complete(struct btdev *btdev, uint16_t handle)
 494 {
 495         struct bt_hci_evt_remote_features_complete rfc;
 496
 497         if (btdev->conn) {
 498                 rfc.status = BT_HCI_ERR_SUCCESS;
 499                 rfc.handle = cpu_to_le16(handle);
 500                 memcpy(rfc.features, btdev->conn->features, 8);
 501         } else {
 502                 rfc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
 503                 rfc.handle = cpu_to_le16(handle);
 504                 memset(rfc.features, 0, 8);
 505         }
 506
 507         send_event(btdev, BT_HCI_EVT_REMOTE_FEATURES_COMPLETE,
 508                                                         &rfc, sizeof(rfc));
 509 }
 510
 511 static void remote_ext_features_complete(struct btdev *btdev, uint16_t handle,
 512                                                                 uint8_t page)
 513 {
 514         struct bt_hci_evt_remote_ext_features_complete refc;
 515
 516         if (btdev->conn && page < 0x02) {
 517                 refc.handle = cpu_to_le16(handle);
 518                 refc.page = page;
 519                 refc.max_page = 0x01;
 520
 521                 switch (page) {
 522                 case 0x00:
 523                         refc.status = BT_HCI_ERR_SUCCESS;
 524                         memcpy(refc.features, btdev->conn->features, 8);
 525                         break;
 526                 case 0x01:
 527                         refc.status = BT_HCI_ERR_SUCCESS;
 528                         memset(refc.features, 0, 8);
 529                         break;
 530                 default:
 531                         refc.status = BT_HCI_ERR_INVALID_PARAMETERS;
 532                         memset(refc.features, 0, 8);
 533                         break;
 534                 }
 535         } else {
 536                 refc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
 537                 refc.handle = cpu_to_le16(handle);
 538                 refc.page = page;
 539                 refc.max_page = 0x01;
 540                 memset(refc.features, 0, 8);
 541         }
 542
 543         send_event(btdev, BT_HCI_EVT_REMOTE_EXT_FEATURES_COMPLETE,
 544                                                         &refc, sizeof(refc));
 545 }
 546
 547 static void remote_version_complete(struct btdev *btdev, uint16_t handle)
 548 {
 549         struct bt_hci_evt_remote_version_complete rvc;
 550
 551         if (btdev->conn) {
 552                 rvc.status = BT_HCI_ERR_SUCCESS;
 553                 rvc.handle = cpu_to_le16(handle);
 554                 rvc.lmp_ver = btdev->conn->version;
 555                 rvc.manufacturer = cpu_to_le16(btdev->conn->manufacturer);
 556                 rvc.lmp_subver = cpu_to_le16(btdev->conn->revision);
 557         } else {
 558                 rvc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
 559                 rvc.handle = cpu_to_le16(handle);
 560                 rvc.lmp_ver = 0x00;
 561                 rvc.manufacturer = cpu_to_le16(0);
 562                 rvc.lmp_subver = cpu_to_le16(0);
 563         }
 564
 565         send_event(btdev, BT_HCI_EVT_REMOTE_VERSION_COMPLETE,
 566                                                         &rvc, sizeof(rvc));
 567 }
 568
 569 static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
 570 {
 571         const struct bt_hci_cmd_hdr *hdr = data;
 572         const struct bt_hci_cmd_create_conn *cc;
 573         const struct bt_hci_cmd_disconnect *dc;
 574         const struct bt_hci_cmd_create_conn_cancel *ccc;
 575         const struct bt_hci_cmd_accept_conn_request *acr;
 576         const struct bt_hci_cmd_reject_conn_request *rcr;
 577         const struct bt_hci_cmd_remote_name_request *rnr;
 578         const struct bt_hci_cmd_remote_name_request_cancel *rnrc;
 579         const struct bt_hci_cmd_read_remote_features *rrf;
 580         const struct bt_hci_cmd_read_remote_ext_features *rref;
 581         const struct bt_hci_cmd_read_remote_version *rrv;
 582         const struct bt_hci_cmd_write_default_link_policy *wdlp;
 583         const struct bt_hci_cmd_set_event_mask *sem;
 584         const struct bt_hci_cmd_set_event_filter *sef;
 585         const struct bt_hci_cmd_write_local_name *wln;
 586         const struct bt_hci_cmd_write_conn_accept_timeout *wcat;
 587         const struct bt_hci_cmd_write_page_timeout *wpt;
 588         const struct bt_hci_cmd_write_scan_enable *wse;
 589         const struct bt_hci_cmd_write_auth_enable *wae;
 590         const struct bt_hci_cmd_write_class_of_dev *wcod;
 591         const struct bt_hci_cmd_write_voice_setting *wvs;
 592         const struct bt_hci_cmd_write_inquiry_mode *wim;
 593         const struct bt_hci_cmd_write_afh_assess_mode *waam;
 594         const struct bt_hci_cmd_write_ext_inquiry_rsp *weir;
 595         const struct bt_hci_cmd_write_simple_pairing_mode *wspm;
 596         const struct bt_hci_cmd_write_le_host_supported *wlhs;
 597         const struct bt_hci_cmd_le_set_event_mask *lsem;
 598         struct bt_hci_rsp_read_default_link_policy rdlp;
 599         struct bt_hci_rsp_read_stored_link_key rslk;
 600         struct bt_hci_rsp_write_stored_link_key wslk;
 601         struct bt_hci_rsp_delete_stored_link_key dslk;
 602         struct bt_hci_rsp_read_local_name rln;
 603         struct bt_hci_rsp_read_conn_accept_timeout rcat;
 604         struct bt_hci_rsp_read_page_timeout rpt;
 605         struct bt_hci_rsp_read_scan_enable rse;
 606         struct bt_hci_rsp_read_auth_enable rae;
 607         struct bt_hci_rsp_read_class_of_dev rcod;
 608         struct bt_hci_rsp_read_voice_setting rvs;
 609         struct bt_hci_rsp_read_inquiry_mode rim;
 610         struct bt_hci_rsp_read_afh_assess_mode raam;
 611         struct bt_hci_rsp_read_ext_inquiry_rsp reir;
 612         struct bt_hci_rsp_read_simple_pairing_mode rspm;
 613         struct bt_hci_rsp_read_inquiry_rsp_tx_power rirtp;
 614         struct bt_hci_rsp_read_le_host_supported rlhs;
 615         struct bt_hci_rsp_read_local_version rlv;
 616         struct bt_hci_rsp_read_local_commands rlc;
 617         struct bt_hci_rsp_read_local_features rlf;
 618         struct bt_hci_rsp_read_local_ext_features rlef;
 619         struct bt_hci_rsp_read_buffer_size rbs;
 620         struct bt_hci_rsp_read_country_code rcc;
 621         struct bt_hci_rsp_read_bd_addr rba;
 622         struct bt_hci_rsp_read_data_block_size rdbs;
 623         struct bt_hci_rsp_le_read_buffer_size lrbs;
 624         struct bt_hci_rsp_le_read_local_features lrlf;
 625         struct bt_hci_rsp_le_read_supported_states lrss;
 626         uint16_t opcode;
 627         uint8_t status, page;
 628
 629         if (len < sizeof(*hdr))
 630                 return;
 631
 632         opcode = le16_to_cpu(hdr->opcode);
 633
 634         switch (opcode) {
 635         case BT_HCI_CMD_INQUIRY:
 636                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
 637                 inquiry_complete(btdev, BT_HCI_ERR_SUCCESS);
 638                 break;
 639
 640         case BT_HCI_CMD_INQUIRY_CANCEL:
 641                 status = BT_HCI_ERR_SUCCESS;
 642                 cmd_complete(btdev, opcode, &status, sizeof(status));
 643                 break;
 644
 645         case BT_HCI_CMD_CREATE_CONN:
 646                 cc = data + sizeof(*hdr);
 647                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
 648                 conn_request(btdev, cc->bdaddr);
 649                 break;
 650
 651         case BT_HCI_CMD_DISCONNECT:
 652                 dc = data + sizeof(*hdr);
 653                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
 654                 disconnect_complete(btdev, le16_to_cpu(dc->handle), dc->reason);
 655                 break;
 656
 657         case BT_HCI_CMD_CREATE_CONN_CANCEL:
 658                 ccc = data + sizeof(*hdr);
 659                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
 660                 conn_complete(btdev, ccc->bdaddr, BT_HCI_ERR_UNKNOWN_CONN_ID);
 661                 break;
 662
 663         case BT_HCI_CMD_ACCEPT_CONN_REQUEST:
 664                 acr = data + sizeof(*hdr);
 665                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
 666                 conn_complete(btdev, acr->bdaddr, BT_HCI_ERR_SUCCESS);
 667                 break;
 668
 669         case BT_HCI_CMD_REJECT_CONN_REQUEST:
 670                 rcr = data + sizeof(*hdr);
 671                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
 672                 conn_complete(btdev, rcr->bdaddr, BT_HCI_ERR_UNKNOWN_CONN_ID);
 673                 break;
 674
 675         case BT_HCI_CMD_REMOTE_NAME_REQUEST:
 676                 rnr = data + sizeof(*hdr);
 677                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
 678                 name_request_complete(btdev, rnr->bdaddr, BT_HCI_ERR_SUCCESS);
 679                 break;
 680
 681         case BT_HCI_CMD_REMOTE_NAME_REQUEST_CANCEL:
 682                 rnrc = data + sizeof(*hdr);
 683                 status = BT_HCI_ERR_SUCCESS;
 684                 cmd_complete(btdev, opcode, &status, sizeof(status));
 685                 name_request_complete(btdev, rnrc->bdaddr,
 686                                                 BT_HCI_ERR_UNKNOWN_CONN_ID);
 687                 break;
 688
 689         case BT_HCI_CMD_READ_REMOTE_FEATURES:
 690                 rrf = data + sizeof(*hdr);
 691                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
 692                 remote_features_complete(btdev, le16_to_cpu(rrf->handle));
 693                 break;
 694
 695         case BT_HCI_CMD_READ_REMOTE_EXT_FEATURES:
 696                 rref = data + sizeof(*hdr);
 697                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
 698                 remote_ext_features_complete(btdev, le16_to_cpu(rref->handle),
 699                                                                 rref->page);
 700                 break;
 701
 702         case BT_HCI_CMD_READ_REMOTE_VERSION:
 703                 rrv = data + sizeof(*hdr);
 704                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
 705                 remote_version_complete(btdev, le16_to_cpu(rrv->handle));
 706                 break;
 707
 708         case BT_HCI_CMD_READ_DEFAULT_LINK_POLICY:
 709                 rdlp.status = BT_HCI_ERR_SUCCESS;
 710                 rdlp.policy = cpu_to_le16(btdev->default_link_policy);
 711                 cmd_complete(btdev, opcode, &rdlp, sizeof(rdlp));
 712                 break;
 713
 714         case BT_HCI_CMD_WRITE_DEFAULT_LINK_POLICY:
 715                 wdlp = data + sizeof(*hdr);
 716                 btdev->default_link_policy = le16_to_cpu(wdlp->policy);
 717                 status = BT_HCI_ERR_SUCCESS;
 718                 cmd_complete(btdev, opcode, &status, sizeof(status));
 719                 break;
 720
 721         case BT_HCI_CMD_SET_EVENT_MASK:
 722                 sem = data + sizeof(*hdr);
 723                 memcpy(btdev->event_mask, sem->mask, 8);
 724                 status = BT_HCI_ERR_SUCCESS;
 725                 cmd_complete(btdev, opcode, &status, sizeof(status));
 726                 break;
 727
 728         case BT_HCI_CMD_RESET:
 729                 status = BT_HCI_ERR_SUCCESS;
 730                 cmd_complete(btdev, opcode, &status, sizeof(status));
 731                 break;
 732
 733         case BT_HCI_CMD_SET_EVENT_FILTER:
 734                 sef = data + sizeof(*hdr);
 735                 btdev->event_filter = sef->type;
 736                 status = BT_HCI_ERR_SUCCESS;
 737                 cmd_complete(btdev, opcode, &status, sizeof(status));
 738                 break;
 739
 740         case BT_HCI_CMD_READ_STORED_LINK_KEY:
 741                 rslk.status = BT_HCI_ERR_SUCCESS;
 742                 rslk.max_num_keys = cpu_to_le16(0);
 743                 rslk.num_keys = cpu_to_le16(0);
 744                 cmd_complete(btdev, opcode, &rslk, sizeof(rslk));
 745                 break;
 746
 747         case BT_HCI_CMD_WRITE_STORED_LINK_KEY:
 748                 wslk.status = BT_HCI_ERR_SUCCESS;
 749                 wslk.num_keys = 0;
 750                 cmd_complete(btdev, opcode, &wslk, sizeof(wslk));
 751                 break;
 752
 753         case BT_HCI_CMD_DELETE_STORED_LINK_KEY:
 754                 dslk.status = BT_HCI_ERR_SUCCESS;
 755                 dslk.num_keys = cpu_to_le16(0);
 756                 cmd_complete(btdev, opcode, &dslk, sizeof(dslk));
 757                 break;
 758
 759         case BT_HCI_CMD_WRITE_LOCAL_NAME:
 760                 wln = data + sizeof(*hdr);
 761                 memcpy(btdev->name, wln->name, 248);
 762                 status = BT_HCI_ERR_SUCCESS;
 763                 cmd_complete(btdev, opcode, &status, sizeof(status));
 764                 break;
 765
 766         case BT_HCI_CMD_READ_LOCAL_NAME:
 767                 rln.status = BT_HCI_ERR_SUCCESS;
 768                 memcpy(rln.name, btdev->name, 248);
 769                 cmd_complete(btdev, opcode, &rln, sizeof(rln));
 770                 break;
 771
 772         case BT_HCI_CMD_READ_CONN_ACCEPT_TIMEOUT:
 773                 rcat.status = BT_HCI_ERR_SUCCESS;
 774                 rcat.timeout = cpu_to_le16(btdev->conn_accept_timeout);
 775                 cmd_complete(btdev, opcode, &rcat, sizeof(rcat));
 776                 break;
 777
 778         case BT_HCI_CMD_WRITE_CONN_ACCEPT_TIMEOUT:
 779                 wcat = data + sizeof(*hdr);
 780                 btdev->conn_accept_timeout = le16_to_cpu(wcat->timeout);
 781                 status = BT_HCI_ERR_SUCCESS;
 782                 cmd_complete(btdev, opcode, &status, sizeof(status));
 783                 break;
 784
 785         case BT_HCI_CMD_READ_PAGE_TIMEOUT:
 786                 rpt.status = BT_HCI_ERR_SUCCESS;
 787                 rpt.timeout = cpu_to_le16(btdev->page_timeout);
 788                 cmd_complete(btdev, opcode, &rpt, sizeof(rpt));
 789                 break;
 790
 791         case BT_HCI_CMD_WRITE_PAGE_TIMEOUT:
 792                 wpt = data + sizeof(*hdr);
 793                 btdev->page_timeout = le16_to_cpu(wpt->timeout);
 794                 status = BT_HCI_ERR_SUCCESS;
 795                 cmd_complete(btdev, opcode, &status, sizeof(status));
 796                 break;
 797
 798         case BT_HCI_CMD_READ_SCAN_ENABLE:
 799                 rse.status = BT_HCI_ERR_SUCCESS;
 800                 rse.enable = btdev->scan_enable;
 801                 cmd_complete(btdev, opcode, &rse, sizeof(rse));
 802                 break;
 803
 804         case BT_HCI_CMD_WRITE_SCAN_ENABLE:
 805                 wse = data + sizeof(*hdr);
 806                 btdev->scan_enable = wse->enable;
 807                 status = BT_HCI_ERR_SUCCESS;
 808                 cmd_complete(btdev, opcode, &status, sizeof(status));
 809                 break;
 810
 811         case BT_HCI_CMD_READ_AUTH_ENABLE:
 812                 rae.status = BT_HCI_ERR_SUCCESS;
 813                 rae.enable = btdev->auth_enable;
 814                 cmd_complete(btdev, opcode, &rae, sizeof(rae));
 815                 break;
 816
 817         case BT_HCI_CMD_WRITE_AUTH_ENABLE:
 818                 wae = data + sizeof(*hdr);
 819                 btdev->auth_enable = wae->enable;
 820                 status = BT_HCI_ERR_SUCCESS;
 821                 cmd_complete(btdev, opcode, &status, sizeof(status));
 822                 break;
 823
 824         case BT_HCI_CMD_READ_CLASS_OF_DEV:
 825                 rcod.status = BT_HCI_ERR_SUCCESS;
 826                 memcpy(rcod.dev_class, btdev->dev_class, 3);
 827                 cmd_complete(btdev, opcode, &rcod, sizeof(rcod));
 828                 break;
 829
 830         case BT_HCI_CMD_WRITE_CLASS_OF_DEV:
 831                 wcod = data + sizeof(*hdr);
 832                 memcpy(btdev->dev_class, wcod->dev_class, 3);
 833                 status = BT_HCI_ERR_SUCCESS;
 834                 cmd_complete(btdev, opcode, &status, sizeof(status));
 835                 break;
 836
 837         case BT_HCI_CMD_READ_VOICE_SETTING:
 838                 rvs.status = BT_HCI_ERR_SUCCESS;
 839                 rvs.setting = cpu_to_le16(btdev->voice_setting);
 840                 cmd_complete(btdev, opcode, &rvs, sizeof(rvs));
 841                 break;
 842
 843         case BT_HCI_CMD_WRITE_VOICE_SETTING:
 844                 wvs = data + sizeof(*hdr);
 845                 btdev->voice_setting = le16_to_cpu(wvs->setting);
 846                 status = BT_HCI_ERR_SUCCESS;
 847                 cmd_complete(btdev, opcode, &status, sizeof(status));
 848                 break;
 849
 850         case BT_HCI_CMD_READ_INQUIRY_MODE:
 851                 rim.status = BT_HCI_ERR_SUCCESS;
 852                 rim.mode = btdev->inquiry_mode;
 853                 cmd_complete(btdev, opcode, &rim, sizeof(rim));
 854                 break;
 855
 856         case BT_HCI_CMD_WRITE_INQUIRY_MODE:
 857                 wim = data + sizeof(*hdr);
 858                 btdev->inquiry_mode = wim->mode;
 859                 status = BT_HCI_ERR_SUCCESS;
 860                 cmd_complete(btdev, opcode, &status, sizeof(status));
 861                 break;
 862
 863         case BT_HCI_CMD_READ_AFH_ASSESS_MODE:
 864                 raam.status = BT_HCI_ERR_SUCCESS;
 865                 raam.mode = btdev->afh_assess_mode;
 866                 cmd_complete(btdev, opcode, &raam, sizeof(raam));
 867                 break;
 868
 869         case BT_HCI_CMD_WRITE_AFH_ASSESS_MODE:
 870                 waam = data + sizeof(*hdr);
 871                 btdev->afh_assess_mode = waam->mode;
 872                 status = BT_HCI_ERR_SUCCESS;
 873                 cmd_complete(btdev, opcode, &status, sizeof(status));
 874                 break;
 875
 876         case BT_HCI_CMD_READ_EXT_INQUIRY_RSP:
 877                 reir.status = BT_HCI_ERR_SUCCESS;
 878                 reir.fec = btdev->ext_inquiry_fec;
 879                 memcpy(reir.data, btdev->ext_inquiry_rsp, 240);
 880                 cmd_complete(btdev, opcode, &reir, sizeof(reir));
 881                 break;
 882
 883         case BT_HCI_CMD_WRITE_EXT_INQUIRY_RSP:
 884                 weir = data + sizeof(*hdr);
 885                 btdev->ext_inquiry_fec = weir->fec;
 886                 memcpy(btdev->ext_inquiry_rsp, weir->data, 240);
 887                 status = BT_HCI_ERR_SUCCESS;
 888                 cmd_complete(btdev, opcode, &status, sizeof(status));
 889                 break;
 890
 891         case BT_HCI_CMD_READ_SIMPLE_PAIRING_MODE:
 892                 rspm.status = BT_HCI_ERR_SUCCESS;
 893                 rspm.mode = btdev->simple_pairing_mode;
 894                 cmd_complete(btdev, opcode, &rspm, sizeof(rspm));
 895                 break;
 896
 897         case BT_HCI_CMD_WRITE_SIMPLE_PAIRING_MODE:
 898                 wspm = data + sizeof(*hdr);
 899                 btdev->simple_pairing_mode = wspm->mode;
 900                 status = BT_HCI_ERR_SUCCESS;
 901                 cmd_complete(btdev, opcode, &status, sizeof(status));
 902                 break;
 903
 904         case BT_HCI_CMD_READ_INQUIRY_RSP_TX_POWER:
 905                 rirtp.status = BT_HCI_ERR_SUCCESS;
 906                 rirtp.level = 0;
 907                 cmd_complete(btdev, opcode, &rirtp, sizeof(rirtp));
 908                 break;
 909
 910         case BT_HCI_CMD_READ_LE_HOST_SUPPORTED:
 911                 rlhs.status = BT_HCI_ERR_SUCCESS;
 912                 rlhs.supported = btdev->le_supported;
 913                 rlhs.simultaneous = btdev->le_simultaneous;
 914                 cmd_complete(btdev, opcode, &rlhs, sizeof(rlhs));
 915                 break;
 916
 917         case BT_HCI_CMD_WRITE_LE_HOST_SUPPORTED:
 918                 wlhs = data + sizeof(*hdr);
 919                 btdev->le_supported = wlhs->supported;
 920                 btdev->le_simultaneous = wlhs->simultaneous;
 921                 status = BT_HCI_ERR_SUCCESS;
 922                 cmd_complete(btdev, opcode, &status, sizeof(status));
 923                 break;
 924
 925         case BT_HCI_CMD_READ_LOCAL_VERSION:
 926                 rlv.status = BT_HCI_ERR_SUCCESS;
 927                 rlv.hci_ver = btdev->version;
 928                 rlv.hci_rev = cpu_to_le16(btdev->revision);
 929                 rlv.lmp_ver = btdev->version;
 930                 rlv.manufacturer = cpu_to_le16(btdev->manufacturer);
 931                 rlv.lmp_subver = cpu_to_le16(btdev->revision);
 932                 cmd_complete(btdev, opcode, &rlv, sizeof(rlv));
 933                 break;
 934
 935         case BT_HCI_CMD_READ_LOCAL_COMMANDS:
 936                 rlc.status = BT_HCI_ERR_SUCCESS;
 937                 memcpy(rlc.commands, btdev->commands, 64);
 938                 cmd_complete(btdev, opcode, &rlc, sizeof(rlc));
 939                 break;
 940
 941         case BT_HCI_CMD_READ_LOCAL_FEATURES:
 942                 rlf.status = BT_HCI_ERR_SUCCESS;
 943                 memcpy(rlf.features, btdev->features, 8);
 944                 cmd_complete(btdev, opcode, &rlf, sizeof(rlf));
 945                 break;
 946
 947         case BT_HCI_CMD_READ_LOCAL_EXT_FEATURES:
 948                 page = ((const uint8_t *) data)[sizeof(*hdr)];
 949                 switch (page) {
 950                 case 0x00:
 951                         rlef.status = BT_HCI_ERR_SUCCESS;
 952                         rlef.page = 0x00;
 953                         rlef.max_page = 0x01;
 954                         memcpy(rlef.features, btdev->features, 8);
 955                         break;
 956                 case 0x01:
 957                         rlef.status = BT_HCI_ERR_SUCCESS;
 958                         rlef.page = 0x01;
 959                         rlef.max_page = 0x01;
 960                         memset(rlef.features, 0, 8);
 961                         if (btdev->simple_pairing_mode)
 962                                 rlef.features[0] |= 0x01;
 963                         if (btdev->le_supported)
 964                                 rlef.features[0] |= 0x02;
 965                         if (btdev->le_simultaneous)
 966                                 rlef.features[0] |= 0x04;
 967                         break;
 968                 default:
 969                         rlef.status = BT_HCI_ERR_INVALID_PARAMETERS;
 970                         rlef.page = page;
 971                         rlef.max_page = 0x01;
 972                         memset(rlef.features, 0, 8);
 973                         break;
 974                 }
 975                 cmd_complete(btdev, opcode, &rlef, sizeof(rlef));
 976                 break;
 977
 978         case BT_HCI_CMD_READ_BUFFER_SIZE:
 979                 rbs.status = BT_HCI_ERR_SUCCESS;
 980                 rbs.acl_mtu = cpu_to_le16(btdev->acl_mtu);
 981                 rbs.sco_mtu = 0;
 982                 rbs.acl_max_pkt = cpu_to_le16(btdev->acl_max_pkt);
 983                 rbs.sco_max_pkt = cpu_to_le16(0);
 984                 cmd_complete(btdev, opcode, &rbs, sizeof(rbs));
 985                 break;
 986
 987         case BT_HCI_CMD_READ_COUNTRY_CODE:
 988                 rcc.status = BT_HCI_ERR_SUCCESS;
 989                 rcc.code = btdev->country_code;
 990                 cmd_complete(btdev, opcode, &rcc, sizeof(rcc));
 991                 break;
 992
 993         case BT_HCI_CMD_READ_BD_ADDR:
 994                 rba.status = BT_HCI_ERR_SUCCESS;
 995                 memcpy(rba.bdaddr, btdev->bdaddr, 6);
 996                 cmd_complete(btdev, opcode, &rba, sizeof(rba));
 997                 break;
 998
 999         case BT_HCI_CMD_READ_DATA_BLOCK_SIZE:
1000                 rdbs.status = BT_HCI_ERR_SUCCESS;
1001                 rdbs.max_acl_len = cpu_to_le16(btdev->acl_mtu);
1002                 rdbs.block_len = cpu_to_le16(btdev->acl_mtu);
1003                 rdbs.num_blocks = cpu_to_le16(btdev->acl_max_pkt);
1004                 cmd_complete(btdev, opcode, &rdbs, sizeof(rdbs));
1005                 break;
1006
1007         case BT_HCI_CMD_LE_SET_EVENT_MASK:
1008                 lsem = data + sizeof(*hdr);
1009                 memcpy(btdev->le_event_mask, lsem->mask, 8);
1010                 status = BT_HCI_ERR_SUCCESS;
1011                 cmd_complete(btdev, opcode, &status, sizeof(status));
1012                 break;
1013
1014         case BT_HCI_CMD_LE_READ_BUFFER_SIZE:
1015                 lrbs.status = BT_HCI_ERR_SUCCESS;
1016                 lrbs.le_mtu = cpu_to_le16(btdev->acl_mtu);
1017                 lrbs.le_max_pkt = btdev->acl_max_pkt;
1018                 cmd_complete(btdev, opcode, &lrbs, sizeof(lrbs));
1019                 break;
1020
1021         case BT_HCI_CMD_LE_READ_LOCAL_FEATURES:
1022                 lrlf.status = BT_HCI_ERR_SUCCESS;
1023                 memcpy(lrlf.features, btdev->le_features, 8);
1024                 cmd_complete(btdev, opcode, &lrlf, sizeof(lrlf));
1025                 break;
1026
1027         case BT_HCI_CMD_LE_SET_SCAN_PARAMETERS:
1028                 status = BT_HCI_ERR_SUCCESS;
1029                 cmd_complete(btdev, opcode, &status, sizeof(status));
1030                 break;
1031
1032         case BT_HCI_CMD_LE_SET_SCAN_ENABLE:
1033                 status = BT_HCI_ERR_SUCCESS;
1034                 cmd_complete(btdev, opcode, &status, sizeof(status));
1035                 break;
1036
1037         case BT_HCI_CMD_LE_READ_SUPPORTED_STATES:
1038                 lrss.status = BT_HCI_ERR_SUCCESS;
1039                 memcpy(lrss.states, btdev->le_states, 8);
1040                 cmd_complete(btdev, opcode, &lrss, sizeof(lrss));
1041                 break;
1042
1043         default:
1044                 printf("Unsupported command 0x%4.4x\n", opcode);
1045                 hexdump(data, len);
1046                 cmd_status(btdev, BT_HCI_ERR_UNKNOWN_COMMAND, opcode);
1047                 break;
1048         }
1049 }
1050
1051 void btdev_receive_h4(struct btdev *btdev, const void *data, uint16_t len)
1052 {
1053         uint8_t pkt_type;
1054
1055         if (!btdev)
1056                 return;
1057
1058         if (len < 1)
1059                 return;
1060
1061         pkt_type = ((const uint8_t *) data)[0];
1062
1063         switch (pkt_type) {
1064         case BT_H4_CMD_PKT:
1065                 process_cmd(btdev, data + 1, len - 1);
1066                 break;
1067         case BT_H4_ACL_PKT:
1068                 if (btdev->conn)
1069                         send_packet(btdev->conn, data, len);
1070                 num_completed_packets(btdev);
1071                 break;
1072         default:
1073                 printf("Unsupported packet 0x%2.2x\n", pkt_type);
1074                 break;
1075         }
1076 }