aboutsummaryrefslogtreecommitdiffstats
path: root/os/hal/src/usbh/hal_usbh_debug.c
diff options
context:
space:
mode:
Diffstat (limited to 'os/hal/src/usbh/hal_usbh_debug.c')
-rw-r--r--os/hal/src/usbh/hal_usbh_debug.c664
1 files changed, 145 insertions, 519 deletions
diff --git a/os/hal/src/usbh/hal_usbh_debug.c b/os/hal/src/usbh/hal_usbh_debug.c
index 57df535..225489d 100644
--- a/os/hal/src/usbh/hal_usbh_debug.c
+++ b/os/hal/src/usbh/hal_usbh_debug.c
@@ -21,573 +21,199 @@
#include "ch.h"
#include "usbh/debug.h"
-#include <stdarg.h>
-#if 0
-#include "debug.h"
-#else
#include "chprintf.h"
-#define dbg_lock()
-#define dbg_unlock()
-#endif
-
-#define MAX_FILLER 11
-#define FLOAT_PRECISION 9
-#define MPRINTF_USE_FLOAT 0
-
-static char *long_to_string_with_divisor(char *p, long num, unsigned radix, long divisor)
-{
- int i;
- char *q;
- long l, ll;
+#include <stdarg.h>
- l = num;
- if (divisor == 0) {
- ll = num;
- } else {
- ll = divisor;
+#define TEMP_BUFF_LEN 255
+
+/* ************************ */
+/* Circular queue structure */
+/* ************************ */
+static int dq_append_string(usbh_dq_t *q, const uint8_t *s, int len) {
+ if (len <= 0) return 0;
+ if (len > TEMP_BUFF_LEN) len = TEMP_BUFF_LEN;
+ if (q->rem < len + 1) return -1;
+ q->rem -= len + 1;
+
+ uint8_t *d = q->next;
+ *d++ = len;
+ if (d == q->end) d = q->start;
+ while (len--) {
+ *d++ = *s++;
+ if (d == q->end) d = q->start;
}
-
- q = p + MAX_FILLER;
- do {
- i = (int)(l % radix);
- i += '0';
- if (i > '9') {
- i += 'A' - '0' - 10;
- }
- *--q = i;
- l /= radix;
- } while ((ll /= radix) != 0);
-
- i = (int)(p + MAX_FILLER - q);
- do {
- *p++ = *q++;
- } while (--i);
-
- return p;
-}
-
-static char *ltoa(char *p, long num, unsigned radix) {
-
- return long_to_string_with_divisor(p, num, radix, 0);
+ q->next = d;
+ return 0;
}
-#if MPRINTF_USE_FLOAT
-static const long _pow10[FLOAT_PRECISION] = {10, 100, 1000, 10000, 100000, 1000000,
- 10000000, 100000000, 1000000000};
-static const double m10[FLOAT_PRECISION] = {5.0/100, 5.0/1000, 5.0/10000, 5.0/100000, 5.0/1000000,
- 5.0/10000000, 5.0/100000000, 5.0/1000000000, 5.0/10000000000};
-
-static char *ftoa(char *p, double num, unsigned long precision, bool dot) {
- long l;
- char *q;
- double r;
-
-
- if (precision == 0) {
- l = (long)(num + 0.5);
- return long_to_string_with_divisor(p, l, 10, 0);
- } else {
- if (precision > FLOAT_PRECISION) precision = FLOAT_PRECISION;
- r = m10[precision - 1];
- precision = _pow10[precision - 1];
-
- l = (long)num;
- p = long_to_string_with_divisor(p, l, 10, 0);
- if (dot) *p++ = '.';
- l = (long)((num - l + r) * precision);
- q = long_to_string_with_divisor(p, l, 10, precision / 10) - 1;
-
- while (q > p) {
- if (*q != '0') {
- break;
- }
- --q;
+static void dq_remove_oldest_string(usbh_dq_t *q) {
+ int len = *q->first;
+ if (len) {
+ ++len;
+ q->rem += len;
+ q->first += len;
+ if (q->first >= q->end) {
+ q->first -= q->sz;
+ }
+ if (q->rem == q->sz) {
+ *q->first = 0;
}
- return ++q;
}
-
-
-
-
}
-#endif
-static inline void _wr(input_queue_t *iqp, char c) {
- *iqp->q_wrptr++ = c;
- if (iqp->q_wrptr >= iqp->q_top)
- iqp->q_wrptr = iqp->q_buffer;
+static int dq_read_oldest_string(usbh_dq_t *q, uint8_t *d) {
+ uint8_t *s = q->first;
+ int len;
+ int sz;
+ len = sz = *s++;
+ while (len--) {
+ *d++ = *s++;
+ if (d == q->end) d = q->start;
+ }
+ *d = 0;
+ return sz;
}
-static inline void _put(char c) {
- input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;
- if (sizeof(USBH_DEBUG_USBHD.dbg_buff) - iqp->q_counter <= 1)
- return;
- iqp->q_counter++;
- _wr(iqp, c);
+static void dq_init(usbh_dq_t *q, uint8_t *buff, int len) {
+ q->start = q->first = q->next = buff;
+ q->end = q->start + len;
+ q->sz = q->rem = len;
+ *buff = 0;
}
-int _dbg_printf(const char *fmt, va_list ap) {
- char *p, *s, c, filler;
- int i, precision, width;
- int n = 0;
- bool is_long, left_align, sign;
- long l;
-#if MPRINTF_USE_FLOAT
- double f;
- char tmpbuf[2*MAX_FILLER + 1];
-#else
- char tmpbuf[MAX_FILLER + 1];
-#endif
-
- for (;;) {
-
- //agarrar nuevo caracter de formato
- c = *fmt++;
- //chequeo eos
- if (c == 0) return n;
+static uint8_t buff[TEMP_BUFF_LEN + 1];
- //copio los caracteres comunes
- if (c != '%') {
- _put(c);
- n++;
- continue;
- }
-
- //encontré un '%'
- p = tmpbuf;
- s = tmpbuf;
-
- //left align
- left_align = FALSE;
- if (*fmt == '-') {
- fmt++;
- left_align = TRUE;
- }
-
- sign = FALSE;
- if (*fmt == '+') {
- fmt++;
- sign = TRUE;
- }
-
- //filler
- filler = ' ';
- if (*fmt == '0') {
- fmt++;
- filler = '0';
- }
-
- //width
- width = 0;
- while (TRUE) {
- c = *fmt++;
- if (c >= '0' && c <= '9')
- c -= '0';
- else if (c == '*')
- c = va_arg(ap, int);
- else
- break;
- width = width * 10 + c;
- }
-
- //precision
- precision = 0;
- if (c == '.') {
-
- if (*fmt == 'n') {
- fmt++;
-
- }
- while (TRUE) {
- c = *fmt++;
- if (c >= '0' && c <= '9')
- c -= '0';
- else if (c == '*')
- c = va_arg(ap, int);
- else
- break;
- precision = precision * 10 + c;
- }
- }
-
- //long modifier
- if (c == 'l' || c == 'L') {
- is_long = TRUE;
- if (*fmt)
- c = *fmt++;
- }
- else
- is_long = (c >= 'A') && (c <= 'Z');
-
- /* Command decoding.*/
- switch (c) {
- //char
- case 'c':
- filler = ' ';
- *p++ = va_arg(ap, int);
- break;
-
- //string
- case 's':
- filler = ' ';
- if ((s = va_arg(ap, char *)) == 0)
- s = (char *)"(null)";
- if (precision == 0)
- precision = 32767;
-
- //strlen con límite hasta precision
- for (p = s; *p && (--precision >= 0); p++)
- ;
- break;
-
-
-
- case 'D':
- case 'd':
- case 'I':
- case 'i':
- if (is_long)
- l = va_arg(ap, long);
- else
- l = va_arg(ap, int);
- if (l < 0) {
- *p++ = '-';
- l = -l;
- sign = TRUE;
- } else if (sign) {
- *p++ = '+';
- }
- p = ltoa(p, l, 10);
- break;
-
-#if MPRINTF_USE_FLOAT
- case 'f':
- f = va_arg(ap, double);
- if (f < 0) {
- *p++ = '-';
- f = -f;
- sign = TRUE;
- } else if (sign) {
- *p++ = '+';
- }
- if (prec == FALSE) precision = 6;
- p = ftoa(p, f, precision, dot);
- break;
-#endif
-
-
- case 'X':
- case 'x':
- c = 16;
- goto unsigned_common;
- case 'U':
- case 'u':
- c = 10;
- goto unsigned_common;
- case 'O':
- case 'o':
- c = 8;
-
-unsigned_common:
- if (is_long)
- l = va_arg(ap, unsigned long);
- else
- l = va_arg(ap, unsigned int);
- p = ltoa(p, l, c);
- break;
-
- //copiar
- default:
- *p++ = c;
- break;
- }
-
- //longitud
- i = (int)(p - s);
-
- //calculo cuántos caracteres de filler debo poner
- if ((width -= i) < 0)
- width = 0;
-
- if (left_align == FALSE)
- width = -width;
-
- if (width < 0) {
- //alineado a la derecha
-
- //poner el signo adelante
- if (sign && filler == '0') {
- _put(*s++);
- n++;
- i--;
- }
-
- //fill a la izquierda
- do {
- _put(filler);
- n++;
- } while (++width != 0);
- }
-
- //copiar los caracteres
- while (--i >= 0) {
- _put(*s++);
- n++;
- }
+static inline syssts_t _dbg_prologue(struct usbh_debug_helper *debug,
+ uint32_t hfnum, uint16_t hfir, const char *s, int *len) {
+ syssts_t sts = chSysGetStatusAndLockX();
- //fill a la derecha
- while (width) {
- _put(filler);
- n++;
- width--;
- }
+ debug->last = osalOsGetSystemTimeX();
+ if (debug->ena) {
+ debug->first = debug->last;
}
- //return n; // can raise 'code is unreachable' warning
+ if (((hfnum & 0x3fff) == 0x3fff) && (hfir == (hfnum >> 16))) {
+ *len = chsnprintf((char *)buff, sizeof(buff), "+%08d ", debug->last - debug->first);
+ debug->ena = FALSE;
+ } else {
+ uint32_t f = hfnum & 0xffff;
+ uint32_t p = 1000 - ((hfnum >> 16) / (hfir / 1000));
+ *len = chsnprintf((char *)buff, sizeof(buff), "%05d.%03d %s", f, p, s);
+ debug->ena = TRUE;
+ }
+ return sts;
}
-static systime_t first, last;
-static bool ena;
-static uint32_t hdr[2];
+static inline void dbg_epilogue(struct usbh_debug_helper *debug,
+ syssts_t sts, int len) {
-static void _build_hdr(void) {
- uint32_t hfnum = USBH_DEBUG_USBHD.otg->HFNUM;
- uint16_t hfir = USBH_DEBUG_USBHD.otg->HFIR;
- last = osalOsGetSystemTimeX();
- if (ena) {
- first = last;
+ while (dq_append_string(&debug->dq, buff, len) < 0) {
+ dq_remove_oldest_string(&debug->dq);
}
- if (((hfnum & 0x3fff) == 0x3fff) && (hfir == (hfnum >> 16))) {
- hdr[0] = 0xfeff;
- hdr[1] = last - first;
- ena = FALSE;
- } else {
- hdr[0] = 0xffff | (hfir << 16);
- hdr[1] = hfnum;
- ena = TRUE;
+ if (debug->on) {
+ chThdResumeI(&debug->tr, MSG_OK);
}
-}
-static void _print_hdr(void)
-{
- _put(hdr[0] & 0xff);
- _put((hdr[0] >> 8) & 0xff);
- _put((hdr[0] >> 16) & 0xff);
- _put((hdr[0] >> 24) & 0xff);
- _put(hdr[1] & 0xff);
- _put((hdr[1] >> 8) & 0xff);
- _put((hdr[1] >> 16) & 0xff);
- _put((hdr[1] >> 24) & 0xff);
-}
-
-void usbDbgPrintf(const char *fmt, ...)
-{
- va_list ap;
- va_start(ap, fmt);
- syssts_t sts = chSysGetStatusAndLockX();
- input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;
- int rem = sizeof(USBH_DEBUG_USBHD.dbg_buff) - iqp->q_counter;
- if (rem >= 9) {
- _build_hdr();
- _print_hdr();
- _dbg_printf(fmt, ap);
- iqp->q_counter++;
- _wr(iqp, 0);
- chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);
- }
chSysRestoreStatusX(sts);
- if (!port_is_isr_context() && chSchIsPreemptionRequired()) {
- chSchRescheduleS();
- }
- va_end(ap);
}
+#if USBH_DEBUG_MULTI_HOST
+void usbDbgPrintf(USBHDriver *host, const char *fmt, ...) {
+ if (!host) return;
+ struct usbh_debug_helper *const debug = &host->debug;
+ uint32_t hfnum = host->otg->HFNUM;
+ uint16_t hfir = host->otg->HFIR;
+#else
+void usbDbgPrintf(const char *fmt, ...) {
+ struct usbh_debug_helper *const debug = &usbh_debug;
+ uint32_t hfnum = USBH_DEBUG_SINGLE_HOST_SELECTION.otg->HFNUM;
+ uint16_t hfir = USBH_DEBUG_SINGLE_HOST_SELECTION.otg->HFIR;
+#endif
+ int len;
-void usbDbgPuts(const char *s)
-{
- _build_hdr();
- uint8_t *p = (uint8_t *)hdr;
- uint8_t *top = p + 8;
+ syssts_t sts = _dbg_prologue(debug, hfnum, hfir, "", &len);
- syssts_t sts = chSysGetStatusAndLockX();
- input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;
- int rem = sizeof(USBH_DEBUG_USBHD.dbg_buff) - iqp->q_counter;
- if (rem >= 9) {
- while (rem) {
- _wr(iqp, *p);
- if (++p == top) break;
- }
- rem -= 9;
- while (rem && *s) {
- _wr(iqp, *s);
- rem--;
- s++;
- }
- _wr(iqp, 0);
- iqp->q_counter = sizeof(USBH_DEBUG_USBHD.dbg_buff) - rem;
- chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);
- }
- chSysRestoreStatusX(sts);
- if (!port_is_isr_context() && chSchIsPreemptionRequired()) {
- chSchRescheduleS();
- }
-}
-
-void usbDbgReset(void) {
- const char *msg = "\r\n\r\n==== DEBUG OUTPUT RESET ====\r\n";
+ va_list ap;
+ va_start(ap, fmt);
+ len += chvsnprintf((char *)buff + len, sizeof(buff) - len, fmt, ap);
+ va_end(ap);
- syssts_t sts = chSysGetStatusAndLockX();
- iqResetI(&USBH_DEBUG_USBHD.iq);
- oqResetI(&USBH_DEBUG_SD.oqueue);
- while (*msg) {
- *USBH_DEBUG_SD.oqueue.q_wrptr++ = *msg++;
- USBH_DEBUG_SD.oqueue.q_counter--;
- }
- chSysRestoreStatusX(sts);
- if (!port_is_isr_context() && chSchIsPreemptionRequired()) {
- chSchRescheduleS();
- }
+ dbg_epilogue(debug, sts, len);
}
-static int _get(void) {
- if (!USBH_DEBUG_USBHD.iq.q_counter) return -1;
- USBH_DEBUG_USBHD.iq.q_counter--;
- uint8_t b = *USBH_DEBUG_USBHD.iq.q_rdptr++;
- if (USBH_DEBUG_USBHD.iq.q_rdptr >= USBH_DEBUG_USBHD.iq.q_top) {
- USBH_DEBUG_USBHD.iq.q_rdptr = USBH_DEBUG_USBHD.iq.q_buffer;
- }
- return b;
+#if USBH_DEBUG_MULTI_HOST
+void usbDbgPuts(USBHDriver *host, const char *s) {
+ if (!host) return;
+ struct usbh_debug_helper *const debug = &host->debug;
+ uint32_t hfnum = host->otg->HFNUM;
+ uint16_t hfir = host->otg->HFIR;
+#else
+void usbDbgPuts(const char *s) {
+ struct usbh_debug_helper *const debug = &usbh_debug;
+ uint32_t hfnum = USBH_DEBUG_SINGLE_HOST_SELECTION.otg->HFNUM;
+ uint16_t hfir = USBH_DEBUG_SINGLE_HOST_SELECTION.otg->HFIR;
+#endif
+ int len;
+ syssts_t sts = _dbg_prologue(debug, hfnum, hfir, s, &len);
+ dbg_epilogue(debug, sts, len);
}
-void usbDbgSystemHalted(void) {
- while (true) {
- if (!((bool)((USBH_DEBUG_SD.oqueue.q_wrptr == USBH_DEBUG_SD.oqueue.q_rdptr) && (USBH_DEBUG_SD.oqueue.q_counter != 0U))))
- break;
- USBH_DEBUG_SD.oqueue.q_counter++;
- while (!(USBH_DEBUG_SD.usart->SR & USART_SR_TXE));
- USBH_DEBUG_SD.usart->DR = *USBH_DEBUG_SD.oqueue.q_rdptr++;
- if (USBH_DEBUG_SD.oqueue.q_rdptr >= USBH_DEBUG_SD.oqueue.q_top) {
- USBH_DEBUG_SD.oqueue.q_rdptr = USBH_DEBUG_SD.oqueue.q_buffer;
- }
- }
-
- int c;
- int state = 0;
- for (;;) {
- c = _get(); if (c < 0) break;
-
- if (state == 0) {
- if (c == 0xff) state = 1;
- } else if (state == 1) {
- if (c == 0xff) state = 2;
- else (state = 0);
- } else {
- c = _get(); if (c < 0) return;
- c = _get(); if (c < 0) return;
- c = _get(); if (c < 0) return;
- c = _get(); if (c < 0) return;
- c = _get(); if (c < 0) return;
-
- while (true) {
- c = _get(); if (c < 0) return;
- if (!c) {
- while (!(USBH_DEBUG_SD.usart->SR & USART_SR_TXE));
- USBH_DEBUG_SD.usart->DR = '\r';
- while (!(USBH_DEBUG_SD.usart->SR & USART_SR_TXE));
- USBH_DEBUG_SD.usart->DR = '\n';
- state = 0;
- break;
- }
- while (!(USBH_DEBUG_SD.usart->SR & USART_SR_TXE));
- USBH_DEBUG_SD.usart->DR = c;
- }
- }
- }
+#if USBH_DEBUG_MULTI_HOST
+void usbDbgEnable(USBHDriver *host, bool enable) {
+ struct usbh_debug_helper *const debug = &host->debug;
+#else
+void usbDbgEnable(bool enable) {
+ struct usbh_debug_helper *const debug = &usbh_debug;
+#endif
+ debug->on = enable;
}
static void usb_debug_thread(void *arg) {
- USBHDriver *host = (USBHDriver *)arg;
- uint8_t state = 0;
+#if USBH_DEBUG_MULTI_HOST
+ USBHDriver *const host = (USBHDriver *)arg;
+ struct usbh_debug_helper *const debug = &host->debug;
+#else
+ (void)arg;
+ struct usbh_debug_helper *const debug = &usbh_debug;
+#endif
+
+ uint8_t rdbuff[TEMP_BUFF_LEN + 1];
chRegSetThreadName("USBH_DBG");
while (true) {
- msg_t c = iqGet(&host->iq);
- if (c < 0) goto reset;
-
- if (state == 0) {
- if (c == 0xff) state = 1;
- } else if (state == 1) {
- if (c == 0xff) state = 2;
- else if (c == 0xfe) state = 3;
- else (state = 0);
- } else if (state == 2) {
- uint16_t hfir;
- uint32_t hfnum;
-
- hfir = c;
- c = iqGet(&host->iq); if (c < 0) goto reset;
- hfir |= c << 8;
-
- c = iqGet(&host->iq); if (c < 0) goto reset;
- hfnum = c;
- c = iqGet(&host->iq); if (c < 0) goto reset;
- hfnum |= c << 8;
- c = iqGet(&host->iq); if (c < 0) goto reset;
- hfnum |= c << 16;
- c = iqGet(&host->iq); if (c < 0) goto reset;
- hfnum |= c << 24;
-
- uint32_t f = hfnum & 0xffff;
- uint32_t p = 1000 - ((hfnum >> 16) / (hfir / 1000));
- dbg_lock();
- chprintf((BaseSequentialStream *)&USBH_DEBUG_SD, "%05d.%03d ", f, p);
- state = 4;
- } else if (state == 3) {
- uint32_t t;
-
- c = iqGet(&host->iq); if (c < 0) goto reset;
- c = iqGet(&host->iq); if (c < 0) goto reset;
-
- t = c;
- c = iqGet(&host->iq); if (c < 0) goto reset;
- t |= c << 8;
- c = iqGet(&host->iq); if (c < 0) goto reset;
- t |= c << 16;
- c = iqGet(&host->iq); if (c < 0) goto reset;
- t |= c << 24;
-
- dbg_lock();
- chprintf((BaseSequentialStream *)&USBH_DEBUG_SD, "+%08d ", t);
- state = 4;
+ chSysLock();
+ int len = dq_read_oldest_string(&debug->dq, rdbuff);
+ if (!len) {
+ chThdSuspendS(&debug->tr);
+ chSysUnlock();
} else {
- while (true) {
- if (!c) {
- sdPut(&USBH_DEBUG_SD, '\r');
- sdPut(&USBH_DEBUG_SD, '\n');
- goto reset;
- }
- sdPut(&USBH_DEBUG_SD, (uint8_t)c);
- c = iqGet(&host->iq); if (c < 0) goto reset;
- }
- }
-
- continue;
-reset:
- if (state == 4) {
- dbg_unlock();
+ dq_remove_oldest_string(&debug->dq);
+ chSysUnlock();
+#if USBH_DEBUG_MULTI_HOST
+ USBH_DEBUG_OUTPUT_CALLBACK(host, rdbuff, len);
+#else
+ USBH_DEBUG_OUTPUT_CALLBACK(rdbuff, len);
+#endif
}
- state = 0;
}
}
+#if USBH_DEBUG_MULTI_HOST
void usbDbgInit(USBHDriver *host) {
- if (host != &USBH_DEBUG_USBHD)
- return;
- iqObjectInit(&USBH_DEBUG_USBHD.iq, USBH_DEBUG_USBHD.dbg_buff, sizeof(USBH_DEBUG_USBHD.dbg_buff), 0, 0);
- chThdCreateStatic(USBH_DEBUG_USBHD.waDebug, sizeof(USBH_DEBUG_USBHD.waDebug), NORMALPRIO, usb_debug_thread, &USBH_DEBUG_USBHD);
+ struct usbh_debug_helper *const debug = &host->debug;
+ void *param = host;
+#else
+void usbDbgInit(void) {
+ struct usbh_debug_helper *const debug = &usbh_debug;
+ void *param = NULL;
+#endif
+ dq_init(&debug->dq, debug->buff, sizeof(debug->buff));
+ debug->on = true;
+ chThdCreateStatic(debug->thd_wa, sizeof(debug->thd_wa),
+ NORMALPRIO, usb_debug_thread, param);
}
#endif
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-18 11:56:32 +0000