#include "main.h"
#include "dprintf.h"
#if PERMISSION_PRINTF == SUPPORT_PRINTF

void dputs(char *str)
{
    while (*str != '\0')
        dputc(*str++);
}

void dputh16(uint16_t val)
{
    const char tlb[] = "0123456789abcdef";
    int i;
    for (i = 0; i < 4; i++) {
        dputc(tlb[val >> 12]);
        val <<= 4;
    }
}

void dputh32(uint32_t val)
{
    dputh16(val >> 16);
    dputh16(val & 0xffff);
}

void _dprintf(char *fmt, ...)
{
    va_list va;
    char ch;

    va_start(va,fmt);

    while ((ch = *(fmt++))) {
        if (ch != '%') {
            dputc(ch);
            continue;
        }

        ch = *(fmt++);
        switch (ch) {
        case 'x':
            dputh16(va_arg(va, uint32_t));
            break;
        case 'X':
            dputh32(va_arg(va, uint32_t));
            break;
#if 0
        case 's' :
            dputs(va_arg(va, char *));
            break;
        case '%' :
            dputc('%');
        default:
            break;
#endif
        }
    }
    va_end(va);
}
#endif
