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;
}
int debugPutc(int c)
/*
returns -1 if output fails
*/
{
chMtxLock(&debugOutLock);
if (chOQGetEmptyI(&debugOutQ) > 1) {
chOQPutTimeout( &debugOutQ, 0, TIME_IMMEDIATE);
chOQPutTimeout( &debugOutQ, c, TIME_IMMEDIATE);
}else
c = -1;
resumeReader();
chMtxUnlock();
return c;
}
size_t OutQueue::getEmptyI(void) {
return chOQGetEmptyI(&oq);
}
