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 library is free software; you can redistribute it and/or
  10  *  modify it under the terms of the GNU Lesser General Public
  11  *  License as published by the Free Software Foundation; either
  12  *  version 2.1 of the License, or (at your option) any later version.
  13  *
  14  *  This library 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 GNU
  17  *  Lesser General Public License for more details.
  18  *
  19  *  You should have received a copy of the GNU Lesser General Public
  20  *  License along with this library; 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 #include <alloca.h>
  34 #include <sys/uio.h>
  35
  36 #include "src/shared/util.h"
  37 #include "src/shared/timeout.h"
  38 #include "monitor/bt.h"
  39 #include "btdev.h"
  40
  41 #define has_bredr(btdev)        (!((btdev)->features[4] & 0x20))
  42 #define has_le(btdev)           (!!((btdev)->features[4] & 0x40))
  43
  44 struct hook {
  45         btdev_hook_func handler;
  46         void *user_data;
  47         enum btdev_hook_type type;
  48         uint16_t opcode;
  49 };
  50
  51 #define MAX_HOOK_ENTRIES 16
  52
  53 struct btdev {
  54         enum btdev_type type;
  55
  56         struct btdev *conn;
  57
  58         bool auth_init;
  59         uint8_t link_key[16];
  60         uint16_t pin[16];
  61         uint8_t pin_len;
  62         uint8_t io_cap;
  63         uint8_t auth_req;
  64         bool ssp_auth_complete;
  65         uint8_t ssp_status;
  66
  67         btdev_command_func command_handler;
  68         void *command_data;
  69
  70         btdev_send_func send_handler;
  71         void *send_data;
  72
  73         unsigned int inquiry_id;
  74         unsigned int inquiry_timeout_id;
  75
  76         struct hook *hook_list[MAX_HOOK_ENTRIES];
  77
  78         uint16_t manufacturer;
  79         uint8_t  version;
  80         uint16_t revision;
  81         uint8_t  commands[64];
  82         uint8_t  max_page;
  83         uint8_t  features[8];
  84         uint8_t  feat_page_2[8];
  85         uint16_t acl_mtu;
  86         uint16_t acl_max_pkt;
  87         uint8_t  country_code;
  88         uint8_t  bdaddr[6];
  89         uint8_t  random_addr[6];
  90         uint8_t  le_features[8];
  91         uint8_t  le_states[8];
  92
  93         uint16_t default_link_policy;
  94         uint8_t  event_mask[8];
  95         uint8_t  event_mask_page2[8];
  96         uint8_t  event_filter;
  97         uint8_t  name[248];
  98         uint8_t  dev_class[3];
  99         uint16_t voice_setting;
 100         uint16_t conn_accept_timeout;
 101         uint16_t page_timeout;
 102         uint8_t  scan_enable;
 103         uint16_t page_scan_interval;
 104         uint16_t page_scan_window;
 105         uint16_t page_scan_type;
 106         uint8_t  auth_enable;
 107         uint16_t inquiry_scan_interval;
 108         uint16_t inquiry_scan_window;
 109         uint8_t  inquiry_mode;
 110         uint8_t  afh_assessment_mode;
 111         uint8_t  ext_inquiry_fec;
 112         uint8_t  ext_inquiry_rsp[240];
 113         uint8_t  simple_pairing_mode;
 114         uint8_t  ssp_debug_mode;
 115         uint8_t  secure_conn_support;
 116         uint8_t  le_supported;
 117         uint8_t  le_simultaneous;
 118         uint8_t  le_event_mask[8];
 119         uint8_t  le_adv_data[31];
 120         uint8_t  le_adv_data_len;
 121         uint8_t  le_adv_type;
 122         uint8_t  le_adv_own_addr;
 123         uint8_t  le_adv_direct_addr_type;
 124         uint8_t  le_adv_direct_addr[6];
 125         uint8_t  le_scan_data[31];
 126         uint8_t  le_scan_data_len;
 127         uint8_t  le_scan_enable;
 128         uint8_t  le_scan_type;
 129         uint8_t  le_scan_own_addr_type;
 130         uint8_t  le_filter_dup;
 131         uint8_t  le_adv_enable;
 132         uint8_t  le_ltk[16];
 133
 134         uint16_t sync_train_interval;
 135         uint32_t sync_train_timeout;
 136         uint8_t  sync_train_service_data;
 137 };
 138
 139 struct inquiry_data {
 140         struct btdev *btdev;
 141         int num_resp;
 142
 143         int sent_count;
 144         int iter;
 145 };
 146
 147 #define DEFAULT_INQUIRY_INTERVAL 100 /* 100 miliseconds */
 148
 149 #define MAX_BTDEV_ENTRIES 16
 150
 151 static const uint8_t LINK_KEY_NONE[16] = { 0 };
 152 static const uint8_t LINK_KEY_DUMMY[16] = {     0, 1, 2, 3, 4, 5, 6, 7,
 153                                                 8, 9, 0, 1, 2, 3, 4, 5 };
 154
 155 static struct btdev *btdev_list[MAX_BTDEV_ENTRIES] = { };
 156
 157 static int get_hook_index(struct btdev *btdev, enum btdev_hook_type type,
 158                                                                 uint16_t opcode)
 159 {
 160         int i;
 161
 162         for (i = 0; i < MAX_HOOK_ENTRIES; i++) {
 163                 if (btdev->hook_list[i] == NULL)
 164                         continue;
 165
 166                 if (btdev->hook_list[i]->type == type &&
 167                                         btdev->hook_list[i]->opcode == opcode)
 168                         return i;
 169         }
 170
 171         return -1;
 172 }
 173
 174 static bool run_hooks(struct btdev *btdev, enum btdev_hook_type type,
 175                                 uint16_t opcode, const void *data, uint16_t len)
 176 {
 177         int index = get_hook_index(btdev, type, opcode);
 178         if (index < 0)
 179                 return true;
 180
 181         return btdev->hook_list[index]->handler(data, len,
 182                                         btdev->hook_list[index]->user_data);
 183 }
 184
 185 static inline int add_btdev(struct btdev *btdev)
 186 {
 187         int i, index = -1;
 188
 189         for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
 190                 if (btdev_list[i] == NULL) {
 191                         index = i;
 192                         btdev_list[index] = btdev;
 193                         break;
 194                 }
 195         }
 196
 197         return index;
 198 }
 199
 200 static inline int del_btdev(struct btdev *btdev)
 201 {
 202         int i, index = -1;
 203
 204         for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
 205                 if (btdev_list[i] == btdev) {
 206                         index = i;
 207                         btdev_list[index] = NULL;
 208                         break;
 209                 }
 210         }
 211
 212         return index;
 213 }
 214
 215 static inline struct btdev *find_btdev_by_bdaddr(const uint8_t *bdaddr)
 216 {
 217         int i;
 218
 219         for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
 220                 if (btdev_list[i] && !memcmp(btdev_list[i]->bdaddr, bdaddr, 6))
 221                         return btdev_list[i];
 222         }
 223
 224         return NULL;
 225 }
 226
 227 static inline struct btdev *find_btdev_by_bdaddr_type(const uint8_t *bdaddr,
 228                                                         uint8_t bdaddr_type)
 229 {
 230         int i;
 231
 232         for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
 233                 int cmp;
 234
 235                 if (!btdev_list[i])
 236                         continue;
 237
 238                 if (bdaddr_type == 0x01)
 239                         cmp = memcmp(btdev_list[i]->random_addr, bdaddr, 6);
 240                 else
 241                         cmp = memcmp(btdev_list[i]->bdaddr, bdaddr, 6);
 242
 243                 if (!cmp)
 244                         return btdev_list[i];
 245         }
 246
 247         return NULL;
 248 }
 249
 250 static void hexdump(const unsigned char *buf, uint16_t len)
 251 {
 252         static const char hexdigits[] = "0123456789abcdef";
 253         char str[68];
 254         uint16_t i;
 255
 256         if (!len)
 257                 return;
 258
 259         for (i = 0; i < len; i++) {
 260                 str[((i % 16) * 3) + 0] = hexdigits[buf[i] >> 4];
 261                 str[((i % 16) * 3) + 1] = hexdigits[buf[i] & 0xf];
 262                 str[((i % 16) * 3) + 2] = ' ';
 263                 str[(i % 16) + 49] = isprint(buf[i]) ? buf[i] : '.';
 264
 265                 if ((i + 1) % 16 == 0) {
 266                         str[47] = ' ';
 267                         str[48] = ' ';
 268                         str[65] = '\0';
 269                         printf("%-12c%s\n", ' ', str);
 270                         str[0] = ' ';
 271                 }
 272         }
 273
 274         if (i % 16 > 0) {
 275                 uint16_t j;
 276                 for (j = (i % 16); j < 16; j++) {
 277                         str[(j * 3) + 0] = ' ';
 278                         str[(j * 3) + 1] = ' ';
 279                         str[(j * 3) + 2] = ' ';
 280                         str[j + 49] = ' ';
 281                 }
 282                 str[47] = ' ';
 283                 str[48] = ' ';
 284                 str[65] = '\0';
 285                 printf("%-12c%s\n", ' ', str);
 286         }
 287 }
 288
 289 static void get_bdaddr(uint16_t id, uint8_t index, uint8_t *bdaddr)
 290 {
 291         bdaddr[0] = id & 0xff;
 292         bdaddr[1] = id >> 8;
 293         bdaddr[2] = index;
 294         bdaddr[3] = 0x01;
 295         bdaddr[4] = 0xaa;
 296         bdaddr[5] = 0x00;
 297 }
 298
 299 static void set_common_commands_all(struct btdev *btdev)
 300 {
 301         btdev->commands[5]  |= 0x40;    /* Set Event Mask */
 302         btdev->commands[5]  |= 0x80;    /* Reset */
 303         btdev->commands[14] |= 0x08;    /* Read Local Version */
 304         btdev->commands[14] |= 0x10;    /* Read Local Supported Commands */
 305         btdev->commands[14] |= 0x20;    /* Read Local Supported Features */
 306         btdev->commands[14] |= 0x80;    /* Read Buffer Size */
 307 }
 308
 309 static void set_common_commands_bredrle(struct btdev *btdev)
 310 {
 311         btdev->commands[0]  |= 0x20;    /* Disconnect */
 312         btdev->commands[2]  |= 0x80;    /* Read Remote Version Information */
 313         btdev->commands[10] |= 0x40;    /* Host Buffer Size */
 314         btdev->commands[15] |= 0x02;    /* Read BD ADDR */
 315 }
 316
 317 static void set_bredr_commands(struct btdev *btdev)
 318 {
 319         set_common_commands_all(btdev);
 320         set_common_commands_bredrle(btdev);
 321
 322         btdev->commands[0]  |= 0x01;    /* Inquiry */
 323         btdev->commands[0]  |= 0x02;    /* Inquiry Cancel */
 324         btdev->commands[0]  |= 0x10;    /* Create Connection */
 325         btdev->commands[0]  |= 0x40;    /* Add SCO Connection */
 326         btdev->commands[0]  |= 0x80;    /* Cancel Create Connection */
 327         btdev->commands[1]  |= 0x01;    /* Accept Connection Request */
 328         btdev->commands[1]  |= 0x02;    /* Reject Connection Request */
 329         btdev->commands[1]  |= 0x04;    /* Link Key Request Reply */
 330         btdev->commands[1]  |= 0x08;    /* Link Key Request Negative Reply */
 331         btdev->commands[1]  |= 0x10;    /* PIN Code Request Reply */
 332         btdev->commands[1]  |= 0x20;    /* PIN Code Request Negative Reply */
 333         btdev->commands[1]  |= 0x80;    /* Authentication Requested */
 334         btdev->commands[2]  |= 0x01;    /* Set Connection Encryption */
 335         btdev->commands[2]  |= 0x08;    /* Remote Name Request */
 336         btdev->commands[2]  |= 0x10;    /* Cancel Remote Name Request */
 337         btdev->commands[2]  |= 0x20;    /* Read Remote Supported Features */
 338         btdev->commands[2]  |= 0x40;    /* Read Remote Extended Features */
 339         btdev->commands[3]  |= 0x01;    /* Read Clock Offset */
 340         btdev->commands[5]  |= 0x08;    /* Read Default Link Policy */
 341         btdev->commands[5]  |= 0x10;    /* Write Default Link Policy */
 342         btdev->commands[6]  |= 0x01;    /* Set Event Filter */
 343         btdev->commands[6]  |= 0x20;    /* Read Stored Link Key */
 344         btdev->commands[6]  |= 0x40;    /* Write Stored Link Key */
 345         btdev->commands[6]  |= 0x80;    /* Delete Stored Link Key */
 346         btdev->commands[7]  |= 0x01;    /* Write Local Name */
 347         btdev->commands[7]  |= 0x02;    /* Read Local Name */
 348         btdev->commands[7]  |= 0x04;    /* Read Connection Accept Timeout */
 349         btdev->commands[7]  |= 0x08;    /* Write Connection Accept Timeout */
 350         btdev->commands[7]  |= 0x10;    /* Read Page Timeout */
 351         btdev->commands[7]  |= 0x20;    /* Write Page Timeout */
 352         btdev->commands[7]  |= 0x40;    /* Read Scan Enable */
 353         btdev->commands[7]  |= 0x80;    /* Write Scan Enable */
 354         btdev->commands[8]  |= 0x01;    /* Read Page Scan Activity */
 355         btdev->commands[8]  |= 0x02;    /* Write Page Scan Activity */
 356         btdev->commands[8]  |= 0x04;    /* Read Inquiry Scan Activity */
 357         btdev->commands[8]  |= 0x08;    /* Write Inquiry Scan Activity */
 358         btdev->commands[8]  |= 0x10;    /* Read Authentication Enable */
 359         btdev->commands[8]  |= 0x20;    /* Write Authentication Enable */
 360         btdev->commands[9]  |= 0x01;    /* Read Class Of Device */
 361         btdev->commands[9]  |= 0x02;    /* Write Class Of Device */
 362         btdev->commands[9]  |= 0x04;    /* Read Voice Setting */
 363         btdev->commands[9]  |= 0x08;    /* Write Voice Setting */
 364         btdev->commands[11] |= 0x10;    /* Write Current IAC LAP */
 365         btdev->commands[12] |= 0x40;    /* Read Inquiry Mode */
 366         btdev->commands[12] |= 0x80;    /* Write Inquiry Mode */
 367         btdev->commands[13] |= 0x01;    /* Read Page Scan Type */
 368         btdev->commands[13] |= 0x02;    /* Write Page Scan Type */
 369         btdev->commands[13] |= 0x04;    /* Read AFH Assess Mode */
 370         btdev->commands[13] |= 0x08;    /* Write AFH Assess Mode */
 371         btdev->commands[14] |= 0x40;    /* Read Local Extended Features */
 372         btdev->commands[15] |= 0x01;    /* Read Country Code */
 373         btdev->commands[16] |= 0x04;    /* Enable Device Under Test Mode */
 374         btdev->commands[16] |= 0x08;    /* Setup Synchronous Connection */
 375         btdev->commands[17] |= 0x01;    /* Read Extended Inquiry Response */
 376         btdev->commands[17] |= 0x02;    /* Write Extended Inquiry Response */
 377         btdev->commands[17] |= 0x20;    /* Read Simple Pairing Mode */
 378         btdev->commands[17] |= 0x40;    /* Write Simple Pairing Mode */
 379         btdev->commands[17] |= 0x80;    /* Read Local OOB Data */
 380         btdev->commands[18] |= 0x01;    /* Read Inquiry Response TX Power */
 381         btdev->commands[18] |= 0x02;    /* Write Inquiry Response TX Power */
 382         btdev->commands[18] |= 0x80;    /* IO Capability Request Reply */
 383         btdev->commands[23] |= 0x04;    /* Read Data Block Size */
 384         btdev->commands[29] |= 0x20;    /* Read Local Supported Codecs */
 385         btdev->commands[30] |= 0x08;    /* Get MWS Transport Layer Config */
 386 }
 387
 388 static void set_le_commands(struct btdev *btdev)
 389 {
 390         set_common_commands_all(btdev);
 391         set_common_commands_bredrle(btdev);
 392
 393         btdev->commands[24] |= 0x20;    /* Read LE Host Supported */
 394         btdev->commands[24] |= 0x20;    /* Write LE Host Supported */
 395         btdev->commands[25] |= 0x01;    /* LE Set Event Mask */
 396         btdev->commands[25] |= 0x02;    /* LE Read Buffer Size */
 397         btdev->commands[25] |= 0x04;    /* LE Read Local Features */
 398         btdev->commands[25] |= 0x20;    /* LE Set Adv Parameters */
 399         btdev->commands[25] |= 0x40;    /* LE Read Adv TX Power */
 400         btdev->commands[25] |= 0x80;    /* LE Set Adv Data */
 401         btdev->commands[26] |= 0x02;    /* LE Set Adv Enable */
 402         btdev->commands[26] |= 0x04;    /* LE Set Scan Parameters */
 403         btdev->commands[26] |= 0x08;    /* LE Set Scan Enable */
 404         btdev->commands[26] |= 0x40;    /* LE Read White List Size */
 405         btdev->commands[27] |= 0x80;    /* LE Rand */
 406         btdev->commands[28] |= 0x08;    /* LE Read Supported States */
 407         btdev->commands[28] |= 0x10;    /* LE Receiver Test */
 408         btdev->commands[28] |= 0x20;    /* LE Transmitter Test */
 409         btdev->commands[28] |= 0x40;    /* LE Test End */
 410 }
 411
 412 static void set_bredrle_commands(struct btdev *btdev)
 413 {
 414         set_bredr_commands(btdev);
 415         set_le_commands(btdev);
 416
 417         /* Extra BR/EDR commands we want to only support for >= 4.0
 418          * adapters.
 419          */
 420         btdev->commands[22] |= 0x04;    /* Set Event Mask Page 2 */
 421         btdev->commands[31] |= 0x80;    /* Read Sync Train Parameters */
 422         btdev->commands[32] |= 0x04;    /* Read Secure Connections Support */
 423         btdev->commands[32] |= 0x08;    /* Write Secure Connections Support */
 424         btdev->commands[32] |= 0x10;    /* Read Auth Payload Timeout */
 425         btdev->commands[32] |= 0x20;    /* Write Auth Payload Timeout */
 426         btdev->commands[32] |= 0x40;    /* Read Local OOB Extended Data */
 427 }
 428
 429 static void set_amp_commands(struct btdev *btdev)
 430 {
 431         set_common_commands_all(btdev);
 432
 433         btdev->commands[22] |= 0x20;    /* Read Local AMP Info */
 434 }
 435
 436 static void set_bredrle_features(struct btdev *btdev)
 437 {
 438         btdev->features[0] |= 0x04;     /* Encryption */
 439         btdev->features[0] |= 0x20;     /* Role switch */
 440         btdev->features[0] |= 0x80;     /* Sniff mode */
 441         btdev->features[1] |= 0x08;     /* SCO link */
 442         btdev->features[2] |= 0x08;     /* Transparent SCO */
 443         btdev->features[3] |= 0x40;     /* RSSI with inquiry results */
 444         btdev->features[3] |= 0x80;     /* Extended SCO link */
 445         btdev->features[4] |= 0x08;     /* AFH capable slave */
 446         btdev->features[4] |= 0x10;     /* AFH classification slave */
 447         btdev->features[4] |= 0x40;     /* LE Supported */
 448         btdev->features[5] |= 0x02;     /* Sniff subrating */
 449         btdev->features[5] |= 0x04;     /* Pause encryption */
 450         btdev->features[5] |= 0x08;     /* AFH capable master */
 451         btdev->features[5] |= 0x10;     /* AFH classification master */
 452         btdev->features[6] |= 0x01;     /* Extended Inquiry Response */
 453         btdev->features[6] |= 0x02;     /* Simultaneous LE and BR/EDR */
 454         btdev->features[6] |= 0x08;     /* Secure Simple Pairing */
 455         btdev->features[6] |= 0x10;     /* Encapsulated PDU */
 456         btdev->features[6] |= 0x20;     /* Erroneous Data Reporting */
 457         btdev->features[6] |= 0x40;     /* Non-flushable Packet Boundary Flag */
 458         btdev->features[7] |= 0x01;     /* Link Supervision Timeout Event */
 459         btdev->features[7] |= 0x02;     /* Inquiry TX Power Level */
 460         btdev->features[7] |= 0x80;     /* Extended features */
 461
 462         btdev->feat_page_2[0] |= 0x01;  /* CSB - Master Operation */
 463         btdev->feat_page_2[0] |= 0x02;  /* CSB - Slave Operation */
 464         btdev->feat_page_2[0] |= 0x04;  /* Synchronization Train */
 465         btdev->feat_page_2[0] |= 0x08;  /* Synchronization Scan */
 466         btdev->feat_page_2[0] |= 0x10;  /* Inquiry Response Notification */
 467         btdev->feat_page_2[1] |= 0x01;  /* Secure Connections */
 468         btdev->feat_page_2[1] |= 0x02;  /* Ping */
 469
 470         btdev->max_page = 2;
 471 }
 472
 473 static void set_bredr_features(struct btdev *btdev)
 474 {
 475         btdev->features[0] |= 0x04;     /* Encryption */
 476         btdev->features[0] |= 0x20;     /* Role switch */
 477         btdev->features[0] |= 0x80;     /* Sniff mode */
 478         btdev->features[1] |= 0x08;     /* SCO link */
 479         btdev->features[3] |= 0x40;     /* RSSI with inquiry results */
 480         btdev->features[3] |= 0x80;     /* Extended SCO link */
 481         btdev->features[4] |= 0x08;     /* AFH capable slave */
 482         btdev->features[4] |= 0x10;     /* AFH classification slave */
 483         btdev->features[5] |= 0x02;     /* Sniff subrating */
 484         btdev->features[5] |= 0x04;     /* Pause encryption */
 485         btdev->features[5] |= 0x08;     /* AFH capable master */
 486         btdev->features[5] |= 0x10;     /* AFH classification master */
 487         btdev->features[6] |= 0x01;     /* Extended Inquiry Response */
 488         btdev->features[6] |= 0x08;     /* Secure Simple Pairing */
 489         btdev->features[6] |= 0x10;     /* Encapsulated PDU */
 490         btdev->features[6] |= 0x20;     /* Erroneous Data Reporting */
 491         btdev->features[6] |= 0x40;     /* Non-flushable Packet Boundary Flag */
 492         btdev->features[7] |= 0x01;     /* Link Supervision Timeout Event */
 493         btdev->features[7] |= 0x02;     /* Inquiry TX Power Level */
 494         btdev->features[7] |= 0x80;     /* Extended features */
 495
 496         btdev->max_page = 1;
 497 }
 498
 499 static void set_le_features(struct btdev *btdev)
 500 {
 501         btdev->features[4] |= 0x20;     /* BR/EDR Not Supported */
 502         btdev->features[4] |= 0x40;     /* LE Supported */
 503
 504         btdev->max_page = 1;
 505 }
 506
 507 static void set_amp_features(struct btdev *btdev)
 508 {
 509 }
 510
 511 struct btdev *btdev_create(enum btdev_type type, uint16_t id)
 512 {
 513         struct btdev *btdev;
 514         int index;
 515
 516         btdev = malloc(sizeof(*btdev));
 517         if (!btdev)
 518                 return NULL;
 519
 520         memset(btdev, 0, sizeof(*btdev));
 521         btdev->type = type;
 522
 523         btdev->manufacturer = 63;
 524
 525         if (type == BTDEV_TYPE_BREDR)
 526                 btdev->version = 0x05;
 527         else
 528                 btdev->version = 0x08;
 529
 530         btdev->revision = 0x0000;
 531
 532         switch (btdev->type) {
 533         case BTDEV_TYPE_BREDRLE:
 534                 set_bredrle_features(btdev);
 535                 set_bredrle_commands(btdev);
 536                 break;
 537         case BTDEV_TYPE_BREDR:
 538                 set_bredr_features(btdev);
 539                 set_bredr_commands(btdev);
 540                 break;
 541         case BTDEV_TYPE_LE:
 542                 set_le_features(btdev);
 543                 set_le_commands(btdev);
 544                 break;
 545         case BTDEV_TYPE_AMP:
 546                 set_amp_features(btdev);
 547                 set_amp_commands(btdev);
 548                 break;
 549         }
 550
 551         btdev->page_scan_interval = 0x0800;
 552         btdev->page_scan_window = 0x0012;
 553         btdev->page_scan_type = 0x00;
 554
 555         btdev->sync_train_interval = 0x0080;
 556         btdev->sync_train_timeout = 0x0002ee00;
 557         btdev->sync_train_service_data = 0x00;
 558
 559         btdev->acl_mtu = 192;
 560         btdev->acl_max_pkt = 1;
 561
 562         btdev->country_code = 0x00;
 563
 564         index = add_btdev(btdev);
 565         if (index < 0) {
 566                 free(btdev);
 567                 return NULL;
 568         }
 569
 570         get_bdaddr(id, index, btdev->bdaddr);
 571
 572         return btdev;
 573 }
 574
 575 void btdev_destroy(struct btdev *btdev)
 576 {
 577         if (!btdev)
 578                 return;
 579
 580         if (btdev->inquiry_id > 0)
 581                 timeout_remove(btdev->inquiry_id);
 582
 583         del_btdev(btdev);
 584
 585         free(btdev);
 586 }
 587
 588 const uint8_t *btdev_get_bdaddr(struct btdev *btdev)
 589 {
 590         return btdev->bdaddr;
 591 }
 592
 593 uint8_t *btdev_get_features(struct btdev *btdev)
 594 {
 595         return btdev->features;
 596 }
 597
 598 static bool use_ssp(struct btdev *btdev1, struct btdev *btdev2)
 599 {
 600         if (btdev1->auth_enable || btdev2->auth_enable)
 601                 return false;
 602
 603         return (btdev1->simple_pairing_mode && btdev2->simple_pairing_mode);
 604 }
 605
 606 void btdev_set_command_handler(struct btdev *btdev, btdev_command_func handler,
 607                                                         void *user_data)
 608 {
 609         if (!btdev)
 610                 return;
 611
 612         btdev->command_handler = handler;
 613         btdev->command_data = user_data;
 614 }
 615
 616 void btdev_set_send_handler(struct btdev *btdev, btdev_send_func handler,
 617                                                         void *user_data)
 618 {
 619         if (!btdev)
 620                 return;
 621
 622         btdev->send_handler = handler;
 623         btdev->send_data = user_data;
 624 }
 625
 626 static void send_packet(struct btdev *btdev, const struct iovec *iov,
 627                                                                 int iovlen)
 628 {
 629         if (!btdev->send_handler)
 630                 return;
 631
 632         btdev->send_handler(iov, iovlen, btdev->send_data);
 633 }
 634
 635 static void send_event(struct btdev *btdev, uint8_t event,
 636                                                 const void *data, uint8_t len)
 637 {
 638         struct bt_hci_evt_hdr hdr;
 639         struct iovec iov[3];
 640         uint8_t pkt = BT_H4_EVT_PKT;
 641
 642         iov[0].iov_base = &pkt;
 643         iov[0].iov_len = sizeof(pkt);
 644
 645         hdr.evt = event;
 646         hdr.plen = len;
 647
 648         iov[1].iov_base = &hdr;
 649         iov[1].iov_len = sizeof(hdr);
 650
 651         if (len > 0) {
 652                 iov[2].iov_base = (void *) data;
 653                 iov[2].iov_len = len;
 654         }
 655
 656         if (run_hooks(btdev, BTDEV_HOOK_POST_EVT, event, data, len))
 657                 send_packet(btdev, iov, len > 0 ? 3 : 2);
 658 }
 659
 660 static void send_cmd(struct btdev *btdev, uint8_t evt, uint16_t opcode,
 661                                         const struct iovec *iov, int iovlen)
 662 {
 663         struct bt_hci_evt_hdr hdr;
 664         struct iovec iov2[2 + iovlen];
 665         uint8_t pkt = BT_H4_EVT_PKT;
 666         int i;
 667
 668         iov2[0].iov_base = &pkt;
 669         iov2[0].iov_len = sizeof(pkt);
 670
 671         hdr.evt = evt;
 672         hdr.plen = 0;
 673
 674         iov2[1].iov_base = &hdr;
 675         iov2[1].iov_len = sizeof(hdr);
 676
 677         for (i = 0; i < iovlen; i++) {
 678                 hdr.plen += iov[i].iov_len;
 679                 iov2[2 + i].iov_base = iov[i].iov_base;
 680                 iov2[2 + i].iov_len = iov[i].iov_len;
 681         }
 682
 683         if (run_hooks(btdev, BTDEV_HOOK_POST_CMD, opcode, iov[i -1].iov_base,
 684                                                         iov[i -1].iov_len))
 685                 send_packet(btdev, iov2, 2 + iovlen);
 686 }
 687
 688 static void cmd_complete(struct btdev *btdev, uint16_t opcode,
 689                                                 const void *data, uint8_t len)
 690 {
 691         struct bt_hci_evt_cmd_complete cc;
 692         struct iovec iov[2];
 693
 694         cc.ncmd = 0x01;
 695         cc.opcode = cpu_to_le16(opcode);
 696
 697         iov[0].iov_base = &cc;
 698         iov[0].iov_len = sizeof(cc);
 699
 700         iov[1].iov_base = (void *) data;
 701         iov[1].iov_len = len;
 702
 703         send_cmd(btdev, BT_HCI_EVT_CMD_COMPLETE, opcode, iov, 2);
 704 }
 705
 706 static void cmd_status(struct btdev *btdev, uint8_t status, uint16_t opcode)
 707 {
 708         struct bt_hci_evt_cmd_status cs;
 709         struct iovec iov;
 710
 711         cs.status = status;
 712         cs.ncmd = 0x01;
 713         cs.opcode = cpu_to_le16(opcode);
 714
 715         iov.iov_base = &cs;
 716         iov.iov_len = sizeof(cs);
 717
 718         send_cmd(btdev, BT_HCI_EVT_CMD_STATUS, opcode, &iov, 1);
 719 }
 720
 721 static void num_completed_packets(struct btdev *btdev)
 722 {
 723         if (btdev->conn) {
 724                 struct bt_hci_evt_num_completed_packets ncp;
 725
 726                 ncp.num_handles = 1;
 727                 ncp.handle = cpu_to_le16(42);
 728                 ncp.count = cpu_to_le16(1);
 729
 730                 send_event(btdev, BT_HCI_EVT_NUM_COMPLETED_PACKETS,
 731                                                         &ncp, sizeof(ncp));
 732         }
 733 }
 734
 735 static bool inquiry_callback(void *user_data)
 736 {
 737         struct inquiry_data *data = user_data;
 738         struct btdev *btdev = data->btdev;
 739         struct bt_hci_evt_inquiry_complete ic;
 740         int sent = data->sent_count;
 741         int i;
 742
 743         /*Report devices only once and wait for inquiry timeout*/
 744         if (data->iter == MAX_BTDEV_ENTRIES)
 745                 return true;
 746
 747         for (i = data->iter; i < MAX_BTDEV_ENTRIES; i++) {
 748                 /*Lets sent 10 inquiry results at once */
 749                 if (sent + 10 == data->sent_count)
 750                         break;
 751
 752                 if (!btdev_list[i] || btdev_list[i] == btdev)
 753                         continue;
 754
 755                 if (!(btdev_list[i]->scan_enable & 0x02))
 756                         continue;
 757
 758                 if (btdev->inquiry_mode == 0x02 &&
 759                                         btdev_list[i]->ext_inquiry_rsp[0]) {
 760                         struct bt_hci_evt_ext_inquiry_result ir;
 761
 762                         ir.num_resp = 0x01;
 763                         memcpy(ir.bdaddr, btdev_list[i]->bdaddr, 6);
 764                         ir.pscan_rep_mode = 0x00;
 765                         ir.pscan_period_mode = 0x00;
 766                         memcpy(ir.dev_class, btdev_list[i]->dev_class, 3);
 767                         ir.clock_offset = 0x0000;
 768                         ir.rssi = -60;
 769                         memcpy(ir.data, btdev_list[i]->ext_inquiry_rsp, 240);
 770
 771                         send_event(btdev, BT_HCI_EVT_EXT_INQUIRY_RESULT,
 772                                                         &ir, sizeof(ir));
 773                         data->sent_count++;
 774                         continue;
 775                 }
 776
 777                 if (btdev->inquiry_mode > 0x00) {
 778                         struct bt_hci_evt_inquiry_result_with_rssi ir;
 779
 780                         ir.num_resp = 0x01;
 781                         memcpy(ir.bdaddr, btdev_list[i]->bdaddr, 6);
 782                         ir.pscan_rep_mode = 0x00;
 783                         ir.pscan_period_mode = 0x00;
 784                         memcpy(ir.dev_class, btdev_list[i]->dev_class, 3);
 785                         ir.clock_offset = 0x0000;
 786                         ir.rssi = -60;
 787
 788                         send_event(btdev, BT_HCI_EVT_INQUIRY_RESULT_WITH_RSSI,
 789                                                         &ir, sizeof(ir));
 790                         data->sent_count++;
 791                 } else {
 792                         struct bt_hci_evt_inquiry_result ir;
 793
 794                         ir.num_resp = 0x01;
 795                         memcpy(ir.bdaddr, btdev_list[i]->bdaddr, 6);
 796                         ir.pscan_rep_mode = 0x00;
 797                         ir.pscan_period_mode = 0x00;
 798                         ir.pscan_mode = 0x00;
 799                         memcpy(ir.dev_class, btdev_list[i]->dev_class, 3);
 800                         ir.clock_offset = 0x0000;
 801
 802                         send_event(btdev, BT_HCI_EVT_INQUIRY_RESULT,
 803                                                         &ir, sizeof(ir));
 804                         data->sent_count++;
 805                 }
 806         }
 807         data->iter = i;
 808
 809         /* Check if we sent already required amount of responses*/
 810         if (data->num_resp && data->sent_count == data->num_resp)
 811                 goto finish;
 812
 813         return true;
 814
 815 finish:
 816         /* Note that destroy will be called */
 817         ic.status = BT_HCI_ERR_SUCCESS;
 818         send_event(btdev, BT_HCI_EVT_INQUIRY_COMPLETE, &ic, sizeof(ic));
 819
 820         return false;
 821 }
 822
 823 static void inquiry_destroy(void *user_data)
 824 {
 825         struct inquiry_data *data = user_data;
 826         struct btdev *btdev = data->btdev;
 827
 828         if (!btdev)
 829                 goto finish;
 830
 831         btdev->inquiry_id = 0;
 832
 833         if (btdev->inquiry_timeout_id > 0) {
 834                 timeout_remove(btdev->inquiry_timeout_id);
 835                 btdev->inquiry_timeout_id = 0;
 836         }
 837
 838 finish:
 839         free(data);
 840 }
 841
 842 static bool inquiry_timeout(void *user_data)
 843 {
 844         struct inquiry_data *data = user_data;
 845         struct btdev *btdev = data->btdev;
 846         struct bt_hci_evt_inquiry_complete ic;
 847
 848         timeout_remove(btdev->inquiry_id);
 849         btdev->inquiry_timeout_id = 0;
 850
 851         /* Inquiry is stopped, send Inquiry complete event. */
 852         ic.status = BT_HCI_ERR_SUCCESS;
 853         send_event(btdev, BT_HCI_EVT_INQUIRY_COMPLETE, &ic, sizeof(ic));
 854
 855         return false;
 856 }
 857
 858 static void inquiry_cmd(struct btdev *btdev, const void *cmd)
 859 {
 860         const struct bt_hci_cmd_inquiry *inq_cmd = cmd;
 861         struct inquiry_data *data;
 862         struct bt_hci_evt_inquiry_complete ic;
 863         int status = BT_HCI_ERR_HARDWARE_FAILURE;
 864         unsigned int inquiry_len_ms;
 865
 866         if (btdev->inquiry_id > 0) {
 867                 status = BT_HCI_ERR_COMMAND_DISALLOWED;
 868                 goto failed;
 869         }
 870
 871         data = malloc(sizeof(*data));
 872         if (!data)
 873                 goto failed;
 874
 875         memset(data, 0, sizeof(*data));
 876         data->btdev = btdev;
 877         data->num_resp = inq_cmd->num_resp;
 878
 879         /* Add timeout to cancel inquiry */
 880         inquiry_len_ms = 1280 * inq_cmd->length;
 881         if (inquiry_len_ms)
 882                 btdev->inquiry_timeout_id = timeout_add(inquiry_len_ms,
 883                                                         inquiry_timeout,
 884                                                         data, NULL);
 885
 886         btdev->inquiry_id = timeout_add(DEFAULT_INQUIRY_INTERVAL,
 887                                                         inquiry_callback, data,
 888                                                         inquiry_destroy);
 889         /* Return if success */
 890         if (btdev->inquiry_id > 0)
 891                 return;
 892
 893 failed:
 894         ic.status = status;
 895         send_event(btdev, BT_HCI_EVT_INQUIRY_COMPLETE, &ic, sizeof(ic));
 896 }
 897
 898 static void inquiry_cancel(struct btdev *btdev)
 899 {
 900         uint8_t status;
 901
 902         if (!btdev->inquiry_id) {
 903                 status = BT_HCI_ERR_COMMAND_DISALLOWED;
 904                 cmd_complete(btdev, BT_HCI_CMD_INQUIRY_CANCEL, &status,
 905                                                         sizeof(status));
 906                 return;
 907         }
 908
 909         timeout_remove(btdev->inquiry_timeout_id);
 910         btdev->inquiry_timeout_id = 0;
 911         timeout_remove(btdev->inquiry_id);
 912         btdev->inquiry_id = 0;
 913
 914         status = BT_HCI_ERR_SUCCESS;
 915         cmd_complete(btdev, BT_HCI_CMD_INQUIRY_CANCEL, &status,
 916                                                         sizeof(status));
 917 }
 918
 919 static void conn_complete(struct btdev *btdev,
 920                                         const uint8_t *bdaddr, uint8_t status)
 921 {
 922         struct bt_hci_evt_conn_complete cc;
 923
 924         if (!status) {
 925                 struct btdev *remote = find_btdev_by_bdaddr(bdaddr);
 926
 927                 btdev->conn = remote;
 928                 remote->conn = btdev;
 929
 930                 cc.status = status;
 931                 memcpy(cc.bdaddr, btdev->bdaddr, 6);
 932                 cc.encr_mode = 0x00;
 933
 934                 cc.handle = cpu_to_le16(42);
 935                 cc.link_type = 0x01;
 936
 937                 send_event(remote, BT_HCI_EVT_CONN_COMPLETE, &cc, sizeof(cc));
 938
 939                 cc.handle = cpu_to_le16(42);
 940                 cc.link_type = 0x01;
 941         } else {
 942                 cc.handle = cpu_to_le16(0x0000);
 943                 cc.link_type = 0x01;
 944         }
 945
 946         cc.status = status;
 947         memcpy(cc.bdaddr, bdaddr, 6);
 948         cc.encr_mode = 0x00;
 949
 950         send_event(btdev, BT_HCI_EVT_CONN_COMPLETE, &cc, sizeof(cc));
 951 }
 952
 953 static void accept_conn_request_complete(struct btdev *btdev,
 954                                                         const uint8_t *bdaddr)
 955 {
 956         struct btdev *remote = find_btdev_by_bdaddr(bdaddr);
 957
 958         if (!remote)
 959                 return;
 960
 961         if (btdev->auth_enable || remote->auth_enable)
 962                 send_event(remote, BT_HCI_EVT_LINK_KEY_REQUEST,
 963                                                         btdev->bdaddr, 6);
 964         else
 965                 conn_complete(btdev, bdaddr, BT_HCI_ERR_SUCCESS);
 966 }
 967
 968 static void sync_conn_complete(struct btdev *btdev, uint16_t voice_setting,
 969                                                                 uint8_t status)
 970 {
 971         struct bt_hci_evt_sync_conn_complete cc;
 972
 973         if (!btdev->conn)
 974                 return;
 975
 976         cc.status = status;
 977         memcpy(cc.bdaddr, btdev->conn->bdaddr, 6);
 978
 979         cc.handle = cpu_to_le16(status == BT_HCI_ERR_SUCCESS ? 257 : 0);
 980         cc.link_type = 0x02;
 981         cc.tx_interval = 0x000c;
 982         cc.retrans_window = 0x06;
 983         cc.rx_pkt_len = 60;
 984         cc.tx_pkt_len = 60;
 985         cc.air_mode = (voice_setting == 0x0060) ? 0x02 : 0x03;
 986
 987         send_event(btdev, BT_HCI_EVT_SYNC_CONN_COMPLETE, &cc, sizeof(cc));
 988 }
 989
 990 static void sco_conn_complete(struct btdev *btdev, uint8_t status)
 991 {
 992         struct bt_hci_evt_conn_complete cc;
 993
 994         if (!btdev->conn)
 995                 return;
 996
 997         cc.status = status;
 998         memcpy(cc.bdaddr, btdev->conn->bdaddr, 6);
 999         cc.handle = cpu_to_le16(status == BT_HCI_ERR_SUCCESS ? 257 : 0);
1000         cc.link_type = 0x00;
1001         cc.encr_mode = 0x00;
1002
1003         send_event(btdev, BT_HCI_EVT_CONN_COMPLETE, &cc, sizeof(cc));
1004 }
1005
1006 static void le_conn_complete(struct btdev *btdev,
1007                                         const uint8_t *bdaddr, uint8_t bdaddr_type,
1008                                         uint8_t status)
1009 {
1010         char buf[1 + sizeof(struct bt_hci_evt_le_conn_complete)];
1011         struct bt_hci_evt_le_conn_complete *cc = (void *) &buf[1];
1012
1013         memset(buf, 0, sizeof(buf));
1014
1015         buf[0] = BT_HCI_EVT_LE_CONN_COMPLETE;
1016
1017         if (!status) {
1018                 struct btdev *remote = find_btdev_by_bdaddr_type(bdaddr,
1019                                                                 bdaddr_type);
1020
1021                 btdev->conn = remote;
1022                 btdev->le_adv_enable = 0;
1023                 remote->conn = btdev;
1024                 remote->le_adv_enable = 0;
1025
1026                 cc->status = status;
1027                 cc->peer_addr_type = btdev->le_scan_own_addr_type;
1028                 if (cc->peer_addr_type == 0x01)
1029                         memcpy(cc->peer_addr, btdev->random_addr, 6);
1030                 else
1031                         memcpy(cc->peer_addr, btdev->bdaddr, 6);
1032
1033                 cc->role = 0x01;
1034                 cc->handle = cpu_to_le16(42);
1035
1036                 send_event(remote, BT_HCI_EVT_LE_META_EVENT, buf, sizeof(buf));
1037
1038                 cc->handle = cpu_to_le16(42);
1039         }
1040
1041         cc->status = status;
1042         cc->peer_addr_type = bdaddr_type;
1043         memcpy(cc->peer_addr, bdaddr, 6);
1044         cc->role = 0x00;
1045
1046         send_event(btdev, BT_HCI_EVT_LE_META_EVENT, buf, sizeof(buf));
1047 }
1048
1049 static const uint8_t *scan_addr(const struct btdev *btdev)
1050 {
1051         if (btdev->le_scan_own_addr_type == 0x01)
1052                 return btdev->random_addr;
1053
1054         return btdev->bdaddr;
1055 }
1056
1057 static const uint8_t *adv_addr(const struct btdev *btdev)
1058 {
1059         if (btdev->le_adv_own_addr == 0x01)
1060                 return btdev->random_addr;
1061
1062         return btdev->bdaddr;
1063 }
1064
1065 static bool adv_match(struct btdev *scan, struct btdev *adv)
1066 {
1067         /* Match everything if this is not directed advertising */
1068         if (adv->le_adv_type != 0x01 && adv->le_adv_type != 0x04)
1069                 return true;
1070
1071         if (scan->le_scan_own_addr_type != adv->le_adv_direct_addr_type)
1072                 return false;
1073
1074         return !memcmp(scan_addr(scan), adv->le_adv_direct_addr, 6);
1075 }
1076
1077 static bool adv_connectable(struct btdev *btdev)
1078 {
1079         if (!btdev->le_adv_enable)
1080                 return false;
1081
1082         return btdev->le_adv_type != 0x03;
1083 }
1084
1085 static void le_conn_request(struct btdev *btdev, const uint8_t *bdaddr,
1086                                                         uint8_t bdaddr_type)
1087 {
1088         struct btdev *remote = find_btdev_by_bdaddr_type(bdaddr, bdaddr_type);
1089
1090         if (remote && adv_connectable(remote) && adv_match(btdev, remote) &&
1091                                         remote->le_adv_own_addr == bdaddr_type)
1092                 le_conn_complete(btdev, bdaddr, bdaddr_type, 0);
1093         else
1094                 le_conn_complete(btdev, bdaddr, bdaddr_type,
1095                                         BT_HCI_ERR_CONN_FAILED_TO_ESTABLISH);
1096 }
1097
1098 static void conn_request(struct btdev *btdev, const uint8_t *bdaddr)
1099 {
1100         struct btdev *remote = find_btdev_by_bdaddr(bdaddr);
1101
1102         if (remote && remote->scan_enable & 0x02) {
1103                 struct bt_hci_evt_conn_request cr;
1104
1105                 memcpy(cr.bdaddr, btdev->bdaddr, 6);
1106                 memcpy(cr.dev_class, btdev->dev_class, 3);
1107                 cr.link_type = 0x01;
1108
1109                 send_event(remote, BT_HCI_EVT_CONN_REQUEST, &cr, sizeof(cr));
1110         } else {
1111                 conn_complete(btdev, bdaddr, BT_HCI_ERR_PAGE_TIMEOUT);
1112         }
1113 }
1114
1115 static void le_conn_update(struct btdev *btdev, uint16_t handle,
1116                                 uint16_t max_interval, uint16_t min_interval,
1117                                 uint16_t latency, uint16_t supv_timeout,
1118                                 uint16_t min_length, uint16_t max_length)
1119 {
1120         struct btdev *remote = btdev->conn;
1121         struct __packed {
1122                 uint8_t subevent;
1123                 struct bt_hci_evt_le_conn_update_complete ev;
1124         } ev;
1125
1126         ev.subevent = BT_HCI_EVT_LE_CONN_UPDATE_COMPLETE;
1127         ev.ev.handle = cpu_to_le16(handle);
1128         ev.ev.interval = cpu_to_le16(min_interval);
1129         ev.ev.latency = cpu_to_le16(latency);
1130         ev.ev.supv_timeout = cpu_to_le16(supv_timeout);
1131
1132         if (remote)
1133                 ev.ev.status = BT_HCI_ERR_SUCCESS;
1134         else
1135                 ev.ev.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1136
1137         send_event(btdev, BT_HCI_EVT_LE_META_EVENT, &ev, sizeof(ev));
1138
1139         if (remote)
1140                 send_event(remote, BT_HCI_EVT_LE_META_EVENT, &ev, sizeof(ev));
1141 }
1142
1143 static void disconnect_complete(struct btdev *btdev, uint16_t handle,
1144                                                         uint8_t reason)
1145 {
1146         struct bt_hci_evt_disconnect_complete dc;
1147         struct btdev *remote = btdev->conn;
1148
1149         if (!remote) {
1150                 dc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1151                 dc.handle = cpu_to_le16(handle);
1152                 dc.reason = 0x00;
1153
1154                 send_event(btdev, BT_HCI_EVT_DISCONNECT_COMPLETE,
1155                                                         &dc, sizeof(dc));
1156                 return;
1157         }
1158
1159         dc.status = BT_HCI_ERR_SUCCESS;
1160         dc.handle = cpu_to_le16(handle);
1161         dc.reason = reason;
1162
1163         btdev->conn = NULL;
1164         remote->conn = NULL;
1165
1166         send_event(btdev, BT_HCI_EVT_DISCONNECT_COMPLETE, &dc, sizeof(dc));
1167         send_event(remote, BT_HCI_EVT_DISCONNECT_COMPLETE, &dc, sizeof(dc));
1168 }
1169
1170 static void link_key_req_reply_complete(struct btdev *btdev,
1171                                         const uint8_t *bdaddr,
1172                                         const uint8_t *link_key)
1173 {
1174         struct btdev *remote = btdev->conn;
1175         struct bt_hci_evt_auth_complete ev;
1176
1177         memcpy(btdev->link_key, link_key, 16);
1178
1179         if (!remote) {
1180                 remote = find_btdev_by_bdaddr(bdaddr);
1181                 if (!remote)
1182                         return;
1183         }
1184
1185         if (!memcmp(remote->link_key, LINK_KEY_NONE, 16)) {
1186                 send_event(remote, BT_HCI_EVT_LINK_KEY_REQUEST,
1187                                                         btdev->bdaddr, 6);
1188                 return;
1189         }
1190
1191         ev.handle = cpu_to_le16(42);
1192
1193         if (!memcmp(btdev->link_key, remote->link_key, 16))
1194                 ev.status = BT_HCI_ERR_SUCCESS;
1195         else
1196                 ev.status = BT_HCI_ERR_AUTH_FAILURE;
1197
1198         send_event(btdev, BT_HCI_EVT_AUTH_COMPLETE, &ev, sizeof(ev));
1199         send_event(remote, BT_HCI_EVT_AUTH_COMPLETE, &ev, sizeof(ev));
1200 }
1201
1202 static void link_key_req_neg_reply_complete(struct btdev *btdev,
1203                                                         const uint8_t *bdaddr)
1204 {
1205         struct btdev *remote = btdev->conn;
1206
1207         if (!remote) {
1208                 remote = find_btdev_by_bdaddr(bdaddr);
1209                 if (!remote)
1210                         return;
1211         }
1212
1213         if (use_ssp(btdev, remote)) {
1214                 struct bt_hci_evt_io_capability_request io_req;
1215
1216                 memcpy(io_req.bdaddr, bdaddr, 6);
1217                 send_event(btdev, BT_HCI_EVT_IO_CAPABILITY_REQUEST, &io_req,
1218                                                         sizeof(io_req));
1219         } else {
1220                 struct bt_hci_evt_pin_code_request pin_req;
1221
1222                 memcpy(pin_req.bdaddr, bdaddr, 6);
1223                 send_event(btdev, BT_HCI_EVT_PIN_CODE_REQUEST, &pin_req,
1224                                                         sizeof(pin_req));
1225         }
1226 }
1227
1228 static uint8_t get_link_key_type(struct btdev *btdev)
1229 {
1230         struct btdev *remote = btdev->conn;
1231         uint8_t auth, unauth;
1232
1233         if (!remote)
1234                 return 0x00;
1235
1236         if (!btdev->simple_pairing_mode)
1237                 return 0x00;
1238
1239         if (btdev->ssp_debug_mode || remote->ssp_debug_mode)
1240                 return 0x03;
1241
1242         if (btdev->secure_conn_support && remote->secure_conn_support) {
1243                 unauth = 0x07;
1244                 auth = 0x08;
1245         } else {
1246                 unauth = 0x04;
1247                 auth = 0x05;
1248         }
1249
1250         if (btdev->io_cap == 0x03 || remote->io_cap == 0x03)
1251                 return unauth;
1252
1253         if (!(btdev->auth_req & 0x01) && !(remote->auth_req & 0x01))
1254                 return unauth;
1255
1256         /* DisplayOnly only produces authenticated with KeyboardOnly */
1257         if (btdev->io_cap == 0x00 && remote->io_cap != 0x02)
1258                 return unauth;
1259
1260         /* DisplayOnly only produces authenticated with KeyboardOnly */
1261         if (remote->io_cap == 0x00 && btdev->io_cap != 0x02)
1262                 return unauth;
1263
1264         return auth;
1265 }
1266
1267 static void link_key_notify(struct btdev *btdev, const uint8_t *bdaddr,
1268                                                         const uint8_t *key)
1269 {
1270         struct bt_hci_evt_link_key_notify ev;
1271
1272         memcpy(btdev->link_key, key, 16);
1273
1274         memcpy(ev.bdaddr, bdaddr, 6);
1275         memcpy(ev.link_key, key, 16);
1276         ev.key_type = get_link_key_type(btdev);
1277
1278         send_event(btdev, BT_HCI_EVT_LINK_KEY_NOTIFY, &ev, sizeof(ev));
1279 }
1280
1281 static void encrypt_change(struct btdev *btdev, uint8_t mode, uint8_t status)
1282 {
1283         struct bt_hci_evt_encrypt_change ev;
1284
1285         ev.status = status;
1286         ev.handle = cpu_to_le16(42);
1287         ev.encr_mode = mode;
1288
1289         send_event(btdev, BT_HCI_EVT_ENCRYPT_CHANGE, &ev, sizeof(ev));
1290 }
1291
1292 static void pin_code_req_reply_complete(struct btdev *btdev,
1293                                         const uint8_t *bdaddr, uint8_t pin_len,
1294                                         const uint8_t *pin_code)
1295 {
1296         struct bt_hci_evt_auth_complete ev;
1297         struct btdev *remote = btdev->conn;
1298
1299         if (!remote) {
1300                 remote = find_btdev_by_bdaddr(bdaddr);
1301                 if (!remote)
1302                         return;
1303         }
1304
1305         memcpy(btdev->pin, pin_code, pin_len);
1306         btdev->pin_len = pin_len;
1307
1308         if (!remote->pin_len) {
1309                 struct bt_hci_evt_pin_code_request pin_req;
1310
1311                 memcpy(pin_req.bdaddr, btdev->bdaddr, 6);
1312                 send_event(remote, BT_HCI_EVT_PIN_CODE_REQUEST, &pin_req,
1313                                                         sizeof(pin_req));
1314                 return;
1315         }
1316
1317         if (btdev->pin_len == remote->pin_len &&
1318                         !memcmp(btdev->pin, remote->pin, btdev->pin_len)) {
1319                 link_key_notify(btdev, remote->bdaddr, LINK_KEY_DUMMY);
1320                 link_key_notify(remote, btdev->bdaddr, LINK_KEY_DUMMY);
1321                 ev.status = BT_HCI_ERR_SUCCESS;
1322         } else {
1323                 ev.status = BT_HCI_ERR_AUTH_FAILURE;
1324         }
1325
1326         if (remote->conn) {
1327                 ev.handle = cpu_to_le16(42);
1328                 send_event(remote, BT_HCI_EVT_AUTH_COMPLETE, &ev, sizeof(ev));
1329         } else {
1330                 conn_complete(remote, btdev->bdaddr, ev.status);
1331         }
1332
1333         btdev->pin_len = 0;
1334         remote->pin_len = 0;
1335 }
1336
1337 static void pin_code_req_neg_reply_complete(struct btdev *btdev,
1338                                                         const uint8_t *bdaddr)
1339 {
1340         struct bt_hci_evt_auth_complete ev;
1341         struct btdev *remote = btdev->conn;
1342
1343         if (!remote) {
1344                 remote = find_btdev_by_bdaddr(bdaddr);
1345                 if (!remote)
1346                         return;
1347         }
1348
1349         ev.status = BT_HCI_ERR_PIN_OR_KEY_MISSING;
1350         ev.handle = cpu_to_le16(42);
1351
1352         if (btdev->conn)
1353                 send_event(btdev, BT_HCI_EVT_AUTH_COMPLETE, &ev, sizeof(ev));
1354         else
1355                 conn_complete(btdev, bdaddr, BT_HCI_ERR_PIN_OR_KEY_MISSING);
1356
1357         if (remote->conn) {
1358                 if (remote->pin_len)
1359                         send_event(remote, BT_HCI_EVT_AUTH_COMPLETE, &ev,
1360                                                                 sizeof(ev));
1361         } else {
1362                 conn_complete(remote, btdev->bdaddr,
1363                                         BT_HCI_ERR_PIN_OR_KEY_MISSING);
1364         }
1365 }
1366
1367 static void auth_request_complete(struct btdev *btdev, uint16_t handle)
1368 {
1369         struct btdev *remote = btdev->conn;
1370
1371         if (!remote) {
1372                 struct bt_hci_evt_auth_complete ev;
1373
1374                 ev.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1375                 ev.handle = cpu_to_le16(handle);
1376
1377                 send_event(btdev, BT_HCI_EVT_AUTH_COMPLETE, &ev, sizeof(ev));
1378
1379                 return;
1380         }
1381
1382         btdev->auth_init = true;
1383
1384         send_event(btdev, BT_HCI_EVT_LINK_KEY_REQUEST, remote->bdaddr, 6);
1385 }
1386
1387 static void name_request_complete(struct btdev *btdev,
1388                                         const uint8_t *bdaddr, uint8_t status)
1389 {
1390         struct bt_hci_evt_remote_name_request_complete nc;
1391
1392         nc.status = status;
1393         memcpy(nc.bdaddr, bdaddr, 6);
1394         memset(nc.name, 0, 248);
1395
1396         if (!status) {
1397                 struct btdev *remote = find_btdev_by_bdaddr(bdaddr);
1398
1399                 if (remote)
1400                         memcpy(nc.name, remote->name, 248);
1401                 else
1402                         nc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1403         }
1404
1405         send_event(btdev, BT_HCI_EVT_REMOTE_NAME_REQUEST_COMPLETE,
1406                                                         &nc, sizeof(nc));
1407 }
1408
1409 static void remote_features_complete(struct btdev *btdev, uint16_t handle)
1410 {
1411         struct bt_hci_evt_remote_features_complete rfc;
1412
1413         if (btdev->conn) {
1414                 rfc.status = BT_HCI_ERR_SUCCESS;
1415                 rfc.handle = cpu_to_le16(handle);
1416                 memcpy(rfc.features, btdev->conn->features, 8);
1417         } else {
1418                 rfc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1419                 rfc.handle = cpu_to_le16(handle);
1420                 memset(rfc.features, 0, 8);
1421         }
1422
1423         send_event(btdev, BT_HCI_EVT_REMOTE_FEATURES_COMPLETE,
1424                                                         &rfc, sizeof(rfc));
1425 }
1426
1427 static void btdev_get_host_features(struct btdev *btdev, uint8_t features[8])
1428 {
1429         memset(features, 0, 8);
1430         if (btdev->simple_pairing_mode)
1431                 features[0] |= 0x01;
1432         if (btdev->le_supported)
1433                 features[0] |= 0x02;
1434         if (btdev->le_simultaneous)
1435                 features[0] |= 0x04;
1436         if (btdev->secure_conn_support)
1437                 features[0] |= 0x08;
1438 }
1439
1440 static void remote_ext_features_complete(struct btdev *btdev, uint16_t handle,
1441                                                                 uint8_t page)
1442 {
1443         struct bt_hci_evt_remote_ext_features_complete refc;
1444
1445         if (btdev->conn && page < 0x02) {
1446                 refc.handle = cpu_to_le16(handle);
1447                 refc.page = page;
1448                 refc.max_page = 0x01;
1449
1450                 switch (page) {
1451                 case 0x00:
1452                         refc.status = BT_HCI_ERR_SUCCESS;
1453                         memcpy(refc.features, btdev->conn->features, 8);
1454                         break;
1455                 case 0x01:
1456                         refc.status = BT_HCI_ERR_SUCCESS;
1457                         btdev_get_host_features(btdev->conn, refc.features);
1458                         break;
1459                 default:
1460                         refc.status = BT_HCI_ERR_INVALID_PARAMETERS;
1461                         memset(refc.features, 0, 8);
1462                         break;
1463                 }
1464         } else {
1465                 refc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1466                 refc.handle = cpu_to_le16(handle);
1467                 refc.page = page;
1468                 refc.max_page = 0x01;
1469                 memset(refc.features, 0, 8);
1470         }
1471
1472         send_event(btdev, BT_HCI_EVT_REMOTE_EXT_FEATURES_COMPLETE,
1473                                                         &refc, sizeof(refc));
1474 }
1475
1476 static void remote_version_complete(struct btdev *btdev, uint16_t handle)
1477 {
1478         struct bt_hci_evt_remote_version_complete rvc;
1479
1480         if (btdev->conn) {
1481                 rvc.status = BT_HCI_ERR_SUCCESS;
1482                 rvc.handle = cpu_to_le16(handle);
1483                 rvc.lmp_ver = btdev->conn->version;
1484                 rvc.manufacturer = cpu_to_le16(btdev->conn->manufacturer);
1485                 rvc.lmp_subver = cpu_to_le16(btdev->conn->revision);
1486         } else {
1487                 rvc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1488                 rvc.handle = cpu_to_le16(handle);
1489                 rvc.lmp_ver = 0x00;
1490                 rvc.manufacturer = cpu_to_le16(0);
1491                 rvc.lmp_subver = cpu_to_le16(0);
1492         }
1493
1494         send_event(btdev, BT_HCI_EVT_REMOTE_VERSION_COMPLETE,
1495                                                         &rvc, sizeof(rvc));
1496 }
1497
1498 static void remote_clock_offset_complete(struct btdev *btdev, uint16_t handle)
1499 {
1500         struct bt_hci_evt_clock_offset_complete coc;
1501
1502         if (btdev->conn) {
1503                 coc.status = BT_HCI_ERR_SUCCESS;
1504                 coc.handle = cpu_to_le16(handle);
1505                 coc.clock_offset = 0;
1506         } else {
1507                 coc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1508                 coc.handle = cpu_to_le16(handle);
1509                 coc.clock_offset = 0;
1510         }
1511
1512         send_event(btdev, BT_HCI_EVT_CLOCK_OFFSET_COMPLETE,
1513                                                         &coc, sizeof(coc));
1514 }
1515
1516 static void io_cap_req_reply_complete(struct btdev *btdev,
1517                                         const uint8_t *bdaddr,
1518                                         uint8_t capability, uint8_t oob_data,
1519                                         uint8_t authentication)
1520 {
1521         struct btdev *remote = btdev->conn;
1522         struct bt_hci_evt_io_capability_response ev;
1523         struct bt_hci_rsp_io_capability_request_reply rsp;
1524         uint8_t status;
1525
1526         if (!remote) {
1527                 status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1528                 goto done;
1529         }
1530
1531         status = BT_HCI_ERR_SUCCESS;
1532
1533         btdev->io_cap = capability;
1534         btdev->auth_req = authentication;
1535
1536         memcpy(ev.bdaddr, btdev->bdaddr, 6);
1537         ev.capability = capability;
1538         ev.oob_data = oob_data;
1539         ev.authentication = authentication;
1540
1541         send_event(remote, BT_HCI_EVT_IO_CAPABILITY_RESPONSE, &ev, sizeof(ev));
1542
1543         if (remote->io_cap) {
1544                 struct bt_hci_evt_user_confirm_request cfm;
1545
1546                 memcpy(cfm.bdaddr, btdev->bdaddr, 6);
1547                 cfm.passkey = 0;
1548
1549                 send_event(remote, BT_HCI_EVT_USER_CONFIRM_REQUEST,
1550                                                         &cfm, sizeof(cfm));
1551
1552                 memcpy(cfm.bdaddr, bdaddr, 6);
1553                 send_event(btdev, BT_HCI_EVT_USER_CONFIRM_REQUEST,
1554                                                         &cfm, sizeof(cfm));
1555         } else {
1556                 send_event(remote, BT_HCI_EVT_IO_CAPABILITY_REQUEST,
1557                                                         btdev->bdaddr, 6);
1558         }
1559
1560 done:
1561         rsp.status = status;
1562         memcpy(rsp.bdaddr, bdaddr, 6);
1563         cmd_complete(btdev, BT_HCI_CMD_IO_CAPABILITY_REQUEST_REPLY,
1564                                                         &rsp, sizeof(rsp));
1565 }
1566
1567 static void io_cap_req_neg_reply_complete(struct btdev *btdev,
1568                                                         const uint8_t *bdaddr)
1569 {
1570         struct bt_hci_rsp_io_capability_request_neg_reply rsp;
1571
1572         rsp.status = BT_HCI_ERR_SUCCESS;
1573         memcpy(rsp.bdaddr, bdaddr, 6);
1574         cmd_complete(btdev, BT_HCI_CMD_IO_CAPABILITY_REQUEST_NEG_REPLY,
1575                                                         &rsp, sizeof(rsp));
1576 }
1577
1578 static void ssp_complete(struct btdev *btdev, const uint8_t *bdaddr,
1579                                                 uint8_t status, bool wait)
1580 {
1581         struct bt_hci_evt_simple_pairing_complete iev, aev;
1582         struct bt_hci_evt_auth_complete auth;
1583         struct btdev *remote = btdev->conn;
1584         struct btdev *init, *accp;
1585
1586         if (!remote)
1587                 return;
1588
1589         btdev->ssp_status = status;
1590         btdev->ssp_auth_complete = true;
1591
1592         if (!remote->ssp_auth_complete && wait)
1593                 return;
1594
1595         if (status == BT_HCI_ERR_SUCCESS &&
1596                                 remote->ssp_status != BT_HCI_ERR_SUCCESS)
1597                 status = remote->ssp_status;
1598
1599         iev.status = status;
1600         aev.status = status;
1601
1602         if (btdev->auth_init) {
1603                 init = btdev;
1604                 accp = remote;
1605                 memcpy(iev.bdaddr, bdaddr, 6);
1606                 memcpy(aev.bdaddr, btdev->bdaddr, 6);
1607         } else {
1608                 init = remote;
1609                 accp = btdev;
1610                 memcpy(iev.bdaddr, btdev->bdaddr, 6);
1611                 memcpy(aev.bdaddr, bdaddr, 6);
1612         }
1613
1614         send_event(init, BT_HCI_EVT_SIMPLE_PAIRING_COMPLETE, &iev,
1615                                                                 sizeof(iev));
1616         send_event(accp, BT_HCI_EVT_SIMPLE_PAIRING_COMPLETE, &aev,
1617                                                                 sizeof(aev));
1618
1619         if (status == BT_HCI_ERR_SUCCESS) {
1620                 link_key_notify(init, iev.bdaddr, LINK_KEY_DUMMY);
1621                 link_key_notify(accp, aev.bdaddr, LINK_KEY_DUMMY);
1622         }
1623
1624         auth.status = status;
1625         auth.handle = cpu_to_le16(42);
1626         send_event(init, BT_HCI_EVT_AUTH_COMPLETE, &auth, sizeof(auth));
1627 }
1628
1629 static void le_send_adv_report(struct btdev *btdev, const struct btdev *remote,
1630                                                                 uint8_t type)
1631 {
1632         struct __packed {
1633                 uint8_t subevent;
1634                 union {
1635                         struct bt_hci_evt_le_adv_report lar;
1636                         uint8_t raw[10 + 31 + 1];
1637                 };
1638         } meta_event;
1639
1640         meta_event.subevent = BT_HCI_EVT_LE_ADV_REPORT;
1641
1642         memset(&meta_event.lar, 0, sizeof(meta_event.lar));
1643         meta_event.lar.num_reports = 1;
1644         meta_event.lar.event_type = type;
1645         meta_event.lar.addr_type = remote->le_adv_own_addr;
1646         memcpy(meta_event.lar.addr, adv_addr(remote), 6);
1647
1648         /* Scan or advertising response */
1649         if (type == 0x04) {
1650                 meta_event.lar.data_len = remote->le_scan_data_len;
1651                 memcpy(meta_event.lar.data, remote->le_scan_data,
1652                                                 meta_event.lar.data_len);
1653         } else {
1654                 meta_event.lar.data_len = remote->le_adv_data_len;
1655                 memcpy(meta_event.lar.data, remote->le_adv_data,
1656                                                 meta_event.lar.data_len);
1657         }
1658         /* Not available */
1659         meta_event.raw[10 + meta_event.lar.data_len] = 127;
1660         send_event(btdev, BT_HCI_EVT_LE_META_EVENT, &meta_event,
1661                                         1 + 10 + meta_event.lar.data_len + 1);
1662 }
1663
1664 static uint8_t get_adv_report_type(uint8_t adv_type)
1665 {
1666         /*
1667          * Connectable low duty cycle directed advertising creates a
1668          * connectable directed advertising report type.
1669          */
1670         if (adv_type == 0x04)
1671                 return 0x01;
1672
1673         return adv_type;
1674 }
1675
1676 static void le_set_adv_enable_complete(struct btdev *btdev)
1677 {
1678         uint8_t report_type;
1679         int i;
1680
1681         report_type = get_adv_report_type(btdev->le_adv_type);
1682
1683         for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
1684                 if (!btdev_list[i] || btdev_list[i] == btdev)
1685                         continue;
1686
1687                 if (!btdev_list[i]->le_scan_enable)
1688                         continue;
1689
1690                 if (!adv_match(btdev_list[i], btdev))
1691                         continue;
1692
1693                 le_send_adv_report(btdev_list[i], btdev, report_type);
1694
1695                 if (btdev_list[i]->le_scan_type != 0x01)
1696                         continue;
1697
1698                 /* ADV_IND & ADV_SCAN_IND generate a scan response */
1699                 if (btdev->le_adv_type == 0x00 || btdev->le_adv_type == 0x02)
1700                         le_send_adv_report(btdev_list[i], btdev, 0x04);
1701         }
1702 }
1703
1704 static void le_set_scan_enable_complete(struct btdev *btdev)
1705 {
1706         int i;
1707
1708         for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
1709                 uint8_t report_type;
1710
1711                 if (!btdev_list[i] || btdev_list[i] == btdev)
1712                         continue;
1713
1714                 if (!btdev_list[i]->le_adv_enable)
1715                         continue;
1716
1717                 if (!adv_match(btdev, btdev_list[i]))
1718                         continue;
1719
1720                 report_type = get_adv_report_type(btdev_list[i]->le_adv_type);
1721                 le_send_adv_report(btdev, btdev_list[i], report_type);
1722
1723                 if (btdev->le_scan_type != 0x01)
1724                         continue;
1725
1726                 /* ADV_IND & ADV_SCAN_IND generate a scan response */
1727                 if (btdev_list[i]->le_adv_type == 0x00 ||
1728                                         btdev_list[i]->le_adv_type == 0x02)
1729                         le_send_adv_report(btdev, btdev_list[i], 0x04);
1730         }
1731 }
1732
1733 static void le_start_encrypt_complete(struct btdev *btdev)
1734 {
1735         char buf[1 + sizeof(struct bt_hci_evt_le_long_term_key_request)];
1736         struct bt_hci_evt_le_long_term_key_request *ev = (void *) &buf[1];
1737         struct btdev *remote = btdev->conn;
1738
1739         if (!remote) {
1740                 cmd_status(btdev, BT_HCI_ERR_UNKNOWN_CONN_ID,
1741                                                 BT_HCI_CMD_LE_START_ENCRYPT);
1742                 return;
1743         }
1744
1745         cmd_status(btdev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_START_ENCRYPT);
1746
1747         memset(buf, 0, sizeof(buf));
1748         buf[0] = BT_HCI_EVT_LE_LONG_TERM_KEY_REQUEST;
1749         ev->handle = cpu_to_le16(42);
1750
1751         send_event(remote, BT_HCI_EVT_LE_META_EVENT, buf, sizeof(buf));
1752 }
1753
1754 static void le_encrypt_complete(struct btdev *btdev)
1755 {
1756         struct bt_hci_evt_encrypt_change ev;
1757         struct bt_hci_rsp_le_ltk_req_reply rp;
1758         struct btdev *remote = btdev->conn;
1759
1760         memset(&rp, 0, sizeof(rp));
1761         rp.handle = cpu_to_le16(42);
1762
1763         if (!remote) {
1764                 rp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1765                 cmd_complete(btdev, BT_HCI_CMD_LE_LTK_REQ_REPLY, &rp,
1766                                                         sizeof(rp));
1767                 return;
1768         }
1769
1770         rp.status = BT_HCI_ERR_SUCCESS;
1771         cmd_complete(btdev, BT_HCI_CMD_LE_LTK_REQ_REPLY, &rp, sizeof(rp));
1772
1773         memset(&ev, 0, sizeof(ev));
1774
1775         if (memcmp(btdev->le_ltk, remote->le_ltk, 16)) {
1776                 ev.status = BT_HCI_ERR_AUTH_FAILURE;
1777                 ev.encr_mode = 0x00;
1778         } else {
1779                 ev.status = BT_HCI_ERR_SUCCESS;
1780                 ev.encr_mode = 0x01;
1781         }
1782
1783         ev.handle = cpu_to_le16(42);
1784
1785         send_event(btdev, BT_HCI_EVT_ENCRYPT_CHANGE, &ev, sizeof(ev));
1786         send_event(remote, BT_HCI_EVT_ENCRYPT_CHANGE, &ev, sizeof(ev));
1787 }
1788
1789 static void ltk_neg_reply_complete(struct btdev *btdev)
1790 {
1791         struct bt_hci_rsp_le_ltk_req_neg_reply rp;
1792         struct bt_hci_evt_encrypt_change ev;
1793         struct btdev *remote = btdev->conn;
1794
1795         memset(&rp, 0, sizeof(rp));
1796         rp.handle = cpu_to_le16(42);
1797
1798         if (!remote) {
1799                 rp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1800                 cmd_complete(btdev, BT_HCI_CMD_LE_LTK_REQ_NEG_REPLY, &rp,
1801                                                         sizeof(rp));
1802                 return;
1803         }
1804
1805         rp.status = BT_HCI_ERR_SUCCESS;
1806         cmd_complete(btdev, BT_HCI_CMD_LE_LTK_REQ_NEG_REPLY, &rp, sizeof(rp));
1807
1808         memset(&ev, 0, sizeof(ev));
1809         ev.status = BT_HCI_ERR_PIN_OR_KEY_MISSING;
1810         ev.handle = cpu_to_le16(42);
1811
1812         send_event(remote, BT_HCI_EVT_ENCRYPT_CHANGE, &ev, sizeof(ev));
1813 }
1814
1815 static void default_cmd(struct btdev *btdev, uint16_t opcode,
1816                                                 const void *data, uint8_t len)
1817 {
1818         const struct bt_hci_cmd_remote_name_request_cancel *rnrc;
1819         const struct bt_hci_cmd_write_default_link_policy *wdlp;
1820         const struct bt_hci_cmd_set_event_mask *sem;
1821         const struct bt_hci_cmd_set_event_filter *sef;
1822         const struct bt_hci_cmd_write_local_name *wln;
1823         const struct bt_hci_cmd_write_conn_accept_timeout *wcat;
1824         const struct bt_hci_cmd_write_page_timeout *wpt;
1825         const struct bt_hci_cmd_write_scan_enable *wse;
1826         const struct bt_hci_cmd_write_page_scan_activity *wpsa;
1827         const struct bt_hci_cmd_write_inquiry_scan_activity *wisa;
1828         const struct bt_hci_cmd_write_page_scan_type *wpst;
1829         const struct bt_hci_cmd_write_auth_enable *wae;
1830         const struct bt_hci_cmd_write_class_of_dev *wcod;
1831         const struct bt_hci_cmd_write_voice_setting *wvs;
1832         const struct bt_hci_cmd_write_inquiry_mode *wim;
1833         const struct bt_hci_cmd_write_afh_assessment_mode *waam;
1834         const struct bt_hci_cmd_write_ext_inquiry_response *weir;
1835         const struct bt_hci_cmd_write_simple_pairing_mode *wspm;
1836         const struct bt_hci_cmd_io_capability_request_reply *icrr;
1837         const struct bt_hci_cmd_io_capability_request_reply *icrnr;
1838         const struct bt_hci_cmd_write_le_host_supported *wlhs;
1839         const struct bt_hci_cmd_write_secure_conn_support *wscs;
1840         const struct bt_hci_cmd_set_event_mask_page2 *semp2;
1841         const struct bt_hci_cmd_le_set_event_mask *lsem;
1842         const struct bt_hci_cmd_le_set_random_address *lsra;
1843         const struct bt_hci_cmd_le_set_adv_parameters *lsap;
1844         const struct bt_hci_cmd_le_set_adv_data *lsad;
1845         const struct bt_hci_cmd_le_set_scan_rsp_data *lssrd;
1846         const struct bt_hci_cmd_setup_sync_conn *ssc;
1847         const struct bt_hci_cmd_write_ssp_debug_mode *wsdm;
1848         const struct bt_hci_cmd_le_set_adv_enable *lsae;
1849         const struct bt_hci_cmd_le_set_scan_parameters *lssp;
1850         const struct bt_hci_cmd_le_set_scan_enable *lsse;
1851         const struct bt_hci_cmd_le_start_encrypt *lse;
1852         const struct bt_hci_cmd_le_ltk_req_reply *llrr;
1853         const struct bt_hci_cmd_read_local_amp_assoc *rlaa_cmd;
1854         const struct bt_hci_cmd_read_rssi *rrssi;
1855         const struct bt_hci_cmd_read_tx_power *rtxp;
1856         struct bt_hci_rsp_read_default_link_policy rdlp;
1857         struct bt_hci_rsp_read_stored_link_key rslk;
1858         struct bt_hci_rsp_write_stored_link_key wslk;
1859         struct bt_hci_rsp_delete_stored_link_key dslk;
1860         struct bt_hci_rsp_read_local_name rln;
1861         struct bt_hci_rsp_read_conn_accept_timeout rcat;
1862         struct bt_hci_rsp_read_page_timeout rpt;
1863         struct bt_hci_rsp_read_scan_enable rse;
1864         struct bt_hci_rsp_read_page_scan_activity rpsa;
1865         struct bt_hci_rsp_read_inquiry_scan_activity risa;
1866         struct bt_hci_rsp_read_page_scan_type rpst;
1867         struct bt_hci_rsp_read_auth_enable rae;
1868         struct bt_hci_rsp_read_class_of_dev rcod;
1869         struct bt_hci_rsp_read_voice_setting rvs;
1870         struct bt_hci_rsp_read_num_supported_iac rnsi;
1871         struct bt_hci_rsp_read_current_iac_lap *rcil;
1872         struct bt_hci_rsp_read_inquiry_mode rim;
1873         struct bt_hci_rsp_read_afh_assessment_mode raam;
1874         struct bt_hci_rsp_read_ext_inquiry_response reir;
1875         struct bt_hci_rsp_read_simple_pairing_mode rspm;
1876         struct bt_hci_rsp_read_local_oob_data rlod;
1877         struct bt_hci_rsp_read_inquiry_resp_tx_power rirtp;
1878         struct bt_hci_rsp_read_le_host_supported rlhs;
1879         struct bt_hci_rsp_read_secure_conn_support rscs;
1880         struct bt_hci_rsp_read_local_oob_ext_data rloed;
1881         struct bt_hci_rsp_read_sync_train_params rstp;
1882         struct bt_hci_rsp_read_local_version rlv;
1883         struct bt_hci_rsp_read_local_commands rlc;
1884         struct bt_hci_rsp_read_local_features rlf;
1885         struct bt_hci_rsp_read_local_ext_features rlef;
1886         struct bt_hci_rsp_read_buffer_size rbs;
1887         struct bt_hci_rsp_read_country_code rcc;
1888         struct bt_hci_rsp_read_bd_addr rba;
1889         struct bt_hci_rsp_read_data_block_size rdbs;
1890         struct bt_hci_rsp_read_local_codecs *rlsc;
1891         struct bt_hci_rsp_read_local_amp_info rlai;
1892         struct bt_hci_rsp_read_local_amp_assoc rlaa_rsp;
1893         struct bt_hci_rsp_get_mws_transport_config *gmtc;
1894         struct bt_hci_rsp_le_read_buffer_size lrbs;
1895         struct bt_hci_rsp_le_read_local_features lrlf;
1896         struct bt_hci_rsp_le_read_adv_tx_power lratp;
1897         struct bt_hci_rsp_le_read_supported_states lrss;
1898         struct bt_hci_rsp_le_read_white_list_size lrwls;
1899         struct bt_hci_rsp_le_rand lr;
1900         struct bt_hci_rsp_le_test_end lte;
1901         struct bt_hci_rsp_remote_name_request_cancel rnrc_rsp;
1902         struct bt_hci_rsp_link_key_request_reply lkrr_rsp;
1903         struct bt_hci_rsp_link_key_request_neg_reply lkrnr_rsp;
1904         struct bt_hci_rsp_pin_code_request_neg_reply pcrr_rsp;
1905         struct bt_hci_rsp_pin_code_request_neg_reply pcrnr_rsp;
1906         struct bt_hci_rsp_user_confirm_request_reply ucrr_rsp;
1907         struct bt_hci_rsp_user_confirm_request_neg_reply ucrnr_rsp;
1908         struct bt_hci_rsp_read_rssi rrssi_rsp;
1909         struct bt_hci_rsp_read_tx_power rtxp_rsp;
1910         uint8_t status, page;
1911
1912         switch (opcode) {
1913         case BT_HCI_CMD_INQUIRY:
1914                 if (btdev->type == BTDEV_TYPE_LE)
1915                         goto unsupported;
1916                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
1917                 break;
1918
1919         case BT_HCI_CMD_INQUIRY_CANCEL:
1920                 if (btdev->type == BTDEV_TYPE_LE)
1921                         goto unsupported;
1922                 inquiry_cancel(btdev);
1923                 break;
1924
1925         case BT_HCI_CMD_CREATE_CONN:
1926                 if (btdev->type == BTDEV_TYPE_LE)
1927                         goto unsupported;
1928                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
1929                 break;
1930
1931         case BT_HCI_CMD_DISCONNECT:
1932                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
1933                 break;
1934
1935         case BT_HCI_CMD_CREATE_CONN_CANCEL:
1936                 if (btdev->type == BTDEV_TYPE_LE)
1937                         goto unsupported;
1938                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
1939                 break;
1940
1941         case BT_HCI_CMD_ACCEPT_CONN_REQUEST:
1942                 if (btdev->type == BTDEV_TYPE_LE)
1943                         goto unsupported;
1944                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
1945                 break;
1946
1947         case BT_HCI_CMD_REJECT_CONN_REQUEST:
1948                 if (btdev->type == BTDEV_TYPE_LE)
1949                         goto unsupported;
1950                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
1951                 break;
1952
1953         case BT_HCI_CMD_LINK_KEY_REQUEST_REPLY:
1954                 if (btdev->type == BTDEV_TYPE_LE)
1955                         goto unsupported;
1956                 lkrr_rsp.status = BT_HCI_ERR_SUCCESS;
1957                 memcpy(lkrr_rsp.bdaddr, data, 6);
1958                 cmd_complete(btdev, opcode, &lkrr_rsp, sizeof(lkrr_rsp));
1959                 break;
1960
1961         case BT_HCI_CMD_LINK_KEY_REQUEST_NEG_REPLY:
1962                 if (btdev->type == BTDEV_TYPE_LE)
1963                         goto unsupported;
1964                 lkrnr_rsp.status = BT_HCI_ERR_SUCCESS;
1965                 memcpy(lkrnr_rsp.bdaddr, data, 6);
1966                 cmd_complete(btdev, opcode, &lkrnr_rsp, sizeof(lkrnr_rsp));
1967                 break;
1968
1969         case BT_HCI_CMD_PIN_CODE_REQUEST_REPLY:
1970                 if (btdev->type == BTDEV_TYPE_LE)
1971                         goto unsupported;
1972                 pcrr_rsp.status = BT_HCI_ERR_SUCCESS;
1973                 memcpy(pcrr_rsp.bdaddr, data, 6);
1974                 cmd_complete(btdev, opcode, &pcrr_rsp, sizeof(pcrr_rsp));
1975                 break;
1976
1977         case BT_HCI_CMD_PIN_CODE_REQUEST_NEG_REPLY:
1978                 if (btdev->type == BTDEV_TYPE_LE)
1979                         goto unsupported;
1980                 pcrnr_rsp.status = BT_HCI_ERR_SUCCESS;
1981                 memcpy(pcrnr_rsp.bdaddr, data, 6);
1982                 cmd_complete(btdev, opcode, &pcrnr_rsp, sizeof(pcrnr_rsp));
1983                 break;
1984
1985         case BT_HCI_CMD_AUTH_REQUESTED:
1986                 if (btdev->type == BTDEV_TYPE_LE)
1987                         goto unsupported;
1988                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
1989                 break;
1990
1991         case BT_HCI_CMD_SET_CONN_ENCRYPT:
1992                 if (btdev->type == BTDEV_TYPE_LE)
1993                         goto unsupported;
1994                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
1995                 break;
1996
1997         case BT_HCI_CMD_REMOTE_NAME_REQUEST:
1998                 if (btdev->type == BTDEV_TYPE_LE)
1999                         goto unsupported;
2000                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
2001                 break;
2002
2003         case BT_HCI_CMD_REMOTE_NAME_REQUEST_CANCEL:
2004                 if (btdev->type == BTDEV_TYPE_LE)
2005                         goto unsupported;
2006                 rnrc = data;
2007                 rnrc_rsp.status = BT_HCI_ERR_SUCCESS;
2008                 memcpy(rnrc_rsp.bdaddr, rnrc->bdaddr, 6);
2009                 cmd_complete(btdev, opcode, &rnrc_rsp, sizeof(rnrc_rsp));
2010                 break;
2011
2012         case BT_HCI_CMD_READ_REMOTE_FEATURES:
2013                 if (btdev->type == BTDEV_TYPE_LE)
2014                         goto unsupported;
2015                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
2016                 break;
2017
2018         case BT_HCI_CMD_READ_REMOTE_EXT_FEATURES:
2019                 if (btdev->type == BTDEV_TYPE_LE)
2020                         goto unsupported;
2021                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
2022                 break;
2023
2024         case BT_HCI_CMD_READ_REMOTE_VERSION:
2025                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
2026                 break;
2027
2028         case BT_HCI_CMD_READ_CLOCK_OFFSET:
2029                 if (btdev->type == BTDEV_TYPE_LE)
2030                         goto unsupported;
2031                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
2032                 break;
2033
2034         case BT_HCI_CMD_READ_DEFAULT_LINK_POLICY:
2035                 if (btdev->type == BTDEV_TYPE_LE)
2036                         goto unsupported;
2037                 rdlp.status = BT_HCI_ERR_SUCCESS;
2038                 rdlp.policy = cpu_to_le16(btdev->default_link_policy);
2039                 cmd_complete(btdev, opcode, &rdlp, sizeof(rdlp));
2040                 break;
2041
2042         case BT_HCI_CMD_WRITE_DEFAULT_LINK_POLICY:
2043                 if (btdev->type == BTDEV_TYPE_LE)
2044                         goto unsupported;
2045                 wdlp = data;
2046                 btdev->default_link_policy = le16_to_cpu(wdlp->policy);
2047                 status = BT_HCI_ERR_SUCCESS;
2048                 cmd_complete(btdev, opcode, &status, sizeof(status));
2049                 break;
2050
2051         case BT_HCI_CMD_SET_EVENT_MASK:
2052                 sem = data;
2053                 memcpy(btdev->event_mask, sem->mask, 8);
2054                 status = BT_HCI_ERR_SUCCESS;
2055                 cmd_complete(btdev, opcode, &status, sizeof(status));
2056                 break;
2057
2058         case BT_HCI_CMD_RESET:
2059                 status = BT_HCI_ERR_SUCCESS;
2060                 cmd_complete(btdev, opcode, &status, sizeof(status));
2061                 break;
2062
2063         case BT_HCI_CMD_SET_EVENT_FILTER:
2064                 if (btdev->type == BTDEV_TYPE_LE)
2065                         goto unsupported;
2066                 sef = data;
2067                 btdev->event_filter = sef->type;
2068                 status = BT_HCI_ERR_SUCCESS;
2069                 cmd_complete(btdev, opcode, &status, sizeof(status));
2070                 break;
2071
2072         case BT_HCI_CMD_READ_STORED_LINK_KEY:
2073                 if (btdev->type == BTDEV_TYPE_LE)
2074                         goto unsupported;
2075                 rslk.status = BT_HCI_ERR_SUCCESS;
2076                 rslk.max_num_keys = cpu_to_le16(0);
2077                 rslk.num_keys = cpu_to_le16(0);
2078                 cmd_complete(btdev, opcode, &rslk, sizeof(rslk));
2079                 break;
2080
2081         case BT_HCI_CMD_WRITE_STORED_LINK_KEY:
2082                 if (btdev->type == BTDEV_TYPE_LE)
2083                         goto unsupported;
2084                 wslk.status = BT_HCI_ERR_SUCCESS;
2085                 wslk.num_keys = 0;
2086                 cmd_complete(btdev, opcode, &wslk, sizeof(wslk));
2087                 break;
2088
2089         case BT_HCI_CMD_DELETE_STORED_LINK_KEY:
2090                 if (btdev->type == BTDEV_TYPE_LE)
2091                         goto unsupported;
2092                 dslk.status = BT_HCI_ERR_SUCCESS;
2093                 dslk.num_keys = cpu_to_le16(0);
2094                 cmd_complete(btdev, opcode, &dslk, sizeof(dslk));
2095                 break;
2096
2097         case BT_HCI_CMD_WRITE_LOCAL_NAME:
2098                 if (btdev->type == BTDEV_TYPE_LE)
2099                         goto unsupported;
2100                 wln = data;
2101                 memcpy(btdev->name, wln->name, 248);
2102                 status = BT_HCI_ERR_SUCCESS;
2103                 cmd_complete(btdev, opcode, &status, sizeof(status));
2104                 break;
2105
2106         case BT_HCI_CMD_READ_LOCAL_NAME:
2107                 if (btdev->type == BTDEV_TYPE_LE)
2108                         goto unsupported;
2109                 rln.status = BT_HCI_ERR_SUCCESS;
2110                 memcpy(rln.name, btdev->name, 248);
2111                 cmd_complete(btdev, opcode, &rln, sizeof(rln));
2112                 break;
2113
2114         case BT_HCI_CMD_READ_CONN_ACCEPT_TIMEOUT:
2115                 if (btdev->type == BTDEV_TYPE_LE)
2116                         goto unsupported;
2117                 rcat.status = BT_HCI_ERR_SUCCESS;
2118                 rcat.timeout = cpu_to_le16(btdev->conn_accept_timeout);
2119                 cmd_complete(btdev, opcode, &rcat, sizeof(rcat));
2120                 break;
2121
2122         case BT_HCI_CMD_WRITE_CONN_ACCEPT_TIMEOUT:
2123                 if (btdev->type == BTDEV_TYPE_LE)
2124                         goto unsupported;
2125                 wcat = data;
2126                 btdev->conn_accept_timeout = le16_to_cpu(wcat->timeout);
2127                 status = BT_HCI_ERR_SUCCESS;
2128                 cmd_complete(btdev, opcode, &status, sizeof(status));
2129                 break;
2130
2131         case BT_HCI_CMD_READ_PAGE_TIMEOUT:
2132                 if (btdev->type == BTDEV_TYPE_LE)
2133                         goto unsupported;
2134                 rpt.status = BT_HCI_ERR_SUCCESS;
2135                 rpt.timeout = cpu_to_le16(btdev->page_timeout);
2136                 cmd_complete(btdev, opcode, &rpt, sizeof(rpt));
2137                 break;
2138
2139         case BT_HCI_CMD_WRITE_PAGE_TIMEOUT:
2140                 if (btdev->type == BTDEV_TYPE_LE)
2141                         goto unsupported;
2142                 wpt = data;
2143                 btdev->page_timeout = le16_to_cpu(wpt->timeout);
2144                 status = BT_HCI_ERR_SUCCESS;
2145                 cmd_complete(btdev, opcode, &status, sizeof(status));
2146                 break;
2147
2148         case BT_HCI_CMD_READ_SCAN_ENABLE:
2149                 if (btdev->type == BTDEV_TYPE_LE)
2150                         goto unsupported;
2151                 rse.status = BT_HCI_ERR_SUCCESS;
2152                 rse.enable = btdev->scan_enable;
2153                 cmd_complete(btdev, opcode, &rse, sizeof(rse));
2154                 break;
2155
2156         case BT_HCI_CMD_WRITE_SCAN_ENABLE:
2157                 if (btdev->type == BTDEV_TYPE_LE)
2158                         goto unsupported;
2159                 wse = data;
2160                 btdev->scan_enable = wse->enable;
2161                 status = BT_HCI_ERR_SUCCESS;
2162                 cmd_complete(btdev, opcode, &status, sizeof(status));
2163                 break;
2164
2165         case BT_HCI_CMD_READ_PAGE_SCAN_ACTIVITY:
2166                 if (btdev->type == BTDEV_TYPE_LE)
2167                         goto unsupported;
2168                 rpsa.status = BT_HCI_ERR_SUCCESS;
2169                 rpsa.interval = cpu_to_le16(btdev->page_scan_interval);
2170                 rpsa.window = cpu_to_le16(btdev->page_scan_window);
2171                 cmd_complete(btdev, opcode, &rpsa, sizeof(rpsa));
2172                 break;
2173
2174         case BT_HCI_CMD_WRITE_PAGE_SCAN_ACTIVITY:
2175                 if (btdev->type == BTDEV_TYPE_LE)
2176                         goto unsupported;
2177                 wpsa = data;
2178                 btdev->page_scan_interval = le16_to_cpu(wpsa->interval);
2179                 btdev->page_scan_window = le16_to_cpu(wpsa->window);
2180                 status = BT_HCI_ERR_SUCCESS;
2181                 cmd_complete(btdev, opcode, &status, sizeof(status));
2182                 break;
2183
2184         case BT_HCI_CMD_READ_INQUIRY_SCAN_ACTIVITY:
2185                 if (btdev->type == BTDEV_TYPE_LE)
2186                         goto unsupported;
2187                 risa.status = BT_HCI_ERR_SUCCESS;
2188                 risa.interval = cpu_to_le16(btdev->inquiry_scan_interval);
2189                 risa.window = cpu_to_le16(btdev->inquiry_scan_window);
2190                 cmd_complete(btdev, opcode, &risa, sizeof(risa));
2191                 break;
2192
2193         case BT_HCI_CMD_WRITE_INQUIRY_SCAN_ACTIVITY:
2194                 if (btdev->type == BTDEV_TYPE_LE)
2195                         goto unsupported;
2196                 wisa = data;
2197                 btdev->inquiry_scan_interval = le16_to_cpu(wisa->interval);
2198                 btdev->inquiry_scan_window = le16_to_cpu(wisa->window);
2199                 status = BT_HCI_ERR_SUCCESS;
2200                 cmd_complete(btdev, opcode, &status, sizeof(status));
2201                 break;
2202
2203         case BT_HCI_CMD_READ_PAGE_SCAN_TYPE:
2204                 if (btdev->type == BTDEV_TYPE_LE)
2205                         goto unsupported;
2206                 rpst.status = BT_HCI_ERR_SUCCESS;
2207                 rpst.type = btdev->page_scan_type;
2208                 cmd_complete(btdev, opcode, &rpst, sizeof(rpst));
2209                 break;
2210
2211         case BT_HCI_CMD_WRITE_PAGE_SCAN_TYPE:
2212                 if (btdev->type == BTDEV_TYPE_LE)
2213                         goto unsupported;
2214                 wpst = data;
2215                 btdev->page_scan_type = wpst->type;
2216                 status = BT_HCI_ERR_SUCCESS;
2217                 cmd_complete(btdev, opcode, &status, sizeof(status));
2218                 break;
2219
2220         case BT_HCI_CMD_READ_AUTH_ENABLE:
2221                 if (btdev->type == BTDEV_TYPE_LE)
2222                         goto unsupported;
2223                 rae.status = BT_HCI_ERR_SUCCESS;
2224                 rae.enable = btdev->auth_enable;
2225                 cmd_complete(btdev, opcode, &rae, sizeof(rae));
2226                 break;
2227
2228         case BT_HCI_CMD_WRITE_AUTH_ENABLE:
2229                 if (btdev->type == BTDEV_TYPE_LE)
2230                         goto unsupported;
2231                 wae = data;
2232                 btdev->auth_enable = wae->enable;
2233                 status = BT_HCI_ERR_SUCCESS;
2234                 cmd_complete(btdev, opcode, &status, sizeof(status));
2235                 break;
2236
2237         case BT_HCI_CMD_READ_CLASS_OF_DEV:
2238                 if (btdev->type == BTDEV_TYPE_LE)
2239                         goto unsupported;
2240                 rcod.status = BT_HCI_ERR_SUCCESS;
2241                 memcpy(rcod.dev_class, btdev->dev_class, 3);
2242                 cmd_complete(btdev, opcode, &rcod, sizeof(rcod));
2243                 break;
2244
2245         case BT_HCI_CMD_WRITE_CLASS_OF_DEV:
2246                 if (btdev->type == BTDEV_TYPE_LE)
2247                         goto unsupported;
2248                 wcod = data;
2249                 memcpy(btdev->dev_class, wcod->dev_class, 3);
2250                 status = BT_HCI_ERR_SUCCESS;
2251                 cmd_complete(btdev, opcode, &status, sizeof(status));
2252                 break;
2253
2254         case BT_HCI_CMD_READ_VOICE_SETTING:
2255                 if (btdev->type == BTDEV_TYPE_LE)
2256                         goto unsupported;
2257                 rvs.status = BT_HCI_ERR_SUCCESS;
2258                 rvs.setting = cpu_to_le16(btdev->voice_setting);
2259                 cmd_complete(btdev, opcode, &rvs, sizeof(rvs));
2260                 break;
2261
2262         case BT_HCI_CMD_WRITE_VOICE_SETTING:
2263                 if (btdev->type == BTDEV_TYPE_LE)
2264                         goto unsupported;
2265                 wvs = data;
2266                 btdev->voice_setting = le16_to_cpu(wvs->setting);
2267                 status = BT_HCI_ERR_SUCCESS;
2268                 cmd_complete(btdev, opcode, &status, sizeof(status));
2269                 break;
2270
2271         case BT_HCI_CMD_HOST_BUFFER_SIZE:
2272                 status = BT_HCI_ERR_SUCCESS;
2273                 cmd_complete(btdev, opcode, &status, sizeof(status));
2274                 break;
2275
2276         case BT_HCI_CMD_READ_NUM_SUPPORTED_IAC:
2277                 if (btdev->type == BTDEV_TYPE_LE)
2278                         goto unsupported;
2279                 rnsi.status = BT_HCI_ERR_SUCCESS;
2280                 rnsi.num_iac = 0x01;
2281                 cmd_complete(btdev, opcode, &rnsi, sizeof(rnsi));
2282                 break;
2283
2284         case BT_HCI_CMD_READ_CURRENT_IAC_LAP:
2285                 if (btdev->type == BTDEV_TYPE_LE)
2286                         goto unsupported;
2287                 rcil = alloca(sizeof(*rcil) + 3);
2288                 rcil->status = BT_HCI_ERR_SUCCESS;
2289                 rcil->num_iac = 0x01;
2290                 rcil->iac_lap[0] = 0x33;
2291                 rcil->iac_lap[1] = 0x8b;
2292                 rcil->iac_lap[2] = 0x9e;
2293                 cmd_complete(btdev, opcode, rcil, sizeof(*rcil) + 3);
2294                 break;
2295
2296         case BT_HCI_CMD_WRITE_CURRENT_IAC_LAP:
2297                 if (btdev->type == BTDEV_TYPE_LE)
2298                         goto unsupported;
2299                 status = BT_HCI_ERR_SUCCESS;
2300                 cmd_complete(btdev, opcode, &status, sizeof(status));
2301                 break;
2302
2303         case BT_HCI_CMD_READ_INQUIRY_MODE:
2304                 if (btdev->type == BTDEV_TYPE_LE)
2305                         goto unsupported;
2306                 rim.status = BT_HCI_ERR_SUCCESS;
2307                 rim.mode = btdev->inquiry_mode;
2308                 cmd_complete(btdev, opcode, &rim, sizeof(rim));
2309                 break;
2310
2311         case BT_HCI_CMD_WRITE_INQUIRY_MODE:
2312                 if (btdev->type == BTDEV_TYPE_LE)
2313                         goto unsupported;
2314                 wim = data;
2315                 btdev->inquiry_mode = wim->mode;
2316                 status = BT_HCI_ERR_SUCCESS;
2317                 cmd_complete(btdev, opcode, &status, sizeof(status));
2318                 break;
2319
2320         case BT_HCI_CMD_READ_AFH_ASSESSMENT_MODE:
2321                 if (btdev->type == BTDEV_TYPE_LE)
2322                         goto unsupported;
2323                 raam.status = BT_HCI_ERR_SUCCESS;
2324                 raam.mode = btdev->afh_assessment_mode;
2325                 cmd_complete(btdev, opcode, &raam, sizeof(raam));
2326                 break;
2327
2328         case BT_HCI_CMD_WRITE_AFH_ASSESSMENT_MODE:
2329                 if (btdev->type == BTDEV_TYPE_LE)
2330                         goto unsupported;
2331                 waam = data;
2332                 btdev->afh_assessment_mode = waam->mode;
2333                 status = BT_HCI_ERR_SUCCESS;
2334                 cmd_complete(btdev, opcode, &status, sizeof(status));
2335                 break;
2336
2337         case BT_HCI_CMD_READ_EXT_INQUIRY_RESPONSE:
2338                 if (btdev->type == BTDEV_TYPE_LE)
2339                         goto unsupported;
2340                 reir.status = BT_HCI_ERR_SUCCESS;
2341                 reir.fec = btdev->ext_inquiry_fec;
2342                 memcpy(reir.data, btdev->ext_inquiry_rsp, 240);
2343                 cmd_complete(btdev, opcode, &reir, sizeof(reir));
2344                 break;
2345
2346         case BT_HCI_CMD_WRITE_EXT_INQUIRY_RESPONSE:
2347                 if (btdev->type == BTDEV_TYPE_LE)
2348                         goto unsupported;
2349                 weir = data;
2350                 btdev->ext_inquiry_fec = weir->fec;
2351                 memcpy(btdev->ext_inquiry_rsp, weir->data, 240);
2352                 status = BT_HCI_ERR_SUCCESS;
2353                 cmd_complete(btdev, opcode, &status, sizeof(status));
2354                 break;
2355
2356         case BT_HCI_CMD_READ_SIMPLE_PAIRING_MODE:
2357                 if (btdev->type == BTDEV_TYPE_LE)
2358                         goto unsupported;
2359                 rspm.status = BT_HCI_ERR_SUCCESS;
2360                 rspm.mode = btdev->simple_pairing_mode;
2361                 cmd_complete(btdev, opcode, &rspm, sizeof(rspm));
2362                 break;
2363
2364         case BT_HCI_CMD_WRITE_SIMPLE_PAIRING_MODE:
2365                 if (btdev->type == BTDEV_TYPE_LE)
2366                         goto unsupported;
2367                 wspm = data;
2368                 btdev->simple_pairing_mode = wspm->mode;
2369                 status = BT_HCI_ERR_SUCCESS;
2370                 cmd_complete(btdev, opcode, &status, sizeof(status));
2371                 break;
2372
2373         case BT_HCI_CMD_IO_CAPABILITY_REQUEST_REPLY:
2374                 if (btdev->type == BTDEV_TYPE_LE)
2375                         goto unsupported;
2376                 icrr = data;
2377                 io_cap_req_reply_complete(btdev, icrr->bdaddr,
2378                                                         icrr->capability,
2379                                                         icrr->oob_data,
2380                                                         icrr->authentication);
2381                 break;
2382
2383         case BT_HCI_CMD_IO_CAPABILITY_REQUEST_NEG_REPLY:
2384                 if (btdev->type == BTDEV_TYPE_LE)
2385                         goto unsupported;
2386                 icrnr = data;
2387                 io_cap_req_neg_reply_complete(btdev, icrnr->bdaddr);
2388                 ssp_complete(btdev, icrnr->bdaddr, BT_HCI_ERR_AUTH_FAILURE,
2389                                                                         false);
2390                 break;
2391
2392         case BT_HCI_CMD_USER_CONFIRM_REQUEST_REPLY:
2393                 if (btdev->type == BTDEV_TYPE_LE)
2394                         goto unsupported;
2395                 ucrr_rsp.status = BT_HCI_ERR_SUCCESS;
2396                 memcpy(ucrr_rsp.bdaddr, data, 6);
2397                 cmd_complete(btdev, opcode, &ucrr_rsp, sizeof(ucrr_rsp));
2398                 ssp_complete(btdev, data, BT_HCI_ERR_SUCCESS, true);
2399                 break;
2400
2401         case BT_HCI_CMD_USER_CONFIRM_REQUEST_NEG_REPLY:
2402                 if (btdev->type == BTDEV_TYPE_LE)
2403                         goto unsupported;
2404                 ucrnr_rsp.status = BT_HCI_ERR_SUCCESS;
2405                 memcpy(ucrnr_rsp.bdaddr, data, 6);
2406                 cmd_complete(btdev, opcode, &ucrnr_rsp, sizeof(ucrnr_rsp));
2407                 ssp_complete(btdev, data, BT_HCI_ERR_AUTH_FAILURE, true);
2408                 break;
2409
2410         case BT_HCI_CMD_READ_LOCAL_OOB_DATA:
2411                 if (btdev->type == BTDEV_TYPE_LE)
2412                         goto unsupported;
2413                 rlod.status = BT_HCI_ERR_SUCCESS;
2414                 cmd_complete(btdev, opcode, &rlod, sizeof(rlod));
2415                 break;
2416
2417         case BT_HCI_CMD_READ_INQUIRY_RESP_TX_POWER:
2418                 if (btdev->type == BTDEV_TYPE_LE)
2419                         goto unsupported;
2420                 rirtp.status = BT_HCI_ERR_SUCCESS;
2421                 rirtp.level = 0;
2422                 cmd_complete(btdev, opcode, &rirtp, sizeof(rirtp));
2423                 break;
2424
2425         case BT_HCI_CMD_READ_LE_HOST_SUPPORTED:
2426                 if (btdev->type != BTDEV_TYPE_BREDRLE)
2427                         goto unsupported;
2428                 rlhs.status = BT_HCI_ERR_SUCCESS;
2429                 rlhs.supported = btdev->le_supported;
2430                 rlhs.simultaneous = btdev->le_simultaneous;
2431                 cmd_complete(btdev, opcode, &rlhs, sizeof(rlhs));
2432                 break;
2433
2434         case BT_HCI_CMD_WRITE_LE_HOST_SUPPORTED:
2435                 if (btdev->type != BTDEV_TYPE_BREDRLE)
2436                         goto unsupported;
2437                 wlhs = data;
2438                 btdev->le_supported = wlhs->supported;
2439                 btdev->le_simultaneous = wlhs->simultaneous;
2440                 status = BT_HCI_ERR_SUCCESS;
2441                 cmd_complete(btdev, opcode, &status, sizeof(status));
2442                 break;
2443
2444         case BT_HCI_CMD_READ_SECURE_CONN_SUPPORT:
2445                 if (btdev->type != BTDEV_TYPE_BREDRLE)
2446                         goto unsupported;
2447                 rscs.status = BT_HCI_ERR_SUCCESS;
2448                 rscs.support = btdev->secure_conn_support;
2449                 cmd_complete(btdev, opcode, &rscs, sizeof(rscs));
2450                 break;
2451
2452         case BT_HCI_CMD_WRITE_SECURE_CONN_SUPPORT:
2453                 if (btdev->type != BTDEV_TYPE_BREDRLE)
2454                         goto unsupported;
2455                 wscs = data;
2456                 btdev->secure_conn_support = wscs->support;
2457                 status = BT_HCI_ERR_SUCCESS;
2458                 cmd_complete(btdev, opcode, &status, sizeof(status));
2459                 break;
2460
2461         case BT_HCI_CMD_READ_LOCAL_OOB_EXT_DATA:
2462                 if (btdev->type != BTDEV_TYPE_BREDRLE)
2463                         goto unsupported;
2464                 rloed.status = BT_HCI_ERR_SUCCESS;
2465                 cmd_complete(btdev, opcode, &rloed, sizeof(rloed));
2466                 break;
2467
2468         case BT_HCI_CMD_READ_SYNC_TRAIN_PARAMS:
2469                 if (btdev->type != BTDEV_TYPE_BREDRLE)
2470                         goto unsupported;
2471                 rstp.status = BT_HCI_ERR_SUCCESS;
2472                 rstp.interval = cpu_to_le16(btdev->sync_train_interval);
2473                 rstp.timeout = cpu_to_le32(btdev->sync_train_timeout);
2474                 rstp.service_data = btdev->sync_train_service_data;
2475                 cmd_complete(btdev, opcode, &rstp, sizeof(rstp));
2476                 break;
2477
2478         case BT_HCI_CMD_READ_LOCAL_VERSION:
2479                 rlv.status = BT_HCI_ERR_SUCCESS;
2480                 rlv.hci_ver = btdev->version;
2481                 rlv.hci_rev = cpu_to_le16(btdev->revision);
2482                 rlv.lmp_ver = btdev->version;
2483                 rlv.manufacturer = cpu_to_le16(btdev->manufacturer);
2484                 rlv.lmp_subver = cpu_to_le16(btdev->revision);
2485                 cmd_complete(btdev, opcode, &rlv, sizeof(rlv));
2486                 break;
2487
2488         case BT_HCI_CMD_READ_LOCAL_COMMANDS:
2489                 rlc.status = BT_HCI_ERR_SUCCESS;
2490                 memcpy(rlc.commands, btdev->commands, 64);
2491                 cmd_complete(btdev, opcode, &rlc, sizeof(rlc));
2492                 break;
2493
2494         case BT_HCI_CMD_READ_LOCAL_FEATURES:
2495                 rlf.status = BT_HCI_ERR_SUCCESS;
2496                 memcpy(rlf.features, btdev->features, 8);
2497                 cmd_complete(btdev, opcode, &rlf, sizeof(rlf));
2498                 break;
2499
2500         case BT_HCI_CMD_READ_LOCAL_EXT_FEATURES:
2501                 if (btdev->type == BTDEV_TYPE_LE)
2502                         goto unsupported;
2503
2504                 page = ((const uint8_t *) data)[0];
2505
2506                 rlef.page = page;
2507                 rlef.max_page = btdev->max_page;
2508
2509                 if (page > btdev->max_page) {
2510                         rlef.status = BT_HCI_ERR_INVALID_PARAMETERS;
2511                         memset(rlef.features, 0, 8);
2512                         cmd_complete(btdev, opcode, &rlef, sizeof(rlef));
2513                         break;
2514                 }
2515
2516                 switch (page) {
2517                 case 0x00:
2518                         rlef.status = BT_HCI_ERR_SUCCESS;
2519                         memcpy(rlef.features, btdev->features, 8);
2520                         break;
2521                 case 0x01:
2522                         rlef.status = BT_HCI_ERR_SUCCESS;
2523                         btdev_get_host_features(btdev, rlef.features);
2524                         break;
2525                 case 0x02:
2526                         rlef.status = BT_HCI_ERR_SUCCESS;
2527                         memcpy(rlef.features, btdev->feat_page_2, 8);
2528                         break;
2529                 default:
2530                         rlef.status = BT_HCI_ERR_INVALID_PARAMETERS;
2531                         memset(rlef.features, 0, 8);
2532                         break;
2533                 }
2534                 cmd_complete(btdev, opcode, &rlef, sizeof(rlef));
2535                 break;
2536
2537         case BT_HCI_CMD_READ_BUFFER_SIZE:
2538                 rbs.status = BT_HCI_ERR_SUCCESS;
2539                 rbs.acl_mtu = cpu_to_le16(btdev->acl_mtu);
2540                 rbs.sco_mtu = 0;
2541                 rbs.acl_max_pkt = cpu_to_le16(btdev->acl_max_pkt);
2542                 rbs.sco_max_pkt = cpu_to_le16(0);
2543                 cmd_complete(btdev, opcode, &rbs, sizeof(rbs));
2544                 break;
2545
2546         case BT_HCI_CMD_READ_COUNTRY_CODE:
2547                 rcc.status = BT_HCI_ERR_SUCCESS;
2548                 rcc.code = btdev->country_code;
2549                 cmd_complete(btdev, opcode, &rcc, sizeof(rcc));
2550                 break;
2551
2552         case BT_HCI_CMD_READ_BD_ADDR:
2553                 rba.status = BT_HCI_ERR_SUCCESS;
2554                 memcpy(rba.bdaddr, btdev->bdaddr, 6);
2555                 cmd_complete(btdev, opcode, &rba, sizeof(rba));
2556                 break;
2557
2558         case BT_HCI_CMD_READ_DATA_BLOCK_SIZE:
2559                 if (btdev->type == BTDEV_TYPE_LE)
2560                         goto unsupported;
2561                 rdbs.status = BT_HCI_ERR_SUCCESS;
2562                 rdbs.max_acl_len = cpu_to_le16(btdev->acl_mtu);
2563                 rdbs.block_len = cpu_to_le16(btdev->acl_mtu);
2564                 rdbs.num_blocks = cpu_to_le16(btdev->acl_max_pkt);
2565                 cmd_complete(btdev, opcode, &rdbs, sizeof(rdbs));
2566                 break;
2567
2568         case BT_HCI_CMD_READ_LOCAL_CODECS:
2569                 if (btdev->type == BTDEV_TYPE_LE)
2570                         goto unsupported;
2571                 rlsc = alloca(sizeof(*rlsc) + 7);
2572                 rlsc->status = BT_HCI_ERR_SUCCESS;
2573                 rlsc->num_codecs = 0x06;
2574                 rlsc->codec[0] = 0x00;
2575                 rlsc->codec[1] = 0x01;
2576                 rlsc->codec[2] = 0x02;
2577                 rlsc->codec[3] = 0x03;
2578                 rlsc->codec[4] = 0x04;
2579                 rlsc->codec[5] = 0x05;
2580                 rlsc->codec[6] = 0x00;
2581                 cmd_complete(btdev, opcode, rlsc, sizeof(*rlsc) + 7);
2582                 break;
2583
2584         case BT_HCI_CMD_READ_RSSI:
2585                 rrssi = data;
2586
2587                 rrssi_rsp.status = BT_HCI_ERR_SUCCESS;
2588                 rrssi_rsp.handle = rrssi->handle;
2589                 rrssi_rsp.rssi = -1; /* non-zero so we can see it in tester */
2590                 cmd_complete(btdev, opcode, &rrssi_rsp, sizeof(rrssi_rsp));
2591                 break;
2592
2593         case BT_HCI_CMD_READ_TX_POWER:
2594                 rtxp = data;
2595
2596                 switch (rtxp->type) {
2597                 case 0x00:
2598                         rtxp_rsp.status = BT_HCI_ERR_SUCCESS;
2599                         rtxp_rsp.level =  -1; /* non-zero */
2600                         break;
2601
2602                 case 0x01:
2603                         rtxp_rsp.status = BT_HCI_ERR_SUCCESS;
2604                         rtxp_rsp.level = 4; /* max for class 2 radio */
2605                         break;
2606
2607                 default:
2608                         rtxp_rsp.level = 0;
2609                         rtxp_rsp.status = BT_HCI_ERR_INVALID_PARAMETERS;
2610                         break;
2611                 }
2612
2613                 rtxp_rsp.handle = rtxp->handle;
2614                 cmd_complete(btdev, opcode, &rtxp_rsp, sizeof(rtxp_rsp));
2615                 break;
2616
2617         case BT_HCI_CMD_READ_LOCAL_AMP_INFO:
2618                 if (btdev->type != BTDEV_TYPE_AMP)
2619                         goto unsupported;
2620                 rlai.status = BT_HCI_ERR_SUCCESS;
2621                 rlai.amp_status = 0x01;         /* Used for Bluetooth only */
2622                 rlai.total_bw = cpu_to_le32(0);
2623                 rlai.max_bw = cpu_to_le32(0);
2624                 rlai.min_latency = cpu_to_le32(0);
2625                 rlai.max_pdu = cpu_to_le32(672);
2626                 rlai.amp_type = 0x01;           /* 802.11 AMP Controller */
2627                 rlai.pal_cap = cpu_to_le16(0x0000);
2628                 rlai.max_assoc_len = cpu_to_le16(672);
2629                 rlai.max_flush_to = cpu_to_le32(0xffffffff);
2630                 rlai.be_flush_to = cpu_to_le32(0xffffffff);
2631                 cmd_complete(btdev, opcode, &rlai, sizeof(rlai));
2632                 break;
2633
2634         case BT_HCI_CMD_READ_LOCAL_AMP_ASSOC:
2635                 if (btdev->type != BTDEV_TYPE_AMP)
2636                         goto unsupported;
2637                 rlaa_cmd = data;
2638                 rlaa_rsp.status = BT_HCI_ERR_SUCCESS;
2639                 rlaa_rsp.phy_handle = rlaa_cmd->phy_handle;
2640                 rlaa_rsp.remain_assoc_len = cpu_to_le16(1);
2641                 rlaa_rsp.assoc_fragment[0] = 0x42;
2642                 memset(rlaa_rsp.assoc_fragment + 1, 0,
2643                                         sizeof(rlaa_rsp.assoc_fragment) - 1);
2644                 cmd_complete(btdev, opcode, &rlaa_rsp, sizeof(rlaa_rsp));
2645                 break;
2646
2647         case BT_HCI_CMD_GET_MWS_TRANSPORT_CONFIG:
2648                 if (btdev->type == BTDEV_TYPE_LE)
2649                         goto unsupported;
2650                 gmtc = alloca(sizeof(*gmtc));
2651                 gmtc->status = BT_HCI_ERR_SUCCESS;
2652                 gmtc->num_transports = 0x00;
2653                 cmd_complete(btdev, opcode, gmtc, sizeof(*gmtc));
2654                 break;
2655
2656         case BT_HCI_CMD_SET_EVENT_MASK_PAGE2:
2657                 if (btdev->type != BTDEV_TYPE_BREDRLE)
2658                         goto unsupported;
2659                 semp2 = data;
2660                 memcpy(btdev->event_mask_page2, semp2->mask, 8);
2661                 status = BT_HCI_ERR_SUCCESS;
2662                 cmd_complete(btdev, opcode, &status, sizeof(status));
2663                 break;
2664
2665         case BT_HCI_CMD_LE_SET_EVENT_MASK:
2666                 if (btdev->type == BTDEV_TYPE_BREDR)
2667                         goto unsupported;
2668                 lsem = data;
2669                 memcpy(btdev->le_event_mask, lsem->mask, 8);
2670                 status = BT_HCI_ERR_SUCCESS;
2671                 cmd_complete(btdev, opcode, &status, sizeof(status));
2672                 break;
2673
2674         case BT_HCI_CMD_LE_READ_BUFFER_SIZE:
2675                 if (btdev->type == BTDEV_TYPE_BREDR)
2676                         goto unsupported;
2677                 lrbs.status = BT_HCI_ERR_SUCCESS;
2678                 lrbs.le_mtu = cpu_to_le16(btdev->acl_mtu);
2679                 lrbs.le_max_pkt = btdev->acl_max_pkt;
2680                 cmd_complete(btdev, opcode, &lrbs, sizeof(lrbs));
2681                 break;
2682
2683         case BT_HCI_CMD_LE_READ_LOCAL_FEATURES:
2684                 if (btdev->type == BTDEV_TYPE_BREDR)
2685                         goto unsupported;
2686                 lrlf.status = BT_HCI_ERR_SUCCESS;
2687                 memcpy(lrlf.features, btdev->le_features, 8);
2688                 cmd_complete(btdev, opcode, &lrlf, sizeof(lrlf));
2689                 break;
2690
2691         case BT_HCI_CMD_LE_SET_RANDOM_ADDRESS:
2692                 if (btdev->type == BTDEV_TYPE_BREDR)
2693                         goto unsupported;
2694                 lsra = data;
2695                 memcpy(btdev->random_addr, lsra->addr, 6);
2696                 status = BT_HCI_ERR_SUCCESS;
2697                 cmd_complete(btdev, opcode, &status, sizeof(status));
2698                 break;
2699
2700         case BT_HCI_CMD_LE_SET_ADV_PARAMETERS:
2701                 if (btdev->type == BTDEV_TYPE_BREDR)
2702                         goto unsupported;
2703
2704                 if (btdev->le_adv_enable) {
2705                         status = BT_HCI_ERR_COMMAND_DISALLOWED;
2706                         cmd_complete(btdev, opcode, &status, sizeof(status));
2707                         break;
2708                 }
2709
2710                 lsap = data;
2711                 btdev->le_adv_type = lsap->type;
2712                 btdev->le_adv_own_addr = lsap->own_addr_type;
2713                 btdev->le_adv_direct_addr_type = lsap->direct_addr_type;
2714                 memcpy(btdev->le_adv_direct_addr, lsap->direct_addr, 6);
2715
2716                 status = BT_HCI_ERR_SUCCESS;
2717                 cmd_complete(btdev, opcode, &status, sizeof(status));
2718                 break;
2719
2720         case BT_HCI_CMD_LE_READ_ADV_TX_POWER:
2721                 if (btdev->type == BTDEV_TYPE_BREDR)
2722                         goto unsupported;
2723                 lratp.status = BT_HCI_ERR_SUCCESS;
2724                 lratp.level = 0;
2725                 cmd_complete(btdev, opcode, &lratp, sizeof(lratp));
2726                 break;
2727
2728         case BT_HCI_CMD_LE_SET_ADV_ENABLE:
2729                 if (btdev->type == BTDEV_TYPE_BREDR)
2730                         goto unsupported;
2731                 lsae = data;
2732                 if (btdev->le_adv_enable == lsae->enable)
2733                         status = BT_HCI_ERR_COMMAND_DISALLOWED;
2734                 else {
2735                         btdev->le_adv_enable = lsae->enable;
2736                         status = BT_HCI_ERR_SUCCESS;
2737                 }
2738                 cmd_complete(btdev, opcode, &status, sizeof(status));
2739                 if (status == BT_HCI_ERR_SUCCESS && btdev->le_adv_enable)
2740                         le_set_adv_enable_complete(btdev);
2741                 break;
2742
2743         case BT_HCI_CMD_LE_SET_SCAN_PARAMETERS:
2744                 if (btdev->type == BTDEV_TYPE_BREDR)
2745                         goto unsupported;
2746
2747                 lssp = data;
2748
2749                 if (btdev->le_scan_enable)
2750                         status = BT_HCI_ERR_COMMAND_DISALLOWED;
2751                 else {
2752                         status = BT_HCI_ERR_SUCCESS;
2753                         btdev->le_scan_type = lssp->type;
2754                         btdev->le_scan_own_addr_type = lssp->own_addr_type;
2755                 }
2756
2757                 cmd_complete(btdev, opcode, &status, sizeof(status));
2758                 break;
2759
2760         case BT_HCI_CMD_LE_SET_SCAN_ENABLE:
2761                 if (btdev->type == BTDEV_TYPE_BREDR)
2762                         goto unsupported;
2763                 lsse = data;
2764                 if (btdev->le_scan_enable == lsse->enable)
2765                         status = BT_HCI_ERR_COMMAND_DISALLOWED;
2766                 else {
2767                         btdev->le_scan_enable = lsse->enable;
2768                         btdev->le_filter_dup = lsse->filter_dup;
2769                         status = BT_HCI_ERR_SUCCESS;
2770                 }
2771                 cmd_complete(btdev, opcode, &status, sizeof(status));
2772                 if (status == BT_HCI_ERR_SUCCESS && btdev->le_scan_enable)
2773                         le_set_scan_enable_complete(btdev);
2774                 break;
2775
2776         case BT_HCI_CMD_LE_CREATE_CONN:
2777                 if (btdev->type == BTDEV_TYPE_BREDR)
2778                         goto unsupported;
2779                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
2780                 break;
2781
2782         case BT_HCI_CMD_LE_READ_WHITE_LIST_SIZE:
2783                 if (btdev->type == BTDEV_TYPE_BREDR)
2784                         goto unsupported;
2785                 lrwls.status = BT_HCI_ERR_SUCCESS;
2786                 lrwls.size = 0;
2787                 cmd_complete(btdev, opcode, &lrwls, sizeof(lrwls));
2788                 break;
2789
2790         case BT_HCI_CMD_LE_CONN_UPDATE:
2791                 if (btdev->type == BTDEV_TYPE_BREDR)
2792                         goto unsupported;
2793                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
2794                 break;
2795
2796         case BT_HCI_CMD_LE_CLEAR_WHITE_LIST:
2797                 if (btdev->type == BTDEV_TYPE_BREDR)
2798                         goto unsupported;
2799                 status = BT_HCI_ERR_SUCCESS;
2800                 cmd_complete(btdev, opcode, &status, sizeof(status));
2801                 break;
2802
2803         case BT_HCI_CMD_LE_READ_SUPPORTED_STATES:
2804                 if (btdev->type == BTDEV_TYPE_BREDR)
2805                         goto unsupported;
2806                 lrss.status = BT_HCI_ERR_SUCCESS;
2807                 memcpy(lrss.states, btdev->le_states, 8);
2808                 cmd_complete(btdev, opcode, &lrss, sizeof(lrss));
2809                 break;
2810
2811         case BT_HCI_CMD_LE_SET_ADV_DATA:
2812                 if (btdev->type == BTDEV_TYPE_BREDR)
2813                         goto unsupported;
2814                 lsad = data;
2815                 btdev->le_adv_data_len = lsad->len;
2816                 memcpy(btdev->le_adv_data, lsad->data, 31);
2817                 status = BT_HCI_ERR_SUCCESS;
2818                 cmd_complete(btdev, opcode, &status, sizeof(status));
2819                 break;
2820
2821         case BT_HCI_CMD_LE_SET_SCAN_RSP_DATA:
2822                 if (btdev->type == BTDEV_TYPE_BREDR)
2823                         goto unsupported;
2824                 lssrd = data;
2825                 btdev->le_scan_data_len = lssrd->len;
2826                 memcpy(btdev->le_scan_data, lssrd->data, 31);
2827                 status = BT_HCI_ERR_SUCCESS;
2828                 cmd_complete(btdev, opcode, &status, sizeof(status));
2829                 break;
2830
2831         case BT_HCI_CMD_LE_RAND:
2832                 if (btdev->type == BTDEV_TYPE_BREDR)
2833                         goto unsupported;
2834                 lr.status = BT_HCI_ERR_SUCCESS;
2835                 lr.number = rand();
2836                 cmd_complete(btdev, opcode, &lr, sizeof(lr));
2837                 break;
2838
2839         case BT_HCI_CMD_LE_START_ENCRYPT:
2840                 if (btdev->type == BTDEV_TYPE_BREDR)
2841                         goto unsupported;
2842                 lse = data;
2843                 memcpy(btdev->le_ltk, lse->ltk, 16);
2844                 le_start_encrypt_complete(btdev);
2845                 break;
2846
2847         case BT_HCI_CMD_LE_LTK_REQ_REPLY:
2848                 if (btdev->type == BTDEV_TYPE_BREDR)
2849                         goto unsupported;
2850                 llrr = data;
2851                 memcpy(btdev->le_ltk, llrr->ltk, 16);
2852                 le_encrypt_complete(btdev);
2853                 break;
2854
2855         case BT_HCI_CMD_LE_LTK_REQ_NEG_REPLY:
2856                 if (btdev->type == BTDEV_TYPE_BREDR)
2857                         goto unsupported;
2858                 ltk_neg_reply_complete(btdev);
2859                 break;
2860
2861         case BT_HCI_CMD_SETUP_SYNC_CONN:
2862                 if (btdev->type == BTDEV_TYPE_LE)
2863                         goto unsupported;
2864                 ssc = data;
2865                 status = BT_HCI_ERR_SUCCESS;
2866                 cmd_status(btdev, BT_HCI_ERR_SUCCESS, opcode);
2867                 sync_conn_complete(btdev, ssc->voice_setting,
2868                                                         BT_HCI_ERR_SUCCESS);
2869                 break;
2870
2871         case BT_HCI_CMD_ADD_SCO_CONN:
2872                 if (btdev->type == BTDEV_TYPE_LE)
2873                         goto unsupported;
2874                 sco_conn_complete(btdev, BT_HCI_ERR_SUCCESS);
2875                 break;
2876
2877         case BT_HCI_CMD_ENABLE_DUT_MODE:
2878                 status = BT_HCI_ERR_SUCCESS;
2879                 cmd_complete(btdev, opcode, &status, sizeof(status));
2880                 break;
2881
2882         case BT_HCI_CMD_WRITE_SSP_DEBUG_MODE:
2883                 if (btdev->type == BTDEV_TYPE_LE)
2884                         goto unsupported;
2885                 wsdm = data;
2886                 btdev->ssp_debug_mode = wsdm->mode;
2887                 status = BT_HCI_ERR_SUCCESS;
2888                 cmd_complete(btdev, opcode, &status, sizeof(status));
2889                 break;
2890
2891         case BT_HCI_CMD_LE_RECEIVER_TEST:
2892                 if (btdev->type == BTDEV_TYPE_BREDR)
2893                         goto unsupported;
2894                 status = BT_HCI_ERR_SUCCESS;
2895                 cmd_complete(btdev, opcode, &status, sizeof(status));
2896                 break;
2897
2898         case BT_HCI_CMD_LE_TRANSMITTER_TEST:
2899                 if (btdev->type == BTDEV_TYPE_BREDR)
2900                         goto unsupported;
2901                 status = BT_HCI_ERR_SUCCESS;
2902                 cmd_complete(btdev, opcode, &status, sizeof(status));
2903                 break;
2904
2905         case BT_HCI_CMD_LE_TEST_END:
2906                 if (btdev->type == BTDEV_TYPE_BREDR)
2907                         goto unsupported;
2908                 lte.status = BT_HCI_ERR_SUCCESS;
2909                 lte.num_packets = 0;
2910                 cmd_complete(btdev, opcode, &lte, sizeof(lte));
2911                 break;
2912
2913         default:
2914                 goto unsupported;
2915         }
2916
2917         return;
2918
2919 unsupported:
2920         printf("Unsupported command 0x%4.4x\n", opcode);
2921         hexdump(data, len);
2922         cmd_status(btdev, BT_HCI_ERR_UNKNOWN_COMMAND, opcode);
2923 }
2924
2925 static void default_cmd_completion(struct btdev *btdev, uint16_t opcode,
2926                                                 const void *data, uint8_t len)
2927 {
2928         const struct bt_hci_cmd_create_conn *cc;
2929         const struct bt_hci_cmd_disconnect *dc;
2930         const struct bt_hci_cmd_create_conn_cancel *ccc;
2931         const struct bt_hci_cmd_accept_conn_request *acr;
2932         const struct bt_hci_cmd_reject_conn_request *rcr;
2933         const struct bt_hci_cmd_auth_requested *ar;
2934         const struct bt_hci_cmd_set_conn_encrypt *sce;
2935         const struct bt_hci_cmd_link_key_request_reply *lkrr;
2936         const struct bt_hci_cmd_link_key_request_neg_reply *lkrnr;
2937         const struct bt_hci_cmd_pin_code_request_neg_reply *pcrnr;
2938         const struct bt_hci_cmd_pin_code_request_reply *pcrr;
2939         const struct bt_hci_cmd_remote_name_request *rnr;
2940         const struct bt_hci_cmd_remote_name_request_cancel *rnrc;
2941         const struct bt_hci_cmd_read_remote_features *rrf;
2942         const struct bt_hci_cmd_read_remote_ext_features *rref;
2943         const struct bt_hci_cmd_read_remote_version *rrv;
2944         const struct bt_hci_cmd_read_clock_offset *rco;
2945         const struct bt_hci_cmd_le_create_conn *lecc;
2946         const struct bt_hci_cmd_le_conn_update *lecu;
2947
2948         switch (opcode) {
2949         case BT_HCI_CMD_INQUIRY:
2950                 if (btdev->type == BTDEV_TYPE_LE)
2951                         return;
2952                 inquiry_cmd(btdev, data);
2953                 break;
2954
2955         case BT_HCI_CMD_CREATE_CONN:
2956                 if (btdev->type == BTDEV_TYPE_LE)
2957                         return;
2958                 cc = data;
2959                 conn_request(btdev, cc->bdaddr);
2960                 break;
2961
2962         case BT_HCI_CMD_DISCONNECT:
2963                 dc = data;
2964                 disconnect_complete(btdev, le16_to_cpu(dc->handle), dc->reason);
2965                 break;
2966
2967         case BT_HCI_CMD_CREATE_CONN_CANCEL:
2968                 if (btdev->type == BTDEV_TYPE_LE)
2969                         return;
2970                 ccc = data;
2971                 conn_complete(btdev, ccc->bdaddr, BT_HCI_ERR_UNKNOWN_CONN_ID);
2972                 break;
2973
2974         case BT_HCI_CMD_ACCEPT_CONN_REQUEST:
2975                 if (btdev->type == BTDEV_TYPE_LE)
2976                         return;
2977                 acr = data;
2978                 accept_conn_request_complete(btdev, acr->bdaddr);
2979                 break;
2980
2981         case BT_HCI_CMD_REJECT_CONN_REQUEST:
2982                 if (btdev->type == BTDEV_TYPE_LE)
2983                         return;
2984                 rcr = data;
2985                 conn_complete(btdev, rcr->bdaddr, BT_HCI_ERR_UNKNOWN_CONN_ID);
2986                 break;
2987
2988         case BT_HCI_CMD_LINK_KEY_REQUEST_REPLY:
2989                 if (btdev->type == BTDEV_TYPE_LE)
2990                         return;
2991                 lkrr = data;
2992                 link_key_req_reply_complete(btdev, lkrr->bdaddr, lkrr->link_key);
2993                 break;
2994
2995         case BT_HCI_CMD_LINK_KEY_REQUEST_NEG_REPLY:
2996                 if (btdev->type == BTDEV_TYPE_LE)
2997                         return;
2998                 lkrnr = data;
2999                 link_key_req_neg_reply_complete(btdev, lkrnr->bdaddr);
3000                 break;
3001
3002         case BT_HCI_CMD_PIN_CODE_REQUEST_REPLY:
3003                 if (btdev->type == BTDEV_TYPE_LE)
3004                         return;
3005                 pcrr = data;
3006                 pin_code_req_reply_complete(btdev, pcrr->bdaddr, pcrr->pin_len,
3007                                                         pcrr->pin_code);
3008                 break;
3009
3010         case BT_HCI_CMD_PIN_CODE_REQUEST_NEG_REPLY:
3011                 if (btdev->type == BTDEV_TYPE_LE)
3012                         return;
3013                 pcrnr = data;
3014                 pin_code_req_neg_reply_complete(btdev, pcrnr->bdaddr);
3015                 break;
3016
3017         case BT_HCI_CMD_AUTH_REQUESTED:
3018                 if (btdev->type == BTDEV_TYPE_LE)
3019                         return;
3020                 ar = data;
3021                 auth_request_complete(btdev, le16_to_cpu(ar->handle));
3022                 break;
3023
3024         case BT_HCI_CMD_SET_CONN_ENCRYPT:
3025                 if (btdev->type == BTDEV_TYPE_LE)
3026                         return;
3027                 sce = data;
3028                 if (btdev->conn) {
3029                         uint8_t mode;
3030
3031                         if (!sce->encr_mode)
3032                                 mode = 0x00;
3033                         else if (btdev->secure_conn_support &&
3034                                         btdev->conn->secure_conn_support)
3035                                 mode = 0x02;
3036                         else
3037                                 mode = 0x01;
3038
3039                         encrypt_change(btdev, mode, BT_HCI_ERR_SUCCESS);
3040                         encrypt_change(btdev->conn, mode, BT_HCI_ERR_SUCCESS);
3041                 }
3042                 break;
3043
3044         case BT_HCI_CMD_REMOTE_NAME_REQUEST:
3045                 if (btdev->type == BTDEV_TYPE_LE)
3046                         return;
3047                 rnr = data;
3048                 name_request_complete(btdev, rnr->bdaddr, BT_HCI_ERR_SUCCESS);
3049                 break;
3050
3051         case BT_HCI_CMD_REMOTE_NAME_REQUEST_CANCEL:
3052                 if (btdev->type == BTDEV_TYPE_LE)
3053                         return;
3054                 rnrc = data;
3055                 name_request_complete(btdev, rnrc->bdaddr,
3056                                                 BT_HCI_ERR_UNKNOWN_CONN_ID);
3057                 break;
3058
3059         case BT_HCI_CMD_READ_REMOTE_FEATURES:
3060                 if (btdev->type == BTDEV_TYPE_LE)
3061                         return;
3062                 rrf = data;
3063                 remote_features_complete(btdev, le16_to_cpu(rrf->handle));
3064                 break;
3065
3066         case BT_HCI_CMD_READ_REMOTE_EXT_FEATURES:
3067                 if (btdev->type == BTDEV_TYPE_LE)
3068                         return;
3069                 rref = data;
3070                 remote_ext_features_complete(btdev, le16_to_cpu(rref->handle),
3071                                                                 rref->page);
3072                 break;
3073
3074         case BT_HCI_CMD_READ_REMOTE_VERSION:
3075                 rrv = data;
3076                 remote_version_complete(btdev, le16_to_cpu(rrv->handle));
3077                 break;
3078
3079         case BT_HCI_CMD_READ_CLOCK_OFFSET:
3080                 if (btdev->type == BTDEV_TYPE_LE)
3081                         return;
3082                 rco = data;
3083                 remote_clock_offset_complete(btdev, le16_to_cpu(rco->handle));
3084                 break;
3085
3086         case BT_HCI_CMD_LE_CREATE_CONN:
3087                 if (btdev->type == BTDEV_TYPE_BREDR)
3088                         return;
3089                 lecc = data;
3090                 btdev->le_scan_own_addr_type = lecc->own_addr_type;
3091                 le_conn_request(btdev, lecc->peer_addr, lecc->peer_addr_type);
3092                 break;
3093
3094         case BT_HCI_CMD_LE_CONN_UPDATE:
3095                 if (btdev->type == BTDEV_TYPE_BREDR)
3096                         return;
3097                 lecu = data;
3098                 le_conn_update(btdev, le16_to_cpu(lecu->handle),
3099                                 le16_to_cpu(lecu->min_interval),
3100                                 le16_to_cpu(lecu->max_interval),
3101                                 le16_to_cpu(lecu->latency),
3102                                 le16_to_cpu(lecu->supv_timeout),
3103                                 le16_to_cpu(lecu->min_length),
3104                                 le16_to_cpu(lecu->max_length));
3105                 break;
3106         }
3107 }
3108
3109 struct btdev_callback {
3110         void (*function)(btdev_callback callback, uint8_t response,
3111                                 uint8_t status, const void *data, uint8_t len);
3112         void *user_data;
3113         uint16_t opcode;
3114         const void *data;
3115         uint8_t len;
3116 };
3117
3118 void btdev_command_response(btdev_callback callback, uint8_t response,
3119                                 uint8_t status, const void *data, uint8_t len)
3120 {
3121         callback->function(callback, response, status, data, len);
3122 }
3123
3124 static void handler_callback(btdev_callback callback, uint8_t response,
3125                                 uint8_t status, const void *data, uint8_t len)
3126 {
3127         struct btdev *btdev = callback->user_data;
3128
3129         switch (response) {
3130         case BTDEV_RESPONSE_DEFAULT:
3131                 if (!run_hooks(btdev, BTDEV_HOOK_PRE_CMD, callback->opcode,
3132                                                 callback->data, callback->len))
3133                         return;
3134                 default_cmd(btdev, callback->opcode,
3135                                         callback->data, callback->len);
3136
3137                 if (!run_hooks(btdev, BTDEV_HOOK_PRE_EVT, callback->opcode,
3138                                                 callback->data, callback->len))
3139                         return;
3140                 default_cmd_completion(btdev, callback->opcode,
3141                                         callback->data, callback->len);
3142                 break;
3143         case BTDEV_RESPONSE_COMMAND_STATUS:
3144                 cmd_status(btdev, status, callback->opcode);
3145                 break;
3146         case BTDEV_RESPONSE_COMMAND_COMPLETE:
3147                 cmd_complete(btdev, callback->opcode, data, len);
3148                 break;
3149         default:
3150                 cmd_status(btdev, BT_HCI_ERR_UNKNOWN_COMMAND,
3151                                                 callback->opcode);
3152                 break;
3153         }
3154 }
3155
3156 static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
3157 {
3158         struct btdev_callback callback;
3159         const struct bt_hci_cmd_hdr *hdr = data;
3160
3161         if (len < sizeof(*hdr))
3162                 return;
3163
3164         callback.function = handler_callback;
3165         callback.user_data = btdev;
3166         callback.opcode = le16_to_cpu(hdr->opcode);
3167         callback.data = data + sizeof(*hdr);
3168         callback.len = hdr->plen;
3169
3170         if (btdev->command_handler)
3171                 btdev->command_handler(callback.opcode,
3172                                         callback.data, callback.len,
3173                                         &callback, btdev->command_data);
3174         else {
3175                 if (!run_hooks(btdev, BTDEV_HOOK_PRE_CMD, callback.opcode,
3176                                                 callback.data, callback.len))
3177                         return;
3178                 default_cmd(btdev, callback.opcode,
3179                                         callback.data, callback.len);
3180
3181                 if (!run_hooks(btdev, BTDEV_HOOK_PRE_EVT, callback.opcode,
3182                                                 callback.data, callback.len))
3183                         return;
3184                 default_cmd_completion(btdev, callback.opcode,
3185                                         callback.data, callback.len);
3186         }
3187 }
3188
3189 void btdev_receive_h4(struct btdev *btdev, const void *data, uint16_t len)
3190 {
3191         uint8_t pkt_type;
3192         struct iovec iov;
3193
3194         if (!btdev)
3195                 return;
3196
3197         if (len < 1)
3198                 return;
3199
3200         pkt_type = ((const uint8_t *) data)[0];
3201
3202         switch (pkt_type) {
3203         case BT_H4_CMD_PKT:
3204                 process_cmd(btdev, data + 1, len - 1);
3205                 break;
3206         case BT_H4_ACL_PKT:
3207                 if (btdev->conn) {
3208                         iov.iov_base = (void *) data;
3209                         iov.iov_len = len;
3210                         send_packet(btdev->conn, &iov, 1);
3211                 }
3212                 num_completed_packets(btdev);
3213                 break;
3214         default:
3215                 printf("Unsupported packet 0x%2.2x\n", pkt_type);
3216                 break;
3217         }
3218 }
3219
3220 int btdev_add_hook(struct btdev *btdev, enum btdev_hook_type type,
3221                                 uint16_t opcode, btdev_hook_func handler,
3222                                 void *user_data)
3223 {
3224         int i;
3225
3226         if (!btdev)
3227                 return -1;
3228
3229         if (get_hook_index(btdev, type, opcode) > 0)
3230                 return -1;
3231
3232         for (i = 0; i < MAX_HOOK_ENTRIES; i++) {
3233                 if (btdev->hook_list[i] == NULL) {
3234                         btdev->hook_list[i] = malloc(sizeof(struct hook));
3235                         if (btdev->hook_list[i] == NULL)
3236                                 return -1;
3237
3238                         btdev->hook_list[i]->handler = handler;
3239                         btdev->hook_list[i]->user_data = user_data;
3240                         btdev->hook_list[i]->opcode = opcode;
3241                         btdev->hook_list[i]->type = type;
3242                         return i;
3243                 }
3244         }
3245
3246         return -1;
3247 }
3248
3249 bool btdev_del_hook(struct btdev *btdev, enum btdev_hook_type type,
3250                                                                 uint16_t opcode)
3251 {
3252         int i;
3253
3254         if (!btdev)
3255                 return false;
3256
3257         for (i = 0; i < MAX_HOOK_ENTRIES; i++) {
3258                 if (btdev->hook_list[i] == NULL)
3259                         continue;
3260
3261                 if (btdev->hook_list[i]->type != type ||
3262                                         btdev->hook_list[i]->opcode != opcode)
3263                         continue;
3264
3265                 free(btdev->hook_list[i]);
3266                 btdev->hook_list[i] = NULL;
3267
3268                 return true;
3269         }
3270
3271         return false;
3272 }