1 /* bttester.c - Bluetooth Tester */
2 
3 /*
4  * Copyright (c) 2015-2016 Intel Corporation
5  * Copyright (c) 2022 Codecoup
6  *
7  * SPDX-License-Identifier: Apache-2.0
8  */
9 #include <errno.h>
10 #include <stdio.h>
11 #include <stdint.h>
12 #include <string.h>
13 
14 #include <zephyr/autoconf.h>
15 #include <zephyr/bluetooth/bluetooth.h>
16 #include <zephyr/device.h>
17 #include <zephyr/drivers/uart.h>
18 #include <zephyr/drivers/uart_pipe.h>
19 #include <zephyr/kernel.h>
20 #include <zephyr/kernel/thread_stack.h>
21 #include <zephyr/logging/log.h>
22 #include <zephyr/net_buf.h>
23 #include <zephyr/sys/__assert.h>
24 #include <zephyr/sys/byteorder.h>
25 #include <zephyr/sys/util.h>
26 #include <zephyr/sys_clock.h>
27 #include <zephyr/toolchain.h>
28 #include <zephyr/types.h>
29 
30 #define LOG_MODULE_NAME bttester
31 LOG_MODULE_REGISTER(LOG_MODULE_NAME, CONFIG_BTTESTER_LOG_LEVEL);
32 
33 #include "btp/btp.h"
34 
35 #define STACKSIZE CONFIG_BTTESTER_BTP_CMD_THREAD_STACK_SIZE
36 static K_THREAD_STACK_DEFINE(stack, STACKSIZE);
37 static struct k_thread cmd_thread;
38 
39 #define CMD_QUEUED 2
40 struct btp_buf {
41 	intptr_t _reserved;
42 	uint8_t data[BTP_MTU]; /* includes btp header */
43 	uint8_t rsp[BTP_DATA_MAX_SIZE];
44 };
45 
46 static struct btp_buf cmd_buf[CMD_QUEUED];
47 
48 static K_FIFO_DEFINE(cmds_queue);
49 static K_FIFO_DEFINE(avail_queue);
50 
51 static struct btp_buf *delayed_cmd;
52 
53 static struct {
54 	const struct btp_handler *handlers;
55 	size_t num;
56 } service_handler[BTP_SERVICE_ID_MAX + 1];
57 
58 static struct net_buf_simple *rsp_buf = NET_BUF_SIMPLE(BTP_MTU);
59 static K_MUTEX_DEFINE(rsp_buf_mutex);
60 
61 static void tester_send_with_index(uint8_t service, uint8_t opcode, uint8_t index,
62 				   const uint8_t *data, size_t len);
63 static void tester_rsp_with_index(uint8_t service, uint8_t opcode, uint8_t index,
64 				  uint8_t status);
65 
tester_register_command_handlers(uint8_t service,const struct btp_handler * handlers,size_t num)66 void tester_register_command_handlers(uint8_t service,
67 				      const struct btp_handler *handlers,
68 				      size_t num)
69 {
70 	__ASSERT_NO_MSG(service <= BTP_SERVICE_ID_MAX);
71 	__ASSERT_NO_MSG(service_handler[service].handlers == NULL);
72 
73 	service_handler[service].handlers = handlers;
74 	service_handler[service].num = num;
75 }
76 
find_btp_handler(uint8_t service,uint8_t opcode)77 static const struct btp_handler *find_btp_handler(uint8_t service, uint8_t opcode)
78 {
79 	if ((service > BTP_SERVICE_ID_MAX) ||
80 	    (service_handler[service].handlers == NULL)) {
81 		return NULL;
82 	}
83 
84 	for (uint8_t i = 0; i < service_handler[service].num; i++) {
85 		if (service_handler[service].handlers[i].opcode == opcode) {
86 			return &service_handler[service].handlers[i];
87 		}
88 	}
89 
90 	return NULL;
91 }
92 
cmd_handler(void * p1,void * p2,void * p3)93 static void cmd_handler(void *p1, void *p2, void *p3)
94 {
95 	while (1) {
96 		const struct btp_handler *btp;
97 		struct btp_buf *cmd;
98 		struct btp_hdr *hdr;
99 		uint8_t status;
100 		uint16_t rsp_len = 0;
101 
102 		cmd = k_fifo_get(&cmds_queue, K_FOREVER);
103 		hdr = (struct btp_hdr *)cmd->data;
104 
105 		LOG_DBG("cmd service 0x%02x opcode 0x%02x index 0x%02x",
106 			hdr->service, hdr->opcode, hdr->index);
107 
108 		btp = find_btp_handler(hdr->service, hdr->opcode);
109 		if (btp) {
110 			uint16_t len = sys_le16_to_cpu(hdr->len);
111 
112 			if (len > BTP_DATA_MAX_SIZE) {
113 				status = BTP_STATUS_FAILED;
114 			} else if (btp->index != hdr->index) {
115 				status = BTP_STATUS_FAILED;
116 			} else if ((btp->expect_len >= 0) && (btp->expect_len != len)) {
117 				status = BTP_STATUS_FAILED;
118 			} else {
119 				status = btp->func(hdr->data, len,
120 						   cmd->rsp, &rsp_len);
121 			}
122 
123 			/* This means that caller likely overwrote rsp buffer */
124 			__ASSERT_NO_MSG(rsp_len <= BTP_DATA_MAX_SIZE);
125 		} else {
126 			status = BTP_STATUS_UNKNOWN_CMD;
127 		}
128 		/* Allow to delay only 1 command. This is for convenience only
129 		 * of using cmd data without need of copying those in async
130 		 * functions. Should be not needed eventually.
131 		 */
132 		if (status == BTP_STATUS_DELAY_REPLY) {
133 			__ASSERT_NO_MSG(delayed_cmd == NULL);
134 			delayed_cmd = cmd;
135 			continue;
136 		}
137 
138 		if ((status == BTP_STATUS_SUCCESS) && rsp_len > 0) {
139 			tester_send_with_index(hdr->service, hdr->opcode,
140 					       hdr->index, cmd->rsp, rsp_len);
141 		} else {
142 			tester_rsp_with_index(hdr->service, hdr->opcode,
143 					      hdr->index, status);
144 		}
145 
146 		(void)memset(cmd, 0, sizeof(*cmd));
147 		k_fifo_put(&avail_queue, cmd);
148 	}
149 }
150 
recv_cb(uint8_t * buf,size_t * off)151 static uint8_t *recv_cb(uint8_t *buf, size_t *off)
152 {
153 	struct btp_hdr *cmd = (void *) buf;
154 	struct btp_buf *new_buf;
155 	uint16_t len;
156 
157 	if (*off < sizeof(*cmd)) {
158 		return buf;
159 	}
160 
161 	len = sys_le16_to_cpu(cmd->len);
162 	if (len > BTP_MTU - sizeof(*cmd)) {
163 		LOG_ERR("BT tester: invalid packet length");
164 		*off = 0;
165 		return buf;
166 	}
167 
168 	if (*off < sizeof(*cmd) + len) {
169 		return buf;
170 	}
171 
172 	new_buf =  k_fifo_get(&avail_queue, K_NO_WAIT);
173 	if (!new_buf) {
174 		LOG_ERR("BT tester: RX overflow");
175 		*off = 0;
176 		return buf;
177 	}
178 
179 	k_fifo_put(&cmds_queue, CONTAINER_OF(buf, struct btp_buf, data[0]));
180 
181 	*off = 0;
182 	return new_buf->data;
183 }
184 
185 #if defined(CONFIG_UART_PIPE)
186 /* Uart Pipe */
uart_init(uint8_t * data)187 static void uart_init(uint8_t *data)
188 {
189 	uart_pipe_register(data, BTP_MTU, recv_cb);
190 }
191 
uart_send(const uint8_t * data,size_t len)192 static void uart_send(const uint8_t *data, size_t len)
193 {
194 	uart_pipe_send(data, len);
195 }
196 #else /* !CONFIG_UART_PIPE */
197 static uint8_t *recv_buf;
198 static size_t recv_off;
199 static const struct device *const dev =
200 	DEVICE_DT_GET(DT_CHOSEN(zephyr_console));
201 
timer_expiry_cb(struct k_timer * timer)202 static void timer_expiry_cb(struct k_timer *timer)
203 {
204 	uint8_t c;
205 
206 	while (uart_poll_in(dev, &c) == 0) {
207 		recv_buf[recv_off++] = c;
208 		recv_buf = recv_cb(recv_buf, &recv_off);
209 	}
210 }
211 
212 K_TIMER_DEFINE(timer, timer_expiry_cb, NULL);
213 
214 /* Uart Poll */
uart_init(uint8_t * data)215 static void uart_init(uint8_t *data)
216 {
217 	__ASSERT_NO_MSG(device_is_ready(dev));
218 
219 	recv_buf = data;
220 
221 	k_timer_start(&timer, K_MSEC(10), K_MSEC(10));
222 }
223 
uart_send(const uint8_t * data,size_t len)224 static void uart_send(const uint8_t *data, size_t len)
225 {
226 	int i;
227 
228 	for (i = 0; i < len; i++) {
229 		uart_poll_out(dev, data[i]);
230 	}
231 }
232 #endif /* CONFIG_UART_PIPE */
233 
tester_init(void)234 void tester_init(void)
235 {
236 	int i;
237 	struct btp_buf *buf;
238 
239 	LOG_DBG("Initializing tester");
240 
241 	for (i = 0; i < CMD_QUEUED; i++) {
242 		k_fifo_put(&avail_queue, &cmd_buf[i]);
243 	}
244 
245 	k_thread_create(&cmd_thread, stack, STACKSIZE, cmd_handler,
246 			NULL, NULL, NULL, K_PRIO_COOP(7), 0, K_NO_WAIT);
247 
248 	buf = k_fifo_get(&avail_queue, K_NO_WAIT);
249 
250 	uart_init(buf->data);
251 
252 	/* core service is always available */
253 	tester_init_core();
254 
255 	tester_send_with_index(BTP_SERVICE_ID_CORE, BTP_CORE_EV_IUT_READY,
256 			      BTP_INDEX_NONE, NULL, 0);
257 }
258 
tester_rsp_buffer_lock(void)259 int tester_rsp_buffer_lock(void)
260 {
261 	if (k_mutex_lock(&rsp_buf_mutex, Z_FOREVER) != 0) {
262 		LOG_ERR("Cannot lock rsp_ring_buf");
263 
264 		return -EACCES;
265 	}
266 
267 	return 0;
268 }
269 
tester_rsp_buffer_unlock(void)270 void tester_rsp_buffer_unlock(void)
271 {
272 	k_mutex_unlock(&rsp_buf_mutex);
273 }
274 
tester_rsp_buffer_free(void)275 void tester_rsp_buffer_free(void)
276 {
277 	net_buf_simple_init(rsp_buf, 0);
278 }
279 
tester_rsp_buffer_allocate(size_t len,uint8_t ** data)280 void tester_rsp_buffer_allocate(size_t len, uint8_t **data)
281 {
282 	tester_rsp_buffer_free();
283 
284 	*data = net_buf_simple_add(rsp_buf, len);
285 }
286 
287 K_MUTEX_DEFINE(uart_mutex);
tester_send_with_index(uint8_t service,uint8_t opcode,uint8_t index,const uint8_t * data,size_t len)288 static void tester_send_with_index(uint8_t service, uint8_t opcode, uint8_t index,
289 				   const uint8_t *data, size_t len)
290 {
291 	struct btp_hdr msg;
292 
293 	msg.service = service;
294 	msg.opcode = opcode;
295 	msg.index = index;
296 	msg.len = sys_cpu_to_le16(len);
297 
298 	k_mutex_lock(&uart_mutex, K_FOREVER);
299 	uart_send((uint8_t *)&msg, sizeof(msg));
300 	if (data && len) {
301 		uart_send(data, len);
302 	}
303 	k_mutex_unlock(&uart_mutex);
304 }
305 
tester_rsp_with_index(uint8_t service,uint8_t opcode,uint8_t index,uint8_t status)306 static void tester_rsp_with_index(uint8_t service, uint8_t opcode, uint8_t index,
307 				  uint8_t status)
308 {
309 	struct btp_status s;
310 
311 	LOG_DBG("service 0x%02x opcode 0x%02x index 0x%02x status 0x%02x", service, opcode, index,
312 		status);
313 
314 	if (status == BTP_STATUS_SUCCESS) {
315 		tester_send_with_index(service, opcode, index, NULL, 0);
316 		return;
317 	}
318 
319 	s.code = status;
320 	tester_send_with_index(service, BTP_STATUS, index, (uint8_t *) &s, sizeof(s));
321 }
322 
tester_event(uint8_t service,uint8_t opcode,const void * data,size_t len)323 void tester_event(uint8_t service, uint8_t opcode, const void *data, size_t len)
324 {
325 	__ASSERT_NO_MSG(opcode >= 0x80);
326 
327 	LOG_DBG("service 0x%02x opcode 0x%02x", service, opcode);
328 
329 	tester_send_with_index(service, opcode, BTP_INDEX, data, len);
330 }
331 
tester_rsp_full(uint8_t service,uint8_t opcode,const void * rsp,size_t len)332 void tester_rsp_full(uint8_t service, uint8_t opcode, const void *rsp, size_t len)
333 {
334 	struct btp_buf *cmd;
335 
336 	__ASSERT_NO_MSG(opcode < 0x80);
337 	__ASSERT_NO_MSG(delayed_cmd != NULL);
338 
339 	LOG_DBG("service 0x%02x opcode 0x%02x", service, opcode);
340 
341 	tester_send_with_index(service, opcode, BTP_INDEX, rsp, len);
342 
343 	cmd = delayed_cmd;
344 	delayed_cmd = NULL;
345 
346 	(void)memset(cmd, 0, sizeof(*cmd));
347 	k_fifo_put(&avail_queue, cmd);
348 }
349 
tester_rsp(uint8_t service,uint8_t opcode,uint8_t status)350 void tester_rsp(uint8_t service, uint8_t opcode, uint8_t status)
351 {
352 	struct btp_buf *cmd;
353 
354 	__ASSERT_NO_MSG(opcode < 0x80);
355 	__ASSERT_NO_MSG(delayed_cmd != NULL);
356 
357 	LOG_DBG("service 0x%02x opcode 0x%02x status 0x%02x", service, opcode, status);
358 
359 	tester_rsp_with_index(service, opcode, BTP_INDEX, status);
360 
361 	cmd = delayed_cmd;
362 	delayed_cmd = NULL;
363 
364 	(void)memset(cmd, 0, sizeof(*cmd));
365 	k_fifo_put(&avail_queue, cmd);
366 }
367 
tester_supported_commands(uint8_t service,uint8_t * cmds)368 uint16_t tester_supported_commands(uint8_t service, uint8_t *cmds)
369 {
370 	uint8_t opcode_max = 0;
371 
372 	__ASSERT_NO_MSG(service <= BTP_SERVICE_ID_MAX);
373 
374 	for (size_t i = 0; i < service_handler[service].num; i++) {
375 		const struct btp_handler *handler = &service_handler[service].handlers[i];
376 
377 		tester_set_bit(cmds, handler->opcode);
378 
379 		if (handler->opcode > opcode_max) {
380 			opcode_max = handler->opcode;
381 		}
382 	}
383 
384 	/* bytes used to store supported commands bitmask */
385 	return (opcode_max / 8) + 1;
386 }
387