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