int usbprintf(USBSerial& usbs, const char* format, ...) { int32_t* argp = (int32_t*)&format; int num = 0; while(*format) { if(*format != '%') { usbs.putc(*format++); num++; continue; } format++; bool is_signed = false; bool zero_pad = false; int radix = 16; int min_len = 0; if(*format == '0') { zero_pad = true; } while(*format >= '0' && *format <= '9') { min_len = min_len * 10 + *format++ - '0'; } switch(*format++) { case '%': usbs.putc('%'); num++; break; case 'c': usbs.putc((char)*++argp); num++; break; case 's': { char* str = (char*)*++argp; while(*str) { usbs.putc(*str++); num++; } } break; case 'd': is_signed = true; case 'u': radix = 10; case 'x': ; uint32_t x = (uint32_t)*(++argp); if(is_signed && (int32_t)x < 0) { x = -x; usbs.putc('-'); min_len--; } char buf[11]; char* bufp = &buf[sizeof(buf)]; *--bufp = 0; do { int d = x % radix; *--bufp = d < 10 ? '0' + d : 'a' - 10 + d; x /= radix; min_len--; } while(x); while(min_len > 0) { usbs.putc(zero_pad ? '0' : ' '); num++; min_len--; } while(*bufp) { usbs.putc(*bufp++); num++; } break; } } return num; }