diff options
Diffstat (limited to 'os/hal/src/usbh/hal_usbh_debug.c')
-rw-r--r-- | os/hal/src/usbh/hal_usbh_debug.c | 664 |
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 |