GCC Code Coverage Report

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 <string.h>
8			#include "zephyr/types.h"
9			#include "zephyr/sdp.h"
10			#include "tools/util.h"
11			#include "mon.h"
12			#include "host.h"
13			#include "adapter/hid_parser.h"
14			#include "adapter/mapping_quirks.h"
15			#include "l2cap.h"
16			#include "sdp.h"
17
18			static const uint8_t l2cap_attr_req[] = {
19			/* Service Search Pattern */
20			/* Data Element */
21			/* Type */ BT_SDP_SEQ8,
22			/* Size */ 0x03,
23			/* Data Value */
24			/* Data Element */
25			/* Type */ BT_SDP_UUID16,
26			/* Data Value */
27			/* UUID */ 0x01, 0x00, /* L2CAP */
28			/* Max Att Byte */ 0x04, 0x00, /* 1024 */
29			/* Att ID List */
30			/* Data Element */
31			/* Type */ BT_SDP_SEQ8,
32			/* Size */ 0x05,
33			/* Data Value */
34			/* Data Element */
35			/* Type */ BT_SDP_UINT32,
36			/* Data Value */
37			/* Att Range */ 0x00, 0x00, /* to */ 0xff, 0xff,
38			};
39
40			static const uint8_t pnp_attr_req[] = {
41			/* Service Search Pattern */
42			/* Data Element */
43			/* Type */ BT_SDP_SEQ8,
44			/* Size */ 0x03,
45			/* Data Value */
46			/* Data Element */
47			/* Type */ BT_SDP_UUID16,
48			/* Data Value */
49			/* UUID */ (BT_SDP_PNP_INFO_SVCLASS >> 8), (BT_SDP_PNP_INFO_SVCLASS & 0xff),
50			/* Max Att Byte */ 0x04, 0x00, /* 1024 */
51			/* Att ID List */
52			/* Data Element */
53			/* Type */ BT_SDP_SEQ8,
54			/* Size */ 0x05,
55			/* Data Value */
56			/* Data Element */
57			/* Type */ BT_SDP_UINT32,
58			/* Data Value */
59			/* Att Range */ 0x00, 0x00, /* to */ 0xff, 0xff,
60			};
61
62			// static const uint8_t xb1_svc_search_attr_rsp[] = {
63			// 0x35, 0x0a, 0x35, 0x08, 0x09, 0x00, 0x01, 0x35, 0x03, 0x19, 0x12, 0x00
64			// };
65
66			static const uint8_t xb1_svc_search_rsp[] = {
67			0x00, 0x01, 0x00, 0x00
68			};
69
70			static const uint8_t xb1_svc_attr_rsp[] = {
71			0x35, 0x95, 0x09, 0x00, 0x00, 0x0a, 0x00, 0x01,
72			0x00, 0x00, 0x09, 0x00, 0x01, 0x35, 0x03, 0x19,
73			0x12, 0x00, 0x09, 0x00, 0x04, 0x35, 0x0d, 0x35,
74			0x06, 0x19, 0x01, 0x00, 0x09, 0x00, 0x01, 0x35,
75			0x03, 0x19, 0x00, 0x01, 0x09, 0x00, 0x05, 0x35,
76			0x03, 0x19, 0x10, 0x02, 0x09, 0x00, 0x06, 0x35,
77			0x09, 0x09, 0x65, 0x6e, 0x09, 0x00, 0x6a, 0x09,
78			0x01, 0x00, 0x09, 0x01, 0x00, 0x25, 0x18, 0x44,
79			0x65, 0x76, 0x69, 0x63, 0x65, 0x20, 0x49, 0x44,
80			0x20, 0x53, 0x65, 0x72, 0x76, 0x69, 0x63, 0x65,
81			0x20, 0x52, 0x65, 0x63, 0x6f, 0x72, 0x64, 0x09,
82			0x01, 0x01, 0x25, 0x18, 0x44, 0x65, 0x76, 0x69,
83			0x63, 0x65, 0x20, 0x49, 0x44, 0x20, 0x53, 0x65,
84			0x72, 0x76, 0x69, 0x63, 0x65, 0x20, 0x52, 0x65,
85			0x63, 0x6f, 0x72, 0x64, 0x09, 0x02, 0x00, 0x09,
86			0x01, 0x03, 0x09, 0x02, 0x01, 0x09, 0x00, 0x06,
87			0x09, 0x02, 0x02, 0x09, 0x00, 0x01, 0x09, 0x02,
88			0x03, 0x09, 0x0a, 0x00, 0x09, 0x02, 0x04, 0x28,
89			0x01, 0x09, 0x02, 0x05, 0x09, 0x00, 0x01
90			};
91
92			static uint16_t tx_tid = 0;
93
94		✗	static void bt_sdp_cmd(uint16_t handle, uint16_t cid, uint8_t code, uint16_t tid, uint16_t len) {
95		✗	uint16_t packet_len = (BT_HCI_H4_HDR_SIZE + BT_HCI_ACL_HDR_SIZE
96			+ sizeof(struct bt_l2cap_hdr) + sizeof(struct bt_sdp_hdr) + len);
97
98		✗	bt_hci_pkt_tmp.h4_hdr.type = BT_HCI_H4_TYPE_ACL;
99
100		✗	bt_hci_pkt_tmp.acl_hdr.handle = bt_acl_handle_pack(handle, 0x2);
101		✗	bt_hci_pkt_tmp.acl_hdr.len = packet_len - BT_HCI_H4_HDR_SIZE - BT_HCI_ACL_HDR_SIZE;
102
103		✗	bt_hci_pkt_tmp.l2cap_hdr.len = bt_hci_pkt_tmp.acl_hdr.len - sizeof(bt_hci_pkt_tmp.l2cap_hdr);
104		✗	bt_hci_pkt_tmp.l2cap_hdr.cid = cid;
105
106		✗	bt_hci_pkt_tmp.sdp_hdr.op_code = code;
107		✗	bt_hci_pkt_tmp.sdp_hdr.tid = sys_cpu_to_be16(tid);
108		✗	bt_hci_pkt_tmp.sdp_hdr.param_len = sys_cpu_to_be16(len);
109
110		✗	bt_host_txq_add((uint8_t *)&bt_hci_pkt_tmp, packet_len);
111		✗	}
112
113		✗	static void bt_sdp_cmd_svc_search_rsp(uint16_t handle, uint16_t cid, uint16_t tid, const uint8_t *data, uint32_t len) {
114		✗	struct bt_sdp_svc_rsp *svc_rsp = (struct bt_sdp_svc_rsp *)bt_hci_pkt_tmp.sdp_data;
115		✗	uint8_t *sdp_data = bt_hci_pkt_tmp.sdp_data + sizeof(struct bt_sdp_svc_rsp);
116		✗	uint8_t *sdp_con_state = sdp_data + len;
117
118		✗	svc_rsp->total_recs = sys_cpu_to_be16(0x0001);
119		✗	svc_rsp->current_recs = sys_cpu_to_be16(0x0001);
120		✗	if (data) {
121		✗	memcpy(sdp_data, data, len);
122			}
123		✗	*sdp_con_state = 0;
124
125		✗	bt_sdp_cmd(handle, cid, BT_SDP_SVC_SEARCH_RSP, tid, sizeof(struct bt_sdp_svc_rsp) + len + 1);
126		✗	}
127
128		✗	static void bt_sdp_cmd_svc_attr_rsp(uint16_t handle, uint16_t cid, uint16_t tid, const uint8_t *data, uint32_t len) {
129		✗	struct bt_sdp_att_rsp *att_rsp = (struct bt_sdp_att_rsp *)bt_hci_pkt_tmp.sdp_data;
130		✗	uint8_t *sdp_data = bt_hci_pkt_tmp.sdp_data + sizeof(struct bt_sdp_att_rsp);
131		✗	uint8_t *sdp_con_state = sdp_data + len;
132
133		✗	att_rsp->att_list_len = sys_cpu_to_be16(len);
134		✗	if (data) {
135		✗	memcpy(sdp_data, data, len);
136			}
137		✗	*sdp_con_state = 0;
138
139		✗	bt_sdp_cmd(handle, cid, BT_SDP_SVC_ATTR_RSP, tid, sizeof(struct bt_sdp_att_rsp) + len + 1);
140		✗	}
141
142		✗	static void bt_sdp_cmd_svc_search_attr_rsp(uint16_t handle, uint16_t cid, uint16_t tid, const uint8_t *data, uint32_t len) {
143		✗	struct bt_sdp_att_rsp *att_rsp = (struct bt_sdp_att_rsp *)bt_hci_pkt_tmp.sdp_data;
144		✗	uint8_t *sdp_data = bt_hci_pkt_tmp.sdp_data + sizeof(struct bt_sdp_att_rsp);
145		✗	uint8_t *sdp_con_state = sdp_data + len;
146
147		✗	att_rsp->att_list_len = sys_cpu_to_be16(len);
148		✗	if (data) {
149		✗	memcpy(sdp_data, data, len);
150			}
151		✗	*sdp_con_state = 0;
152
153		✗	bt_sdp_cmd(handle, cid, BT_SDP_SVC_SEARCH_ATTR_RSP, tid, sizeof(struct bt_sdp_att_rsp) + len + 1);
154		✗	}
155
156		✗	void bt_sdp_cmd_svc_search_attr_req(struct bt_dev *device, uint8_t *cont_data, uint32_t cont_len) {
157		✗	memcpy(bt_hci_pkt_tmp.sdp_data, l2cap_attr_req, sizeof(l2cap_attr_req));
158		✗	memcpy(bt_hci_pkt_tmp.sdp_data + sizeof(l2cap_attr_req), cont_data, cont_len);
159
160		✗	bt_sdp_cmd(device->acl_handle, device->sdp_tx_chan.dcid, BT_SDP_SVC_SEARCH_ATTR_REQ, tx_tid++, sizeof(l2cap_attr_req) + cont_len);
161		✗	}
162		✗	static void bt_sdp_cmd_pnp_vendor_svc_search_attr_req(struct bt_dev *device, uint8_t *cont_data, uint32_t cont_len) {
163		✗	memcpy(bt_hci_pkt_tmp.sdp_data, pnp_attr_req, sizeof(pnp_attr_req));
164		✗	memcpy(bt_hci_pkt_tmp.sdp_data + sizeof(pnp_attr_req), cont_data, cont_len);
165
166		✗	bt_sdp_cmd(device->acl_handle, device->sdp_tx_chan.dcid, BT_SDP_SVC_SEARCH_ATTR_REQ, tx_tid++, sizeof(pnp_attr_req) + cont_len);
167		✗	}
168
169		✗	void bt_sdp_parser(struct bt_data *bt_data) {
170		✗	const uint8_t sdp_vid[] = {0x09, 0x02, 0x01};
171		✗	const uint8_t sdp_pid[] = {0x09, 0x02, 0x02};
172		✗	const uint8_t sdp_hid_desc_list[] = {0x09, 0x02, 0x06};
173		✗	uint8_t *hid_desc = NULL;
174		✗	uint32_t hid_desc_len = 0;
175		✗	uint32_t hid_offset = 0;
176
177		✗	bt_data->base.vid = sys_be16_to_cpu(*(uint16_t *)(memmem(bt_data->base.pnp_data, bt_data->base.pnp_len, sdp_vid, sizeof(sdp_vid)) + 4));
178		✗	bt_data->base.pid = sys_be16_to_cpu(*(uint16_t *)(memmem(bt_data->base.pnp_data, bt_data->base.pnp_len, sdp_pid, sizeof(sdp_pid)) + 4));
179		✗	if (bt_data->base.pnp_data) {
180		✗	free(bt_data->base.pnp_data);
181		✗	bt_data->base.pnp_data = NULL;
182			}
183		✗	printf("%s: VID: 0x%04X PID: 0x%04X\n", __FUNCTION__, bt_data->base.vid, bt_data->base.pid);
184		✗	bt_mon_log(true, "%s: VID: 0x%04X PID: 0x%04X\n", __FUNCTION__, bt_data->base.vid, bt_data->base.pid);
185
186		✗	hid_desc = memmem(bt_data->base.sdp_data, bt_data->base.sdp_len, sdp_hid_desc_list, sizeof(sdp_hid_desc_list));
187		✗	hid_offset = hid_desc - bt_data->base.sdp_data;
188
189		✗	if (hid_desc) {
190		✗	hid_desc += 3;
191
192		✗	switch (*hid_desc) {
193		✗	case BT_SDP_SEQ8:
194		✗	hid_desc += 2;
195		✗	break;
196		✗	case BT_SDP_SEQ16:
197		✗	hid_desc += 3;
198		✗	break;
199		✗	case BT_SDP_SEQ32:
200		✗	hid_desc += 5;
201		✗	break;
202			}
203
204		✗	switch (*hid_desc) {
205		✗	case BT_SDP_SEQ8:
206		✗	hid_desc += 4;
207		✗	break;
208		✗	case BT_SDP_SEQ16:
209		✗	hid_desc += 5;
210		✗	break;
211		✗	case BT_SDP_SEQ32:
212		✗	hid_desc += 7;
213		✗	break;
214			}
215
216		✗	switch (*hid_desc++) {
217		✗	case BT_SDP_TEXT_STR8:
218		✗	hid_desc_len = *hid_desc;
219		✗	hid_desc++;
220		✗	break;
221		✗	case BT_SDP_TEXT_STR16:
222		✗	hid_desc_len = sys_be16_to_cpu(*(uint16_t *)hid_desc);
223		✗	hid_desc += 2;
224		✗	break;
225		✗	case BT_SDP_TEXT_STR32:
226		✗	hid_desc_len = sys_be32_to_cpu(*(uint32_t *)hid_desc);
227		✗	hid_desc += 4;
228		✗	break;
229			}
230		✗	printf("# %s HID descriptor size: %lu Usage page: %02X%02X\n", __FUNCTION__, hid_desc_len, hid_desc[0], hid_desc[1]);
231		✗	if ((hid_offset + hid_desc_len) > bt_data->base.sdp_len) {
232		✗	printf("# %s HID descriptor size exceed buffer size: %ld, trunc\n", __FUNCTION__, bt_data->base.sdp_len);
233		✗	hid_desc_len = bt_data->base.sdp_len - hid_offset;
234			}
235		✗	hid_parser(bt_data, hid_desc, hid_desc_len);
236		✗	if (bt_data->base.sdp_data) {
237		✗	free(bt_data->base.sdp_data);
238		✗	bt_data->base.sdp_data = NULL;
239			}
240			}
241		✗	}
242
243		✗	void bt_sdp_hdlr(struct bt_dev *device, struct bt_hci_pkt *bt_hci_acl_pkt) {
244		✗	switch (bt_hci_acl_pkt->sdp_hdr.op_code) {
245		✗	case BT_SDP_SVC_SEARCH_REQ:
246		✗	bt_sdp_cmd_svc_search_rsp(device->acl_handle, device->sdp_rx_chan.dcid,
247		✗	sys_be16_to_cpu(bt_hci_acl_pkt->sdp_hdr.tid), xb1_svc_search_rsp, sizeof(xb1_svc_search_rsp));
248		✗	break;
249		✗	case BT_SDP_SVC_ATTR_REQ:
250		✗	bt_sdp_cmd_svc_attr_rsp(device->acl_handle, device->sdp_rx_chan.dcid,
251		✗	sys_be16_to_cpu(bt_hci_acl_pkt->sdp_hdr.tid), xb1_svc_attr_rsp, sizeof(xb1_svc_attr_rsp));
252		✗	break;
253		✗	case BT_SDP_SVC_SEARCH_ATTR_REQ:
254		✗	bt_sdp_cmd_svc_search_attr_rsp(device->acl_handle, device->sdp_rx_chan.dcid,
255		✗	sys_be16_to_cpu(bt_hci_acl_pkt->sdp_hdr.tid), NULL, 0);
256		✗	break;
257		✗	case BT_SDP_SVC_SEARCH_ATTR_RSP:
258			{
259		✗	struct bt_sdp_att_rsp *att_rsp = (struct bt_sdp_att_rsp *)bt_hci_acl_pkt->sdp_data;
260		✗	uint8_t *sdp_data = bt_hci_acl_pkt->sdp_data + sizeof(struct bt_sdp_att_rsp);
261		✗	uint8_t *sdp_con_state = sdp_data + sys_be16_to_cpu(att_rsp->att_list_len);
262		✗	uint32_t free_len = BT_SDP_DATA_SIZE - bt_adapter.data[device->ids.id].base.sdp_len;
263		✗	uint32_t free_pnp_len = BT_PNP_DATA_SIZE - bt_adapter.data[device->ids.id].base.pnp_len;
264		✗	uint32_t cp_len = sys_be16_to_cpu(att_rsp->att_list_len);
265
266		✗	switch (device->sdp_state) {
267		✗	case 0:
268		✗	if (cp_len > free_len) {
269		✗	cp_len = free_len;
270		✗	printf("# %s SDP data > buffer will be trunc to %d, cp_len %ld\n", __FUNCTION__, BT_SDP_DATA_SIZE, cp_len);
271			}
272
273		✗	if (bt_adapter.data[device->ids.id].base.sdp_data == NULL) {
274		✗	bt_adapter.data[device->ids.id].base.sdp_data = malloc(BT_SDP_DATA_SIZE);
275		✗	if (bt_adapter.data[device->ids.id].base.sdp_data == NULL) {
276		✗	printf("# dev: %ld Failed to alloc report memory\n", device->ids.id);
277		✗	break;
278			}
279			}
280		✗	memcpy(bt_adapter.data[device->ids.id].base.sdp_data + bt_adapter.data[device->ids.id].base.sdp_len, sdp_data, cp_len);
281		✗	bt_adapter.data[device->ids.id].base.sdp_len += cp_len;
282		✗	if (*sdp_con_state) {
283		✗	bt_sdp_cmd_svc_search_attr_req(device, sdp_con_state, 1 + *sdp_con_state);
284			}
285			else {
286		✗	uint8_t cont = 0x00;
287		✗	bt_sdp_cmd_pnp_vendor_svc_search_attr_req(device, &cont, 1);
288		✗	device->sdp_state++;
289			}
290			break;
291		✗	case 1:
292		✗	if (cp_len > free_pnp_len) {
293		✗	cp_len = free_pnp_len;
294		✗	printf("# %s PNP data > buffer will be trunc to %d, cp_len %ld\n", __FUNCTION__, BT_PNP_DATA_SIZE, cp_len);
295			}
296
297		✗	if (bt_adapter.data[device->ids.id].base.pnp_data == NULL) {
298		✗	bt_adapter.data[device->ids.id].base.pnp_data = malloc(BT_PNP_DATA_SIZE);
299		✗	if (bt_adapter.data[device->ids.id].base.pnp_data == NULL) {
300		✗	printf("# dev: %ld Failed to alloc pnp memory\n", device->ids.id);
301		✗	break;
302			}
303			}
304		✗	memcpy(bt_adapter.data[device->ids.id].base.pnp_data + bt_adapter.data[device->ids.id].base.pnp_len, sdp_data, cp_len);
305		✗	bt_adapter.data[device->ids.id].base.pnp_len += cp_len;
306		✗	if (*sdp_con_state) {
307		✗	bt_sdp_cmd_pnp_vendor_svc_search_attr_req(device, sdp_con_state, 1 + *sdp_con_state);
308			}
309			else {
310		✗	bt_l2cap_cmd_sdp_disconn_req(device);
311		✗	atomic_set_bit(&device->flags, BT_DEV_SDP_DATA);
312			}
313			break;
314			}
315			break;
316			}
317			}
318		✗	}
319