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