size_t debugPrint(const char *fmt, ...)
/*
printf style debugging output
outputs a trailing newline
*/
{
va_list ap;
va_start(ap, fmt);
NullStream lenStream = {&nullVmt, 0};
chvprintf((BaseSequentialStream *) &lenStream, fmt, ap);
size_t len = lenStream.len;
if (len) {
chMtxLock(&debugOutLock);
size_t qspace = chOQGetEmptyI(&debugOutQ);
if (qspace) {
if (len >= qspace)
len = qspace - 1; //truncate string if it won't fit in queue
if (len) {
if (len > 255)
len = 255;
qStream dbgStream = {&qVmt, len};
chOQPutTimeout(&debugOutQ, len, TIME_IMMEDIATE);
chvprintf((BaseSequentialStream *) &dbgStream, fmt, ap);
resumeReader();
len++;
}
}else
len=0;
chMtxUnlock();
}else
if (debugPutc('\n') >= 0)
len=1;
va_end(ap);
return len;
}
size_t debugPut(const uint8_t *block, size_t n)
/*
truncate any block > 255 bytes
returns # of characters actually output (including the trailing newline)
*/
{
if (n) {
chMtxLock(&debugOutLock);
size_t space = chOQGetEmptyI(&debugOutQ);
if (space) {
if (n >= space)
n = space - 1;
if (n) {
if (n > 255)
n = 255;
chOQPutTimeout( &debugOutQ, n, TIME_IMMEDIATE);
chOQWriteTimeout( &debugOutQ, block, n, TIME_IMMEDIATE);
resumeReader();
n++;
}
}else
n = 0;
chMtxUnlock();
}else
if (debugPutc('\n') >= 0)
n = 1;
return n;
}
// Send a single 4 bit value to the RS232 port as a hex digit
void hex_digit_send( unsigned char c )
{
c &= 0x0F ;
if ( c > 9 )
{
c += 7 ;
}
c += '0' ;
debugPutc( c ) ;
}
// Outputs a string to the UART
void debugPuts(const char *format, ...)
{
va_list arglist;
char tmp[256];
va_start(arglist, format);
vsnprintf(tmp, 256, format, arglist);
va_end(arglist);
const char *t = tmp;
while (*t) {
debugPutc(*t++);
}
}
// Outputs a string to the UART
void debugPuts(const char *format, ...)
{
va_list arglist;
char tmp[PRINTF_BUFFER_SIZE];
va_start(arglist, format);
vsnprintf(tmp, PRINTF_BUFFER_SIZE, format, arglist);
va_end(arglist);
#if defined(SIMU)
fputs(tmp, stdout);
fflush(stdout);
if (traceCallback) {
traceCallback(tmp);
}
#else
const char *t = tmp;
while (*t) {
debugPutc(*t++);
}
#endif
}
void crlf()
{
debugPutc( 13 ) ;
debugPutc( 10 ) ;
}