Espresso/lib/printf.c

#include <stdarg.h>
#include <stddef.h>
#include <stdbool.h>
#include <stdint.h>
#include <drivers/serio.h>

#include <printf.h>

static uint8_t color = 0xFF;

void printwc(const char* str, uint8_t color)
{
  uint8_t c = terminal_getcolor();
  
  terminal_setcolor(color);
  
  printf(str);
  
  terminal_setcolor(c);
}

void printd(const char* str)
{
  terminal_debug_writestring(str);
  
  serial_puts("[ DEBUG ] ");
  serial_puts(str);
}

void printdc(const char* str, uint8_t color)
{
  uint8_t c = terminal_getcolor();
  terminal_setcolor(color);
  
  printd(str);
  
  terminal_setcolor(c);
}

void printf_set_color(uint8_t _color)
{
  if (_color == 0xFF)
  {
    return;
  }

  color = _color;
}

void printf(const char* format, ...)
{
  va_list args;
  va_start(args, format);
  
  if (color != 0xFF)
  {
    terminal_setcolor(color);
  }

  for (size_t i = 0; format[i] != '\0'; ++i)
  {
    if (format[i] == '%' && format[i + 1] != '\0')
    {
      ++i;

      int width = 0;
      if (format[i] == '0')
      {
        ++i;
        while (format[i] >= '0' && format[i] <= '9')
        {
          width = width * 10 + (format[i] - '0');
          ++i;
        }
      }

      bool is_ll = false;
      if (format[i] == 'l' && format[i + 1] == 'l')
      {
        is_ll = true;
        i += 2;
      }

      switch (format[i])
      {
        case 's': {
          const char* str = va_arg(args, const char*);
          
          terminal_writestring(str ? str : "(null)");
          
          if (use_serial())
          {
            serial_puts(str ? str : "(null)");
          }
          
          break;
        }
        case 'c': {
          char c = (char) va_arg(args, int);
          terminal_putchar(c);
          
          if (use_serial())
          {
            serial_write(c);
          }
          
          break;
        }
        case 'd':
        case 'i': {
          if (is_ll)
          {
            int64_t val = va_arg(args, int64_t);
            print_lint(val);
          }
          else
          {
            int32_t val = va_arg(args, int32_t);
            print_int(val);
          }
          
          break;
        }
        case 'u': {
          if (is_ll)
          {
            uint64_t val = va_arg(args, uint64_t);
            print_luint(val);
          }
          else
          {
            uint32_t val = va_arg(args, uint32_t);
            print_uint(val);
          }
          
          break;
        }
        case 'x': {
          if (is_ll)
          {
            uint64_t val = va_arg(args, uint64_t);
            print_hex64(val, width ? width : 16, false);
          }
          else
          {
            uint32_t val = va_arg(args, uint32_t);
            print_hex(val, width ? width : 8, false);
          }
          break;
        }
        case 'X': {
          if (is_ll)
          {
            uint64_t val = va_arg(args, uint64_t);
            print_hex64(val, width ? width : 16, true);
          }
          else
          {
            uint32_t val = va_arg(args, uint32_t);
            print_hex(val, width ? width : 8, true);
          }
          break;
        }
        case 'p': {
          void* ptr = va_arg(args, void*);
          terminal_writestring("0x");
          
          if (use_serial())
          {
            serial_write('0');
            serial_write('x');
          }
          
          print_hex((uint32_t)(uintptr_t)ptr, 8, true);
          
          break;
        }
        case 'f':
        case 'F': {
          double val = va_arg(args, double);
          print_double(val, 2);
          break;
        }
        case '%': {
          terminal_putchar('%');
          
          if (use_serial())
          {
            serial_write('%');
          }
          
          break;
        }
        default: {
          terminal_putchar('%');
          terminal_putchar(format[i]);
          
          if (use_serial())
          {
            serial_write('%');
            serial_write(format[i]);
          }
          
          break;
        }
      }
    }
    else
    {
      terminal_putchar(format[i]);
      
      if (use_serial())
      {
        serial_write(format[i]);
      }
    }
  }

  va_end(args);
}


void print_int(int32_t value)
{
  char buffer[12];
  int i = 0;
  uint32_t u;

  if (value < 0)
  {
    terminal_putchar('-');
    
    if (use_serial())
    {
      serial_write('-');
    }
    
    u = (uint32_t)(-value);
  }
  else
  {
    u = (uint32_t)value;
  }

  do
  {
    buffer[i++] = '0' + (u % 10);
    u /= 10;
  } while (u > 0);

  while (i--)
  {
    terminal_putchar(buffer[i]);
    
    if (use_serial())
    {
      serial_write(buffer[i]);
    }
  }
}

void print_lint(int64_t value)
{
  char buffer[21];
  int i = 0;
  uint64_t u;

  if (value < 0)
  {
    terminal_putchar('-');
    
    if (use_serial())
    {
      serial_write('-');
    }
    
    u = (uint64_t) (-value);
  }
  else
  {
    u = (uint64_t) value;
  }

  do
  {
    buffer[i++] = '0' + (u % 10);
    u /= 10;
  } while (u > 0);

  while (i--)
  {
    terminal_putchar(buffer[i]);
    
    if (use_serial())
    {
      serial_write(buffer[i]);
    }
  }
}

void print_hex(uint32_t value, int width, bool uppercase)
{
  const char* hex_chars = uppercase ? "0123456789ABCDEF" : "0123456789abcdef";
  char buffer[9]; // 8 hex digits max for 32-bit
  int i = 0;

  do
  {
    buffer[i++] = hex_chars[value & 0xF];
    value >>= 4;
  } while (value || i < width); // ensure at least 'width' digits

  while (i--)
  {
    terminal_putchar(buffer[i]);
    
    if (use_serial())
    {
      serial_write(buffer[i]);
    }
  }
}

void print_double(double value, int precision)
{
  // Handle the integer part
  int32_t integer_part = (int32_t)value;
  double fractional_part = value - integer_part;

  // Print the integer part
  print_int(integer_part);

  // Print the decimal point
  terminal_putchar('.');
  
  if (use_serial())
  {
    serial_write('.');
  }

  // Print the fractional part (scaled up)
  fractional_part *= 1;
  for (int i = 0; i < precision; i++)
  {
    fractional_part *= 10;
  }

  int32_t frac_int = (int32_t)fractional_part;
  print_int(frac_int);
}

void print_uint(uint32_t value)
{
  char buffer[11];
  int i = 0;

  do
  {
    buffer[i++] = '0' + (value % 10);
    value /= 10;
  } while (value > 0);

  while (i--)
  {
    terminal_putchar(buffer[i]);
    
    if (use_serial())
    {
      serial_write(buffer[i]);
    }
  }
}

void print_luint(uint64_t value)
{
  char buffer[21];
  int i = 0;

  do
  {
    buffer[i++] = '0' + (value % 10);
    value /= 10;
  } while (value > 0);

  while (i--)
  {
    terminal_putchar(buffer[i]);
    
    if (use_serial())
    {
      serial_write(buffer[i]);
    }
  }
}

void print_hex64(uint64_t value, int width, bool uppercase)
{
  char buffer[17] = {0};
  const char* digits = uppercase ? "0123456789ABCDEF" : "0123456789abcdef";
  int i = 0;

  do
  {
    buffer[i++] = digits[value % 16];
    value /= 16;
  } while (value > 0);

  while (i < width)
  {
    buffer[i++] = '0';
  }

  while (i--)
  {
    terminal_putchar(buffer[i]);
    
    if (use_serial())
    {
      serial_write(buffer[i]);
    }
  }
}