133 lines
2.3 KiB
C
133 lines
2.3 KiB
C
#include <debug.h>
|
|
#include <stdarg.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
#include <vga.h>
|
|
#include <serial.h>
|
|
|
|
void debug_init()
|
|
{
|
|
vga_init();
|
|
serial_init(PORT_COM1);
|
|
}
|
|
|
|
uint64_t num2str(uint64_t num, uint32_t base, char *buf)
|
|
{
|
|
// Convert a number to string
|
|
if(num == 0)
|
|
{
|
|
buf[0] = '0';
|
|
buf[1] = '\0';
|
|
return 0;
|
|
}
|
|
uint32_t i=0, j=0;
|
|
char chars[] = "0123456789ABCDEF";
|
|
// Build the string starting with the least significant digit
|
|
while(num > 0)
|
|
{
|
|
buf[i++] = chars[num%base];
|
|
num /= base;
|
|
}
|
|
|
|
// Invert the string to get digits in right order
|
|
i--;
|
|
j=0;
|
|
while(j<i)
|
|
{
|
|
char t = buf[i];
|
|
buf[i--] = buf[j];
|
|
buf[j++] = t;
|
|
}
|
|
|
|
// Zero terminate
|
|
buf[i+j+1] = '\0';
|
|
return i+j+1;
|
|
}
|
|
|
|
void debug_putch(char c)
|
|
{
|
|
vga_printch(c);
|
|
serial_write(PORT_COM1, c);
|
|
}
|
|
void debug_putsn(char *s, size_t n)
|
|
{
|
|
while(n--)
|
|
debug_putch(*s++);
|
|
}
|
|
void debug_puts(char *s)
|
|
{
|
|
debug_putsn(s, strlen(s));
|
|
}
|
|
|
|
void debug_vprintf(char *str, va_list args)
|
|
{
|
|
size_t len = 0;
|
|
if(!(*str))
|
|
return;
|
|
if(*str != '%')
|
|
{
|
|
while(str[len] && str[len] != '%')
|
|
len++;
|
|
debug_putsn(str, len);
|
|
return debug_vprintf(&str[len], args);
|
|
}
|
|
str++;
|
|
char *prefix;
|
|
uint32_t base = 0;
|
|
#define DEBUG_VPRINTF_BASE_CHAR 255
|
|
#define DEBUG_VPRINTF_BASE_STR 1024
|
|
switch(*str)
|
|
{
|
|
case 'b':
|
|
prefix = "0b";
|
|
base = 2;
|
|
break;
|
|
case 'o':
|
|
prefix = "0";
|
|
base = 8;
|
|
break;
|
|
case 'd':
|
|
prefix = "";
|
|
base = 10;
|
|
break;
|
|
case 'x':
|
|
prefix = "0x";
|
|
base = 16;
|
|
break;
|
|
case 'c':
|
|
prefix = "";
|
|
base = DEBUG_VPRINTF_BASE_CHAR;
|
|
break;
|
|
case 's':
|
|
prefix = "";
|
|
base = DEBUG_VPRINTF_BASE_STR;
|
|
break;
|
|
case '%':
|
|
default:
|
|
debug_putch('%');
|
|
break;
|
|
}
|
|
if(base == DEBUG_VPRINTF_BASE_STR)
|
|
debug_puts(va_arg(args, char *));
|
|
else if(base == DEBUG_VPRINTF_BASE_CHAR)
|
|
debug_putch((char)va_arg(args, uint64_t));
|
|
else if(base)
|
|
{
|
|
uint64_t num = va_arg(args, uint64_t);
|
|
char buf[128];
|
|
num2str(num, base, buf);
|
|
debug_puts(prefix);
|
|
debug_puts(buf);
|
|
}
|
|
str++;
|
|
return debug_vprintf(str, args);
|
|
}
|
|
|
|
void debug_printf(char *str, ...)
|
|
{
|
|
va_list args;
|
|
va_start(args, str);
|
|
debug_vprintf(str, args);
|
|
va_end(args);
|
|
}
|