3 * BlueZ - Bluetooth protocol stack for Linux
5 * Copyright (C) 2011-2012 Intel Corporation
6 * Copyright (C) 2004-2010 Marcel Holtmann <marcel@holtmann.org>
9 * This library is free software; you can redistribute it and/or
10 * modify it under the terms of the GNU Lesser General Public
11 * License as published by the Free Software Foundation; either
12 * version 2.1 of the License, or (at your option) any later version.
14 * This library is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17 * Lesser General Public License for more details.
19 * You should have received a copy of the GNU Lesser General Public
20 * License along with this library; if not, write to the Free Software
21 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
33 #include <sys/socket.h>
38 #include "lib/bluetooth.h"
41 #include "src/shared/util.h"
42 #include "src/shared/crypto.h"
43 #include "src/shared/ecc.h"
44 #include "src/shared/mainloop.h"
45 #include "monitor/bt.h"
50 #define WHITE_LIST_SIZE 16
51 #define RESOLV_LIST_SIZE 16
52 #define SCAN_CACHE_SIZE 64
54 #define DEFAULT_TX_LEN 0x001b
55 #define DEFAULT_TX_TIME 0x0148
56 #define MAX_TX_LEN 0x00fb
57 #define MAX_TX_TIME 0x0848
58 #define MAX_RX_LEN 0x00fb
59 #define MAX_RX_TIME 0x0848
67 volatile int ref_count;
70 struct bt_crypto *crypto;
73 bool scan_window_active;
74 uint8_t scan_chan_idx;
76 uint8_t event_mask[16];
77 uint16_t manufacturer;
82 uint8_t le_event_mask[8];
85 uint8_t le_features[8];
86 uint8_t le_random_addr[6];
87 uint16_t le_adv_min_interval;
88 uint16_t le_adv_max_interval;
90 uint8_t le_adv_own_addr_type;
91 uint8_t le_adv_direct_addr_type;
92 uint8_t le_adv_direct_addr[6];
93 uint8_t le_adv_channel_map;
94 uint8_t le_adv_filter_policy;
95 int8_t le_adv_tx_power;
96 uint8_t le_adv_data_len;
97 uint8_t le_adv_data[31];
98 uint8_t le_scan_rsp_data_len;
99 uint8_t le_scan_rsp_data[31];
100 uint8_t le_adv_enable;
101 uint8_t le_scan_type;
102 uint16_t le_scan_interval;
103 uint16_t le_scan_window;
104 uint8_t le_scan_own_addr_type;
105 uint8_t le_scan_filter_policy;
106 uint8_t le_scan_enable;
107 uint8_t le_scan_filter_dup;
109 uint8_t le_conn_peer_addr_type;
110 uint8_t le_conn_peer_addr[6];
111 uint8_t le_conn_own_addr_type;
112 uint8_t le_conn_enable;
114 uint8_t le_white_list_size;
115 uint8_t le_white_list[WHITE_LIST_SIZE][7];
116 uint8_t le_states[8];
118 uint16_t le_default_tx_len;
119 uint16_t le_default_tx_time;
120 uint8_t le_local_sk256[32];
121 uint8_t le_resolv_list[RESOLV_LIST_SIZE][39];
122 uint8_t le_resolv_list_size;
123 uint8_t le_resolv_enable;
124 uint16_t le_resolv_timeout;
126 struct bt_peer scan_cache[SCAN_CACHE_SIZE];
127 uint8_t scan_cache_count;
130 static bool is_in_white_list(struct bt_le *hci, uint8_t addr_type,
131 const uint8_t addr[6])
135 for (i = 0; i < hci->le_white_list_size; i++) {
136 if (hci->le_white_list[i][0] == addr_type &&
137 !memcmp(&hci->le_white_list[i][1], addr, 6))
144 static void clear_white_list(struct bt_le *hci)
148 for (i = 0; i < hci->le_white_list_size; i++) {
149 hci->le_white_list[i][0] = 0xff;
150 memset(&hci->le_white_list[i][1], 0, 6);
154 static void resolve_peer_addr(struct bt_le *hci, uint8_t peer_addr_type,
155 const uint8_t peer_addr[6],
156 uint8_t *addr_type, uint8_t addr[6])
160 if (!hci->le_resolv_enable)
163 if (peer_addr_type != 0x01)
166 if ((peer_addr[5] & 0xc0) != 0x40)
169 for (i = 0; i < hci->le_resolv_list_size; i++) {
170 uint8_t local_hash[3];
172 if (hci->le_resolv_list[i][0] == 0xff)
175 bt_crypto_ah(hci->crypto, &hci->le_resolv_list[i][7],
176 peer_addr + 3, local_hash);
178 if (!memcmp(peer_addr, local_hash, 3)) {
179 switch (hci->le_resolv_list[i][0]) {
189 memcpy(addr, &hci->le_resolv_list[i][1], 6);
195 *addr_type = peer_addr_type;
196 memcpy(addr, peer_addr, 6);
199 static void clear_resolv_list(struct bt_le *hci)
203 for (i = 0; i < hci->le_resolv_list_size; i++) {
204 hci->le_resolv_list[i][0] = 0xff;
205 memset(&hci->le_resolv_list[i][1], 0, 38);
209 static void reset_defaults(struct bt_le *hci)
211 memset(hci->event_mask, 0, sizeof(hci->event_mask));
212 hci->event_mask[0] |= 0x10; /* Disconnection Complete */
213 hci->event_mask[0] |= 0x80; /* Encryption Change */
214 hci->event_mask[1] |= 0x08; /* Read Remote Version Information Complete */
215 hci->event_mask[1] |= 0x20; /* Command Complete */
216 hci->event_mask[1] |= 0x40; /* Command Status */
217 hci->event_mask[1] |= 0x80; /* Hardware Error */
218 hci->event_mask[2] |= 0x04; /* Number of Completed Packets */
219 hci->event_mask[3] |= 0x02; /* Data Buffer Overflow */
220 hci->event_mask[5] |= 0x80; /* Encryption Key Refresh Complete */
221 //hci->event_mask[7] |= 0x20; /* LE Meta Event */
223 hci->manufacturer = 0x003f; /* Bluetooth SIG (63) */
225 memset(hci->commands, 0, sizeof(hci->commands));
226 hci->commands[0] |= 0x20; /* Disconnect */
227 //hci->commands[2] |= 0x80; /* Read Remote Version Information */
228 hci->commands[5] |= 0x40; /* Set Event Mask */
229 hci->commands[5] |= 0x80; /* Reset */
230 //hci->commands[10] |= 0x04; /* Read Transmit Power Level */
231 hci->commands[14] |= 0x08; /* Read Local Version Information */
232 hci->commands[14] |= 0x10; /* Read Local Supported Commands */
233 hci->commands[14] |= 0x20; /* Read Local Supported Features */
234 hci->commands[14] |= 0x80; /* Read Buffer Size */
235 hci->commands[15] |= 0x02; /* Read BD ADDR */
236 //hci->commands[15] |= 0x20; /* Read RSSI */
237 hci->commands[22] |= 0x04; /* Set Event Mask Page 2 */
238 hci->commands[25] |= 0x01; /* LE Set Event Mask */
239 hci->commands[25] |= 0x02; /* LE Read Buffer Size */
240 hci->commands[25] |= 0x04; /* LE Read Local Supported Features */
241 hci->commands[25] |= 0x10; /* LE Set Random Address */
242 hci->commands[25] |= 0x20; /* LE Set Advertising Parameters */
243 hci->commands[25] |= 0x40; /* LE Read Advertising Channel TX Power */
244 hci->commands[25] |= 0x80; /* LE Set Advertising Data */
245 hci->commands[26] |= 0x01; /* LE Set Scan Response Data */
246 hci->commands[26] |= 0x02; /* LE Set Advertise Enable */
247 hci->commands[26] |= 0x04; /* LE Set Scan Parameters */
248 hci->commands[26] |= 0x08; /* LE Set Scan Enable */
249 hci->commands[26] |= 0x10; /* LE Create Connection */
250 hci->commands[26] |= 0x20; /* LE Create Connection Cancel */
251 hci->commands[26] |= 0x40; /* LE Read White List Size */
252 hci->commands[26] |= 0x80; /* LE Clear White List */
253 hci->commands[27] |= 0x01; /* LE Add Device To White List */
254 hci->commands[27] |= 0x02; /* LE Remove Device From White List */
255 //hci->commands[27] |= 0x04; /* LE Connection Update */
256 //hci->commands[27] |= 0x08; /* LE Set Host Channel Classification */
257 //hci->commands[27] |= 0x10; /* LE Read Channel Map */
258 //hci->commands[27] |= 0x20; /* LE Read Remote Used Features */
259 hci->commands[27] |= 0x40; /* LE Encrypt */
260 hci->commands[27] |= 0x80; /* LE Rand */
261 //hci->commands[28] |= 0x01; /* LE Start Encryption */
262 //hci->commands[28] |= 0x02; /* LE Long Term Key Request Reply */
263 //hci->commands[28] |= 0x04; /* LE Long Term Key Request Negative Reply */
264 hci->commands[28] |= 0x08; /* LE Read Supported States */
265 //hci->commands[28] |= 0x10; /* LE Receiver Test */
266 //hci->commands[28] |= 0x20; /* LE Transmitter Test */
267 //hci->commands[28] |= 0x40; /* LE Test End */
268 //hci->commands[33] |= 0x10; /* LE Remote Connection Parameter Request Reply */
269 //hci->commands[33] |= 0x20; /* LE Remote Connection Parameter Request Negative Reply */
270 hci->commands[33] |= 0x40; /* LE Set Data Length */
271 hci->commands[33] |= 0x80; /* LE Read Suggested Default Data Length */
272 hci->commands[34] |= 0x01; /* LE Write Suggested Default Data Length */
273 hci->commands[34] |= 0x02; /* LE Read Local P-256 Public Key */
274 hci->commands[34] |= 0x04; /* LE Generate DHKey */
275 hci->commands[34] |= 0x08; /* LE Add Device To Resolving List */
276 hci->commands[34] |= 0x10; /* LE Remove Device From Resolving List */
277 hci->commands[34] |= 0x20; /* LE Clear Resolving List */
278 hci->commands[34] |= 0x40; /* LE Read Resolving List Size */
279 hci->commands[34] |= 0x80; /* LE Read Peer Resolvable Address */
280 hci->commands[35] |= 0x01; /* LE Read Local Resolvable Address */
281 hci->commands[35] |= 0x02; /* LE Set Address Resolution Enable */
282 hci->commands[35] |= 0x04; /* LE Set Resolvable Private Address Timeout */
283 hci->commands[35] |= 0x08; /* LE Read Maximum Data Length */
285 memset(hci->features, 0, sizeof(hci->features));
286 hci->features[4] |= 0x20; /* BR/EDR Not Supported */
287 hci->features[4] |= 0x40; /* LE Supported */
289 memset(hci->bdaddr, 0, sizeof(hci->bdaddr));
291 memset(hci->le_event_mask, 0, sizeof(hci->le_event_mask));
292 hci->le_event_mask[0] |= 0x01; /* LE Connection Complete */
293 hci->le_event_mask[0] |= 0x02; /* LE Advertising Report */
294 hci->le_event_mask[0] |= 0x04; /* LE Connection Update Complete */
295 hci->le_event_mask[0] |= 0x08; /* LE Read Remote Used Features Complete */
296 hci->le_event_mask[0] |= 0x10; /* LE Long Term Key Request */
297 //hci->le_event_mask[0] |= 0x20; /* LE Remote Connection Parameter Request */
298 //hci->le_event_mask[0] |= 0x40; /* LE Data Length Change */
299 //hci->le_event_mask[0] |= 0x80; /* LE Read Local P-256 Public Key Complete */
300 //hci->le_event_mask[1] |= 0x01; /* LE Generate DHKey Complete */
301 //hci->le_event_mask[1] |= 0x02; /* LE Enhanced Connection Complete */
302 //hci->le_event_mask[1] |= 0x04; /* LE Direct Advertising Report */
307 memset(hci->le_features, 0, sizeof(hci->le_features));
308 hci->le_features[0] |= 0x01; /* LE Encryption */
309 //hci->le_features[0] |= 0x02; /* Connection Parameter Request Procedure */
310 //hci->le_features[0] |= 0x04; /* Extended Reject Indication */
311 //hci->le_features[0] |= 0x08; /* Slave-initiated Features Exchange */
312 hci->le_features[0] |= 0x10; /* LE Ping */
313 hci->le_features[0] |= 0x20; /* LE Data Packet Length Extension */
314 hci->le_features[0] |= 0x40; /* LL Privacy */
315 hci->le_features[0] |= 0x80; /* Extended Scanner Filter Policies */
317 memset(hci->le_random_addr, 0, sizeof(hci->le_random_addr));
319 hci->le_adv_min_interval = 0x0800;
320 hci->le_adv_max_interval = 0x0800;
321 hci->le_adv_type = 0x00;
322 hci->le_adv_own_addr_type = 0x00;
323 hci->le_adv_direct_addr_type = 0x00;
324 memset(hci->le_adv_direct_addr, 0, 6);
325 hci->le_adv_channel_map = 0x07;
326 hci->le_adv_filter_policy = 0x00;
328 hci->le_adv_tx_power = 0;
330 memset(hci->le_adv_data, 0, sizeof(hci->le_adv_data));
331 hci->le_adv_data_len = 0;
333 memset(hci->le_scan_rsp_data, 0, sizeof(hci->le_scan_rsp_data));
334 hci->le_scan_rsp_data_len = 0;
336 hci->le_adv_enable = 0x00;
338 hci->le_scan_type = 0x00; /* Passive Scanning */
339 hci->le_scan_interval = 0x0010; /* 10 ms */
340 hci->le_scan_window = 0x0010; /* 10 ms */
341 hci->le_scan_own_addr_type = 0x00; /* Public Device Address */
342 hci->le_scan_filter_policy = 0x00;
343 hci->le_scan_enable = 0x00;
344 hci->le_scan_filter_dup = 0x00;
346 hci->le_conn_enable = 0x00;
348 hci->le_white_list_size = WHITE_LIST_SIZE;
349 clear_white_list(hci);
351 memset(hci->le_states, 0, sizeof(hci->le_states));
352 hci->le_states[0] |= 0x01; /* Non-connectable Advertising */
353 hci->le_states[0] |= 0x02; /* Scannable Advertising */
354 hci->le_states[0] |= 0x04; /* Connectable Advertising */
355 hci->le_states[0] |= 0x08; /* High Duty Cycle Directed Advertising */
356 hci->le_states[0] |= 0x10; /* Passive Scanning */
357 hci->le_states[0] |= 0x20; /* Active Scanning */
358 hci->le_states[0] |= 0x40; /* Initiating + Connection (Master Role) */
359 hci->le_states[0] |= 0x80; /* Connection (Slave Role) */
360 hci->le_states[1] |= 0x01; /* Passive Scanning +
361 * Non-connectable Advertising */
363 hci->le_default_tx_len = DEFAULT_TX_LEN;
364 hci->le_default_tx_time = DEFAULT_TX_TIME;
366 memset(hci->le_local_sk256, 0, sizeof(hci->le_local_sk256));
368 hci->le_resolv_list_size = RESOLV_LIST_SIZE;
369 clear_resolv_list(hci);
370 hci->le_resolv_enable = 0x00;
371 hci->le_resolv_timeout = 0x0384; /* 900 secs or 15 minutes */
374 static void clear_scan_cache(struct bt_le *hci)
376 memset(hci->scan_cache, 0, sizeof(hci->scan_cache));
377 hci->scan_cache_count = 0;
380 static bool add_to_scan_cache(struct bt_le *hci, uint8_t addr_type,
381 const uint8_t addr[6])
385 for (i = 0; i < hci->scan_cache_count; i++) {
386 if (hci->scan_cache[i].addr_type == addr_type &&
387 !memcmp(hci->scan_cache[i].addr, addr, 6))
391 if (hci->scan_cache_count >= SCAN_CACHE_SIZE)
394 hci->scan_cache[hci->scan_cache_count].addr_type = addr_type;
395 memcpy(hci->scan_cache[hci->scan_cache_count].addr, addr, 6);
396 hci->scan_cache_count++;
401 static void send_event(struct bt_le *hci, uint8_t event,
402 void *data, uint8_t size)
404 uint8_t type = BT_H4_EVT_PKT;
405 struct bt_hci_evt_hdr hdr;
412 iov[0].iov_base = &type;
414 iov[1].iov_base = &hdr;
415 iov[1].iov_len = sizeof(hdr);
418 iov[2].iov_base = data;
419 iov[2].iov_len = size;
424 if (writev(hci->vhci_fd, iov, iovcnt) < 0)
425 fprintf(stderr, "Write to /dev/vhci failed (%m)\n");
428 static void send_adv_pkt(struct bt_le *hci, uint8_t channel)
430 struct bt_phy_pkt_adv pkt;
432 memset(&pkt, 0, sizeof(pkt));
433 pkt.chan_idx = channel;
434 pkt.pdu_type = hci->le_adv_type;
435 pkt.tx_addr_type = hci->le_adv_own_addr_type;
436 switch (hci->le_adv_own_addr_type) {
439 memcpy(pkt.tx_addr, hci->bdaddr, 6);
443 memcpy(pkt.tx_addr, hci->le_random_addr, 6);
446 pkt.rx_addr_type = hci->le_adv_direct_addr_type;
447 memcpy(pkt.rx_addr, hci->le_adv_direct_addr, 6);
448 pkt.adv_data_len = hci->le_adv_data_len;
449 pkt.scan_rsp_len = hci->le_scan_rsp_data_len;
451 bt_phy_send_vector(hci->phy, BT_PHY_PKT_ADV, &pkt, sizeof(pkt),
452 hci->le_adv_data, pkt.adv_data_len,
453 hci->le_scan_rsp_data, pkt.scan_rsp_len);
456 static unsigned int get_adv_delay(void)
458 /* The advertising delay is a pseudo-random value with a range
459 * of 0 ms to 10 ms generated for each advertising event.
462 return (rand() % 11);
465 static void adv_timeout_callback(int id, void *user_data)
467 struct bt_le *hci = user_data;
468 unsigned int msec, min_msec, max_msec;
470 if (hci->le_adv_channel_map & 0x01)
471 send_adv_pkt(hci, 37);
472 if (hci->le_adv_channel_map & 0x02)
473 send_adv_pkt(hci, 38);
474 if (hci->le_adv_channel_map & 0x04)
475 send_adv_pkt(hci, 39);
477 min_msec = (hci->le_adv_min_interval * 625) / 1000;
478 max_msec = (hci->le_adv_max_interval * 625) / 1000;
480 msec = ((min_msec + max_msec) / 2) + get_adv_delay();
482 if (mainloop_modify_timeout(id, msec) < 0) {
483 fprintf(stderr, "Setting advertising timeout failed\n");
484 hci->le_adv_enable = 0x00;
488 static bool start_adv(struct bt_le *hci)
492 if (hci->adv_timeout_id >= 0)
495 msec = ((hci->le_adv_min_interval * 625) / 1000) + get_adv_delay();
497 hci->adv_timeout_id = mainloop_add_timeout(msec, adv_timeout_callback,
499 if (hci->adv_timeout_id < 0)
505 static bool stop_adv(struct bt_le *hci)
507 if (hci->adv_timeout_id < 0)
510 mainloop_remove_timeout(hci->adv_timeout_id);
511 hci->adv_timeout_id = -1;
516 static void scan_timeout_callback(int id, void *user_data)
518 struct bt_le *hci = user_data;
521 if (hci->le_scan_window == hci->le_scan_interval ||
522 !hci->scan_window_active) {
523 msec = (hci->le_scan_window * 625) / 1000;
524 hci->scan_window_active = true;
526 hci->scan_chan_idx++;
527 if (hci->scan_chan_idx > 39)
528 hci->scan_chan_idx = 37;
530 msec = ((hci->le_scan_interval -
531 hci->le_scan_window) * 625) / 1000;
532 hci->scan_window_active = false;
535 if (mainloop_modify_timeout(id, msec) < 0) {
536 fprintf(stderr, "Setting scanning timeout failed\n");
537 hci->le_scan_enable = 0x00;
538 hci->scan_window_active = false;
542 static bool start_scan(struct bt_le *hci)
546 if (hci->scan_timeout_id >= 0)
549 msec = (hci->le_scan_window * 625) / 1000;
551 hci->scan_timeout_id = mainloop_add_timeout(msec, scan_timeout_callback,
553 if (hci->scan_timeout_id < 0)
556 hci->scan_window_active = true;
557 hci->scan_chan_idx = 37;
562 static bool stop_scan(struct bt_le *hci)
564 if (hci->scan_timeout_id < 0)
567 mainloop_remove_timeout(hci->scan_timeout_id);
568 hci->scan_timeout_id = -1;
570 hci->scan_window_active = false;
575 static void cmd_complete(struct bt_le *hci, uint16_t opcode,
576 const void *data, uint8_t len)
578 struct bt_hci_evt_cmd_complete *cc;
581 pkt_data = alloca(sizeof(*cc) + len);
587 cc->opcode = cpu_to_le16(opcode);
590 memcpy(pkt_data + sizeof(*cc), data, len);
592 send_event(hci, BT_HCI_EVT_CMD_COMPLETE, pkt_data, sizeof(*cc) + len);
595 static void cmd_status(struct bt_le *hci, uint8_t status, uint16_t opcode)
597 struct bt_hci_evt_cmd_status cs;
601 cs.opcode = cpu_to_le16(opcode);
603 send_event(hci, BT_HCI_EVT_CMD_STATUS, &cs, sizeof(cs));
606 static void le_meta_event(struct bt_le *hci, uint8_t event,
607 void *data, uint8_t len)
611 if (!(hci->event_mask[7] & 0x20))
614 pkt_data = alloca(1 + len);
618 ((uint8_t *) pkt_data)[0] = event;
621 memcpy(pkt_data + 1, data, len);
623 send_event(hci, BT_HCI_EVT_LE_META_EVENT, pkt_data, 1 + len);
626 static void cmd_disconnect(struct bt_le *hci, const void *data, uint8_t size)
628 cmd_status(hci, BT_HCI_ERR_UNKNOWN_CONN_ID, BT_HCI_CMD_DISCONNECT);
631 static void cmd_set_event_mask(struct bt_le *hci,
632 const void *data, uint8_t size)
634 const struct bt_hci_cmd_set_event_mask *cmd = data;
637 memcpy(hci->event_mask, cmd->mask, 8);
639 status = BT_HCI_ERR_SUCCESS;
640 cmd_complete(hci, BT_HCI_CMD_SET_EVENT_MASK, &status, sizeof(status));
643 static void cmd_reset(struct bt_le *hci, const void *data, uint8_t size)
651 status = BT_HCI_ERR_SUCCESS;
652 cmd_complete(hci, BT_HCI_CMD_RESET, &status, sizeof(status));
655 static void cmd_set_event_mask_page2(struct bt_le *hci,
656 const void *data, uint8_t size)
658 const struct bt_hci_cmd_set_event_mask_page2 *cmd = data;
661 memcpy(hci->event_mask + 8, cmd->mask, 8);
663 status = BT_HCI_ERR_SUCCESS;
664 cmd_complete(hci, BT_HCI_CMD_SET_EVENT_MASK_PAGE2,
665 &status, sizeof(status));
668 static void cmd_read_local_version(struct bt_le *hci,
669 const void *data, uint8_t size)
671 struct bt_hci_rsp_read_local_version rsp;
673 rsp.status = BT_HCI_ERR_SUCCESS;
675 rsp.hci_rev = cpu_to_le16(0x0000);
677 rsp.manufacturer = cpu_to_le16(hci->manufacturer);
678 rsp.lmp_subver = cpu_to_le16(0x0000);
680 cmd_complete(hci, BT_HCI_CMD_READ_LOCAL_VERSION, &rsp, sizeof(rsp));
683 static void cmd_read_local_commands(struct bt_le *hci,
684 const void *data, uint8_t size)
686 struct bt_hci_rsp_read_local_commands rsp;
688 rsp.status = BT_HCI_ERR_SUCCESS;
689 memcpy(rsp.commands, hci->commands, 64);
691 cmd_complete(hci, BT_HCI_CMD_READ_LOCAL_COMMANDS, &rsp, sizeof(rsp));
694 static void cmd_read_local_features(struct bt_le *hci,
695 const void *data, uint8_t size)
697 struct bt_hci_rsp_read_local_features rsp;
699 rsp.status = BT_HCI_ERR_SUCCESS;
700 memcpy(rsp.features, hci->features, 8);
702 cmd_complete(hci, BT_HCI_CMD_READ_LOCAL_FEATURES, &rsp, sizeof(rsp));
705 static void cmd_read_buffer_size(struct bt_le *hci,
706 const void *data, uint8_t size)
708 struct bt_hci_rsp_read_buffer_size rsp;
710 rsp.status = BT_HCI_ERR_SUCCESS;
711 rsp.acl_mtu = cpu_to_le16(0x0000);
713 rsp.acl_max_pkt = cpu_to_le16(0x0000);
714 rsp.sco_max_pkt = cpu_to_le16(0x0000);
716 cmd_complete(hci, BT_HCI_CMD_READ_BUFFER_SIZE, &rsp, sizeof(rsp));
719 static void cmd_read_bd_addr(struct bt_le *hci, const void *data, uint8_t size)
721 struct bt_hci_rsp_read_bd_addr rsp;
723 rsp.status = BT_HCI_ERR_SUCCESS;
724 memcpy(rsp.bdaddr, hci->bdaddr, 6);
726 cmd_complete(hci, BT_HCI_CMD_READ_BD_ADDR, &rsp, sizeof(rsp));
729 static void cmd_le_set_event_mask(struct bt_le *hci,
730 const void *data, uint8_t size)
732 const struct bt_hci_cmd_le_set_event_mask *cmd = data;
735 memcpy(hci->le_event_mask, cmd->mask, 8);
737 status = BT_HCI_ERR_SUCCESS;
738 cmd_complete(hci, BT_HCI_CMD_LE_SET_EVENT_MASK,
739 &status, sizeof(status));
742 static void cmd_le_read_buffer_size(struct bt_le *hci,
743 const void *data, uint8_t size)
745 struct bt_hci_rsp_le_read_buffer_size rsp;
747 rsp.status = BT_HCI_ERR_SUCCESS;
748 rsp.le_mtu = cpu_to_le16(hci->le_mtu);
749 rsp.le_max_pkt = hci->le_max_pkt;
751 cmd_complete(hci, BT_HCI_CMD_LE_READ_BUFFER_SIZE, &rsp, sizeof(rsp));
754 static void cmd_le_read_local_features(struct bt_le *hci,
755 const void *data, uint8_t size)
757 struct bt_hci_rsp_le_read_local_features rsp;
759 rsp.status = BT_HCI_ERR_SUCCESS;
760 memcpy(rsp.features, hci->le_features, 8);
762 cmd_complete(hci, BT_HCI_CMD_LE_READ_LOCAL_FEATURES,
766 static void cmd_le_set_random_address(struct bt_le *hci,
767 const void *data, uint8_t size)
769 const struct bt_hci_cmd_le_set_random_address *cmd = data;
772 memcpy(hci->le_random_addr, cmd->addr, 6);
774 status = BT_HCI_ERR_SUCCESS;
775 cmd_complete(hci, BT_HCI_CMD_LE_SET_RANDOM_ADDRESS,
776 &status, sizeof(status));
779 static void cmd_le_set_adv_parameters(struct bt_le *hci,
780 const void *data, uint8_t size)
782 const struct bt_hci_cmd_le_set_adv_parameters *cmd = data;
783 uint16_t min_interval, max_interval;
786 if (hci->le_adv_enable == 0x01) {
787 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
788 BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
792 min_interval = le16_to_cpu(cmd->min_interval);
793 max_interval = le16_to_cpu(cmd->max_interval);
795 /* Valid range for advertising type is 0x00 to 0x03 */
797 case 0x00: /* ADV_IND */
798 /* Range for advertising interval min is 0x0020 to 0x4000 */
799 if (min_interval < 0x0020 || min_interval > 0x4000) {
800 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
801 BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
804 /* Range for advertising interval max is 0x0020 to 0x4000 */
805 if (max_interval < 0x0020 || max_interval > 0x4000) {
806 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
807 BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
810 /* Advertising interval max shall be less or equal */
811 if (min_interval > max_interval) {
812 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
813 BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
818 case 0x01: /* ADV_DIRECT_IND */
819 /* Range for direct address type is 0x00 to 0x01 */
820 if (cmd->direct_addr_type > 0x01) {
821 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
822 BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
827 case 0x02: /* ADV_SCAN_IND */
828 case 0x03: /* ADV_NONCONN_IND */
829 /* Range for advertising interval min is 0x00a0 to 0x4000 */
830 if (min_interval < 0x00a0 || min_interval > 0x4000) {
831 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
832 BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
835 /* Range for advertising interval max is 0x00a0 to 0x4000 */
836 if (max_interval < 0x00a0 || max_interval > 0x4000) {
837 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
838 BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
841 /* Advertising interval min shall be less or equal */
842 if (min_interval > max_interval) {
843 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
844 BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
850 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
851 BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
855 /* Valid range for own address type is 0x00 to 0x03 */
856 if (cmd->own_addr_type > 0x03) {
857 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
858 BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
862 /* Valid range for advertising channel map is 0x01 to 0x07 */
863 if (cmd->channel_map < 0x01 || cmd->channel_map > 0x07) {
864 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
865 BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
869 /* Valid range for advertising filter policy is 0x00 to 0x03 */
870 if (cmd->filter_policy > 0x03) {
871 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
872 BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
876 hci->le_adv_min_interval = min_interval;
877 hci->le_adv_max_interval = max_interval;
878 hci->le_adv_type = cmd->type;
879 hci->le_adv_own_addr_type = cmd->own_addr_type;
880 hci->le_adv_direct_addr_type = cmd->direct_addr_type;
881 memcpy(hci->le_adv_direct_addr, cmd->direct_addr, 6);
882 hci->le_adv_channel_map = cmd->channel_map;
883 hci->le_adv_filter_policy = cmd->filter_policy;
885 status = BT_HCI_ERR_SUCCESS;
886 cmd_complete(hci, BT_HCI_CMD_LE_SET_ADV_PARAMETERS,
887 &status, sizeof(status));
890 static void cmd_le_read_adv_tx_power(struct bt_le *hci,
891 const void *data, uint8_t size)
893 struct bt_hci_rsp_le_read_adv_tx_power rsp;
895 rsp.status = BT_HCI_ERR_SUCCESS;
896 rsp.level = hci->le_adv_tx_power;
898 cmd_complete(hci, BT_HCI_CMD_LE_READ_ADV_TX_POWER, &rsp, sizeof(rsp));
901 static void cmd_le_set_adv_data(struct bt_le *hci,
902 const void *data, uint8_t size)
904 const struct bt_hci_cmd_le_set_adv_data *cmd = data;
907 /* Valid range for advertising data length is 0x00 to 0x1f */
908 if (cmd->len > 0x1f) {
909 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
910 BT_HCI_CMD_LE_SET_ADV_DATA);
914 hci->le_adv_data_len = cmd->len;
915 memcpy(hci->le_adv_data, cmd->data, 31);
917 status = BT_HCI_ERR_SUCCESS;
918 cmd_complete(hci, BT_HCI_CMD_LE_SET_ADV_DATA, &status, sizeof(status));
921 static void cmd_le_set_scan_rsp_data(struct bt_le *hci,
922 const void *data, uint8_t size)
924 const struct bt_hci_cmd_le_set_scan_rsp_data *cmd = data;
927 /* Valid range for scan response data length is 0x00 to 0x1f */
928 if (cmd->len > 0x1f) {
929 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
930 BT_HCI_CMD_LE_SET_SCAN_RSP_DATA);
934 hci->le_scan_rsp_data_len = cmd->len;
935 memcpy(hci->le_scan_rsp_data, cmd->data, 31);
937 status = BT_HCI_ERR_SUCCESS;
938 cmd_complete(hci, BT_HCI_CMD_LE_SET_SCAN_RSP_DATA,
939 &status, sizeof(status));
942 static void cmd_le_set_adv_enable(struct bt_le *hci,
943 const void *data, uint8_t size)
945 const struct bt_hci_cmd_le_set_adv_enable *cmd = data;
949 /* Valid range for advertising enable is 0x00 to 0x01 */
950 if (cmd->enable > 0x01) {
951 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
952 BT_HCI_CMD_LE_SET_ADV_ENABLE);
956 if (cmd->enable == hci->le_adv_enable) {
957 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
958 BT_HCI_CMD_LE_SET_ADV_ENABLE);
962 if (cmd->enable == 0x01)
963 result = start_adv(hci);
965 result = stop_adv(hci);
968 cmd_status(hci, BT_HCI_ERR_UNSPECIFIED_ERROR,
969 BT_HCI_CMD_LE_SET_ADV_ENABLE);
973 hci->le_adv_enable = cmd->enable;
975 status = BT_HCI_ERR_SUCCESS;
976 cmd_complete(hci, BT_HCI_CMD_LE_SET_ADV_ENABLE,
977 &status, sizeof(status));
980 static void cmd_le_set_scan_parameters(struct bt_le *hci,
981 const void *data, uint8_t size)
983 const struct bt_hci_cmd_le_set_scan_parameters *cmd = data;
984 uint16_t interval, window;
987 if (hci->le_scan_enable == 0x01) {
988 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
989 BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
993 interval = le16_to_cpu(cmd->interval);
994 window = le16_to_cpu(cmd->window);
996 /* Valid range for scan type is 0x00 to 0x01 */
997 if (cmd->type > 0x01) {
998 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
999 BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
1003 /* Valid range for scan interval is 0x0004 to 0x4000 */
1004 if (interval < 0x0004 || interval > 0x4000) {
1005 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1006 BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
1010 /* Valid range for scan window is 0x0004 to 0x4000 */
1011 if (window < 0x0004 || window > 0x4000) {
1012 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1013 BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
1017 /* Scan window shall be less or equal than scan interval */
1018 if (window > interval) {
1019 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1020 BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
1024 /* Valid range for own address type is 0x00 to 0x03 */
1025 if (cmd->own_addr_type > 0x03) {
1026 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1027 BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
1031 /* Valid range for scanning filter policy is 0x00 to 0x03 */
1032 if (cmd->filter_policy > 0x03) {
1033 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1034 BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
1038 hci->le_scan_type = cmd->type;
1039 hci->le_scan_interval = interval;
1040 hci->le_scan_window = window;
1041 hci->le_scan_own_addr_type = cmd->own_addr_type;
1042 hci->le_scan_filter_policy = cmd->filter_policy;
1044 status = BT_HCI_ERR_SUCCESS;
1045 cmd_complete(hci, BT_HCI_CMD_LE_SET_SCAN_PARAMETERS,
1046 &status, sizeof(status));
1049 static void cmd_le_set_scan_enable(struct bt_le *hci,
1050 const void *data, uint8_t size)
1052 const struct bt_hci_cmd_le_set_scan_enable *cmd = data;
1056 /* Valid range for scan enable is 0x00 to 0x01 */
1057 if (cmd->enable > 0x01) {
1058 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1059 BT_HCI_CMD_LE_SET_SCAN_ENABLE);
1063 /* Valid range for filter duplicates is 0x00 to 0x01 */
1064 if (cmd->filter_dup > 0x01) {
1065 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1066 BT_HCI_CMD_LE_SET_SCAN_ENABLE);
1070 if (cmd->enable == hci->le_scan_enable) {
1071 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
1072 BT_HCI_CMD_LE_SET_SCAN_ENABLE);
1076 clear_scan_cache(hci);
1078 if (cmd->enable == 0x01)
1079 result = start_scan(hci);
1081 result = stop_scan(hci);
1084 cmd_status(hci, BT_HCI_ERR_UNSPECIFIED_ERROR,
1085 BT_HCI_CMD_LE_SET_SCAN_ENABLE);
1089 hci->le_scan_enable = cmd->enable;
1090 hci->le_scan_filter_dup = cmd->filter_dup;
1092 status = BT_HCI_ERR_SUCCESS;
1093 cmd_complete(hci, BT_HCI_CMD_LE_SET_SCAN_ENABLE,
1094 &status, sizeof(status));
1097 static void cmd_le_create_conn(struct bt_le *hci,
1098 const void *data, uint8_t size)
1100 const struct bt_hci_cmd_le_create_conn *cmd = data;
1102 if (hci->le_conn_enable == 0x01) {
1103 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
1104 BT_HCI_CMD_LE_CREATE_CONN);
1108 /* Valid range for peer address type is 0x00 to 0x03 */
1109 if (cmd->peer_addr_type > 0x03) {
1110 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1111 BT_HCI_CMD_LE_CREATE_CONN);
1115 /* Valid range for own address type is 0x00 to 0x03 */
1116 if (cmd->own_addr_type > 0x03) {
1117 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1118 BT_HCI_CMD_LE_CREATE_CONN);
1122 hci->le_conn_peer_addr_type = cmd->peer_addr_type;
1123 memcpy(hci->le_conn_peer_addr, cmd->peer_addr, 6);
1124 hci->le_conn_own_addr_type = cmd->own_addr_type;
1125 hci->le_conn_enable = 0x01;
1127 cmd_status(hci, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_CREATE_CONN);
1130 static void cmd_le_create_conn_cancel(struct bt_le *hci,
1131 const void *data, uint8_t size)
1133 struct bt_hci_evt_le_conn_complete evt;
1136 if (hci->le_conn_enable == 0x00) {
1137 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
1138 BT_HCI_CMD_LE_CREATE_CONN_CANCEL);
1142 hci->le_conn_enable = 0x00;
1144 status = BT_HCI_ERR_SUCCESS;
1145 cmd_complete(hci, BT_HCI_CMD_LE_CREATE_CONN_CANCEL,
1146 &status, sizeof(status));
1148 evt.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1149 evt.handle = cpu_to_le16(0x0000);
1151 evt.peer_addr_type = 0x00;
1152 memset(evt.peer_addr, 0, 6);
1153 evt.interval = cpu_to_le16(0x0000);
1154 evt.latency = cpu_to_le16(0x0000);
1155 evt.supv_timeout = cpu_to_le16(0x0000);
1156 evt.clock_accuracy = 0x00;
1158 if (hci->le_event_mask[0] & 0x01)
1159 le_meta_event(hci, BT_HCI_EVT_LE_CONN_COMPLETE,
1163 static void cmd_le_read_white_list_size(struct bt_le *hci,
1164 const void *data, uint8_t size)
1166 struct bt_hci_rsp_le_read_white_list_size rsp;
1168 rsp.status = BT_HCI_ERR_SUCCESS;
1169 rsp.size = hci->le_white_list_size;
1171 cmd_complete(hci, BT_HCI_CMD_LE_READ_WHITE_LIST_SIZE,
1175 static void cmd_le_clear_white_list(struct bt_le *hci,
1176 const void *data, uint8_t size)
1180 clear_white_list(hci);
1182 status = BT_HCI_ERR_SUCCESS;
1183 cmd_complete(hci, BT_HCI_CMD_LE_CLEAR_WHITE_LIST,
1184 &status, sizeof(status));
1187 static void cmd_le_add_to_white_list(struct bt_le *hci,
1188 const void *data, uint8_t size)
1190 const struct bt_hci_cmd_le_add_to_white_list *cmd = data;
1192 bool exists = false;
1195 /* Valid range for address type is 0x00 to 0x01 */
1196 if (cmd->addr_type > 0x01) {
1197 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1198 BT_HCI_CMD_LE_ADD_TO_WHITE_LIST);
1202 for (i = 0; i < hci->le_white_list_size; i++) {
1203 if (hci->le_white_list[i][0] == cmd->addr_type &&
1204 !memcmp(&hci->le_white_list[i][1],
1208 } else if (pos < 0 && hci->le_white_list[i][0] == 0xff)
1213 cmd_status(hci, BT_HCI_ERR_UNSPECIFIED_ERROR,
1214 BT_HCI_CMD_LE_ADD_TO_WHITE_LIST);
1219 cmd_status(hci, BT_HCI_ERR_MEM_CAPACITY_EXCEEDED,
1220 BT_HCI_CMD_LE_ADD_TO_WHITE_LIST);
1224 hci->le_white_list[pos][0] = cmd->addr_type;
1225 memcpy(&hci->le_white_list[pos][1], cmd->addr, 6);
1227 status = BT_HCI_ERR_SUCCESS;
1228 cmd_complete(hci, BT_HCI_CMD_LE_ADD_TO_WHITE_LIST,
1229 &status, sizeof(status));
1232 static void cmd_le_remove_from_white_list(struct bt_le *hci,
1233 const void *data, uint8_t size)
1235 const struct bt_hci_cmd_le_remove_from_white_list *cmd = data;
1239 /* Valid range for address type is 0x00 to 0x01 */
1240 if (cmd->addr_type > 0x01) {
1241 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1242 BT_HCI_CMD_LE_REMOVE_FROM_WHITE_LIST);
1246 for (i = 0; i < hci->le_white_list_size; i++) {
1247 if (hci->le_white_list[i][0] == cmd->addr_type &&
1248 !memcmp(&hci->le_white_list[i][1],
1256 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1257 BT_HCI_CMD_LE_REMOVE_FROM_WHITE_LIST);
1261 hci->le_white_list[pos][0] = 0xff;
1262 memset(&hci->le_white_list[pos][1], 0, 6);
1264 status = BT_HCI_ERR_SUCCESS;
1265 cmd_complete(hci, BT_HCI_CMD_LE_REMOVE_FROM_WHITE_LIST,
1266 &status, sizeof(status));
1269 static void cmd_le_encrypt(struct bt_le *hci, const void *data, uint8_t size)
1271 const struct bt_hci_cmd_le_encrypt *cmd = data;
1272 struct bt_hci_rsp_le_encrypt rsp;
1274 if (!bt_crypto_e(hci->crypto, cmd->key, cmd->plaintext, rsp.data)) {
1275 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
1276 BT_HCI_CMD_LE_ENCRYPT);
1280 rsp.status = BT_HCI_ERR_SUCCESS;
1282 cmd_complete(hci, BT_HCI_CMD_LE_ENCRYPT, &rsp, sizeof(rsp));
1285 static void cmd_le_rand(struct bt_le *hci, const void *data, uint8_t size)
1287 struct bt_hci_rsp_le_rand rsp;
1290 if (!bt_crypto_random_bytes(hci->crypto, value, 8)) {
1291 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
1292 BT_HCI_CMD_LE_RAND);
1296 rsp.status = BT_HCI_ERR_SUCCESS;
1297 memcpy(&rsp.number, value, 8);
1299 cmd_complete(hci, BT_HCI_CMD_LE_RAND, &rsp, sizeof(rsp));
1302 static void cmd_le_read_supported_states(struct bt_le *hci,
1303 const void *data, uint8_t size)
1305 struct bt_hci_rsp_le_read_supported_states rsp;
1307 rsp.status = BT_HCI_ERR_SUCCESS;
1308 memcpy(rsp.states, hci->le_states, 8);
1310 cmd_complete(hci, BT_HCI_CMD_LE_READ_SUPPORTED_STATES,
1314 static void cmd_le_set_data_length(struct bt_le *hci,
1315 const void *data, uint8_t size)
1317 const struct bt_hci_cmd_le_set_data_length *cmd = data;
1318 struct bt_hci_rsp_le_set_data_length rsp;
1319 uint16_t handle, tx_len, tx_time;
1321 handle = le16_to_cpu(cmd->handle);
1322 tx_len = le16_to_cpu(cmd->tx_len);
1323 tx_time = le16_to_cpu(cmd->tx_time);
1325 /* Valid range for connection handle is 0x0000 to 0x0eff */
1326 if (handle > 0x0eff) {
1327 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1328 BT_HCI_CMD_LE_SET_DATA_LENGTH);
1332 /* Valid range for suggested max TX octets is 0x001b to 0x00fb */
1333 if (tx_len < 0x001b || tx_len > 0x00fb) {
1334 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1335 BT_HCI_CMD_LE_SET_DATA_LENGTH);
1339 /* Valid range for suggested max TX time is 0x0148 to 0x0848 */
1340 if (tx_time < 0x0148 || tx_time > 0x0848) {
1341 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1342 BT_HCI_CMD_LE_SET_DATA_LENGTH);
1346 /* Max TX len and time shall be less or equal supported */
1347 if (tx_len > MAX_TX_LEN || tx_time > MAX_TX_TIME) {
1348 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1349 BT_HCI_CMD_LE_SET_DATA_LENGTH);
1353 rsp.status = BT_HCI_ERR_SUCCESS;
1354 rsp.handle = cpu_to_le16(handle);
1356 cmd_complete(hci, BT_HCI_CMD_LE_SET_DATA_LENGTH, &rsp, sizeof(rsp));
1359 static void cmd_le_read_default_data_length(struct bt_le *hci,
1360 const void *data, uint8_t size)
1362 struct bt_hci_rsp_le_read_default_data_length rsp;
1364 rsp.status = BT_HCI_ERR_SUCCESS;
1365 rsp.tx_len = cpu_to_le16(hci->le_default_tx_len);
1366 rsp.tx_time = cpu_to_le16(hci->le_default_tx_time);
1368 cmd_complete(hci, BT_HCI_CMD_LE_READ_DEFAULT_DATA_LENGTH,
1372 static void cmd_le_write_default_data_length(struct bt_le *hci,
1373 const void *data, uint8_t size)
1375 const struct bt_hci_cmd_le_write_default_data_length *cmd = data;
1376 uint16_t tx_len, tx_time;
1379 tx_len = le16_to_cpu(cmd->tx_len);
1380 tx_time = le16_to_cpu(cmd->tx_time);
1382 /* Valid range for suggested max TX octets is 0x001b to 0x00fb */
1383 if (tx_len < 0x001b || tx_len > 0x00fb) {
1384 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1385 BT_HCI_CMD_LE_WRITE_DEFAULT_DATA_LENGTH);
1389 /* Valid range for suggested max TX time is 0x0148 to 0x0848 */
1390 if (tx_time < 0x0148 || tx_time > 0x0848) {
1391 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1392 BT_HCI_CMD_LE_WRITE_DEFAULT_DATA_LENGTH);
1396 /* Suggested max TX len and time shall be less or equal supported */
1397 if (tx_len > MAX_TX_LEN || tx_time > MAX_TX_TIME) {
1398 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1399 BT_HCI_CMD_LE_WRITE_DEFAULT_DATA_LENGTH);
1403 hci->le_default_tx_len = tx_len;
1404 hci->le_default_tx_time = tx_time;
1406 status = BT_HCI_ERR_SUCCESS;
1407 cmd_complete(hci, BT_HCI_CMD_LE_WRITE_DEFAULT_DATA_LENGTH,
1408 &status, sizeof(status));
1411 static void cmd_le_read_local_pk256(struct bt_le *hci,
1412 const void *data, uint8_t size)
1414 struct bt_hci_evt_le_read_local_pk256_complete evt;
1416 cmd_status(hci, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_READ_LOCAL_PK256);
1418 evt.status = BT_HCI_ERR_SUCCESS;
1419 ecc_make_key(evt.local_pk256, hci->le_local_sk256);
1421 if (hci->le_event_mask[0] & 0x80)
1422 le_meta_event(hci, BT_HCI_EVT_LE_READ_LOCAL_PK256_COMPLETE,
1426 static void cmd_le_generate_dhkey(struct bt_le *hci,
1427 const void *data, uint8_t size)
1429 const struct bt_hci_cmd_le_generate_dhkey *cmd = data;
1430 struct bt_hci_evt_le_generate_dhkey_complete evt;
1432 cmd_status(hci, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_GENERATE_DHKEY);
1434 evt.status = BT_HCI_ERR_SUCCESS;
1435 ecdh_shared_secret(cmd->remote_pk256, hci->le_local_sk256, evt.dhkey);
1437 if (hci->le_event_mask[1] & 0x01)
1438 le_meta_event(hci, BT_HCI_EVT_LE_GENERATE_DHKEY_COMPLETE,
1442 static void cmd_le_add_to_resolv_list(struct bt_le *hci,
1443 const void *data, uint8_t size)
1445 const struct bt_hci_cmd_le_add_to_resolv_list *cmd = data;
1447 bool exists = false;
1450 /* Valid range for address type is 0x00 to 0x01 */
1451 if (cmd->addr_type > 0x01) {
1452 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1453 BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST);
1457 for (i = 0; i < hci->le_resolv_list_size; i++) {
1458 if (hci->le_resolv_list[i][0] == cmd->addr_type &&
1459 !memcmp(&hci->le_resolv_list[i][1],
1463 } else if (pos < 0 && hci->le_resolv_list[i][0] == 0xff)
1468 cmd_status(hci, BT_HCI_ERR_UNSPECIFIED_ERROR,
1469 BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST);
1474 cmd_status(hci, BT_HCI_ERR_MEM_CAPACITY_EXCEEDED,
1475 BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST);
1479 hci->le_resolv_list[pos][0] = cmd->addr_type;
1480 memcpy(&hci->le_resolv_list[pos][1], cmd->addr, 6);
1481 memcpy(&hci->le_resolv_list[pos][7], cmd->peer_irk, 16);
1482 memcpy(&hci->le_resolv_list[pos][23], cmd->local_irk, 16);
1484 status = BT_HCI_ERR_SUCCESS;
1485 cmd_complete(hci, BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST,
1486 &status, sizeof(status));
1489 static void cmd_le_remove_from_resolv_list(struct bt_le *hci,
1490 const void *data, uint8_t size)
1492 const struct bt_hci_cmd_le_remove_from_resolv_list *cmd = data;
1496 /* Valid range for address type is 0x00 to 0x01 */
1497 if (cmd->addr_type > 0x01) {
1498 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1499 BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST);
1503 for (i = 0; i < hci->le_resolv_list_size; i++) {
1504 if (hci->le_resolv_list[i][0] == cmd->addr_type &&
1505 !memcmp(&hci->le_resolv_list[i][1],
1513 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1514 BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST);
1518 hci->le_resolv_list[pos][0] = 0xff;
1519 memset(&hci->le_resolv_list[pos][1], 0, 38);
1521 status = BT_HCI_ERR_SUCCESS;
1522 cmd_complete(hci, BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST,
1523 &status, sizeof(status));
1526 static void cmd_le_clear_resolv_list(struct bt_le *hci,
1527 const void *data, uint8_t size)
1531 clear_resolv_list(hci);
1533 status = BT_HCI_ERR_SUCCESS;
1534 cmd_complete(hci, BT_HCI_CMD_LE_CLEAR_RESOLV_LIST,
1535 &status, sizeof(status));
1538 static void cmd_le_read_resolv_list_size(struct bt_le *hci,
1539 const void *data, uint8_t size)
1541 struct bt_hci_rsp_le_read_resolv_list_size rsp;
1543 rsp.status = BT_HCI_ERR_SUCCESS;
1544 rsp.size = hci->le_resolv_list_size;
1546 cmd_complete(hci, BT_HCI_CMD_LE_READ_RESOLV_LIST_SIZE,
1550 static void cmd_le_read_peer_resolv_addr(struct bt_le *hci,
1551 const void *data, uint8_t size)
1553 const struct bt_hci_cmd_le_read_peer_resolv_addr *cmd = data;
1554 struct bt_hci_rsp_le_read_peer_resolv_addr rsp;
1556 /* Valid range for address type is 0x00 to 0x01 */
1557 if (cmd->addr_type > 0x01) {
1558 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1559 BT_HCI_CMD_LE_READ_PEER_RESOLV_ADDR);
1563 rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1564 memset(rsp.addr, 0, 6);
1566 cmd_complete(hci, BT_HCI_CMD_LE_READ_PEER_RESOLV_ADDR,
1570 static void cmd_le_read_local_resolv_addr(struct bt_le *hci,
1571 const void *data, uint8_t size)
1573 const struct bt_hci_cmd_le_read_local_resolv_addr *cmd = data;
1574 struct bt_hci_rsp_le_read_local_resolv_addr rsp;
1576 /* Valid range for address type is 0x00 to 0x01 */
1577 if (cmd->addr_type > 0x01) {
1578 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1579 BT_HCI_CMD_LE_READ_LOCAL_RESOLV_ADDR);
1583 rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1584 memset(rsp.addr, 0, 6);
1586 cmd_complete(hci, BT_HCI_CMD_LE_READ_LOCAL_RESOLV_ADDR,
1590 static void cmd_le_set_resolv_enable(struct bt_le *hci,
1591 const void *data, uint8_t size)
1593 const struct bt_hci_cmd_le_set_resolv_enable *cmd = data;
1596 /* Valid range for address resolution enable is 0x00 to 0x01 */
1597 if (cmd->enable > 0x01) {
1598 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1599 BT_HCI_CMD_LE_SET_RESOLV_ENABLE);
1603 hci->le_resolv_enable = cmd->enable;
1605 status = BT_HCI_ERR_SUCCESS;
1606 cmd_complete(hci, BT_HCI_CMD_LE_SET_RESOLV_ENABLE,
1607 &status, sizeof(status));
1610 static void cmd_le_set_resolv_timeout(struct bt_le *hci,
1611 const void *data, uint8_t size)
1613 const struct bt_hci_cmd_le_set_resolv_timeout *cmd = data;
1617 timeout = le16_to_cpu(cmd->timeout);
1619 /* Valid range for RPA timeout is 0x0001 to 0xa1b8 */
1620 if (timeout < 0x0001 || timeout > 0xa1b8) {
1621 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1622 BT_HCI_CMD_LE_SET_RESOLV_TIMEOUT);
1626 hci->le_resolv_timeout = timeout;
1628 status = BT_HCI_ERR_SUCCESS;
1629 cmd_complete(hci, BT_HCI_CMD_LE_SET_RESOLV_TIMEOUT,
1630 &status, sizeof(status));
1633 static void cmd_le_read_max_data_length(struct bt_le *hci,
1634 const void *data, uint8_t size)
1636 struct bt_hci_rsp_le_read_max_data_length rsp;
1638 rsp.status = BT_HCI_ERR_SUCCESS;
1639 rsp.max_tx_len = cpu_to_le16(MAX_TX_LEN);
1640 rsp.max_tx_time = cpu_to_le16(MAX_TX_TIME);
1641 rsp.max_rx_len = cpu_to_le16(MAX_RX_LEN);
1642 rsp.max_rx_time = cpu_to_le16(MAX_RX_TIME);
1644 cmd_complete(hci, BT_HCI_CMD_LE_READ_MAX_DATA_LENGTH,
1648 static const struct {
1650 void (*func) (struct bt_le *hci, const void *data, uint8_t size);
1654 { BT_HCI_CMD_DISCONNECT, cmd_disconnect, 3, true },
1656 { BT_HCI_CMD_SET_EVENT_MASK, cmd_set_event_mask, 8, true },
1657 { BT_HCI_CMD_RESET, cmd_reset, 0, true },
1658 { BT_HCI_CMD_SET_EVENT_MASK_PAGE2, cmd_set_event_mask_page2, 8, true },
1660 { BT_HCI_CMD_READ_LOCAL_VERSION, cmd_read_local_version, 0, true },
1661 { BT_HCI_CMD_READ_LOCAL_COMMANDS, cmd_read_local_commands, 0, true },
1662 { BT_HCI_CMD_READ_LOCAL_FEATURES, cmd_read_local_features, 0, true },
1663 { BT_HCI_CMD_READ_BUFFER_SIZE, cmd_read_buffer_size, 0, true },
1664 { BT_HCI_CMD_READ_BD_ADDR, cmd_read_bd_addr, 0, true },
1666 { BT_HCI_CMD_LE_SET_EVENT_MASK,
1667 cmd_le_set_event_mask, 8, true },
1668 { BT_HCI_CMD_LE_READ_BUFFER_SIZE,
1669 cmd_le_read_buffer_size, 0, true },
1670 { BT_HCI_CMD_LE_READ_LOCAL_FEATURES,
1671 cmd_le_read_local_features, 0, true },
1672 { BT_HCI_CMD_LE_SET_RANDOM_ADDRESS,
1673 cmd_le_set_random_address, 6, true },
1674 { BT_HCI_CMD_LE_SET_ADV_PARAMETERS,
1675 cmd_le_set_adv_parameters, 15, true },
1676 { BT_HCI_CMD_LE_READ_ADV_TX_POWER,
1677 cmd_le_read_adv_tx_power, 0, true },
1678 { BT_HCI_CMD_LE_SET_ADV_DATA,
1679 cmd_le_set_adv_data, 32, true },
1680 { BT_HCI_CMD_LE_SET_SCAN_RSP_DATA,
1681 cmd_le_set_scan_rsp_data, 32, true },
1682 { BT_HCI_CMD_LE_SET_ADV_ENABLE,
1683 cmd_le_set_adv_enable, 1, true },
1684 { BT_HCI_CMD_LE_SET_SCAN_PARAMETERS,
1685 cmd_le_set_scan_parameters, 7, true },
1686 { BT_HCI_CMD_LE_SET_SCAN_ENABLE,
1687 cmd_le_set_scan_enable, 2, true },
1688 { BT_HCI_CMD_LE_CREATE_CONN,
1689 cmd_le_create_conn, 25, true },
1690 { BT_HCI_CMD_LE_CREATE_CONN_CANCEL,
1691 cmd_le_create_conn_cancel, 0, true },
1692 { BT_HCI_CMD_LE_READ_WHITE_LIST_SIZE,
1693 cmd_le_read_white_list_size, 0, true },
1694 { BT_HCI_CMD_LE_CLEAR_WHITE_LIST,
1695 cmd_le_clear_white_list, 0, true },
1696 { BT_HCI_CMD_LE_ADD_TO_WHITE_LIST,
1697 cmd_le_add_to_white_list, 7, true },
1698 { BT_HCI_CMD_LE_REMOVE_FROM_WHITE_LIST,
1699 cmd_le_remove_from_white_list, 7, true },
1701 { BT_HCI_CMD_LE_ENCRYPT, cmd_le_encrypt, 32, true },
1702 { BT_HCI_CMD_LE_RAND, cmd_le_rand, 0, true },
1704 { BT_HCI_CMD_LE_READ_SUPPORTED_STATES,
1705 cmd_le_read_supported_states, 0, true },
1707 { BT_HCI_CMD_LE_SET_DATA_LENGTH,
1708 cmd_le_set_data_length, 6, true },
1709 { BT_HCI_CMD_LE_READ_DEFAULT_DATA_LENGTH,
1710 cmd_le_read_default_data_length, 0, true },
1711 { BT_HCI_CMD_LE_WRITE_DEFAULT_DATA_LENGTH,
1712 cmd_le_write_default_data_length, 4, true },
1713 { BT_HCI_CMD_LE_READ_LOCAL_PK256,
1714 cmd_le_read_local_pk256, 0, true },
1715 { BT_HCI_CMD_LE_GENERATE_DHKEY,
1716 cmd_le_generate_dhkey, 64, true },
1717 { BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST,
1718 cmd_le_add_to_resolv_list, 39, true },
1719 { BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST,
1720 cmd_le_remove_from_resolv_list, 7, true },
1721 { BT_HCI_CMD_LE_CLEAR_RESOLV_LIST,
1722 cmd_le_clear_resolv_list, 0, true },
1723 { BT_HCI_CMD_LE_READ_RESOLV_LIST_SIZE,
1724 cmd_le_read_resolv_list_size, 0, true },
1725 { BT_HCI_CMD_LE_READ_PEER_RESOLV_ADDR,
1726 cmd_le_read_peer_resolv_addr, 7, true },
1727 { BT_HCI_CMD_LE_READ_LOCAL_RESOLV_ADDR,
1728 cmd_le_read_local_resolv_addr, 7, true },
1729 { BT_HCI_CMD_LE_SET_RESOLV_ENABLE,
1730 cmd_le_set_resolv_enable, 1, true },
1731 { BT_HCI_CMD_LE_SET_RESOLV_TIMEOUT,
1732 cmd_le_set_resolv_timeout, 2, true },
1733 { BT_HCI_CMD_LE_READ_MAX_DATA_LENGTH,
1734 cmd_le_read_max_data_length, 0, true },
1739 static void process_command(struct bt_le *hci, const void *data, size_t size)
1741 const struct bt_hci_cmd_hdr *hdr = data;
1745 if (size < sizeof(*hdr))
1748 data += sizeof(*hdr);
1749 size -= sizeof(*hdr);
1751 opcode = le16_to_cpu(hdr->opcode);
1753 if (hdr->plen != size) {
1754 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS, opcode);
1758 for (i = 0; cmd_table[i].func; i++) {
1759 if (cmd_table[i].opcode != opcode)
1762 if ((cmd_table[i].fixed && size != cmd_table[i].size) ||
1763 size < cmd_table[i].size) {
1764 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS, opcode);
1768 cmd_table[i].func(hci, data, size);
1772 cmd_status(hci, BT_HCI_ERR_UNKNOWN_COMMAND, opcode);
1775 static void vhci_read_callback(int fd, uint32_t events, void *user_data)
1777 struct bt_le *hci = user_data;
1778 unsigned char buf[4096];
1781 if (events & (EPOLLERR | EPOLLHUP))
1784 len = read(hci->vhci_fd, buf, sizeof(buf));
1790 process_command(hci, buf + 1, len - 1);
1795 static void phy_recv_callback(uint16_t type, const void *data,
1796 size_t size, void *user_data)
1798 struct bt_le *hci = user_data;
1801 case BT_PHY_PKT_ADV:
1802 if (!(hci->le_event_mask[0] & 0x02))
1805 if (hci->scan_window_active) {
1806 const struct bt_phy_pkt_adv *pkt = data;
1808 struct bt_hci_evt_le_adv_report *evt = (void *) buf;
1809 uint8_t tx_addr_type, tx_addr[6];
1811 if (hci->scan_chan_idx != pkt->chan_idx)
1814 resolve_peer_addr(hci, pkt->tx_addr_type, pkt->tx_addr,
1815 &tx_addr_type, tx_addr);
1817 if (hci->le_scan_filter_policy == 0x01 ||
1818 hci->le_scan_filter_policy == 0x03) {
1819 if (!is_in_white_list(hci, tx_addr_type,
1824 if (hci->le_scan_filter_dup) {
1825 if (!add_to_scan_cache(hci, tx_addr_type,
1830 memset(buf, 0, sizeof(buf));
1831 evt->num_reports = 0x01;
1832 evt->event_type = pkt->pdu_type;
1833 evt->addr_type = tx_addr_type;
1834 memcpy(evt->addr, tx_addr, 6);
1835 evt->data_len = pkt->adv_data_len;
1836 memcpy(buf + sizeof(*evt), data + sizeof(*pkt),
1839 le_meta_event(hci, BT_HCI_EVT_LE_ADV_REPORT, buf,
1840 sizeof(*evt) + pkt->adv_data_len + 1);
1842 if (hci->le_scan_type == 0x00)
1845 memset(buf, 0, sizeof(buf));
1846 evt->num_reports = 0x01;
1847 evt->event_type = 0x04;
1848 evt->addr_type = tx_addr_type;
1849 memcpy(evt->addr, tx_addr, 6);
1850 evt->data_len = pkt->scan_rsp_len;
1851 memcpy(buf + sizeof(*evt), data + sizeof(*pkt) +
1855 le_meta_event(hci, BT_HCI_EVT_LE_ADV_REPORT, buf,
1856 sizeof(*evt) + pkt->scan_rsp_len + 1);
1862 struct bt_le *bt_le_new(void)
1864 unsigned char setup_cmd[2];
1867 hci = calloc(1, sizeof(*hci));
1871 hci->adv_timeout_id = -1;
1872 hci->scan_timeout_id = -1;
1873 hci->scan_window_active = false;
1875 reset_defaults(hci);
1877 hci->vhci_fd = open("/dev/vhci", O_RDWR);
1878 if (hci->vhci_fd < 0) {
1883 setup_cmd[0] = HCI_VENDOR_PKT;
1884 setup_cmd[1] = HCI_BREDR;
1886 if (write(hci->vhci_fd, setup_cmd, sizeof(setup_cmd)) < 0) {
1887 close(hci->vhci_fd);
1892 mainloop_add_fd(hci->vhci_fd, EPOLLIN, vhci_read_callback, hci, NULL);
1894 hci->phy = bt_phy_new();
1895 hci->crypto = bt_crypto_new();
1897 bt_phy_register(hci->phy, phy_recv_callback, hci);
1899 return bt_le_ref(hci);
1902 struct bt_le *bt_le_ref(struct bt_le *hci)
1907 __sync_fetch_and_add(&hci->ref_count, 1);
1912 void bt_le_unref(struct bt_le *hci)
1917 if (__sync_sub_and_fetch(&hci->ref_count, 1))
1922 bt_crypto_unref(hci->crypto);
1923 bt_phy_unref(hci->phy);
1925 mainloop_remove_fd(hci->vhci_fd);
1927 close(hci->vhci_fd);