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