emulator/le.c

   1 // SPDX-License-Identifier: LGPL-2.1-or-later
   2 /*
   3  *
   4  *  BlueZ - Bluetooth protocol stack for Linux
   5  *
   6  *  Copyright (C) 2011-2012  Intel Corporation
   7  *  Copyright (C) 2004-2010  Marcel Holtmann <marcel@holtmann.org>
   8  *
   9  *
  10  */
  11
  12 #ifdef HAVE_CONFIG_H
  13 #include <config.h>
  14 #endif
  15
  16 #include <fcntl.h>
  17 #include <unistd.h>
  18 #include <stdlib.h>
  19 #include <string.h>
  20 #include <sys/socket.h>
  21 #include <sys/un.h>
  22 #include <sys/uio.h>
  23 #include <time.h>
  24
  25 #include "lib/bluetooth.h"
  26 #include "lib/hci.h"
  27
  28 #include "src/shared/util.h"
  29 #include "src/shared/crypto.h"
  30 #include "src/shared/ecc.h"
  31 #include "src/shared/mainloop.h"
  32 #include "monitor/bt.h"
  33
  34 #include "phy.h"
  35 #include "le.h"
  36
  37 #define ACCEPT_LIST_SIZE        16
  38 #define RESOLV_LIST_SIZE        16
  39 #define SCAN_CACHE_SIZE         64
  40
  41 #define DEFAULT_TX_LEN          0x001b
  42 #define DEFAULT_TX_TIME         0x0148
  43 #define MAX_TX_LEN              0x00fb
  44 #define MAX_TX_TIME             0x0848
  45 #define MAX_RX_LEN              0x00fb
  46 #define MAX_RX_TIME             0x0848
  47
  48 #define DEFAULT_ALL_PHYS        0x03
  49 #define DEFAULT_TX_PHYS         0x00
  50 #define DEFAULT_RX_PHYS         0x00
  51
  52 struct bt_peer {
  53         uint8_t  addr_type;
  54         uint8_t  addr[6];
  55 };
  56
  57 struct bt_le {
  58         volatile int ref_count;
  59         int vhci_fd;
  60         struct bt_phy *phy;
  61         struct bt_crypto *crypto;
  62         int adv_timeout_id;
  63         int scan_timeout_id;
  64         bool scan_window_active;
  65         uint8_t scan_chan_idx;
  66
  67         uint8_t  event_mask[16];
  68         uint16_t manufacturer;
  69         uint8_t  commands[64];
  70         uint8_t  features[8];
  71         uint8_t  bdaddr[6];
  72
  73         uint8_t  le_event_mask[8];
  74         uint16_t le_mtu;
  75         uint8_t  le_max_pkt;
  76         uint8_t  le_features[8];
  77         uint8_t  le_random_addr[6];
  78         uint16_t le_adv_min_interval;
  79         uint16_t le_adv_max_interval;
  80         uint8_t  le_adv_type;
  81         uint8_t  le_adv_own_addr_type;
  82         uint8_t  le_adv_direct_addr_type;
  83         uint8_t  le_adv_direct_addr[6];
  84         uint8_t  le_adv_channel_map;
  85         uint8_t  le_adv_filter_policy;
  86         int8_t   le_adv_tx_power;
  87         uint8_t  le_adv_data_len;
  88         uint8_t  le_adv_data[31];
  89         uint8_t  le_scan_rsp_data_len;
  90         uint8_t  le_scan_rsp_data[31];
  91         uint8_t  le_adv_enable;
  92         uint8_t  le_scan_type;
  93         uint16_t le_scan_interval;
  94         uint16_t le_scan_window;
  95         uint8_t  le_scan_own_addr_type;
  96         uint8_t  le_scan_filter_policy;
  97         uint8_t  le_scan_enable;
  98         uint8_t  le_scan_filter_dup;
  99
 100         uint8_t  le_conn_peer_addr_type;
 101         uint8_t  le_conn_peer_addr[6];
 102         uint8_t  le_conn_own_addr_type;
 103         uint8_t  le_conn_enable;
 104
 105         uint8_t  le_accept_list_size;
 106         uint8_t  le_accept_list[ACCEPT_LIST_SIZE][7];
 107         uint8_t  le_states[8];
 108
 109         uint16_t le_default_tx_len;
 110         uint16_t le_default_tx_time;
 111         uint8_t  le_local_sk256[32];
 112         uint8_t  le_resolv_list[RESOLV_LIST_SIZE][39];
 113         uint8_t  le_resolv_list_size;
 114         uint8_t  le_resolv_enable;
 115         uint16_t le_resolv_timeout;
 116
 117         uint8_t  le_default_all_phys;
 118         uint8_t  le_default_tx_phys;
 119         uint8_t  le_default_rx_phys;
 120
 121         struct bt_peer scan_cache[SCAN_CACHE_SIZE];
 122         uint8_t scan_cache_count;
 123 };
 124
 125 static bool is_in_accept_list(struct bt_le *hci, uint8_t addr_type,
 126                                                         const uint8_t addr[6])
 127 {
 128         int i;
 129
 130         for (i = 0; i < hci->le_accept_list_size; i++) {
 131                 if (hci->le_accept_list[i][0] == addr_type &&
 132                                 !memcmp(&hci->le_accept_list[i][1], addr, 6))
 133                         return true;
 134         }
 135
 136         return false;
 137 }
 138
 139 static void clear_accept_list(struct bt_le *hci)
 140 {
 141         int i;
 142
 143         for (i = 0; i < hci->le_accept_list_size; i++) {
 144                 hci->le_accept_list[i][0] = 0xff;
 145                 memset(&hci->le_accept_list[i][1], 0, 6);
 146         }
 147 }
 148
 149 static void resolve_peer_addr(struct bt_le *hci, uint8_t peer_addr_type,
 150                                         const uint8_t peer_addr[6],
 151                                         uint8_t *addr_type, uint8_t addr[6])
 152 {
 153         int i;
 154
 155         if (!hci->le_resolv_enable)
 156                 goto done;
 157
 158         if (peer_addr_type != 0x01)
 159                 goto done;
 160
 161         if ((peer_addr[5] & 0xc0) != 0x40)
 162                 goto done;
 163
 164         for (i = 0; i < hci->le_resolv_list_size; i++) {
 165                 uint8_t local_hash[3];
 166
 167                 if (hci->le_resolv_list[i][0] == 0xff)
 168                         continue;
 169
 170                 bt_crypto_ah(hci->crypto, &hci->le_resolv_list[i][7],
 171                                                 peer_addr + 3, local_hash);
 172
 173                 if (!memcmp(peer_addr, local_hash, 3)) {
 174                         switch (hci->le_resolv_list[i][0]) {
 175                         case 0x00:
 176                                 *addr_type = 0x02;
 177                                 break;
 178                         case 0x01:
 179                                 *addr_type = 0x03;
 180                                 break;
 181                         default:
 182                                 continue;
 183                         }
 184                         memcpy(addr, &hci->le_resolv_list[i][1], 6);
 185                         return;
 186                 }
 187         }
 188
 189 done:
 190         *addr_type = peer_addr_type;
 191         memcpy(addr, peer_addr, 6);
 192 }
 193
 194 static void clear_resolv_list(struct bt_le *hci)
 195 {
 196         int i;
 197
 198         for (i = 0; i < hci->le_resolv_list_size; i++) {
 199                 hci->le_resolv_list[i][0] = 0xff;
 200                 memset(&hci->le_resolv_list[i][1], 0, 38);
 201         }
 202 }
 203
 204 static void reset_defaults(struct bt_le *hci)
 205 {
 206         memset(hci->event_mask, 0, sizeof(hci->event_mask));
 207         hci->event_mask[0] |= 0x10;     /* Disconnection Complete */
 208         hci->event_mask[0] |= 0x80;     /* Encryption Change */
 209         hci->event_mask[1] |= 0x08;     /* Read Remote Version Information Complete */
 210         hci->event_mask[1] |= 0x20;     /* Command Complete */
 211         hci->event_mask[1] |= 0x40;     /* Command Status */
 212         hci->event_mask[1] |= 0x80;     /* Hardware Error */
 213         hci->event_mask[2] |= 0x04;     /* Number of Completed Packets */
 214         hci->event_mask[3] |= 0x02;     /* Data Buffer Overflow */
 215         hci->event_mask[5] |= 0x80;     /* Encryption Key Refresh Complete */
 216         //hci->event_mask[7] |= 0x20;   /* LE Meta Event */
 217
 218         hci->manufacturer = 0x003f;     /* Bluetooth SIG (63) */
 219
 220         memset(hci->commands, 0, sizeof(hci->commands));
 221         hci->commands[0]  |= 0x20;      /* Disconnect */
 222         //hci->commands[2]  |= 0x80;    /* Read Remote Version Information */
 223         hci->commands[5]  |= 0x40;      /* Set Event Mask */
 224         hci->commands[5]  |= 0x80;      /* Reset */
 225         //hci->commands[10] |= 0x04;    /* Read Transmit Power Level */
 226         hci->commands[14] |= 0x08;      /* Read Local Version Information */
 227         hci->commands[14] |= 0x10;      /* Read Local Supported Commands */
 228         hci->commands[14] |= 0x20;      /* Read Local Supported Features */
 229         hci->commands[14] |= 0x80;      /* Read Buffer Size */
 230         hci->commands[15] |= 0x02;      /* Read BD ADDR */
 231         //hci->commands[15] |= 0x20;    /* Read RSSI */
 232         hci->commands[22] |= 0x04;      /* Set Event Mask Page 2 */
 233         hci->commands[25] |= 0x01;      /* LE Set Event Mask */
 234         hci->commands[25] |= 0x02;      /* LE Read Buffer Size */
 235         hci->commands[25] |= 0x04;      /* LE Read Local Supported Features */
 236         hci->commands[25] |= 0x10;      /* LE Set Random Address */
 237         hci->commands[25] |= 0x20;      /* LE Set Advertising Parameters */
 238         hci->commands[25] |= 0x40;      /* LE Read Advertising Channel TX Power */
 239         hci->commands[25] |= 0x80;      /* LE Set Advertising Data */
 240         hci->commands[26] |= 0x01;      /* LE Set Scan Response Data */
 241         hci->commands[26] |= 0x02;      /* LE Set Advertise Enable */
 242         hci->commands[26] |= 0x04;      /* LE Set Scan Parameters */
 243         hci->commands[26] |= 0x08;      /* LE Set Scan Enable */
 244         hci->commands[26] |= 0x10;      /* LE Create Connection */
 245         hci->commands[26] |= 0x20;      /* LE Create Connection Cancel */
 246         hci->commands[26] |= 0x40;      /* LE Read Accept List Size */
 247         hci->commands[26] |= 0x80;      /* LE Clear Accept List */
 248         hci->commands[27] |= 0x01;      /* LE Add Device To Accept List */
 249         hci->commands[27] |= 0x02;      /* LE Remove Device From Accept List */
 250         //hci->commands[27] |= 0x04;    /* LE Connection Update */
 251         //hci->commands[27] |= 0x08;    /* LE Set Host Channel Classification */
 252         //hci->commands[27] |= 0x10;    /* LE Read Channel Map */
 253         //hci->commands[27] |= 0x20;    /* LE Read Remote Used Features */
 254         hci->commands[27] |= 0x40;      /* LE Encrypt */
 255         hci->commands[27] |= 0x80;      /* LE Rand */
 256         //hci->commands[28] |= 0x01;    /* LE Start Encryption */
 257         //hci->commands[28] |= 0x02;    /* LE Long Term Key Request Reply */
 258         //hci->commands[28] |= 0x04;    /* LE Long Term Key Request Negative Reply */
 259         hci->commands[28] |= 0x08;      /* LE Read Supported States */
 260         //hci->commands[28] |= 0x10;    /* LE Receiver Test */
 261         //hci->commands[28] |= 0x20;    /* LE Transmitter Test */
 262         //hci->commands[28] |= 0x40;    /* LE Test End */
 263         //hci->commands[33] |= 0x10;    /* LE Remote Connection Parameter Request Reply */
 264         //hci->commands[33] |= 0x20;    /* LE Remote Connection Parameter Request Negative Reply */
 265         hci->commands[33] |= 0x40;      /* LE Set Data Length */
 266         hci->commands[33] |= 0x80;      /* LE Read Suggested Default Data Length */
 267         hci->commands[34] |= 0x01;      /* LE Write Suggested Default Data Length */
 268         hci->commands[34] |= 0x02;      /* LE Read Local P-256 Public Key */
 269         hci->commands[34] |= 0x04;      /* LE Generate DHKey */
 270         hci->commands[34] |= 0x08;      /* LE Add Device To Resolving List */
 271         hci->commands[34] |= 0x10;      /* LE Remove Device From Resolving List */
 272         hci->commands[34] |= 0x20;      /* LE Clear Resolving List */
 273         hci->commands[34] |= 0x40;      /* LE Read Resolving List Size */
 274         hci->commands[34] |= 0x80;      /* LE Read Peer Resolvable Address */
 275         hci->commands[35] |= 0x01;      /* LE Read Local Resolvable Address */
 276         hci->commands[35] |= 0x02;      /* LE Set Address Resolution Enable */
 277         hci->commands[35] |= 0x04;      /* LE Set Resolvable Private Address Timeout */
 278         hci->commands[35] |= 0x08;      /* LE Read Maximum Data Length */
 279         hci->commands[35] |= 0x10;      /* LE Read PHY */
 280         hci->commands[35] |= 0x20;      /* LE Set Default PHY */
 281         hci->commands[35] |= 0x40;      /* LE Set PHY */
 282         //hci->commands[35] |= 0x80;    /* LE Enhanced Receiver Test */
 283         //hci->commands[36] |= 0x01;    /* LE Enhanced Transmitter Test */
 284         //hci->commands[36] |= 0x02;    /* LE Set Advertising Set Random Address */
 285         //hci->commands[36] |= 0x04;    /* LE Set Extended Advertising Parameters */
 286         //hci->commands[36] |= 0x08;    /* LE Set Extended Advertising Data */
 287         //hci->commands[36] |= 0x10;    /* LE Set Extended Scan Response Data */
 288         //hci->commands[36] |= 0x20;    /* LE Set Extended Advertising Enable */
 289         //hci->commands[36] |= 0x40;    /* LE Read Maximum Advertising Data Length */
 290         //hci->commands[36] |= 0x80;    /* LE Read Number of Supported Advertising Sets */
 291         //hci->commands[37] |= 0x01;    /* LE Remove Advertising Set */
 292         //hci->commands[37] |= 0x02;    /* LE Clear Advertising Sets */
 293         //hci->commands[37] |= 0x04;    /* LE Set Periodic Advertising Parameters */
 294         //hci->commands[37] |= 0x08;    /* LE Set Periodic Advertising Data */
 295         //hci->commands[37] |= 0x10;    /* LE Set Periodic Advertising Enable */
 296         //hci->commands[37] |= 0x20;    /* LE Set Extended Scan Parameters */
 297         //hci->commands[37] |= 0x40;    /* LE Set Extended Scan Enable */
 298         //hci->commands[37] |= 0x80;    /* LE Extended Create Connection */
 299         //hci->commands[38] |= 0x01;    /* LE Periodic Advertising Create Sync */
 300         //hci->commands[38] |= 0x02;    /* LE Periodic Advertising Create Sync Cancel */
 301         //hci->commands[38] |= 0x04;    /* LE Periodic Advertising Terminate Sync */
 302         //hci->commands[38] |= 0x08;    /* LE Add Device To Periodic Advertiser List */
 303         //hci->commands[38] |= 0x10;    /* LE Remove Device From Periodic Advertiser List */
 304         //hci->commands[38] |= 0x20;    /* LE Clear Periodic Advertiser List */
 305         //hci->commands[38] |= 0x40;    /* LE Read Periodic Advertiser List Size */
 306         //hci->commands[38] |= 0x80;    /* LE Read Transmit Power */
 307         //hci->commands[39] |= 0x01;    /* LE Read RF Path Compensation */
 308         //hci->commands[39] |= 0x02;    /* LE Write RF Path Compensation */
 309         //hci->commands[39] |= 0x04;    /* LE Set Privacy Mode */
 310
 311         memset(hci->features, 0, sizeof(hci->features));
 312         hci->features[4] |= 0x20;       /* BR/EDR Not Supported */
 313         hci->features[4] |= 0x40;       /* LE Supported */
 314
 315         memset(hci->bdaddr, 0, sizeof(hci->bdaddr));
 316
 317         memset(hci->le_event_mask, 0, sizeof(hci->le_event_mask));
 318         hci->le_event_mask[0] |= 0x01;  /* LE Connection Complete */
 319         hci->le_event_mask[0] |= 0x02;  /* LE Advertising Report */
 320         hci->le_event_mask[0] |= 0x04;  /* LE Connection Update Complete */
 321         hci->le_event_mask[0] |= 0x08;  /* LE Read Remote Used Features Complete */
 322         hci->le_event_mask[0] |= 0x10;  /* LE Long Term Key Request */
 323         //hci->le_event_mask[0] |= 0x20;        /* LE Remote Connection Parameter Request */
 324         //hci->le_event_mask[0] |= 0x40;        /* LE Data Length Change */
 325         //hci->le_event_mask[0] |= 0x80;        /* LE Read Local P-256 Public Key Complete */
 326         //hci->le_event_mask[1] |= 0x01;        /* LE Generate DHKey Complete */
 327         //hci->le_event_mask[1] |= 0x02;        /* LE Enhanced Connection Complete */
 328         //hci->le_event_mask[1] |= 0x04;        /* LE Direct Advertising Report */
 329         //hci->le_event_mask[1] |= 0x08;        /* LE PHY Update Complete */
 330         //hci->le_event_mask[1] |= 0x10;        /* LE Extended Advertising Report */
 331         //hci->le_event_mask[1] |= 0x20;        /* LE Periodic Advertising Sync Established */
 332         //hci->le_event_mask[1] |= 0x40;        /* LE Periodic Advertising Report */
 333         //hci->le_event_mask[1] |= 0x80;        /* LE Periodic Advertising Sync Lost */
 334         //hci->le_event_mask[2] |= 0x01;        /* LE Extended Scan Timeout */
 335         //hci->le_event_mask[2] |= 0x02;        /* LE Extended Advertising Set Terminated */
 336         //hci->le_event_mask[2] |= 0x04;        /* LE Scan Request Received */
 337         //hci->le_event_mask[2] |= 0x08;        /* LE Channel Selection Algorithm */
 338
 339         hci->le_mtu = 64;
 340         hci->le_max_pkt = 1;
 341
 342         memset(hci->le_features, 0, sizeof(hci->le_features));
 343         hci->le_features[0] |= 0x01;    /* LE Encryption */
 344         //hci->le_features[0] |= 0x02;  /* Connection Parameter Request Procedure */
 345         //hci->le_features[0] |= 0x04;  /* Extended Reject Indication */
 346         //hci->le_features[0] |= 0x08;  /* Peripheral-initd Features Exchange */
 347         hci->le_features[0] |= 0x10;    /* LE Ping */
 348         hci->le_features[0] |= 0x20;    /* LE Data Packet Length Extension */
 349         hci->le_features[0] |= 0x40;    /* LL Privacy */
 350         hci->le_features[0] |= 0x80;    /* Extended Scanner Filter Policies */
 351         hci->le_features[1] |= 0x01;    /* LE 2M PHY */
 352         hci->le_features[1] |= 0x02;    /* Stable Modulation Index - Transmitter */
 353         hci->le_features[1] |= 0x04;    /* Stable Modulation Index - Receiver */
 354         hci->le_features[1] |= 0x08;    /* LE Coded PHY */
 355         //hci->le_features[1] |= 0x10;  /* LE Extended Advertising */
 356         //hci->le_features[1] |= 0x20;  /* LE Periodic Advertising */
 357         hci->le_features[1] |= 0x40;    /* Channel Selection Algorithm #2 */
 358         hci->le_features[1] |= 0x80;    /* LE Power Class 1 */
 359         hci->le_features[2] |= 0x01;    /* Minimum Number of Used Channels Procedure */
 360
 361         memset(hci->le_random_addr, 0, sizeof(hci->le_random_addr));
 362
 363         hci->le_adv_min_interval = 0x0800;
 364         hci->le_adv_max_interval = 0x0800;
 365         hci->le_adv_type = 0x00;
 366         hci->le_adv_own_addr_type = 0x00;
 367         hci->le_adv_direct_addr_type = 0x00;
 368         memset(hci->le_adv_direct_addr, 0, 6);
 369         hci->le_adv_channel_map = 0x07;
 370         hci->le_adv_filter_policy = 0x00;
 371
 372         hci->le_adv_tx_power = 0;
 373
 374         memset(hci->le_adv_data, 0, sizeof(hci->le_adv_data));
 375         hci->le_adv_data_len = 0;
 376
 377         memset(hci->le_scan_rsp_data, 0, sizeof(hci->le_scan_rsp_data));
 378         hci->le_scan_rsp_data_len = 0;
 379
 380         hci->le_adv_enable = 0x00;
 381
 382         hci->le_scan_type = 0x00;               /* Passive Scanning */
 383         hci->le_scan_interval = 0x0010;         /* 10 ms */
 384         hci->le_scan_window = 0x0010;           /* 10 ms */
 385         hci->le_scan_own_addr_type = 0x00;      /* Public Device Address */
 386         hci->le_scan_filter_policy = 0x00;
 387         hci->le_scan_enable = 0x00;
 388         hci->le_scan_filter_dup = 0x00;
 389
 390         hci->le_conn_enable = 0x00;
 391
 392         hci->le_accept_list_size = ACCEPT_LIST_SIZE;
 393         clear_accept_list(hci);
 394
 395         memset(hci->le_states, 0, sizeof(hci->le_states));
 396         hci->le_states[0] |= 0x01;      /* Non-connectable Advertising */
 397         hci->le_states[0] |= 0x02;      /* Scannable Advertising */
 398         hci->le_states[0] |= 0x04;      /* Connectable Advertising */
 399         hci->le_states[0] |= 0x08;      /* High Duty Cycle Directed Advertising */
 400         hci->le_states[0] |= 0x10;      /* Passive Scanning */
 401         hci->le_states[0] |= 0x20;      /* Active Scanning */
 402         hci->le_states[0] |= 0x40;      /* Initiating + Conn (Central Role) */
 403         hci->le_states[0] |= 0x80;      /* Connection (Peripheral Role) */
 404         hci->le_states[1] |= 0x01;      /* Passive Scanning +
 405                                          * Non-connectable Advertising */
 406
 407         hci->le_default_tx_len = DEFAULT_TX_LEN;
 408         hci->le_default_tx_time = DEFAULT_TX_TIME;
 409
 410         memset(hci->le_local_sk256, 0, sizeof(hci->le_local_sk256));
 411
 412         hci->le_resolv_list_size = RESOLV_LIST_SIZE;
 413         clear_resolv_list(hci);
 414         hci->le_resolv_enable = 0x00;
 415         hci->le_resolv_timeout = 0x0384;        /* 900 secs or 15 minutes */
 416
 417         hci->le_default_all_phys = DEFAULT_ALL_PHYS;
 418         hci->le_default_tx_phys = DEFAULT_TX_PHYS;
 419         hci->le_default_rx_phys = DEFAULT_RX_PHYS;
 420 }
 421
 422 static void clear_scan_cache(struct bt_le *hci)
 423 {
 424         memset(hci->scan_cache, 0, sizeof(hci->scan_cache));
 425         hci->scan_cache_count = 0;
 426 }
 427
 428 static bool add_to_scan_cache(struct bt_le *hci, uint8_t addr_type,
 429                                                         const uint8_t addr[6])
 430 {
 431         int i;
 432
 433         for (i = 0; i < hci->scan_cache_count; i++) {
 434                 if (hci->scan_cache[i].addr_type == addr_type &&
 435                                 !memcmp(hci->scan_cache[i].addr, addr, 6))
 436                         return false;
 437         }
 438
 439         if (hci->scan_cache_count >= SCAN_CACHE_SIZE)
 440                 return true;
 441
 442         hci->scan_cache[hci->scan_cache_count].addr_type = addr_type;
 443         memcpy(hci->scan_cache[hci->scan_cache_count].addr, addr, 6);
 444         hci->scan_cache_count++;
 445
 446         return true;
 447 }
 448
 449 static void send_event(struct bt_le *hci, uint8_t event,
 450                                                 void *data, uint8_t size)
 451 {
 452         uint8_t type = BT_H4_EVT_PKT;
 453         struct bt_hci_evt_hdr hdr;
 454         struct iovec iov[3];
 455         int iovcnt;
 456
 457         hdr.evt  = event;
 458         hdr.plen = size;
 459
 460         iov[0].iov_base = &type;
 461         iov[0].iov_len  = 1;
 462         iov[1].iov_base = &hdr;
 463         iov[1].iov_len  = sizeof(hdr);
 464
 465         if (size > 0) {
 466                 iov[2].iov_base = data;
 467                 iov[2].iov_len  = size;
 468                 iovcnt = 3;
 469         } else
 470                 iovcnt = 2;
 471
 472         if (writev(hci->vhci_fd, iov, iovcnt) < 0)
 473                 fprintf(stderr, "Write to /dev/vhci failed (%m)\n");
 474 }
 475
 476 static void send_adv_pkt(struct bt_le *hci, uint8_t channel)
 477 {
 478         struct bt_phy_pkt_adv pkt;
 479
 480         memset(&pkt, 0, sizeof(pkt));
 481         pkt.chan_idx = channel;
 482         pkt.pdu_type = hci->le_adv_type;
 483         pkt.tx_addr_type = hci->le_adv_own_addr_type;
 484         switch (hci->le_adv_own_addr_type) {
 485         case 0x00:
 486         case 0x02:
 487                 memcpy(pkt.tx_addr, hci->bdaddr, 6);
 488                 break;
 489         case 0x01:
 490         case 0x03:
 491                 memcpy(pkt.tx_addr, hci->le_random_addr, 6);
 492                 break;
 493         }
 494         pkt.rx_addr_type = hci->le_adv_direct_addr_type;
 495         memcpy(pkt.rx_addr, hci->le_adv_direct_addr, 6);
 496         pkt.adv_data_len = hci->le_adv_data_len;
 497         pkt.scan_rsp_len = hci->le_scan_rsp_data_len;
 498
 499         bt_phy_send_vector(hci->phy, BT_PHY_PKT_ADV, &pkt, sizeof(pkt),
 500                                 hci->le_adv_data, pkt.adv_data_len,
 501                                 hci->le_scan_rsp_data, pkt.scan_rsp_len);
 502 }
 503
 504 static unsigned int get_adv_delay(void)
 505 {
 506         /* The advertising delay is a pseudo-random value with a range
 507          * of 0 ms to 10 ms generated for each advertising event.
 508          */
 509         srand(time(NULL));
 510         return (rand() % 11);
 511 }
 512
 513 static void adv_timeout_callback(int id, void *user_data)
 514 {
 515         struct bt_le *hci = user_data;
 516         unsigned int msec, min_msec, max_msec;
 517
 518         if (hci->le_adv_channel_map & 0x01)
 519                 send_adv_pkt(hci, 37);
 520         if (hci->le_adv_channel_map & 0x02)
 521                 send_adv_pkt(hci, 38);
 522         if (hci->le_adv_channel_map & 0x04)
 523                 send_adv_pkt(hci, 39);
 524
 525         min_msec = (hci->le_adv_min_interval * 625) / 1000;
 526         max_msec = (hci->le_adv_max_interval * 625) / 1000;
 527
 528         msec = ((min_msec + max_msec) / 2) + get_adv_delay();
 529
 530         if (mainloop_modify_timeout(id, msec) < 0) {
 531                 fprintf(stderr, "Setting advertising timeout failed\n");
 532                 hci->le_adv_enable = 0x00;
 533         }
 534 }
 535
 536 static bool start_adv(struct bt_le *hci)
 537 {
 538         unsigned int msec;
 539
 540         if (hci->adv_timeout_id >= 0)
 541                 return false;
 542
 543         msec = ((hci->le_adv_min_interval * 625) / 1000) + get_adv_delay();
 544
 545         hci->adv_timeout_id = mainloop_add_timeout(msec, adv_timeout_callback,
 546                                                                 hci, NULL);
 547         if (hci->adv_timeout_id < 0)
 548                 return false;
 549
 550         return true;
 551 }
 552
 553 static bool stop_adv(struct bt_le *hci)
 554 {
 555         if (hci->adv_timeout_id < 0)
 556                 return false;
 557
 558         mainloop_remove_timeout(hci->adv_timeout_id);
 559         hci->adv_timeout_id = -1;
 560
 561         return true;
 562 }
 563
 564 static void scan_timeout_callback(int id, void *user_data)
 565 {
 566         struct bt_le *hci = user_data;
 567         unsigned int msec;
 568
 569         if (hci->le_scan_window == hci->le_scan_interval ||
 570                                                 !hci->scan_window_active) {
 571                 msec = (hci->le_scan_window * 625) / 1000;
 572                 hci->scan_window_active = true;
 573
 574                 hci->scan_chan_idx++;
 575                 if (hci->scan_chan_idx > 39)
 576                         hci->scan_chan_idx = 37;
 577         } else {
 578                 msec = ((hci->le_scan_interval -
 579                                         hci->le_scan_window) * 625) / 1000;
 580                 hci->scan_window_active = false;
 581         }
 582
 583         if (mainloop_modify_timeout(id, msec) < 0) {
 584                 fprintf(stderr, "Setting scanning timeout failed\n");
 585                 hci->le_scan_enable = 0x00;
 586                 hci->scan_window_active = false;
 587         }
 588 }
 589
 590 static bool start_scan(struct bt_le *hci)
 591 {
 592         unsigned int msec;
 593
 594         if (hci->scan_timeout_id >= 0)
 595                 return false;
 596
 597         msec = (hci->le_scan_window * 625) / 1000;
 598
 599         hci->scan_timeout_id = mainloop_add_timeout(msec, scan_timeout_callback,
 600                                                                 hci, NULL);
 601         if (hci->scan_timeout_id < 0)
 602                 return false;
 603
 604         hci->scan_window_active = true;
 605         hci->scan_chan_idx = 37;
 606
 607         return true;
 608 }
 609
 610 static bool stop_scan(struct bt_le *hci)
 611 {
 612         if (hci->scan_timeout_id < 0)
 613                 return false;
 614
 615         mainloop_remove_timeout(hci->scan_timeout_id);
 616         hci->scan_timeout_id = -1;
 617
 618         hci->scan_window_active = false;
 619
 620         return true;
 621 }
 622
 623 static void cmd_complete(struct bt_le *hci, uint16_t opcode,
 624                                                 const void *data, uint8_t len)
 625 {
 626         struct bt_hci_evt_cmd_complete *cc;
 627         void *pkt_data;
 628
 629         pkt_data = alloca(sizeof(*cc) + len);
 630         if (!pkt_data)
 631                 return;
 632
 633         cc = pkt_data;
 634         cc->ncmd = 0x01;
 635         cc->opcode = cpu_to_le16(opcode);
 636
 637         if (len > 0)
 638                 memcpy(pkt_data + sizeof(*cc), data, len);
 639
 640         send_event(hci, BT_HCI_EVT_CMD_COMPLETE, pkt_data, sizeof(*cc) + len);
 641 }
 642
 643 static void cmd_status(struct bt_le *hci, uint8_t status, uint16_t opcode)
 644 {
 645         struct bt_hci_evt_cmd_status cs;
 646
 647         cs.status = status;
 648         cs.ncmd = 0x01;
 649         cs.opcode = cpu_to_le16(opcode);
 650
 651         send_event(hci, BT_HCI_EVT_CMD_STATUS, &cs, sizeof(cs));
 652 }
 653
 654 static void le_meta_event(struct bt_le *hci, uint8_t event,
 655                                                 void *data, uint8_t len)
 656 {
 657         void *pkt_data;
 658
 659         if (!(hci->event_mask[7] & 0x20))
 660                 return;
 661
 662         pkt_data = alloca(1 + len);
 663         if (!pkt_data)
 664                 return;
 665
 666         ((uint8_t *) pkt_data)[0] = event;
 667
 668         if (len > 0)
 669                 memcpy(pkt_data + 1, data, len);
 670
 671         send_event(hci, BT_HCI_EVT_LE_META_EVENT, pkt_data, 1 + len);
 672 }
 673
 674 static void cmd_disconnect(struct bt_le *hci, const void *data, uint8_t size)
 675 {
 676         cmd_status(hci, BT_HCI_ERR_UNKNOWN_CONN_ID, BT_HCI_CMD_DISCONNECT);
 677 }
 678
 679 static void cmd_set_event_mask(struct bt_le *hci,
 680                                                 const void *data, uint8_t size)
 681 {
 682         const struct bt_hci_cmd_set_event_mask *cmd = data;
 683         uint8_t status;
 684
 685         memcpy(hci->event_mask, cmd->mask, 8);
 686
 687         status = BT_HCI_ERR_SUCCESS;
 688         cmd_complete(hci, BT_HCI_CMD_SET_EVENT_MASK, &status, sizeof(status));
 689 }
 690
 691 static void cmd_reset(struct bt_le *hci, const void *data, uint8_t size)
 692 {
 693         uint8_t status;
 694
 695         stop_adv(hci);
 696         stop_scan(hci);
 697         reset_defaults(hci);
 698
 699         status = BT_HCI_ERR_SUCCESS;
 700         cmd_complete(hci, BT_HCI_CMD_RESET, &status, sizeof(status));
 701 }
 702
 703 static void cmd_set_event_mask_page2(struct bt_le *hci,
 704                                                 const void *data, uint8_t size)
 705 {
 706         const struct bt_hci_cmd_set_event_mask_page2 *cmd = data;
 707         uint8_t status;
 708
 709         memcpy(hci->event_mask + 8, cmd->mask, 8);
 710
 711         status = BT_HCI_ERR_SUCCESS;
 712         cmd_complete(hci, BT_HCI_CMD_SET_EVENT_MASK_PAGE2,
 713                                                 &status, sizeof(status));
 714 }
 715
 716 static void cmd_read_local_version(struct bt_le *hci,
 717                                                 const void *data, uint8_t size)
 718 {
 719         struct bt_hci_rsp_read_local_version rsp;
 720
 721         rsp.status = BT_HCI_ERR_SUCCESS;
 722         rsp.hci_ver = 0x09;
 723         rsp.hci_rev = cpu_to_le16(0x0000);
 724         rsp.lmp_ver = 0x09;
 725         rsp.manufacturer = cpu_to_le16(hci->manufacturer);
 726         rsp.lmp_subver = cpu_to_le16(0x0000);
 727
 728         cmd_complete(hci, BT_HCI_CMD_READ_LOCAL_VERSION, &rsp, sizeof(rsp));
 729 }
 730
 731 static void cmd_read_local_commands(struct bt_le *hci,
 732                                                 const void *data, uint8_t size)
 733 {
 734         struct bt_hci_rsp_read_local_commands rsp;
 735
 736         rsp.status = BT_HCI_ERR_SUCCESS;
 737         memcpy(rsp.commands, hci->commands, 64);
 738
 739         cmd_complete(hci, BT_HCI_CMD_READ_LOCAL_COMMANDS, &rsp, sizeof(rsp));
 740 }
 741
 742 static void cmd_read_local_features(struct bt_le *hci,
 743                                                 const void *data, uint8_t size)
 744 {
 745         struct bt_hci_rsp_read_local_features rsp;
 746
 747         rsp.status = BT_HCI_ERR_SUCCESS;
 748         memcpy(rsp.features, hci->features, 8);
 749
 750         cmd_complete(hci, BT_HCI_CMD_READ_LOCAL_FEATURES, &rsp, sizeof(rsp));
 751 }
 752
 753 static void cmd_read_buffer_size(struct bt_le *hci,
 754                                                 const void *data, uint8_t size)
 755 {
 756         struct bt_hci_rsp_read_buffer_size rsp;
 757
 758         rsp.status = BT_HCI_ERR_SUCCESS;
 759         rsp.acl_mtu = cpu_to_le16(0x0000);
 760         rsp.sco_mtu = 0x00;
 761         rsp.acl_max_pkt = cpu_to_le16(0x0000);
 762         rsp.sco_max_pkt = cpu_to_le16(0x0000);
 763
 764         cmd_complete(hci, BT_HCI_CMD_READ_BUFFER_SIZE, &rsp, sizeof(rsp));
 765 }
 766
 767 static void cmd_read_bd_addr(struct bt_le *hci, const void *data, uint8_t size)
 768 {
 769         struct bt_hci_rsp_read_bd_addr rsp;
 770
 771         rsp.status = BT_HCI_ERR_SUCCESS;
 772         memcpy(rsp.bdaddr, hci->bdaddr, 6);
 773
 774         cmd_complete(hci, BT_HCI_CMD_READ_BD_ADDR, &rsp, sizeof(rsp));
 775 }
 776
 777 static void cmd_le_set_event_mask(struct bt_le *hci,
 778                                                 const void *data, uint8_t size)
 779 {
 780         const struct bt_hci_cmd_le_set_event_mask *cmd = data;
 781         uint8_t status;
 782
 783         memcpy(hci->le_event_mask, cmd->mask, 8);
 784
 785         status = BT_HCI_ERR_SUCCESS;
 786         cmd_complete(hci, BT_HCI_CMD_LE_SET_EVENT_MASK,
 787                                                 &status, sizeof(status));
 788 }
 789
 790 static void cmd_le_read_buffer_size(struct bt_le *hci,
 791                                                 const void *data, uint8_t size)
 792 {
 793         struct bt_hci_rsp_le_read_buffer_size rsp;
 794
 795         rsp.status = BT_HCI_ERR_SUCCESS;
 796         rsp.le_mtu = cpu_to_le16(hci->le_mtu);
 797         rsp.le_max_pkt = hci->le_max_pkt;
 798
 799         cmd_complete(hci, BT_HCI_CMD_LE_READ_BUFFER_SIZE, &rsp, sizeof(rsp));
 800 }
 801
 802 static void cmd_le_read_local_features(struct bt_le *hci,
 803                                                 const void *data, uint8_t size)
 804 {
 805         struct bt_hci_rsp_le_read_local_features rsp;
 806
 807         rsp.status = BT_HCI_ERR_SUCCESS;
 808         memcpy(rsp.features, hci->le_features, 8);
 809
 810         cmd_complete(hci, BT_HCI_CMD_LE_READ_LOCAL_FEATURES,
 811                                                         &rsp, sizeof(rsp));
 812 }
 813
 814 static void cmd_le_set_random_address(struct bt_le *hci,
 815                                                 const void *data, uint8_t size)
 816 {
 817         const struct bt_hci_cmd_le_set_random_address *cmd = data;
 818         uint8_t status;
 819
 820         memcpy(hci->le_random_addr, cmd->addr, 6);
 821
 822         status = BT_HCI_ERR_SUCCESS;
 823         cmd_complete(hci, BT_HCI_CMD_LE_SET_RANDOM_ADDRESS,
 824                                                 &status, sizeof(status));
 825 }
 826
 827 static void cmd_le_set_adv_parameters(struct bt_le *hci,
 828                                                 const void *data, uint8_t size)
 829 {
 830         const struct bt_hci_cmd_le_set_adv_parameters *cmd = data;
 831         uint16_t min_interval, max_interval;
 832         uint8_t status;
 833
 834         if (hci->le_adv_enable == 0x01) {
 835                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
 836                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 837                 return;
 838         }
 839
 840         min_interval = le16_to_cpu(cmd->min_interval);
 841         max_interval = le16_to_cpu(cmd->max_interval);
 842
 843         /* Valid range for advertising type is 0x00 to 0x03 */
 844         switch (cmd->type) {
 845         case 0x00:      /* ADV_IND */
 846                 /* Range for advertising interval min is 0x0020 to 0x4000 */
 847                 if (min_interval < 0x0020 || min_interval > 0x4000) {
 848                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 849                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 850                         return;
 851                 }
 852                 /* Range for advertising interval max is 0x0020 to 0x4000 */
 853                 if (max_interval < 0x0020 || max_interval > 0x4000) {
 854                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 855                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 856                         return;
 857                 }
 858                 /* Advertising interval max shall be less or equal */
 859                 if (min_interval > max_interval) {
 860                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 861                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 862                         return;
 863                 }
 864                 break;
 865
 866         case 0x01:      /* ADV_DIRECT_IND */
 867                 /* Range for direct address type is 0x00 to 0x01 */
 868                 if (cmd->direct_addr_type > 0x01) {
 869                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 870                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 871                         return;
 872                 }
 873                 break;
 874
 875         case 0x02:      /* ADV_SCAN_IND */
 876         case 0x03:      /* ADV_NONCONN_IND */
 877                 /* Range for advertising interval min is 0x00a0 to 0x4000 */
 878                 if (min_interval < 0x00a0 || min_interval > 0x4000) {
 879                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 880                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 881                         return;
 882                 }
 883                 /* Range for advertising interval max is 0x00a0 to 0x4000 */
 884                 if (max_interval < 0x00a0 || max_interval > 0x4000) {
 885                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 886                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 887                         return;
 888                 }
 889                 /* Advertising interval min shall be less or equal */
 890                 if (min_interval > max_interval) {
 891                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 892                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 893                         return;
 894                 }
 895                 break;
 896
 897         default:
 898                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 899                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 900                 return;
 901         }
 902
 903         /* Valid range for own address type is 0x00 to 0x03 */
 904         if (cmd->own_addr_type > 0x03) {
 905                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 906                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 907                 return;
 908         }
 909
 910         /* Valid range for advertising channel map is 0x01 to 0x07 */
 911         if (cmd->channel_map < 0x01 || cmd->channel_map > 0x07) {
 912                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 913                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 914                 return;
 915         }
 916
 917         /* Valid range for advertising filter policy is 0x00 to 0x03 */
 918         if (cmd->filter_policy > 0x03) {
 919                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 920                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 921                 return;
 922         }
 923
 924         hci->le_adv_min_interval = min_interval;
 925         hci->le_adv_max_interval = max_interval;
 926         hci->le_adv_type = cmd->type;
 927         hci->le_adv_own_addr_type = cmd->own_addr_type;
 928         hci->le_adv_direct_addr_type = cmd->direct_addr_type;
 929         memcpy(hci->le_adv_direct_addr, cmd->direct_addr, 6);
 930         hci->le_adv_channel_map = cmd->channel_map;
 931         hci->le_adv_filter_policy = cmd->filter_policy;
 932
 933         status = BT_HCI_ERR_SUCCESS;
 934         cmd_complete(hci, BT_HCI_CMD_LE_SET_ADV_PARAMETERS,
 935                                                 &status, sizeof(status));
 936 }
 937
 938 static void cmd_le_read_adv_tx_power(struct bt_le *hci,
 939                                                 const void *data, uint8_t size)
 940 {
 941         struct bt_hci_rsp_le_read_adv_tx_power rsp;
 942
 943         rsp.status = BT_HCI_ERR_SUCCESS;
 944         rsp.level = hci->le_adv_tx_power;
 945
 946         cmd_complete(hci, BT_HCI_CMD_LE_READ_ADV_TX_POWER, &rsp, sizeof(rsp));
 947 }
 948
 949 static void cmd_le_set_adv_data(struct bt_le *hci,
 950                                                 const void *data, uint8_t size)
 951 {
 952         const struct bt_hci_cmd_le_set_adv_data *cmd = data;
 953         uint8_t status;
 954
 955         /* Valid range for advertising data length is 0x00 to 0x1f */
 956         if (cmd->len > 0x1f) {
 957                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 958                                         BT_HCI_CMD_LE_SET_ADV_DATA);
 959                 return;
 960         }
 961
 962         hci->le_adv_data_len = cmd->len;
 963         memcpy(hci->le_adv_data, cmd->data, 31);
 964
 965         status = BT_HCI_ERR_SUCCESS;
 966         cmd_complete(hci, BT_HCI_CMD_LE_SET_ADV_DATA, &status, sizeof(status));
 967 }
 968
 969 static void cmd_le_set_scan_rsp_data(struct bt_le *hci,
 970                                                 const void *data, uint8_t size)
 971 {
 972         const struct bt_hci_cmd_le_set_scan_rsp_data *cmd = data;
 973         uint8_t status;
 974
 975         /* Valid range for scan response data length is 0x00 to 0x1f */
 976         if (cmd->len > 0x1f) {
 977                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 978                                         BT_HCI_CMD_LE_SET_SCAN_RSP_DATA);
 979                 return;
 980         }
 981
 982         hci->le_scan_rsp_data_len = cmd->len;
 983         memcpy(hci->le_scan_rsp_data, cmd->data, 31);
 984
 985         status = BT_HCI_ERR_SUCCESS;
 986         cmd_complete(hci, BT_HCI_CMD_LE_SET_SCAN_RSP_DATA,
 987                                                 &status, sizeof(status));
 988 }
 989
 990 static void cmd_le_set_adv_enable(struct bt_le *hci,
 991                                                 const void *data, uint8_t size)
 992 {
 993         const struct bt_hci_cmd_le_set_adv_enable *cmd = data;
 994         uint8_t status;
 995         bool result;
 996
 997         /* Valid range for advertising enable is 0x00 to 0x01 */
 998         if (cmd->enable > 0x01) {
 999                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1000                                         BT_HCI_CMD_LE_SET_ADV_ENABLE);
1001                 return;
1002         }
1003
1004         if (cmd->enable == hci->le_adv_enable) {
1005                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
1006                                         BT_HCI_CMD_LE_SET_ADV_ENABLE);
1007                 return;
1008         }
1009
1010         if (cmd->enable == 0x01)
1011                 result = start_adv(hci);
1012         else
1013                 result = stop_adv(hci);
1014
1015         if (!result) {
1016                 cmd_status(hci, BT_HCI_ERR_UNSPECIFIED_ERROR,
1017                                         BT_HCI_CMD_LE_SET_ADV_ENABLE);
1018                 return;
1019         }
1020
1021         hci->le_adv_enable = cmd->enable;
1022
1023         status = BT_HCI_ERR_SUCCESS;
1024         cmd_complete(hci, BT_HCI_CMD_LE_SET_ADV_ENABLE,
1025                                                 &status, sizeof(status));
1026 }
1027
1028 static void cmd_le_set_scan_parameters(struct bt_le *hci,
1029                                                 const void *data, uint8_t size)
1030 {
1031         const struct bt_hci_cmd_le_set_scan_parameters *cmd = data;
1032         uint16_t interval, window;
1033         uint8_t status;
1034
1035         if (hci->le_scan_enable == 0x01) {
1036                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
1037                                         BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
1038                 return;
1039         }
1040
1041         interval = le16_to_cpu(cmd->interval);
1042         window = le16_to_cpu(cmd->window);
1043
1044         /* Valid range for scan type is 0x00 to 0x01 */
1045         if (cmd->type > 0x01) {
1046                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1047                                         BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
1048                 return;
1049         }
1050
1051         /* Valid range for scan interval is 0x0004 to 0x4000 */
1052         if (interval < 0x0004 || interval > 0x4000) {
1053                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1054                                         BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
1055                 return;
1056         }
1057
1058         /* Valid range for scan window is 0x0004 to 0x4000 */
1059         if (window < 0x0004 || window > 0x4000) {
1060                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1061                                         BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
1062                 return;
1063         }
1064
1065         /* Scan window shall be less or equal than scan interval */
1066         if (window > interval) {
1067                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1068                                         BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
1069                 return;
1070         }
1071
1072         /* Valid range for own address type is 0x00 to 0x03 */
1073         if (cmd->own_addr_type > 0x03) {
1074                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1075                                         BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
1076                 return;
1077         }
1078
1079         /* Valid range for scanning filter policy is 0x00 to 0x03 */
1080         if (cmd->filter_policy > 0x03) {
1081                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1082                                         BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
1083                 return;
1084         }
1085
1086         hci->le_scan_type = cmd->type;
1087         hci->le_scan_interval = interval;
1088         hci->le_scan_window = window;
1089         hci->le_scan_own_addr_type = cmd->own_addr_type;
1090         hci->le_scan_filter_policy = cmd->filter_policy;
1091
1092         status = BT_HCI_ERR_SUCCESS;
1093         cmd_complete(hci, BT_HCI_CMD_LE_SET_SCAN_PARAMETERS,
1094                                                 &status, sizeof(status));
1095 }
1096
1097 static void cmd_le_set_scan_enable(struct bt_le *hci,
1098                                                 const void *data, uint8_t size)
1099 {
1100         const struct bt_hci_cmd_le_set_scan_enable *cmd = data;
1101         uint8_t status;
1102         bool result;
1103
1104         /* Valid range for scan enable is 0x00 to 0x01 */
1105         if (cmd->enable > 0x01) {
1106                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1107                                         BT_HCI_CMD_LE_SET_SCAN_ENABLE);
1108                 return;
1109         }
1110
1111         /* Valid range for filter duplicates is 0x00 to 0x01 */
1112         if (cmd->filter_dup > 0x01) {
1113                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1114                                         BT_HCI_CMD_LE_SET_SCAN_ENABLE);
1115                 return;
1116         }
1117
1118         if (cmd->enable == hci->le_scan_enable) {
1119                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
1120                                         BT_HCI_CMD_LE_SET_SCAN_ENABLE);
1121                 return;
1122         }
1123
1124         clear_scan_cache(hci);
1125
1126         if (cmd->enable == 0x01)
1127                 result = start_scan(hci);
1128         else
1129                 result = stop_scan(hci);
1130
1131         if (!result) {
1132                 cmd_status(hci, BT_HCI_ERR_UNSPECIFIED_ERROR,
1133                                         BT_HCI_CMD_LE_SET_SCAN_ENABLE);
1134                 return;
1135         }
1136
1137         hci->le_scan_enable = cmd->enable;
1138         hci->le_scan_filter_dup = cmd->filter_dup;
1139
1140         status = BT_HCI_ERR_SUCCESS;
1141         cmd_complete(hci, BT_HCI_CMD_LE_SET_SCAN_ENABLE,
1142                                                 &status, sizeof(status));
1143 }
1144
1145 static void cmd_le_create_conn(struct bt_le *hci,
1146                                                 const void *data, uint8_t size)
1147 {
1148         const struct bt_hci_cmd_le_create_conn *cmd = data;
1149
1150         if (hci->le_conn_enable == 0x01) {
1151                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
1152                                         BT_HCI_CMD_LE_CREATE_CONN);
1153                 return;
1154         }
1155
1156         /* Valid range for peer address type is 0x00 to 0x03 */
1157         if (cmd->peer_addr_type > 0x03) {
1158                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1159                                         BT_HCI_CMD_LE_CREATE_CONN);
1160                 return;
1161         }
1162
1163         /* Valid range for own address type is 0x00 to 0x03 */
1164         if (cmd->own_addr_type > 0x03) {
1165                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1166                                         BT_HCI_CMD_LE_CREATE_CONN);
1167                 return;
1168         }
1169
1170         hci->le_conn_peer_addr_type = cmd->peer_addr_type;
1171         memcpy(hci->le_conn_peer_addr, cmd->peer_addr, 6);
1172         hci->le_conn_own_addr_type = cmd->own_addr_type;
1173         hci->le_conn_enable = 0x01;
1174
1175         cmd_status(hci, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_CREATE_CONN);
1176 }
1177
1178 static void cmd_le_create_conn_cancel(struct bt_le *hci,
1179                                                 const void *data, uint8_t size)
1180 {
1181         struct bt_hci_evt_le_conn_complete evt;
1182         uint8_t status;
1183
1184         if (hci->le_conn_enable == 0x00) {
1185                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
1186                                         BT_HCI_CMD_LE_CREATE_CONN_CANCEL);
1187                 return;
1188         }
1189
1190         hci->le_conn_enable = 0x00;
1191
1192         status = BT_HCI_ERR_SUCCESS;
1193         cmd_complete(hci, BT_HCI_CMD_LE_CREATE_CONN_CANCEL,
1194                                                 &status, sizeof(status));
1195
1196         evt.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1197         evt.handle = cpu_to_le16(0x0000);
1198         evt.role = 0x00;
1199         evt.peer_addr_type = 0x00;
1200         memset(evt.peer_addr, 0, 6);
1201         evt.interval = cpu_to_le16(0x0000);
1202         evt.latency = cpu_to_le16(0x0000);
1203         evt.supv_timeout = cpu_to_le16(0x0000);
1204         evt.clock_accuracy = 0x00;
1205
1206         if (hci->le_event_mask[0] & 0x01)
1207                 le_meta_event(hci, BT_HCI_EVT_LE_CONN_COMPLETE,
1208                                                         &evt, sizeof(evt));
1209 }
1210
1211 static void cmd_le_read_accept_list_size(struct bt_le *hci,
1212                                                 const void *data, uint8_t size)
1213 {
1214         struct bt_hci_rsp_le_read_accept_list_size rsp;
1215
1216         rsp.status = BT_HCI_ERR_SUCCESS;
1217         rsp.size = hci->le_accept_list_size;
1218
1219         cmd_complete(hci, BT_HCI_CMD_LE_READ_ACCEPT_LIST_SIZE,
1220                                                         &rsp, sizeof(rsp));
1221 }
1222
1223 static void cmd_le_clear_accept_list(struct bt_le *hci,
1224                                                 const void *data, uint8_t size)
1225 {
1226         uint8_t status;
1227
1228         clear_accept_list(hci);
1229
1230         status = BT_HCI_ERR_SUCCESS;
1231         cmd_complete(hci, BT_HCI_CMD_LE_CLEAR_ACCEPT_LIST,
1232                                                 &status, sizeof(status));
1233 }
1234
1235 static void cmd_le_add_to_accept_list(struct bt_le *hci,
1236                                                 const void *data, uint8_t size)
1237 {
1238         const struct bt_hci_cmd_le_add_to_accept_list *cmd = data;
1239         uint8_t status;
1240         bool exists = false;
1241         int i, pos = -1;
1242
1243         /* Valid range for address type is 0x00 to 0x01 */
1244         if (cmd->addr_type > 0x01) {
1245                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1246                                         BT_HCI_CMD_LE_ADD_TO_ACCEPT_LIST);
1247                 return;
1248         }
1249
1250         for (i = 0; i < hci->le_accept_list_size; i++) {
1251                 if (hci->le_accept_list[i][0] == cmd->addr_type &&
1252                                 !memcmp(&hci->le_accept_list[i][1],
1253                                                         cmd->addr, 6)) {
1254                         exists = true;
1255                         break;
1256                 } else if (pos < 0 && hci->le_accept_list[i][0] == 0xff)
1257                         pos = i;
1258         }
1259
1260         if (exists) {
1261                 cmd_status(hci, BT_HCI_ERR_UNSPECIFIED_ERROR,
1262                                         BT_HCI_CMD_LE_ADD_TO_ACCEPT_LIST);
1263                 return;
1264         }
1265
1266         if (pos < 0) {
1267                 cmd_status(hci, BT_HCI_ERR_MEM_CAPACITY_EXCEEDED,
1268                                         BT_HCI_CMD_LE_ADD_TO_ACCEPT_LIST);
1269                 return;
1270         }
1271
1272         hci->le_accept_list[pos][0] = cmd->addr_type;
1273         memcpy(&hci->le_accept_list[pos][1], cmd->addr, 6);
1274
1275         status = BT_HCI_ERR_SUCCESS;
1276         cmd_complete(hci, BT_HCI_CMD_LE_ADD_TO_ACCEPT_LIST,
1277                                                 &status, sizeof(status));
1278 }
1279
1280 static void cmd_le_remove_from_accept_list(struct bt_le *hci,
1281                                                 const void *data, uint8_t size)
1282 {
1283         const struct bt_hci_cmd_le_remove_from_accept_list *cmd = data;
1284         uint8_t status;
1285         int i, pos = -1;
1286
1287         /* Valid range for address type is 0x00 to 0x01 */
1288         if (cmd->addr_type > 0x01) {
1289                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1290                                         BT_HCI_CMD_LE_REMOVE_FROM_ACCEPT_LIST);
1291                 return;
1292         }
1293
1294         for (i = 0; i < hci->le_accept_list_size; i++) {
1295                 if (hci->le_accept_list[i][0] == cmd->addr_type &&
1296                                 !memcmp(&hci->le_accept_list[i][1],
1297                                                         cmd->addr, 6)) {
1298                         pos = i;
1299                         break;
1300                 }
1301         }
1302
1303         if (pos < 0) {
1304                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1305                                         BT_HCI_CMD_LE_REMOVE_FROM_ACCEPT_LIST);
1306                 return;
1307         }
1308
1309         hci->le_accept_list[pos][0] = 0xff;
1310         memset(&hci->le_accept_list[pos][1], 0, 6);
1311
1312         status = BT_HCI_ERR_SUCCESS;
1313         cmd_complete(hci, BT_HCI_CMD_LE_REMOVE_FROM_ACCEPT_LIST,
1314                                                 &status, sizeof(status));
1315 }
1316
1317 static void cmd_le_encrypt(struct bt_le *hci, const void *data, uint8_t size)
1318 {
1319         const struct bt_hci_cmd_le_encrypt *cmd = data;
1320         struct bt_hci_rsp_le_encrypt rsp;
1321
1322         if (!bt_crypto_e(hci->crypto, cmd->key, cmd->plaintext, rsp.data)) {
1323                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
1324                                         BT_HCI_CMD_LE_ENCRYPT);
1325                 return;
1326         }
1327
1328         rsp.status = BT_HCI_ERR_SUCCESS;
1329
1330         cmd_complete(hci, BT_HCI_CMD_LE_ENCRYPT, &rsp, sizeof(rsp));
1331 }
1332
1333 static void cmd_le_rand(struct bt_le *hci, const void *data, uint8_t size)
1334 {
1335         struct bt_hci_rsp_le_rand rsp;
1336         uint8_t value[8];
1337
1338         if (!bt_crypto_random_bytes(hci->crypto, value, 8)) {
1339                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
1340                                         BT_HCI_CMD_LE_RAND);
1341                 return;
1342         }
1343
1344         rsp.status = BT_HCI_ERR_SUCCESS;
1345         memcpy(&rsp.number, value, 8);
1346
1347         cmd_complete(hci, BT_HCI_CMD_LE_RAND, &rsp, sizeof(rsp));
1348 }
1349
1350 static void cmd_le_read_supported_states(struct bt_le *hci,
1351                                                 const void *data, uint8_t size)
1352 {
1353         struct bt_hci_rsp_le_read_supported_states rsp;
1354
1355         rsp.status = BT_HCI_ERR_SUCCESS;
1356         memcpy(rsp.states, hci->le_states, 8);
1357
1358         cmd_complete(hci, BT_HCI_CMD_LE_READ_SUPPORTED_STATES,
1359                                                         &rsp, sizeof(rsp));
1360 }
1361
1362 static void cmd_le_set_data_length(struct bt_le *hci,
1363                                                 const void *data, uint8_t size)
1364 {
1365         const struct bt_hci_cmd_le_set_data_length *cmd = data;
1366         struct bt_hci_rsp_le_set_data_length rsp;
1367         uint16_t handle, tx_len, tx_time;
1368
1369         handle = le16_to_cpu(cmd->handle);
1370         tx_len = le16_to_cpu(cmd->tx_len);
1371         tx_time = le16_to_cpu(cmd->tx_time);
1372
1373         /* Valid range for connection handle is 0x0000 to 0x0eff */
1374         if (handle > 0x0eff) {
1375                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1376                                         BT_HCI_CMD_LE_SET_DATA_LENGTH);
1377                 return;
1378         }
1379
1380         /* Valid range for suggested max TX octets is 0x001b to 0x00fb */
1381         if (tx_len < 0x001b || tx_len > 0x00fb) {
1382                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1383                                         BT_HCI_CMD_LE_SET_DATA_LENGTH);
1384                 return;
1385         }
1386
1387         /* Valid range for suggested max TX time is 0x0148 to 0x0848 */
1388         if (tx_time < 0x0148 || tx_time > 0x0848) {
1389                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1390                                         BT_HCI_CMD_LE_SET_DATA_LENGTH);
1391                 return;
1392         }
1393
1394         /* Max TX len and time shall be less or equal supported */
1395         if (tx_len > MAX_TX_LEN || tx_time > MAX_TX_TIME) {
1396                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1397                                         BT_HCI_CMD_LE_SET_DATA_LENGTH);
1398                 return;
1399         }
1400
1401         rsp.status = BT_HCI_ERR_SUCCESS;
1402         rsp.handle = cpu_to_le16(handle);
1403
1404         cmd_complete(hci, BT_HCI_CMD_LE_SET_DATA_LENGTH, &rsp, sizeof(rsp));
1405 }
1406
1407 static void cmd_le_read_default_data_length(struct bt_le *hci,
1408                                                 const void *data, uint8_t size)
1409 {
1410         struct bt_hci_rsp_le_read_default_data_length rsp;
1411
1412         rsp.status = BT_HCI_ERR_SUCCESS;
1413         rsp.tx_len = cpu_to_le16(hci->le_default_tx_len);
1414         rsp.tx_time = cpu_to_le16(hci->le_default_tx_time);
1415
1416         cmd_complete(hci, BT_HCI_CMD_LE_READ_DEFAULT_DATA_LENGTH,
1417                                                         &rsp, sizeof(rsp));
1418 }
1419
1420 static void cmd_le_write_default_data_length(struct bt_le *hci,
1421                                                 const void *data, uint8_t size)
1422 {
1423         const struct bt_hci_cmd_le_write_default_data_length *cmd = data;
1424         uint16_t tx_len, tx_time;
1425         uint8_t status;
1426
1427         tx_len = le16_to_cpu(cmd->tx_len);
1428         tx_time = le16_to_cpu(cmd->tx_time);
1429
1430         /* Valid range for suggested max TX octets is 0x001b to 0x00fb */
1431         if (tx_len < 0x001b || tx_len > 0x00fb) {
1432                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1433                                 BT_HCI_CMD_LE_WRITE_DEFAULT_DATA_LENGTH);
1434                 return;
1435         }
1436
1437         /* Valid range for suggested max TX time is 0x0148 to 0x0848 */
1438         if (tx_time < 0x0148 || tx_time > 0x0848) {
1439                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1440                                 BT_HCI_CMD_LE_WRITE_DEFAULT_DATA_LENGTH);
1441                 return;
1442         }
1443
1444         /* Suggested max TX len and time shall be less or equal supported */
1445         if (tx_len > MAX_TX_LEN || tx_time > MAX_TX_TIME) {
1446                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1447                                 BT_HCI_CMD_LE_WRITE_DEFAULT_DATA_LENGTH);
1448                 return;
1449         }
1450
1451         hci->le_default_tx_len = tx_len;
1452         hci->le_default_tx_time = tx_time;
1453
1454         status = BT_HCI_ERR_SUCCESS;
1455         cmd_complete(hci, BT_HCI_CMD_LE_WRITE_DEFAULT_DATA_LENGTH,
1456                                                 &status, sizeof(status));
1457 }
1458
1459 static void cmd_le_read_local_pk256(struct bt_le *hci,
1460                                                 const void *data, uint8_t size)
1461 {
1462         struct bt_hci_evt_le_read_local_pk256_complete evt;
1463
1464         cmd_status(hci, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_READ_LOCAL_PK256);
1465
1466         evt.status = BT_HCI_ERR_SUCCESS;
1467         ecc_make_key(evt.local_pk256, hci->le_local_sk256);
1468
1469         if (hci->le_event_mask[0] & 0x80)
1470                 le_meta_event(hci, BT_HCI_EVT_LE_READ_LOCAL_PK256_COMPLETE,
1471                                                         &evt, sizeof(evt));
1472 }
1473
1474 static void cmd_le_generate_dhkey(struct bt_le *hci,
1475                                                 const void *data, uint8_t size)
1476 {
1477         const struct bt_hci_cmd_le_generate_dhkey *cmd = data;
1478         struct bt_hci_evt_le_generate_dhkey_complete evt;
1479
1480         cmd_status(hci, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_GENERATE_DHKEY);
1481
1482         evt.status = BT_HCI_ERR_SUCCESS;
1483         ecdh_shared_secret(cmd->remote_pk256, hci->le_local_sk256, evt.dhkey);
1484
1485         if (hci->le_event_mask[1] & 0x01)
1486                 le_meta_event(hci, BT_HCI_EVT_LE_GENERATE_DHKEY_COMPLETE,
1487                                                         &evt, sizeof(evt));
1488 }
1489
1490 static void cmd_le_add_to_resolv_list(struct bt_le *hci,
1491                                                 const void *data, uint8_t size)
1492 {
1493         const struct bt_hci_cmd_le_add_to_resolv_list *cmd = data;
1494         uint8_t status;
1495         bool exists = false;
1496         int i, pos = -1;
1497
1498         /* Valid range for address type is 0x00 to 0x01 */
1499         if (cmd->addr_type > 0x01) {
1500                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1501                                         BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST);
1502                 return;
1503         }
1504
1505         for (i = 0; i < hci->le_resolv_list_size; i++) {
1506                 if (hci->le_resolv_list[i][0] == cmd->addr_type &&
1507                                 !memcmp(&hci->le_resolv_list[i][1],
1508                                                         cmd->addr, 6)) {
1509                         exists = true;
1510                         break;
1511                 } else if (pos < 0 && hci->le_resolv_list[i][0] == 0xff)
1512                         pos = i;
1513         }
1514
1515         if (exists) {
1516                 cmd_status(hci, BT_HCI_ERR_UNSPECIFIED_ERROR,
1517                                         BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST);
1518                 return;
1519         }
1520
1521         if (pos < 0) {
1522                 cmd_status(hci, BT_HCI_ERR_MEM_CAPACITY_EXCEEDED,
1523                                         BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST);
1524                 return;
1525         }
1526
1527         hci->le_resolv_list[pos][0] = cmd->addr_type;
1528         memcpy(&hci->le_resolv_list[pos][1], cmd->addr, 6);
1529         memcpy(&hci->le_resolv_list[pos][7], cmd->peer_irk, 16);
1530         memcpy(&hci->le_resolv_list[pos][23], cmd->local_irk, 16);
1531
1532         status = BT_HCI_ERR_SUCCESS;
1533         cmd_complete(hci, BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST,
1534                                                 &status, sizeof(status));
1535 }
1536
1537 static void cmd_le_remove_from_resolv_list(struct bt_le *hci,
1538                                                 const void *data, uint8_t size)
1539 {
1540         const struct bt_hci_cmd_le_remove_from_resolv_list *cmd = data;
1541         uint8_t status;
1542         int i, pos = -1;
1543
1544         /* Valid range for address type is 0x00 to 0x01 */
1545         if (cmd->addr_type > 0x01) {
1546                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1547                                         BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST);
1548                 return;
1549         }
1550
1551         for (i = 0; i < hci->le_resolv_list_size; i++) {
1552                 if (hci->le_resolv_list[i][0] == cmd->addr_type &&
1553                                 !memcmp(&hci->le_resolv_list[i][1],
1554                                                         cmd->addr, 6)) {
1555                         pos = i;
1556                         break;
1557                 }
1558         }
1559
1560         if (pos < 0) {
1561                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1562                                         BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST);
1563                 return;
1564         }
1565
1566         hci->le_resolv_list[pos][0] = 0xff;
1567         memset(&hci->le_resolv_list[pos][1], 0, 38);
1568
1569         status = BT_HCI_ERR_SUCCESS;
1570         cmd_complete(hci, BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST,
1571                                                 &status, sizeof(status));
1572 }
1573
1574 static void cmd_le_clear_resolv_list(struct bt_le *hci,
1575                                                 const void *data, uint8_t size)
1576 {
1577         uint8_t status;
1578
1579         clear_resolv_list(hci);
1580
1581         status = BT_HCI_ERR_SUCCESS;
1582         cmd_complete(hci, BT_HCI_CMD_LE_CLEAR_RESOLV_LIST,
1583                                                 &status, sizeof(status));
1584 }
1585
1586 static void cmd_le_read_resolv_list_size(struct bt_le *hci,
1587                                                 const void *data, uint8_t size)
1588 {
1589         struct bt_hci_rsp_le_read_resolv_list_size rsp;
1590
1591         rsp.status = BT_HCI_ERR_SUCCESS;
1592         rsp.size = hci->le_resolv_list_size;
1593
1594         cmd_complete(hci, BT_HCI_CMD_LE_READ_RESOLV_LIST_SIZE,
1595                                                         &rsp, sizeof(rsp));
1596 }
1597
1598 static void cmd_le_read_peer_resolv_addr(struct bt_le *hci,
1599                                                 const void *data, uint8_t size)
1600 {
1601         const struct bt_hci_cmd_le_read_peer_resolv_addr *cmd = data;
1602         struct bt_hci_rsp_le_read_peer_resolv_addr rsp;
1603
1604         /* Valid range for address type is 0x00 to 0x01 */
1605         if (cmd->addr_type > 0x01) {
1606                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1607                                         BT_HCI_CMD_LE_READ_PEER_RESOLV_ADDR);
1608                 return;
1609         }
1610
1611         rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1612         memset(rsp.addr, 0, 6);
1613
1614         cmd_complete(hci, BT_HCI_CMD_LE_READ_PEER_RESOLV_ADDR,
1615                                                         &rsp, sizeof(rsp));
1616 }
1617
1618 static void cmd_le_read_local_resolv_addr(struct bt_le *hci,
1619                                                 const void *data, uint8_t size)
1620 {
1621         const struct bt_hci_cmd_le_read_local_resolv_addr *cmd = data;
1622         struct bt_hci_rsp_le_read_local_resolv_addr rsp;
1623
1624         /* Valid range for address type is 0x00 to 0x01 */
1625         if (cmd->addr_type > 0x01) {
1626                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1627                                         BT_HCI_CMD_LE_READ_LOCAL_RESOLV_ADDR);
1628                 return;
1629         }
1630
1631         rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1632         memset(rsp.addr, 0, 6);
1633
1634         cmd_complete(hci, BT_HCI_CMD_LE_READ_LOCAL_RESOLV_ADDR,
1635                                                         &rsp, sizeof(rsp));
1636 }
1637
1638 static void cmd_le_set_resolv_enable(struct bt_le *hci,
1639                                                 const void *data, uint8_t size)
1640 {
1641         const struct bt_hci_cmd_le_set_resolv_enable *cmd = data;
1642         uint8_t status;
1643
1644         /* Valid range for address resolution enable is 0x00 to 0x01 */
1645         if (cmd->enable > 0x01) {
1646                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1647                                         BT_HCI_CMD_LE_SET_RESOLV_ENABLE);
1648                 return;
1649         }
1650
1651         hci->le_resolv_enable = cmd->enable;
1652
1653         status = BT_HCI_ERR_SUCCESS;
1654         cmd_complete(hci, BT_HCI_CMD_LE_SET_RESOLV_ENABLE,
1655                                                 &status, sizeof(status));
1656 }
1657
1658 static void cmd_le_set_resolv_timeout(struct bt_le *hci,
1659                                                 const void *data, uint8_t size)
1660 {
1661         const struct bt_hci_cmd_le_set_resolv_timeout *cmd = data;
1662         uint16_t timeout;
1663         uint8_t status;
1664
1665         timeout = le16_to_cpu(cmd->timeout);
1666
1667         /* Valid range for RPA timeout is 0x0001 to 0xa1b8 */
1668         if (timeout < 0x0001 || timeout > 0xa1b8) {
1669                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1670                                         BT_HCI_CMD_LE_SET_RESOLV_TIMEOUT);
1671                 return;
1672         }
1673
1674         hci->le_resolv_timeout = timeout;
1675
1676         status = BT_HCI_ERR_SUCCESS;
1677         cmd_complete(hci, BT_HCI_CMD_LE_SET_RESOLV_TIMEOUT,
1678                                                 &status, sizeof(status));
1679 }
1680
1681 static void cmd_le_read_max_data_length(struct bt_le *hci,
1682                                                 const void *data, uint8_t size)
1683 {
1684         struct bt_hci_rsp_le_read_max_data_length rsp;
1685
1686         rsp.status = BT_HCI_ERR_SUCCESS;
1687         rsp.max_tx_len = cpu_to_le16(MAX_TX_LEN);
1688         rsp.max_tx_time = cpu_to_le16(MAX_TX_TIME);
1689         rsp.max_rx_len = cpu_to_le16(MAX_RX_LEN);
1690         rsp.max_rx_time = cpu_to_le16(MAX_RX_TIME);
1691
1692         cmd_complete(hci, BT_HCI_CMD_LE_READ_MAX_DATA_LENGTH,
1693                                                         &rsp, sizeof(rsp));
1694 }
1695
1696 static void cmd_le_read_phy(struct bt_le *hci, const void *data, uint8_t size)
1697 {
1698         const struct bt_hci_cmd_le_read_phy *cmd = data;
1699         struct bt_hci_rsp_le_read_phy rsp;
1700
1701         rsp.status = BT_HCI_ERR_SUCCESS;
1702         rsp.handle = cmd->handle;
1703         rsp.tx_phy = 0x01;      /* LE 1M */
1704         rsp.rx_phy = 0x01;      /* LE 1M */
1705
1706         cmd_complete(hci, BT_HCI_CMD_LE_READ_PHY, &rsp, sizeof(rsp));
1707 }
1708
1709 static void cmd_le_set_default_phy(struct bt_le *hci,
1710                                                 const void *data, uint8_t size)
1711 {
1712         const struct bt_hci_cmd_le_set_default_phy *cmd = data;
1713         uint8_t status, tx_phys, rx_phys;
1714         uint8_t phys_mask;
1715
1716         phys_mask = (true << 0) | ((!!(hci->le_features[1] & 0x01)) << 1)
1717                                 | ((!!(hci->le_features[1] & 0x08)) << 2);
1718
1719         if (cmd->all_phys > 0x03) {
1720                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1721                                         BT_HCI_CMD_LE_SET_DEFAULT_PHY);
1722                 return;
1723         }
1724
1725         /* Transmitter PHYs preferences */
1726         if (!(cmd->all_phys & 0x01)) {
1727                 /* At least one preference bit shall be set to 1 */
1728                 if (!cmd->tx_phys) {
1729                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1730                                         BT_HCI_CMD_LE_SET_DEFAULT_PHY);
1731                         return;
1732                 }
1733
1734                 if (cmd->tx_phys & ~phys_mask) {
1735                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1736                                         BT_HCI_CMD_LE_SET_DEFAULT_PHY);
1737                         return;
1738                 }
1739
1740                 tx_phys = cmd->tx_phys;
1741         } else
1742                 tx_phys = 0x00;
1743
1744         /* Transmitter PHYs preferences */
1745         if (!(cmd->all_phys & 0x02)) {
1746                 /* At least one preference bit shall be set to 1 */
1747                 if (!cmd->rx_phys) {
1748                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1749                                         BT_HCI_CMD_LE_SET_DEFAULT_PHY);
1750                         return;
1751                 }
1752
1753                 if (cmd->rx_phys & ~phys_mask) {
1754                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1755                                         BT_HCI_CMD_LE_SET_DEFAULT_PHY);
1756                         return;
1757                 }
1758
1759                 rx_phys = cmd->rx_phys;
1760         } else
1761                 rx_phys = 0x00;
1762
1763         hci->le_default_all_phys = cmd->all_phys;
1764         hci->le_default_tx_phys = tx_phys;
1765         hci->le_default_rx_phys = rx_phys;
1766
1767         status = BT_HCI_ERR_SUCCESS;
1768         cmd_complete(hci, BT_HCI_CMD_LE_SET_DEFAULT_PHY,
1769                                                 &status, sizeof(status));
1770 }
1771
1772 static void cmd_le_set_phy(struct bt_le *hci, const void *data, uint8_t size)
1773 {
1774         const struct bt_hci_cmd_le_set_phy *cmd = data;
1775         struct bt_hci_evt_le_phy_update_complete evt;
1776
1777         cmd_status(hci, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_SET_PHY);
1778
1779         evt.status = BT_HCI_ERR_SUCCESS;
1780         evt.handle = cmd->handle;
1781         evt.tx_phy = 0x01;      /* LE 1M */
1782         evt.rx_phy = 0x01;      /* LE 1M */
1783
1784         if (hci->le_event_mask[1] & 0x08)
1785                 le_meta_event(hci, BT_HCI_EVT_LE_PHY_UPDATE_COMPLETE,
1786                                                         &evt, sizeof(evt));
1787 }
1788
1789 static const struct {
1790         uint16_t opcode;
1791         void (*func) (struct bt_le *hci, const void *data, uint8_t size);
1792         uint8_t size;
1793         bool fixed;
1794 } cmd_table[] = {
1795         { BT_HCI_CMD_DISCONNECT,           cmd_disconnect,           3, true },
1796
1797         { BT_HCI_CMD_SET_EVENT_MASK,       cmd_set_event_mask,       8, true },
1798         { BT_HCI_CMD_RESET,                cmd_reset,                0, true },
1799         { BT_HCI_CMD_SET_EVENT_MASK_PAGE2, cmd_set_event_mask_page2, 8, true },
1800
1801         { BT_HCI_CMD_READ_LOCAL_VERSION,   cmd_read_local_version,   0, true },
1802         { BT_HCI_CMD_READ_LOCAL_COMMANDS,  cmd_read_local_commands,  0, true },
1803         { BT_HCI_CMD_READ_LOCAL_FEATURES,  cmd_read_local_features,  0, true },
1804         { BT_HCI_CMD_READ_BUFFER_SIZE,     cmd_read_buffer_size,     0, true },
1805         { BT_HCI_CMD_READ_BD_ADDR,         cmd_read_bd_addr,         0, true },
1806
1807         { BT_HCI_CMD_LE_SET_EVENT_MASK,
1808                                 cmd_le_set_event_mask, 8, true },
1809         { BT_HCI_CMD_LE_READ_BUFFER_SIZE,
1810                                 cmd_le_read_buffer_size, 0, true },
1811         { BT_HCI_CMD_LE_READ_LOCAL_FEATURES,
1812                                 cmd_le_read_local_features, 0, true },
1813         { BT_HCI_CMD_LE_SET_RANDOM_ADDRESS,
1814                                 cmd_le_set_random_address, 6, true },
1815         { BT_HCI_CMD_LE_SET_ADV_PARAMETERS,
1816                                 cmd_le_set_adv_parameters, 15, true },
1817         { BT_HCI_CMD_LE_READ_ADV_TX_POWER,
1818                                 cmd_le_read_adv_tx_power, 0, true },
1819         { BT_HCI_CMD_LE_SET_ADV_DATA,
1820                                 cmd_le_set_adv_data, 32, true },
1821         { BT_HCI_CMD_LE_SET_SCAN_RSP_DATA,
1822                                 cmd_le_set_scan_rsp_data, 32, true },
1823         { BT_HCI_CMD_LE_SET_ADV_ENABLE,
1824                                 cmd_le_set_adv_enable, 1, true },
1825         { BT_HCI_CMD_LE_SET_SCAN_PARAMETERS,
1826                                 cmd_le_set_scan_parameters, 7, true },
1827         { BT_HCI_CMD_LE_SET_SCAN_ENABLE,
1828                                 cmd_le_set_scan_enable, 2, true },
1829         { BT_HCI_CMD_LE_CREATE_CONN,
1830                                 cmd_le_create_conn, 25, true },
1831         { BT_HCI_CMD_LE_CREATE_CONN_CANCEL,
1832                                 cmd_le_create_conn_cancel, 0, true },
1833         { BT_HCI_CMD_LE_READ_ACCEPT_LIST_SIZE,
1834                                 cmd_le_read_accept_list_size, 0, true },
1835         { BT_HCI_CMD_LE_CLEAR_ACCEPT_LIST,
1836                                 cmd_le_clear_accept_list, 0, true },
1837         { BT_HCI_CMD_LE_ADD_TO_ACCEPT_LIST,
1838                                 cmd_le_add_to_accept_list,  7, true },
1839         { BT_HCI_CMD_LE_REMOVE_FROM_ACCEPT_LIST,
1840                                 cmd_le_remove_from_accept_list, 7, true },
1841
1842         { BT_HCI_CMD_LE_ENCRYPT, cmd_le_encrypt, 32, true },
1843         { BT_HCI_CMD_LE_RAND, cmd_le_rand, 0, true },
1844
1845         { BT_HCI_CMD_LE_READ_SUPPORTED_STATES,
1846                                 cmd_le_read_supported_states, 0, true },
1847
1848         { BT_HCI_CMD_LE_SET_DATA_LENGTH,
1849                                 cmd_le_set_data_length, 6, true },
1850         { BT_HCI_CMD_LE_READ_DEFAULT_DATA_LENGTH,
1851                                 cmd_le_read_default_data_length, 0, true },
1852         { BT_HCI_CMD_LE_WRITE_DEFAULT_DATA_LENGTH,
1853                                 cmd_le_write_default_data_length, 4, true },
1854         { BT_HCI_CMD_LE_READ_LOCAL_PK256,
1855                                 cmd_le_read_local_pk256, 0, true },
1856         { BT_HCI_CMD_LE_GENERATE_DHKEY,
1857                                 cmd_le_generate_dhkey, 64, true },
1858         { BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST,
1859                                 cmd_le_add_to_resolv_list,  39, true },
1860         { BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST,
1861                                 cmd_le_remove_from_resolv_list, 7, true },
1862         { BT_HCI_CMD_LE_CLEAR_RESOLV_LIST,
1863                                 cmd_le_clear_resolv_list, 0, true },
1864         { BT_HCI_CMD_LE_READ_RESOLV_LIST_SIZE,
1865                                 cmd_le_read_resolv_list_size, 0, true },
1866         { BT_HCI_CMD_LE_READ_PEER_RESOLV_ADDR,
1867                                 cmd_le_read_peer_resolv_addr, 7, true },
1868         { BT_HCI_CMD_LE_READ_LOCAL_RESOLV_ADDR,
1869                                 cmd_le_read_local_resolv_addr, 7, true },
1870         { BT_HCI_CMD_LE_SET_RESOLV_ENABLE,
1871                                 cmd_le_set_resolv_enable, 1, true },
1872         { BT_HCI_CMD_LE_SET_RESOLV_TIMEOUT,
1873                                 cmd_le_set_resolv_timeout, 2, true },
1874         { BT_HCI_CMD_LE_READ_MAX_DATA_LENGTH,
1875                                 cmd_le_read_max_data_length, 0, true },
1876         { BT_HCI_CMD_LE_READ_PHY,
1877                                 cmd_le_read_phy, 2, true },
1878         { BT_HCI_CMD_LE_SET_DEFAULT_PHY,
1879                                 cmd_le_set_default_phy, 3, true },
1880         { BT_HCI_CMD_LE_SET_PHY,
1881                                 cmd_le_set_phy, 7, true },
1882
1883         { }
1884 };
1885
1886 static void process_command(struct bt_le *hci, const void *data, size_t size)
1887 {
1888         const struct bt_hci_cmd_hdr *hdr = data;
1889         uint16_t opcode;
1890         unsigned int i;
1891
1892         if (size < sizeof(*hdr))
1893                 return;
1894
1895         data += sizeof(*hdr);
1896         size -= sizeof(*hdr);
1897
1898         opcode = le16_to_cpu(hdr->opcode);
1899
1900         if (hdr->plen != size) {
1901                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS, opcode);
1902                 return;
1903         }
1904
1905         for (i = 0; cmd_table[i].func; i++) {
1906                 if (cmd_table[i].opcode != opcode)
1907                         continue;
1908
1909                 if ((cmd_table[i].fixed && size != cmd_table[i].size) ||
1910                                                 size < cmd_table[i].size) {
1911                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS, opcode);
1912                         return;
1913                 }
1914
1915                 cmd_table[i].func(hci, data, size);
1916                 return;
1917         }
1918
1919         cmd_status(hci, BT_HCI_ERR_UNKNOWN_COMMAND, opcode);
1920 }
1921
1922 static void vhci_read_callback(int fd, uint32_t events, void *user_data)
1923 {
1924         struct bt_le *hci = user_data;
1925         unsigned char buf[4096];
1926         ssize_t len;
1927
1928         if (events & (EPOLLERR | EPOLLHUP))
1929                 return;
1930
1931         len = read(hci->vhci_fd, buf, sizeof(buf));
1932         if (len < 1)
1933                 return;
1934
1935         switch (buf[0]) {
1936         case BT_H4_CMD_PKT:
1937                 process_command(hci, buf + 1, len - 1);
1938                 break;
1939         }
1940 }
1941
1942 static void phy_recv_callback(uint16_t type, const void *data,
1943                                                 size_t size, void *user_data)
1944 {
1945         struct bt_le *hci = user_data;
1946
1947         switch (type) {
1948         case BT_PHY_PKT_ADV:
1949                 if (!(hci->le_event_mask[0] & 0x02))
1950                         return;
1951
1952                 if (hci->scan_window_active) {
1953                         const struct bt_phy_pkt_adv *pkt = data;
1954                         uint8_t buf[100];
1955                         struct bt_hci_evt_le_adv_report *evt = (void *) buf;
1956                         uint8_t tx_addr_type, tx_addr[6];
1957
1958                         if (hci->scan_chan_idx != pkt->chan_idx)
1959                                 break;
1960
1961                         resolve_peer_addr(hci, pkt->tx_addr_type, pkt->tx_addr,
1962                                                         &tx_addr_type, tx_addr);
1963
1964                         if (hci->le_scan_filter_policy == 0x01 ||
1965                                         hci->le_scan_filter_policy == 0x03) {
1966                                 if (!is_in_accept_list(hci, tx_addr_type,
1967                                                                 tx_addr))
1968                                         break;
1969                         }
1970
1971                         if (hci->le_scan_filter_dup) {
1972                                 if (!add_to_scan_cache(hci, tx_addr_type,
1973                                                                 tx_addr))
1974                                         break;
1975                         }
1976
1977                         memset(buf, 0, sizeof(buf));
1978                         evt->num_reports = 0x01;
1979                         evt->event_type = pkt->pdu_type;
1980                         evt->addr_type = tx_addr_type;
1981                         memcpy(evt->addr, tx_addr, 6);
1982                         evt->data_len = pkt->adv_data_len;
1983                         memcpy(buf + sizeof(*evt), data + sizeof(*pkt),
1984                                                         pkt->adv_data_len);
1985
1986                         le_meta_event(hci, BT_HCI_EVT_LE_ADV_REPORT, buf,
1987                                         sizeof(*evt) + pkt->adv_data_len + 1);
1988
1989                         if (hci->le_scan_type == 0x00)
1990                                 break;
1991
1992                         memset(buf, 0, sizeof(buf));
1993                         evt->num_reports = 0x01;
1994                         evt->event_type = 0x04;
1995                         evt->addr_type = tx_addr_type;
1996                         memcpy(evt->addr, tx_addr, 6);
1997                         evt->data_len = pkt->scan_rsp_len;
1998                         memcpy(buf + sizeof(*evt), data + sizeof(*pkt) +
1999                                                         pkt->adv_data_len,
2000                                                         pkt->scan_rsp_len);
2001
2002                         le_meta_event(hci, BT_HCI_EVT_LE_ADV_REPORT, buf,
2003                                         sizeof(*evt) + pkt->scan_rsp_len + 1);
2004                 }
2005                 break;
2006         }
2007 }
2008
2009 struct bt_le *bt_le_new(void)
2010 {
2011         unsigned char setup_cmd[2];
2012         struct bt_le *hci;
2013
2014         hci = calloc(1, sizeof(*hci));
2015         if (!hci)
2016                 return NULL;
2017
2018         hci->adv_timeout_id = -1;
2019         hci->scan_timeout_id = -1;
2020         hci->scan_window_active = false;
2021
2022         reset_defaults(hci);
2023
2024         hci->vhci_fd = open("/dev/vhci", O_RDWR);
2025         if (hci->vhci_fd < 0) {
2026                 free(hci);
2027                 return NULL;
2028         }
2029
2030         setup_cmd[0] = HCI_VENDOR_PKT;
2031         setup_cmd[1] = HCI_PRIMARY;
2032
2033         if (write(hci->vhci_fd, setup_cmd, sizeof(setup_cmd)) < 0) {
2034                 close(hci->vhci_fd);
2035                 free(hci);
2036                 return NULL;
2037         }
2038
2039         mainloop_add_fd(hci->vhci_fd, EPOLLIN, vhci_read_callback, hci, NULL);
2040
2041         hci->phy = bt_phy_new();
2042         hci->crypto = bt_crypto_new();
2043
2044         bt_phy_register(hci->phy, phy_recv_callback, hci);
2045
2046         return bt_le_ref(hci);
2047 }
2048
2049 struct bt_le *bt_le_ref(struct bt_le *hci)
2050 {
2051         if (!hci)
2052                 return NULL;
2053
2054         __sync_fetch_and_add(&hci->ref_count, 1);
2055
2056         return hci;
2057 }
2058
2059 void bt_le_unref(struct bt_le *hci)
2060 {
2061         if (!hci)
2062                 return;
2063
2064         if (__sync_sub_and_fetch(&hci->ref_count, 1))
2065                 return;
2066
2067         stop_adv(hci);
2068
2069         bt_crypto_unref(hci->crypto);
2070         bt_phy_unref(hci->phy);
2071
2072         mainloop_remove_fd(hci->vhci_fd);
2073
2074         close(hci->vhci_fd);
2075
2076         free(hci);
2077 }