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