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