GCC Code Coverage Report

Directory: main/
File: bluetooth/host.c
Date: 2025-10-04 14:03:00
Exec Total Coverage
Lines: 48 355 13.5%
Functions: 6 33 18.2%
Branches: 20 201 10.0%
Line Branch Exec Source
1 /*
2 * Copyright (c) 2019-2025, Jacques Gagnon
3 * SPDX-License-Identifier: Apache-2.0
4 */
5
6 #include <stdio.h>
7 #include <sys/stat.h>
8 #include <freertos/FreeRTOS.h>
9 #include <freertos/task.h>
10 #include <freertos/ringbuf.h>
11 #include <esp_system.h>
12 #include <esp_bt.h>
13 #include <esp_mac.h>
14 #include <esp_timer.h>
15 #include <driver/gpio.h>
16 #include "nvs.h"
17 #include "queue_bss.h"
18 #include "host.h"
19 #include "hci.h"
20 #include "l2cap.h"
21 #include "sdp.h"
22 #include "att_cfg.h"
23 #include "att_hid.h"
24 #include "smp.h"
25 #include "tools/util.h"
26 #include "debug.h"
27 #include "mon.h"
28 #include "system/fs.h"
29 #include "adapter/config.h"
30 #include "adapter/gameid.h"
31 #include "adapter/memory_card.h"
32 #include "adapter/wired/wired.h"
33 #include "adapter/hid_parser.h"
34 #include "bluetooth/hidp/ps.h"
35
36 #define BT_TX 0
37 #define BT_RX 1
38 #define BT_FB_TASK_DELAY_CNT 30
39
40 enum {
41 /* BT CTRL flags */
42 BT_CTRL_READY = 0,
43 BT_HOST_DBG_MODE,
44 };
45
46 struct bt_host_link_keys {
47 uint32_t index;
48 struct bt_hci_evt_link_key_notify link_keys[16];
49 } __packed;
50
51 struct bt_host_le_link_keys {
52 uint32_t index;
53 struct {
54 bt_addr_le_t le_bdaddr;
55 struct bt_smp_encrypt_info ltk;
56 struct bt_smp_master_ident ident;
57 } keys[16];
58 } __packed;
59
60 struct bt_hci_pkt bt_hci_pkt_tmp;
61
62 static struct bt_host_link_keys bt_host_link_keys = {0};
63 static struct bt_host_le_link_keys bt_host_le_link_keys = {0};
64 static RingbufHandle_t txq_hdl;
65 static struct bt_dev bt_dev_conf = {0};
66 static struct bt_dev bt_dev[BT_MAX_DEV] = {0};
67 static atomic_t bt_flags = 0;
68 static uint32_t frag_size = 0;
69 static uint32_t frag_offset = 0;
70 static uint8_t frag_buf[1024];
71
72 static int32_t bt_host_load_bdaddr_from_nvs(void);
73 static int32_t bt_host_load_keys_from_file(struct bt_host_link_keys *data);
74 static int32_t bt_host_store_keys_on_file(struct bt_host_link_keys *data);
75 static int32_t bt_host_load_le_keys_from_file(struct bt_host_le_link_keys *data);
76 static int32_t bt_host_store_le_keys_on_file(struct bt_host_le_link_keys *data);
77 static void bt_host_acl_hdlr(struct bt_hci_pkt *bt_hci_acl_pkt, uint32_t len);
78 static void bt_host_tx_pkt_ready(void);
79 static int bt_host_rx_pkt(uint8_t *data, uint16_t len);
80 static void bt_host_task(void *param);
81
82 static esp_vhci_host_callback_t vhci_host_cb = {
83 bt_host_tx_pkt_ready,
84 bt_host_rx_pkt
85 };
86
87 static int32_t bt_host_load_bdaddr_from_nvs(void) {
88 ✗ int32_t ret = -1;
89
90 // uint8_t test_mac[6] = {0x48, 0xf1, 0xeb, 0xed, 0xf4, 0x1d};
91 // test_mac[5] -= 2; /* Set base mac to BDADDR-2 so that BDADDR end up what we want */
92 // esp_base_mac_addr_set(test_mac);
93 // printf("# %s: Using NVS MAC\n", __FUNCTION__);
94 // ret = 0;
95 return ret;
96 }
97
98 ✗ static int32_t bt_host_load_keys_from_file(struct bt_host_link_keys *data) {
99 ✗ struct stat st;
100 ✗ int32_t ret = -1;
101
102 ✗ if (stat(LINK_KEYS_FILE, &st) != 0) {
103 ✗ printf("# %s: No link keys on SD. Creating...\n", __FUNCTION__);
104 ✗ ret = bt_host_store_keys_on_file(data);
105 }
106 else {
107 ✗ FILE *file = fopen(LINK_KEYS_FILE, "rb");
108 ✗ if (file == NULL) {
109 ✗ printf("# %s: failed to open file for reading\n", __FUNCTION__);
110 }
111 else {
112 ✗ uint32_t count = fread((void *)data, sizeof(*data), 1, file);
113 ✗ fclose(file);
114 ✗ if (count == 1) {
115 ✗ ret = 0;
116 }
117 }
118 }
119 ✗ return ret;
120 }
121
122 ✗ static int32_t bt_host_store_keys_on_file(struct bt_host_link_keys *data) {
123 ✗ int32_t ret = -1;
124
125 ✗ FILE *file = fopen(LINK_KEYS_FILE, "wb");
126 ✗ if (file == NULL) {
127 ✗ printf("# %s: failed to open file for writing\n", __FUNCTION__);
128 }
129 else {
130 ✗ fwrite((void *)data, sizeof(*data), 1, file);
131 ✗ fclose(file);
132 ✗ ret = 0;
133 }
134 ✗ return ret;
135 }
136
137 ✗ static int32_t bt_host_load_le_keys_from_file(struct bt_host_le_link_keys *data) {
138 ✗ struct stat st;
139 ✗ int32_t ret = -1;
140
141 ✗ if (stat(LE_LINK_KEYS_FILE, &st) != 0) {
142 ✗ printf("# %s: No link keys on SD. Creating...\n", __FUNCTION__);
143 ✗ ret = bt_host_store_le_keys_on_file(data);
144 }
145 else {
146 ✗ FILE *file = fopen(LE_LINK_KEYS_FILE, "rb");
147 ✗ if (file == NULL) {
148 ✗ printf("# %s: failed to open file for reading\n", __FUNCTION__);
149 }
150 else {
151 ✗ uint32_t count = fread((void *)data, sizeof(*data), 1, file);
152 ✗ fclose(file);
153 ✗ if (count == 1) {
154 ✗ ret = 0;
155 }
156 }
157 }
158 ✗ return ret;
159 }
160
161 ✗ static int32_t bt_host_store_le_keys_on_file(struct bt_host_le_link_keys *data) {
162 ✗ int32_t ret = -1;
163
164 ✗ FILE *file = fopen(LE_LINK_KEYS_FILE, "wb");
165 ✗ if (file == NULL) {
166 ✗ printf("# %s: failed to open file for writing\n", __FUNCTION__);
167 }
168 else {
169 ✗ fwrite((void *)data, sizeof(*data), 1, file);
170 ✗ fclose(file);
171 ✗ ret = 0;
172 }
173 ✗ return ret;
174 }
175
176 ✗ static void bt_tx_task(void *param) {
177 ✗ size_t packet_len;
178 ✗ uint8_t *packet;
179
180 ✗ while(1) {
181 /* TX packet from Q */
182 ✗ if (atomic_test_bit(&bt_flags, BT_CTRL_READY)) {
183 ✗ packet = (uint8_t *)xRingbufferReceive(txq_hdl, &packet_len, portMAX_DELAY);
184 ✗ if (packet) {
185 ✗ if (packet[0] == 0xFF) {
186 /* Internal wait packet */
187 ✗ vTaskDelay(packet[1] / portTICK_PERIOD_MS);
188 }
189 else {
190 ✗ bt_mon_tx((packet[0] == BT_HCI_H4_TYPE_CMD) ? BT_MON_CMD : BT_MON_ACL_TX,
191 ✗ packet + 1, packet_len - 1);
192 ✗ atomic_clear_bit(&bt_flags, BT_CTRL_READY);
193 ✗ esp_vhci_host_send_packet(packet, packet_len);
194 }
195 ✗ vRingbufferReturnItem(txq_hdl, (void *)packet);
196 }
197 }
198 else {
199 ✗ vTaskDelay(10 / portTICK_PERIOD_MS);
200 }
201 }
202 }
203
204 ✗ static void bt_fb_task(void *param) {
205 ✗ uint32_t *fb_len;
206 ✗ struct raw_fb *fb_data = NULL;
207 ✗ uint32_t delay_cnt = BT_FB_TASK_DELAY_CNT; /* 100ms * 30 = 3sec */
208
209 ✗ while(1) {
210 ✗ bool fb_changed = false;
211 /* Look for rumble/led feedback data */
212 ✗ while ((fb_data = (struct raw_fb *)queue_bss_dequeue(wired_adapter.input_q_hdl, &fb_len))) {
213 ✗ struct bt_dev *device = NULL;
214 ✗ struct bt_data *bt_data = NULL;
215
216 ✗ bt_host_get_active_dev_from_out_idx(fb_data->header.wired_id, &device);
217 ✗ if (device) {
218 ✗ bt_data = &bt_adapter.data[device->ids.id];
219 }
220
221 ✗ switch (fb_data->header.type) {
222 ✗ case FB_TYPE_MEM_WRITE:
223 ✗ mc_storage_update();
224 ✗ break;
225 ✗ case FB_TYPE_STATUS_LED:
226 case FB_TYPE_PLAYER_LED:
227 ✗ if (device && device->ids.subtype == BT_PS5_DS) {
228 /* We need to clear LEDs before setting them */
229 ✗ struct bt_hidp_ps5_set_conf ps5_clear_led = {
230 .conf0 = 0x02,
231 .conf1 = 0x08,
232 };
233 ✗ bt_hid_cmd_ps_set_conf(device, (void *)&ps5_clear_led);
234 }
235 /* Fallthrough */
236 case FB_TYPE_RUMBLE:
237 ✗ if (bt_data) {
238 ✗ fb_changed = adapter_bridge_fb(fb_data, bt_data);
239 ✗ delay_cnt = 0;
240 }
241 break;
242 ✗ case FB_TYPE_GAME_ID:
243 ✗ if (gid_update(fb_data)) {
244 ✗ config_init(GAMEID_CFG);
245 }
246 break;
247 ✗ case FB_TYPE_SYS_ID:
248 ✗ if (gid_update_sys(fb_data)) {
249 ✗ config_init(GAMEID_CFG);
250 }
251 break;
252 default:
253 break;
254 }
255 ✗ queue_bss_return(wired_adapter.input_q_hdl, (uint8_t *)fb_data, fb_len);
256 }
257
258 /* TX Feedback every 10 ms if rumble on, every 3 sec otherwise */
259 ✗ if (delay_cnt-- == 0 || fb_changed) {
260 ✗ for (uint32_t i = 0; i < BT_MAX_DEV; i++) {
261 ✗ struct bt_dev *device = &bt_dev[i];
262
263 ✗ if (atomic_test_bit(&device->flags, BT_DEV_HID_INIT_DONE)) {
264 ✗ struct bt_data *bt_data = &bt_adapter.data[device->ids.id];
265
266 ✗ bt_hid_feedback(device, bt_data->base.output);
267 }
268 }
269 delay_cnt = BT_FB_TASK_DELAY_CNT;
270 }
271 ✗ vTaskDelay(10 / portTICK_PERIOD_MS);
272 }
273 }
274
275 ✗ static void bt_host_task(void *param) {
276 ✗ while (1) {
277 /* Per device housekeeping */
278 ✗ for (uint32_t i = 0; i < BT_MAX_DEV; i++) {
279 ✗ struct bt_dev *device = &bt_dev[i];
280 ✗ struct bt_data *bt_data = &bt_adapter.data[i];
281
282 /* Parse SDP data if available */
283 ✗ if (atomic_test_bit(&device->flags, BT_DEV_DEVICE_FOUND)) {
284 ✗ if (atomic_test_bit(&device->flags, BT_DEV_SDP_DATA)) {
285 ✗ int32_t old_type = device->ids.type;
286
287 ✗ bt_sdp_parser(bt_data);
288 ✗ if (old_type != device->ids.type) {
289 ✗ if (atomic_test_bit(&device->flags, BT_DEV_HID_INTR_READY)) {
290 ✗ bt_hid_init(device);
291 }
292 }
293 ✗ atomic_clear_bit(&device->flags, BT_DEV_SDP_DATA);
294 }
295 }
296 }
297
298 /* Update turbo mask for parallel system */
299 ✗ wired_para_turbo_mask_hdlr();
300
301 #ifdef CONFIG_BLUERETRO_ADAPTER_RUMBLE_DBG
302 adapter_toggle_fb(0, 150000,
303 wired_adapter.data[0].output[16], wired_adapter.data[0].output[17]);
304 #endif
305
306 ✗ vTaskDelay(20 / portTICK_PERIOD_MS);
307 }
308 }
309
310 ✗ static void bt_host_acl_hdlr(struct bt_hci_pkt *bt_hci_acl_pkt, uint32_t len) {
311 ✗ struct bt_dev *device = NULL;
312 ✗ struct bt_hci_pkt *pkt = bt_hci_acl_pkt;
313 ✗ uint32_t pkt_len = len;
314 ✗ bt_host_get_dev_from_handle(pkt->acl_hdr.handle, &device);
315
316 ✗ if (bt_acl_flags(pkt->acl_hdr.handle) == BT_ACL_CONT) {
317 ✗ memcpy(frag_buf + frag_offset, (void *)pkt + BT_HCI_H4_HDR_SIZE + BT_HCI_ACL_HDR_SIZE,
318 ✗ pkt->acl_hdr.len);
319 ✗ frag_offset += pkt->acl_hdr.len;
320 ✗ if (frag_offset < frag_size) {
321 //printf("# %s Waiting for next fragment. offset: %ld size %ld\n", __FUNCTION__, frag_offset, frag_size);
322 ✗ return;
323 }
324 pkt = (struct bt_hci_pkt *)frag_buf;
325 pkt_len = frag_size;
326 //printf("# %s process reassembled frame. offset: %ld size %ld\n", __FUNCTION__, frag_offset, frag_size);
327 }
328 ✗ if (bt_acl_flags(pkt->acl_hdr.handle) == BT_ACL_START
329 ✗ && (pkt_len - (BT_HCI_H4_HDR_SIZE + BT_HCI_ACL_HDR_SIZE + sizeof(struct bt_l2cap_hdr))) < pkt->l2cap_hdr.len) {
330 ✗ memcpy(frag_buf, (void *)pkt, pkt_len);
331 ✗ frag_offset = pkt_len;
332 ✗ frag_size = pkt->l2cap_hdr.len + BT_HCI_H4_HDR_SIZE + BT_HCI_ACL_HDR_SIZE + sizeof(struct bt_l2cap_hdr);
333 //printf("# %s Detected fragmented frame start\n", __FUNCTION__);
334 ✗ return;
335 }
336
337 ✗ if (device == NULL) {
338 ✗ if (pkt->l2cap_hdr.cid == BT_L2CAP_CID_ATT) {
339 ✗ bt_att_cfg_hdlr(&bt_dev_conf, pkt, pkt_len);
340 }
341 else {
342 ✗ printf("# %s dev NULL!\n", __FUNCTION__);
343 }
344 ✗ return;
345 }
346
347 ✗ if (pkt->l2cap_hdr.cid == BT_L2CAP_CID_BR_SIG) {
348 ✗ bt_l2cap_sig_hdlr(device, pkt);
349 }
350 ✗ else if (pkt->l2cap_hdr.cid == BT_L2CAP_CID_ATT) {
351 ✗ bt_att_hid_hdlr(device, pkt, pkt_len);
352 }
353 ✗ else if (pkt->l2cap_hdr.cid == BT_L2CAP_CID_LE_SIG) {
354 ✗ bt_l2cap_le_sig_hdlr(device, pkt, pkt_len);
355 }
356 ✗ else if (pkt->l2cap_hdr.cid == BT_L2CAP_CID_SMP) {
357 ✗ bt_smp_hdlr(device, pkt, pkt_len);
358 }
359 ✗ else if (pkt->l2cap_hdr.cid == device->sdp_tx_chan.scid ||
360 ✗ pkt->l2cap_hdr.cid == device->sdp_rx_chan.scid) {
361 ✗ bt_sdp_hdlr(device, pkt);
362 }
363 ✗ else if (pkt->l2cap_hdr.cid == device->ctrl_chan.scid ||
364 ✗ pkt->l2cap_hdr.cid == device->intr_chan.scid) {
365 ✗ bt_hid_hdlr(device, pkt, pkt_len);
366 }
367 }
368
369 /*
370 * @brief: BT controller callback function, used to notify the upper layer that
371 * controller is ready to receive command
372 */
373 ✗ static void bt_host_tx_pkt_ready(void) {
374 ✗ atomic_set_bit(&bt_flags, BT_CTRL_READY);
375 ✗ }
376
377 /*
378 * @brief: BT controller callback function, to transfer data packet to upper
379 * controller is ready to receive command
380 */
381 ✗ static int bt_host_rx_pkt(uint8_t *data, uint16_t len) {
382 ✗ struct bt_hci_pkt *bt_hci_pkt = (struct bt_hci_pkt *)data;
383 ✗ bt_mon_tx((bt_hci_pkt->h4_hdr.type == BT_HCI_H4_TYPE_EVT) ? BT_MON_EVT : BT_MON_ACL_RX,
384 ✗ data + 1, len - 1);
385
386 #ifdef CONFIG_BLUERETRO_BT_TIMING_TESTS
387 if (atomic_test_bit(&bt_flags, BT_HOST_DBG_MODE)) {
388 bt_dbg(data, len);
389 }
390 else {
391 #endif
392 ✗ switch(bt_hci_pkt->h4_hdr.type) {
393 ✗ case BT_HCI_H4_TYPE_ACL:
394 ✗ bt_host_acl_hdlr(bt_hci_pkt, len);
395 ✗ break;
396 ✗ case BT_HCI_H4_TYPE_EVT:
397 ✗ bt_hci_evt_hdlr(bt_hci_pkt);
398 ✗ break;
399 ✗ default:
400 ✗ printf("# %s unsupported packet type: 0x%02X\n", __FUNCTION__, bt_hci_pkt->h4_hdr.type);
401 ✗ break;
402 }
403 #ifdef CONFIG_BLUERETRO_BT_TIMING_TESTS
404 }
405 #endif
406
407 ✗ return 0;
408 }
409
410 ✗ uint32_t bt_host_get_flag_dev_cnt(uint32_t flag) {
411 ✗ uint32_t cnt = 0;
412 ✗ for (uint32_t i = 0; i < BT_MAX_DEV; i++) {
413 ✗ if (atomic_test_bit(&bt_dev[i].flags, flag)) {
414 ✗ cnt++;
415 }
416 }
417 ✗ return cnt;
418 }
419
420 ✗ void bt_host_disconnect_all(void) {
421 ✗ printf("# %s BOOT SW pressed, DISCONN all devices!\n", __FUNCTION__);
422 ✗ for (uint32_t i = 0; i < BT_MAX_DEV; i++) {
423 ✗ if (atomic_test_bit(&bt_dev[i].flags, BT_DEV_DEVICE_FOUND)) {
424 ✗ bt_hci_disconnect(&bt_dev[i]);
425 }
426 }
427 ✗ }
428
429 92 int32_t bt_host_get_new_dev(struct bt_dev **device) {
430
1/2
✓ Branch 0 (7→3) taken 92 times.
✗ Branch 1 (7→8) not taken.
 92 for (uint32_t i = 0; i < BT_MAX_DEV; i++) {
431
1/2
✓ Branch 0 (4→5) taken 92 times.
✗ Branch 1 (4→6) not taken.
 92 if (!atomic_test_bit(&bt_dev[i].flags, BT_DEV_DEVICE_FOUND)) {
432 92 *device = &bt_dev[i];
433 92 return i;
434 }
435 }
436 return -1;
437 }
438
439 ✗ int32_t bt_host_get_active_dev(struct bt_dev **device) {
440 ✗ for (uint32_t i = 0; i < BT_MAX_DEV; i++) {
441 ✗ if (atomic_test_bit(&bt_dev[i].flags, BT_DEV_DEVICE_FOUND)) {
442 ✗ *device = &bt_dev[i];
443 ✗ return i;
444 }
445 }
446 return -1;
447 }
448
449 ✗ int32_t bt_host_get_hid_init_dev(struct bt_dev **device) {
450 ✗ for (uint32_t i = 0; i < BT_MAX_DEV; i++) {
451 ✗ if (atomic_test_bit(&bt_dev[i].flags, BT_DEV_HID_INIT_DONE)) {
452 ✗ *device = &bt_dev[i];
453 ✗ return i;
454 }
455 }
456 return -1;
457 }
458
459 ✗ int32_t bt_host_get_dev_from_bdaddr(uint8_t *bdaddr, struct bt_dev **device) {
460 ✗ for (uint32_t i = 0; i < BT_MAX_DEV; i++) {
461 ✗ if (atomic_test_bit(&bt_dev[i].flags, BT_DEV_DEVICE_FOUND) && memcmp((void *)bdaddr, bt_dev[i].remote_bdaddr, 6) == 0) {
462 ✗ *device = &bt_dev[i];
463 ✗ return i;
464 }
465 }
466 return -1;
467 }
468
469 ✗ int32_t bt_host_get_dev_from_handle(uint16_t handle, struct bt_dev **device) {
470 ✗ for (uint32_t i = 0; i < BT_MAX_DEV; i++) {
471 ✗ if (atomic_test_bit(&bt_dev[i].flags, BT_DEV_DEVICE_FOUND) && bt_acl_handle(handle) == bt_dev[i].acl_handle) {
472 ✗ *device = &bt_dev[i];
473 ✗ return i;
474 }
475 }
476 return -1;
477 }
478
479 42 int32_t bt_host_get_dev_from_id(uint8_t id, struct bt_dev **device) {
480
1/2
✓ Branch 0 (2→3) taken 42 times.
✗ Branch 1 (2→4) not taken.
 42 if (id < BT_MAX_DEV) {
481 42 *device = &bt_dev[id];
482 42 return id;
483 }
484 return -1;
485 }
486
487 ✗ int32_t bt_host_get_dev_from_out_idx(uint8_t out_idx, struct bt_dev **device) {
488 ✗ for (uint32_t i = 0; i < BT_MAX_DEV; i++) {
489 ✗ if (bt_dev[i].ids.out_idx == out_idx) {
490 ✗ *device = &bt_dev[i];
491 ✗ return i;
492 }
493 }
494 return -1;
495 }
496
497 82 int32_t bt_host_get_active_dev_from_out_idx(uint8_t out_idx, struct bt_dev **device) {
498
2/2
✓ Branch 0 (8→3) taken 244 times.
✓ Branch 1 (8→9) taken 27 times.
 271 for (uint32_t i = 0; i < BT_MAX_DEV; i++) {
499
3/4
✓ Branch 0 (4→5) taken 55 times.
✓ Branch 1 (4→7) taken 189 times.
✓ Branch 2 (5→6) taken 55 times.
✗ Branch 3 (5→7) not taken.
 244 if (atomic_test_bit(&bt_dev[i].flags, BT_DEV_HID_INIT_DONE) && bt_dev[i].ids.out_idx == out_idx) {
500 55 *device = &bt_dev[i];
501 55 return i;
502 }
503 }
504 return -1;
505 }
506
507 ✗ int32_t bt_host_get_dev_conf(struct bt_dev **device) {
508 ✗ *device = &bt_dev_conf;
509 ✗ return 0;
510 }
511
512 184 void bt_host_reset_dev(struct bt_dev *device) {
513 184 int32_t dev_id = 0;
514
515
1/2
✓ Branch 0 (11→3) taken 184 times.
✗ Branch 1 (11→12) not taken.
 184 for (; dev_id < BT_MAX_DEV; dev_id++) {
516
1/2
✓ Branch 0 (3→4) taken 184 times.
✗ Branch 1 (3→10) not taken.
 184 if (device == &bt_dev[dev_id]) {
517 184 goto reset_dev;
518 }
519 }
520 return;
521
522 184 reset_dev:
523 184 adapter_init_buffer(dev_id);
524 184 memset(bt_adapter.data[dev_id].raw_src_mappings, 0, sizeof(*bt_adapter.data[0].raw_src_mappings) * REPORT_MAX);
525 184 memset(bt_adapter.data[dev_id].reports, 0, sizeof(bt_adapter.data[0].reports));
526 184 memset(&bt_adapter.data[dev_id].base, 0, sizeof(bt_adapter.data[0].base));
527 184 memset(device, 0, sizeof(*device));
528
529 184 device->ids.id = dev_id;
530 184 device->ids.out_idx = dev_id;
531 184 device->ids.type = BT_NONE;
532 184 device->ids.subtype = BT_SUBTYPE_DEFAULT;
533 184 bt_adapter.data[dev_id].base.pids = &device->ids;
534 }
535
536 ✗ void bt_host_q_wait_pkt(uint32_t ms) {
537 ✗ uint8_t packet[2] = {0xFF, ms};
538
539 ✗ bt_host_txq_add(packet, sizeof(packet));
540 ✗ }
541
542 ✗ int32_t bt_host_init(void) {
543 ✗ int32_t ret;
544
545 #ifdef CONFIG_BLUERETRO_BT_TIMING_TESTS
546 gpio_config_t io_conf = {0};
547 io_conf.intr_type = GPIO_INTR_DISABLE;
548 io_conf.mode = GPIO_MODE_OUTPUT;
549 io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
550 io_conf.pull_up_en = GPIO_PULLUP_ENABLE;
551 io_conf.pin_bit_mask = 1ULL << 26;
552 gpio_config(&io_conf);
553 gpio_set_level(26, 1);
554 #endif
555
556 ✗ bt_host_load_bdaddr_from_nvs();
557
558 ✗ bt_mon_init();
559
560 ✗ esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT();
561
562 ✗ if ((ret = esp_bt_controller_init(&bt_cfg)) != ESP_OK) {
563 ✗ printf("# Bluetooth controller initialize failed: %s", esp_err_to_name(ret));
564 ✗ return ret;
565 }
566
567 ✗ if ((ret = esp_bt_controller_enable(ESP_BT_MODE_BTDM)) != ESP_OK) {
568 ✗ printf("# Bluetooth controller enable failed: %s", esp_err_to_name(ret));
569 ✗ return ret;
570 }
571
572 ✗ esp_vhci_host_register_callback(&vhci_host_cb);
573
574 ✗ bt_host_tx_pkt_ready();
575
576 ✗ txq_hdl = xRingbufferCreate(256*8, RINGBUF_TYPE_NOSPLIT);
577 ✗ if (txq_hdl == NULL) {
578 ✗ printf("# Failed to create txq ring buffer\n");
579 ✗ return -1;
580 }
581
582 ✗ bt_host_load_keys_from_file(&bt_host_link_keys);
583 ✗ bt_host_load_le_keys_from_file(&bt_host_le_link_keys);
584
585 ✗ xTaskCreatePinnedToCore(&bt_host_task, "bt_host_task", 3072, NULL, 5, NULL, 0);
586 ✗ xTaskCreatePinnedToCore(&bt_fb_task, "bt_fb_task", 3072, NULL, 10, NULL, 0);
587 ✗ xTaskCreatePinnedToCore(&bt_tx_task, "bt_tx_task", 2048, NULL, 11, NULL, 0);
588
589 ✗ if (bt_hci_init()) {
590 ✗ printf("# HCI init fail.\n");
591 ✗ return -1;
592 }
593
594 return ret;
595 }
596
597 196 int32_t bt_host_txq_add(uint8_t *packet, uint32_t packet_len) {
598 #ifdef CONFIG_BLUERETRO_QEMU
599 196 return 0;
600 #else
601 UBaseType_t ret = xRingbufferSend(txq_hdl, (void *)packet, packet_len, portMAX_DELAY);
602 if (ret != pdTRUE) {
603 printf("# %s txq full!\n", __FUNCTION__);
604 }
605 return (ret == pdTRUE ? 0 : -1);
606 #endif
607 }
608
609 ✗ int32_t bt_host_load_link_key(struct bt_hci_cp_link_key_reply *link_key_reply) {
610 ✗ int32_t ret = -1;
611 ✗ for (uint32_t i = 0; i < ARRAY_SIZE(bt_host_link_keys.link_keys); i++) {
612 ✗ if (memcmp((void *)&link_key_reply->bdaddr, (void *)&bt_host_link_keys.link_keys[i].bdaddr, sizeof(link_key_reply->bdaddr)) == 0) {
613 ✗ memcpy((void *)link_key_reply->link_key, &bt_host_link_keys.link_keys[i].link_key, sizeof(link_key_reply->link_key));
614 ✗ ret = 0;
615 }
616 }
617 ✗ return ret;
618 }
619
620 ✗ int32_t bt_host_store_link_key(struct bt_hci_evt_link_key_notify *link_key_notify) {
621 ✗ int32_t ret = -1;
622 ✗ uint32_t index = bt_host_link_keys.index;
623 ✗ for (uint32_t i = 0; i < ARRAY_SIZE(bt_host_link_keys.link_keys); i++) {
624 ✗ if (memcmp((void *)&link_key_notify->bdaddr, (void *)&bt_host_link_keys.link_keys[i].bdaddr, sizeof(link_key_notify->bdaddr)) == 0) {
625 ✗ index = i;
626 }
627 }
628 ✗ memcpy((void *)&bt_host_link_keys.link_keys[index], (void *)link_key_notify, sizeof(bt_host_link_keys.link_keys[0]));
629 ✗ if (index == bt_host_link_keys.index) {
630 ✗ bt_host_link_keys.index++;
631 ✗ bt_host_link_keys.index &= 0xF;
632 }
633 ✗ ret = bt_host_store_keys_on_file(&bt_host_link_keys);
634 ✗ return ret;
635 }
636
637 ✗ void bt_host_clear_le_ltk(bt_addr_le_t *le_bdaddr) {
638 ✗ for (uint32_t i = 0; i < ARRAY_SIZE(bt_host_le_link_keys.keys); i++) {
639 ✗ if (memcmp((void *)le_bdaddr, (void *)&bt_host_le_link_keys.keys[i].le_bdaddr, sizeof(*le_bdaddr)) == 0) {
640 ✗ memset(&bt_host_le_link_keys.keys[i].le_bdaddr, 0, sizeof(bt_host_le_link_keys.keys[0].le_bdaddr));
641 ✗ memset(&bt_host_le_link_keys.keys[i].ltk, 0, sizeof(bt_host_le_link_keys.keys[i].ltk));
642 ✗ memset(&bt_host_le_link_keys.keys[i].ident, 0, sizeof(bt_host_le_link_keys.keys[i].ident));
643 }
644 }
645 ✗ bt_host_store_le_keys_on_file(&bt_host_le_link_keys);
646 ✗ }
647
648 ✗ int32_t bt_host_load_le_ltk(bt_addr_le_t *le_bdaddr, struct bt_smp_encrypt_info *encrypt_info, struct bt_smp_master_ident *master_ident) {
649 ✗ int32_t ret = -1;
650 ✗ for (uint32_t i = 0; i < ARRAY_SIZE(bt_host_le_link_keys.keys); i++) {
651 ✗ if (memcmp((void *)le_bdaddr, (void *)&bt_host_le_link_keys.keys[i].le_bdaddr, sizeof(*le_bdaddr)) == 0) {
652 ✗ if (encrypt_info) {
653 ✗ memcpy((void *)encrypt_info, &bt_host_le_link_keys.keys[i].ltk, sizeof(*encrypt_info));
654 }
655 ✗ if (master_ident) {
656 ✗ memcpy((void *)master_ident, &bt_host_le_link_keys.keys[i].ident, sizeof(*master_ident));
657 }
658 ret = 0;
659 }
660 }
661 ✗ return ret;
662 }
663
664 ✗ int32_t bt_host_store_le_ltk(bt_addr_le_t *le_bdaddr, struct bt_smp_encrypt_info *encrypt_info) {
665 ✗ int32_t ret = -1;
666 ✗ uint32_t index = bt_host_le_link_keys.index;
667 ✗ for (uint32_t i = 0; i < ARRAY_SIZE(bt_host_le_link_keys.keys); i++) {
668 ✗ if (memcmp((void *)le_bdaddr, (void *)&bt_host_le_link_keys.keys[i].le_bdaddr, sizeof(*le_bdaddr)) == 0) {
669 ✗ index = i;
670 }
671 }
672 ✗ memcpy((void *)&bt_host_le_link_keys.keys[index].ltk, (void *)encrypt_info, sizeof(bt_host_le_link_keys.keys[0].ltk));
673 ✗ memcpy((void *)&bt_host_le_link_keys.keys[index].le_bdaddr, (void *)le_bdaddr, sizeof(bt_host_le_link_keys.keys[0].le_bdaddr));
674 ✗ if (index == bt_host_le_link_keys.index) {
675 ✗ bt_host_le_link_keys.index++;
676 ✗ bt_host_le_link_keys.index &= 0xF;
677 }
678 ✗ ret = bt_host_store_le_keys_on_file(&bt_host_le_link_keys);
679 ✗ return ret;
680 }
681
682 ✗ int32_t bt_host_store_le_ident(bt_addr_le_t *le_bdaddr, struct bt_smp_master_ident *master_ident) {
683 ✗ int32_t ret = -1;
684 ✗ uint32_t index = bt_host_le_link_keys.index;
685 ✗ for (uint32_t i = 0; i < ARRAY_SIZE(bt_host_le_link_keys.keys); i++) {
686 ✗ if (memcmp((void *)le_bdaddr, (void *)&bt_host_le_link_keys.keys[i].le_bdaddr, sizeof(*le_bdaddr)) == 0) {
687 ✗ index = i;
688 }
689 }
690 ✗ memcpy((void *)&bt_host_le_link_keys.keys[index].ident, (void *)master_ident, sizeof(bt_host_le_link_keys.keys[0].ident));
691 ✗ memcpy((void *)&bt_host_le_link_keys.keys[index].le_bdaddr, (void *)le_bdaddr, sizeof(bt_host_le_link_keys.keys[0].le_bdaddr));
692 ✗ if (index == bt_host_le_link_keys.index) {
693 ✗ bt_host_le_link_keys.index++;
694 ✗ bt_host_le_link_keys.index &= 0xF;
695 }
696 ✗ ret = bt_host_store_le_keys_on_file(&bt_host_le_link_keys);
697 ✗ return ret;
698 }
699
700 ✗ int32_t bt_host_get_next_accept_le_bdaddr(bt_addr_le_t *le_bdaddr) {
701 ✗ static uint32_t index = 0;
702 ✗ while (index < ARRAY_SIZE(bt_host_le_link_keys.keys)) {
703 ✗ if (bt_addr_le_cmp(&bt_host_le_link_keys.keys[index].le_bdaddr, BT_ADDR_LE_ANY) != 0) {
704 ✗ bt_addr_le_copy(le_bdaddr, &bt_host_le_link_keys.keys[index].le_bdaddr);
705 ✗ index++;
706 ✗ return 0;
707 }
708 ✗ index++;
709 }
710 return -1;
711 }
712
713 1502 void bt_host_bridge(struct bt_dev *device, uint8_t report_id, uint8_t *data, uint32_t len) {
714 1502 struct bt_data *bt_data = &bt_adapter.data[device->ids.id];
715 1502 uint32_t report_type = PAD;
716
717 #ifdef CONFIG_BLUERETRO_BT_TIMING_TESTS
718 atomic_set_bit(&bt_flags, BT_HOST_DBG_MODE);
719 bt_dbg_init(device->ids.type);
720 #else
721 #ifdef CONFIG_BLUERETRO_RAW_REPORT_DUMP
722 printf("# ");
723 for (uint32_t i = 0; i < len; i++) {
724 printf("%02X ", data[i]);
725 }
726 printf("\n");
727 #else
728
2/2
✓ Branch 0 (2→3) taken 261 times.
✓ Branch 1 (2→9) taken 1241 times.
 1502 if (device->ids.type == BT_HID_GENERIC) {
729 261 struct hid_report *report = hid_parser_get_report(device->ids.id, report_id);
730
2/4
✓ Branch 0 (4→5) taken 261 times.
✗ Branch 1 (4→14) not taken.
✓ Branch 2 (5→6) taken 261 times.
✗ Branch 3 (5→14) not taken.
 261 if (report == NULL || report->type == REPORT_NONE) {
731 return;
732 }
733 261 bt_data->base.report_type = report_type = report->type;
734 261 len = report->len;
735
2/2
✓ Branch 0 (6→7) taken 222 times.
✓ Branch 1 (6→8) taken 39 times.
 261 if (report_id != bt_data->base.report_id) {
736 39 atomic_clear_bit(&bt_data->base.flags[report_type], BT_INIT);
737 }
738 }
739
4/4
✓ Branch 0 (10→11) taken 266 times.
✓ Branch 1 (10→12) taken 1236 times.
✓ Branch 2 (11→12) taken 94 times.
✓ Branch 3 (11→13) taken 172 times.
 1502 if (atomic_test_bit(&bt_data->base.flags[report_type], BT_INIT) || bt_data->base.report_cnt > 1) {
740 1330 bt_data->base.report_id = report_id;
741 1330 bt_data->base.input = data;
742 1330 bt_data->base.input_len = len;
743 1330 adapter_bridge(bt_data);
744 }
745 1502 bt_data->base.report_cnt++;
746 #endif
747 #endif
748 }
749