hos/src-old/hos_printf.c

#include "hos_printf.h"
#include <stdint.h>
#include <stdbool.h>
#include "string.h"

static size_t format_dec(char * buffer, int32_t v)
{
    size_t sz = 0u;
    bool printing = false;
    if (v < 0)
    {
        buffer[sz++] = '-';
        v = -v;
    }
    for (int32_t div = 1000000000; div >= 1; div /= 10)
    {
        int32_t digit = v / div;
        v %= div;
        if ((digit != 0) || (div == 1))
        {
            printing = true;
        }
        if (printing)
        {
            buffer[sz++] = digit + '0';
        }
    }
    return sz;
}

static size_t format_dec64(char * buffer, int64_t v)
{
    size_t sz = 0u;
    bool printing = false;
    if (v < 0)
    {
        buffer[sz++] = '-';
        v = -v;
    }
    for (int64_t div = 1000000000000000000; div >= 1; div /= 10)
    {
        int64_t digit = v / div;
        v %= div;
        if ((digit != 0) || (div == 1))
        {
            printing = true;
        }
        if (printing)
        {
            buffer[sz++] = digit + '0';
        }
    }
    return sz;
}

static size_t format_udec(char * buffer, int32_t v)
{
    size_t sz = 0u;
    for (int32_t div = 1000000000u; div >= 1u; div /= 10u)
    {
        int32_t digit = v / div;
        v %= div;
        if ((digit != 0) || (sz > 0u) || (div == 1))
        {
            buffer[sz++] = digit + '0';
        }
    }
    return sz;
}

static size_t format_udec64(char * buffer, uint64_t v)
{
    size_t sz = 0u;
    for (uint64_t div = 10000000000000000000u; div >= 1u; div /= 10u)
    {
        uint64_t digit = v / div;
        v %= div;
        if ((digit != 0) || (sz > 0u) || (div == 1))
        {
            buffer[sz++] = digit + '0';
        }
    }
    return sz;
}

static size_t format_hex(char * buffer, uint32_t v, bool upper)
{
    const char upper_hex[] = "0123456789ABCDEF";
    const char lower_hex[] = "0123456789abcdef";
    size_t sz = 0u;
    for (int i = 28; i >= 0; i -= 4)
    {
        uint8_t n = (v >> i) & 0xFu;
        if ((sz > 0u) || (n != 0u) || (i == 0))
        {
            buffer[sz] = upper ? upper_hex[n] : lower_hex[n];
            sz++;
        }
    }
    return sz;
}

static size_t format_hex64(char * buffer, uint64_t v, bool upper)
{
    const char upper_hex[] = "0123456789ABCDEF";
    const char lower_hex[] = "0123456789abcdef";
    size_t sz = 0u;
    for (int i = 60; i >= 0; i -= 4)
    {
        uint8_t n = (v >> i) & 0xFu;
        if ((sz > 0u) || (n != 0u) || (i == 0))
        {
            buffer[sz] = upper ? upper_hex[n] : lower_hex[n];
            sz++;
        }
    }
    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[22];
    size_t width;
    bool leading_zero;
    bool left_just;
    bool long_flag;
    size_t length;
    while ((c = *fmt))
    {
        if (in_conv)
        {
            switch (c)
            {
            case '%':
                stream->write1('%');
                break;
            case 'X':
                {
                    if (long_flag)
                    {
                        uint64_t v = va_arg(va, uint64_t);
                        length = format_hex(buffer, v, true);
                    }
                    else
                    {
                        uint32_t v = va_arg(va, uint32_t);
                        length = format_hex(buffer, v, true);
                    }
                    pad_write(stream, buffer, length, leading_zero, left_just, width);
                }
                break;
            case 'c':
                {
                    char ch = va_arg(va, int);
                    pad_write(stream, &ch, 1u, leading_zero, left_just, width);
                }
                break;
            case 'd':
                {
                    if (long_flag)
                    {
                        int64_t v = va_arg(va, int64_t);
                        length = format_dec64(buffer, v);
                    }
                    else
                    {
                        int32_t v = va_arg(va, int32_t);
                        length = format_dec(buffer, v);
                    }
                    pad_write(stream, buffer, length, leading_zero, left_just, width);
                }
                break;
            case 's':
                {
                    const char * s = va_arg(va, const char *);
                    pad_write(stream, s, strlen(s), leading_zero, left_just, width);
                }
                break;
            case 'u':
                {
                    if (long_flag)
                    {
                        uint64_t v = va_arg(va, uint64_t);
                        length = format_udec64(buffer, v);
                    }
                    else
                    {
                        uint32_t v = va_arg(va, uint32_t);
                        length = format_udec(buffer, v);
                    }
                    pad_write(stream, buffer, length, leading_zero, left_just, width);
                }
                break;
            case 'x':
                {
                    if (long_flag)
                    {
                        uint64_t v = va_arg(va, uint64_t);
                        length = format_hex64(buffer, v, false);
                    }
                    else
                    {
                        uint32_t v = va_arg(va, uint32_t);
                        length = format_hex(buffer, v, false);
                    }
                    pad_write(stream, buffer, length, leading_zero, left_just, width);
                }
                break;
            }
            in_conv = false;
        }
        else if (c == '%')
        {
            in_conv = true;
            width = 0u;
            leading_zero = false;
            left_just = false;
            long_flag = 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++;
            }
            if (fmt[1] == 'l')
            {
                long_flag = true;
                fmt++;
            }
        }
        else
        {
            stream->write1(c);
        }
        fmt++;
    }
}

void hos_printf(const stream_t * stream, const char * fmt, ...)
{
    va_list va;
    va_start(va, fmt);
    hos_vprintf(stream, fmt, va);
    va_end(va);
}