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