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); }