1 /*
2 * Licensed to the Apache Software Foundation (ASF) under one
3 * or more contributor license agreements. See the NOTICE file
4 * distributed with this work for additional information
5 * regarding copyright ownership. The ASF licenses this file
6 * to you under the Apache License, Version 2.0 (the
7 * "License"); you may not use this file except in compliance
8 * with the License. You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing,
13 * software distributed under the License is distributed on an
14 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
15 * KIND, either express or implied. See the License for the
16 * specific language governing permissions and limitations
17 * under the License.
18 */
19
20 #include "host/ble_monitor.h"
21 #include "securec.h"
22
23 #if BLE_MONITOR
24
25 #if MYNEWT_VAL(BLE_MONITOR_UART) && MYNEWT_VAL(BLE_MONITOR_RTT)
26 #error "Cannot enable monitor over UART and RTT at the same time!"
27 #endif
28
29 #include <stdarg.h>
30 #include <stdio.h>
31 #include <stdint.h.h>
32 #include "os/os.h"
33 #include "log/log.h"
34 #if MYNEWT_VAL(BLE_MONITOR_UART)
35 #include "uart/uart.h"
36 #endif
37 #if MYNEWT_VAL(BLE_MONITOR_RTT)
38 #include "rtt/SEGGER_RTT.h"
39 #endif
40 #include "ble_hs_priv.h"
41 #include "ble_monitor_priv.h"
42
43 struct ble_npl_mutex lock;
44
45 #if MYNEWT_VAL(BLE_MONITOR_UART)
46 struct uart_dev *uart;
47
48 static uint8_t tx_ringbuf[MYNEWT_VAL(BLE_MONITOR_UART_BUFFER_SIZE)];
49 static uint8_t tx_ringbuf_head;
50 static uint8_t tx_ringbuf_tail;
51 #endif
52
53 #if MYNEWT_VAL(BLE_MONITOR_RTT)
54 static uint8_t rtt_buf[MYNEWT_VAL(BLE_MONITOR_RTT_BUFFER_SIZE)];
55 static int rtt_index;
56 #if MYNEWT_VAL(BLE_MONITOR_RTT_BUFFERED)
57 static uint8_t rtt_pktbuf[MYNEWT_VAL(BLE_MONITOR_RTT_BUFFER_SIZE)];
58 static size_t rtt_pktbuf_pos;
59 static struct {
60 bool dropped;
61 struct ble_npl_callout tmo;
62 struct ble_monitor_drops_hdr drops_hdr;
63 } rtt_drops;
64
65 #endif
66 #endif
67
68 #if MYNEWT_VAL(BLE_MONITOR_UART)
inc_and_wrap(int i,int max)69 static inline int inc_and_wrap(int i, int max)
70 {
71 return (i + 1) & (max - 1);
72 }
73
monitor_uart_rx_discard(void * arg,uint8_t ch)74 static int monitor_uart_rx_discard(void *arg, uint8_t ch)
75 {
76 return 0;
77 }
78
monitor_uart_tx_char(void * arg)79 static int monitor_uart_tx_char(void *arg)
80 {
81 uint8_t ch;
82
83 /* No more data */
84 if (tx_ringbuf_head == tx_ringbuf_tail) {
85 return -1;
86 }
87
88 ch = tx_ringbuf[tx_ringbuf_tail];
89 tx_ringbuf_tail = inc_and_wrap(tx_ringbuf_tail, sizeof(tx_ringbuf));
90 return ch;
91 }
92
monitor_uart_queue_char(uint8_t ch)93 static void monitor_uart_queue_char(uint8_t ch)
94 {
95 int sr;
96 OS_ENTER_CRITICAL(sr);
97
98 /* We need to try flush some data from ringbuffer if full */
99 while (inc_and_wrap(tx_ringbuf_head, sizeof(tx_ringbuf)) ==
100 tx_ringbuf_tail) {
101 uart_start_tx(uart);
102 OS_EXIT_CRITICAL(sr);
103
104 if (os_started()) {
105 os_time_delay(1);
106 }
107
108 OS_ENTER_CRITICAL(sr);
109 }
110
111 tx_ringbuf[tx_ringbuf_head] = ch;
112 tx_ringbuf_head = inc_and_wrap(tx_ringbuf_head, sizeof(tx_ringbuf));
113 OS_EXIT_CRITICAL(sr);
114 }
115
monitor_write(const void * buf,size_t len)116 static void monitor_write(const void *buf, size_t len)
117 {
118 const uint8_t *ch = buf;
119
120 while (len--) {
121 monitor_uart_queue_char(*ch++);
122 }
123
124 uart_start_tx(uart);
125 }
126 #endif
127
128 #if MYNEWT_VAL(BLE_MONITOR_RTT)
129
130 #if MYNEWT_VAL(BLE_MONITOR_RTT_BUFFERED)
update_drop_counters(struct ble_monitor_hdr * failed_hdr)131 static void update_drop_counters(struct ble_monitor_hdr *failed_hdr)
132 {
133 uint8_t *cnt;
134 rtt_drops.dropped = true;
135
136 switch (failed_hdr->opcode) {
137 case BLE_MONITOR_OPCODE_COMMAND_PKT:
138 cnt = &rtt_drops.drops_hdr.cmd;
139 break;
140
141 case BLE_MONITOR_OPCODE_EVENT_PKT:
142 cnt = &rtt_drops.drops_hdr.evt;
143 break;
144
145 case BLE_MONITOR_OPCODE_ACL_TX_PKT:
146 cnt = &rtt_drops.drops_hdr.acl_tx;
147 break;
148
149 case BLE_MONITOR_OPCODE_ACL_RX_PKT:
150 cnt = &rtt_drops.drops_hdr.acl_rx;
151 break;
152
153 default:
154 cnt = &rtt_drops.drops_hdr.other;
155 break;
156 }
157
158 if (*cnt < UINT8_MAX) {
159 (*cnt)++;
160 ble_npl_callout_reset(&rtt_drops.tmo, OS_TICKS_PER_SEC);
161 }
162 }
163
reset_drop_counters(void)164 static void reset_drop_counters(void)
165 {
166 rtt_drops.dropped = false;
167 rtt_drops.drops_hdr.cmd = 0;
168 rtt_drops.drops_hdr.evt = 0;
169 rtt_drops.drops_hdr.acl_tx = 0;
170 rtt_drops.drops_hdr.acl_rx = 0;
171 rtt_drops.drops_hdr.other = 0;
172 ble_npl_callout_stop(&rtt_drops.tmo);
173 }
174 #endif
175
monitor_write(const void * buf,size_t len)176 static void monitor_write(const void *buf, size_t len)
177 {
178 #if MYNEWT_VAL(BLE_MONITOR_RTT_BUFFERED)
179 struct ble_monitor_hdr *hdr = (struct ble_monitor_hdr *) rtt_pktbuf;
180 bool discard;
181 unsigned ret = 0;
182 /* We will discard any packet which exceeds length of intermediate buffer */
183 discard = rtt_pktbuf_pos + len > sizeof(rtt_pktbuf);
184 if (!discard) {
185 memcpy_s(rtt_pktbuf + rtt_pktbuf_pos, sizeof(rtt_pktbuf + rtt_pktbuf_pos), buf, len);
186 }
187
188 rtt_pktbuf_pos += len;
189 if (rtt_pktbuf_pos < sizeof(hdr->data_len) + hdr->data_len) {
190 return;
191 }
192
193 if (!discard) {
194 ret = SEGGER_RTT_WriteNoLock(rtt_index, rtt_pktbuf, rtt_pktbuf_pos);
195 }
196
197 if (ret > 0) {
198 reset_drop_counters();
199 } else {
200 update_drop_counters(hdr);
201 }
202
203 rtt_pktbuf_pos = 0;
204 #else
205 SEGGER_RTT_WriteNoLock(rtt_index, buf, len);
206 #endif
207 }
208 #endif
209
monitor_write_header(uint16_t opcode,uint16_t len)210 static void monitor_write_header(uint16_t opcode, uint16_t len)
211 {
212 struct ble_monitor_hdr hdr;
213 struct ble_monitor_ts_hdr ts_hdr;
214 uint8_t hdr_len;
215 int64_t ts;
216 hdr_len = sizeof(ts_hdr);
217 #if MYNEWT_VAL(BLE_MONITOR_RTT) && MYNEWT_VAL(BLE_MONITOR_RTT_BUFFERED)
218
219 if (rtt_drops.dropped) {
220 hdr_len += sizeof(rtt_drops.drops_hdr);
221 }
222
223 #endif
224 hdr.data_len = htole16(4 + hdr_len + len); // 4:byte alignment
225 hdr.hdr_len = hdr_len;
226 hdr.opcode = htole16(opcode);
227 hdr.flags = 0;
228 /* Use uptime for timestamp */
229 ts = os_get_uptime_usec();
230 /*
231 * btsnoop specification states that fields of extended header must be
232 * sorted in increasing order so we will send drops (if any) headers before
233 * timestamp header.
234 */
235 monitor_write(&hdr, sizeof(hdr));
236 #if MYNEWT_VAL(BLE_MONITOR_RTT) && MYNEWT_VAL(BLE_MONITOR_RTT_BUFFERED)
237
238 if (rtt_drops.dropped) {
239 monitor_write(&rtt_drops.drops_hdr, sizeof(rtt_drops.drops_hdr));
240 }
241
242 #endif
243 ts_hdr.type = BLE_MONITOR_EXTHDR_TS32;
244 ts_hdr.ts32 = htole32(ts / 100); // 100:byte alignment
245 monitor_write(&ts_hdr, sizeof(ts_hdr));
246 }
247
btmon_write(FILE * instance,const char * bp,size_t n)248 static size_t btmon_write(FILE *instance, const char *bp, size_t n)
249 {
250 monitor_write(bp, n);
251 return n;
252 }
253
254 static FILE *btmon = (FILE *) & (struct File) {
255 .vmt = &(struct File_methods) {
256 .write = btmon_write,
257 },
258 };
259
260 #if MYNEWT_VAL(BLE_MONITOR_RTT) && MYNEWT_VAL(BLE_MONITOR_RTT_BUFFERED)
drops_tmp_cb(struct ble_npl_event * ev)261 static void drops_tmp_cb(struct ble_npl_event *ev)
262 {
263 ble_npl_mutex_pend(&lock, OS_TIMEOUT_NEVER);
264 /*
265 * There's no "nop" in btsnoop protocol so we just send empty system note
266 * to indicate drops.
267 */
268 monitor_write_header(BLE_MONITOR_OPCODE_SYSTEM_NOTE, 1);
269 monitor_write("", 1);
270 ble_npl_mutex_release(&lock);
271 }
272 #endif
273
ble_monitor_init(void)274 int ble_monitor_init(void)
275 {
276 #if MYNEWT_VAL(BLE_MONITOR_UART)
277 struct uart_conf uc = {
278 .uc_speed = MYNEWT_VAL(BLE_MONITOR_UART_BAUDRATE),
279 .uc_databits = 8,
280 .uc_stopbits = 1,
281 .uc_parity = UART_PARITY_NONE,
282 .uc_flow_ctl = UART_FLOW_CTL_NONE,
283 .uc_tx_char = monitor_uart_tx_char,
284 .uc_rx_char = monitor_uart_rx_discard,
285 .uc_cb_arg = NULL,
286 };
287 uart = (struct uart_dev *)os_dev_open(MYNEWT_VAL(BLE_MONITOR_UART_DEV),
288 OS_TIMEOUT_NEVER, &uc);
289 if (!uart) {
290 return -1;
291 }
292
293 #endif
294 #if MYNEWT_VAL(BLE_MONITOR_RTT)
295 #if MYNEWT_VAL(BLE_MONITOR_RTT_BUFFERED)
296 ble_npl_callout_init(&rtt_drops.tmo, ble_hs_evq_get(), drops_tmp_cb, NULL);
297 /* Initialize types in header (we won't touch them later) */
298 rtt_drops.drops_hdr.type_cmd = BLE_MONITOR_EXTHDR_COMMAND_DROPS;
299 rtt_drops.drops_hdr.type_evt = BLE_MONITOR_EXTHDR_EVENT_DROPS;
300 rtt_drops.drops_hdr.type_acl_tx = BLE_MONITOR_EXTHDR_ACL_TX_DROPS;
301 rtt_drops.drops_hdr.type_acl_rx = BLE_MONITOR_EXTHDR_ACL_RX_DROPS;
302 rtt_drops.drops_hdr.type_other = BLE_MONITOR_EXTHDR_OTHER_DROPS;
303 rtt_index = SEGGER_RTT_AllocUpBuffer(MYNEWT_VAL(BLE_MONITOR_RTT_BUFFER_NAME),
304 rtt_buf, sizeof(rtt_buf),
305 SEGGER_RTT_MODE_NO_BLOCK_SKIP);
306 #else
307 rtt_index = SEGGER_RTT_AllocUpBuffer(MYNEWT_VAL(BLE_MONITOR_RTT_BUFFER_NAME),
308 rtt_buf, sizeof(rtt_buf),
309 SEGGER_RTT_MODE_BLOCK_IF_FIFO_FULL);
310 #endif
311 if (rtt_index < 0) {
312 return -1;
313 }
314
315 #endif
316 ble_npl_mutex_init(&lock);
317 return 0;
318 }
319
ble_monitor_send(uint16_t opcode,const void * data,size_t len)320 int ble_monitor_send(uint16_t opcode, const void *data, size_t len)
321 {
322 ble_npl_mutex_pend(&lock, OS_TIMEOUT_NEVER);
323 monitor_write_header(opcode, len);
324 monitor_write(data, len);
325 ble_npl_mutex_release(&lock);
326 return 0;
327 }
328
ble_monitor_send_om(uint16_t opcode,const struct os_mbuf * om)329 int ble_monitor_send_om(uint16_t opcode, const struct os_mbuf *om)
330 {
331 const struct os_mbuf *om_tmp;
332 uint16_t length = 0;
333 om_tmp = om;
334 while (om_tmp) {
335 length += om_tmp->om_len;
336 om_tmp = SLIST_NEXT(om_tmp, om_next);
337 }
338
339 ble_npl_mutex_pend(&lock, OS_TIMEOUT_NEVER);
340 monitor_write_header(opcode, length);
341
342 while (om) {
343 monitor_write(om->om_data, om->om_len);
344 om = SLIST_NEXT(om, om_next);
345 }
346
347 ble_npl_mutex_release(&lock);
348 return 0;
349 }
350
ble_monitor_new_index(uint8_t bus,uint8_t * addr,const char * name)351 int ble_monitor_new_index(uint8_t bus, uint8_t *addr, const char *name)
352 {
353 struct ble_monitor_new_index pkt;
354 pkt.type = 0; /* Primary controller, we don't support other */
355 pkt.bus = bus;
356 memcpy_s(pkt.bdaddr, sizeof(pkt.bdaddr), addr, 6); // 6:size
357 strncpy_s(pkt.name, sizeof(pkt.name), name, sizeof(pkt.name) - 1);
358 pkt.name[sizeof(pkt.name) - 1] = '\0';
359 ble_monitor_send(BLE_MONITOR_OPCODE_NEW_INDEX, &pkt, sizeof(pkt));
360 return 0;
361 }
362
ble_monitor_log(int level,const char * fmt,...)363 int ble_monitor_log(int level, const char *fmt, ...)
364 {
365 static const char id[] = "nimble";
366 struct ble_monitor_user_logging ulog;
367 va_list va;
368 int len;
369 va_start(va, fmt);
370 len = vsnprintf_s(NULL, 0, 0, fmt, va);
371 va_end(va);
372
373 switch (level) {
374 case LOG_LEVEL_ERROR:
375 ulog.priority = 3; // 3:byte alignment
376 break;
377
378 case LOG_LEVEL_WARN:
379 ulog.priority = 4; // 4:byte alignment
380 break;
381
382 case LOG_LEVEL_INFO:
383 ulog.priority = 6; // 6:byte alignment
384 break;
385
386 case LOG_LEVEL_DEBUG:
387 ulog.priority = 7; // 7:byte alignment
388 break;
389
390 default:
391 ulog.priority = 8; // 8:byte alignment
392 break;
393 }
394
395 ulog.ident_len = sizeof(id);
396 ble_npl_mutex_pend(&lock, OS_TIMEOUT_NEVER);
397 monitor_write_header(BLE_MONITOR_OPCODE_USER_LOGGING,
398 sizeof(ulog) + sizeof(id) + len + 1);
399 monitor_write(&ulog, sizeof(ulog));
400 monitor_write(id, sizeof(id));
401 va_start(va, fmt);
402 int ret = vfprintf(btmon, fmt, va);
403 if (ret == -1) {
404 return -1;
405 }
406 va_end(va);
407 /* null-terminate string */
408 monitor_write("", 1);
409 ble_npl_mutex_release(&lock);
410 return 0;
411 }
412
ble_monitor_out(int c)413 int ble_monitor_out(int c)
414 {
415 static char buf[MYNEWT_VAL(BLE_MONITOR_CONSOLE_BUFFER_SIZE)];
416 static size_t len;
417
418 if (c != '\n') {
419 buf[len++] = c;
420
421 if (len < sizeof(buf) - 1) {
422 return c;
423 }
424 }
425
426 buf[len++] = '\0';
427 ble_monitor_send(BLE_MONITOR_OPCODE_SYSTEM_NOTE, buf, len);
428 len = 0;
429 return c;
430 }
431
432 #endif