diff --git a/src/hos_printf.c b/src/hos_printf.c index 681d3b6..6013d53 100644 --- a/src/hos_printf.c +++ b/src/hos_printf.c @@ -60,11 +60,38 @@ static size_t format_hex(char * buffer, int v, bool upper) return sz; } +static void pad_write(const stream_t * stream, const char * data, size_t length, bool leading_zero, bool left_just, size_t width) +{ + if (left_just) + { + stream->write(data, length); + while (length < width) + { + stream->write1(' '); + length++; + } + } + else + { + char fill = leading_zero ? '0' : ' '; + size_t l = length; + while (l < width) + { + stream->write1(fill); + l++; + } + stream->write(data, length); + } +} + void hos_vprintf(const stream_t * stream, const char * fmt, va_list va) { bool in_conv = false; char c; char buffer[12]; + size_t width; + bool leading_zero; + bool left_just; while ((c = *fmt)) { if (in_conv) @@ -78,40 +105,40 @@ void hos_vprintf(const stream_t * stream, const char * fmt, va_list va) { unsigned int v = va_arg(va, unsigned int); size_t length = format_hex(buffer, v, true); - stream->write(buffer, length); + pad_write(stream, buffer, length, leading_zero, left_just, width); } break; case 'c': { char ch = va_arg(va, int); - stream->write1(ch); + pad_write(stream, &ch, 1u, leading_zero, left_just, width); } break; case 'd': { int v = va_arg(va, int); size_t length = format_dec(buffer, v); - stream->write(buffer, length); + pad_write(stream, buffer, length, leading_zero, left_just, width); } break; case 's': { const char * s = va_arg(va, const char *); - stream->write(s, strlen(s)); + pad_write(stream, s, strlen(s), leading_zero, left_just, width); } break; case 'u': { unsigned int v = va_arg(va, unsigned int); size_t length = format_udec(buffer, v); - stream->write(buffer, length); + pad_write(stream, buffer, length, leading_zero, left_just, width); } break; case 'x': { unsigned int v = va_arg(va, unsigned int); size_t length = format_hex(buffer, v, false); - stream->write(buffer, length); + pad_write(stream, buffer, length, leading_zero, left_just, width); } break; } @@ -120,6 +147,25 @@ void hos_vprintf(const stream_t * stream, const char * fmt, va_list va) else if (c == '%') { in_conv = true; + width = 0u; + leading_zero = false; + left_just = false; + if (fmt[1] == '-') + { + left_just = true; + fmt++; + } + if (fmt[1] == '0') + { + leading_zero = true; + fmt++; + } + while (('0' <= fmt[1]) && (fmt[1] <= '9')) + { + width *= 10u; + width += (fmt[1] - '0'); + fmt++; + } } else {