#include <utils.h>
#include <errcode.h>
#include <uart.h>

uint32_t msb32(uint32_t x)
{
  uint32_t res = 0x0;
  asm volatile(
    "clz %[res], %[val]"
    : [res] "=r" (res)
    : [val] "r"  (x)
  );
  return res == 32 ? 0 : (1 << (31 - res));
}

uint64_t msb64(uint64_t x)
{
  uint64_t res = 0x0;
  asm volatile(
    "clz %[res], %[val]"
    : [res] "=r" (res)
    : [val] "r"  (x)
  );
  return res == 64 ? 0 : ((uint64_t)1 << (63 - res));
}

uint64_t lsb64(uint64_t x)
{
  uint64_t res = 0x0;
  asm volatile(
    "rbit x0, %[val]" ENDL
    "clz %[res], x0"
    : [res] "=r" (res)
    : [val] "r"  (x)
    : "x0"
  );
  return res == 64 ? 0 : ((uint64_t)1 << res);
}

uint32_t hton32(const uint32_t h)
{
  const uint8_t *p = (const void *)&h;

  return (uint32_t)p[0] << 24 |
         (uint32_t)p[1] << 16 |
         (uint32_t)p[2] << 8 |
         (uint32_t)p[3];
}

uint32_t ntoh32(const uint32_t h)
{
  return hton32(h);
}

uint64_t hton64(const uint64_t h)
{
  return (uint64_t)hton32(h & 0xffffffff) << 32 |
         (uint64_t)hton32(h >> 32);
}

uint64_t ntoh64(const uint64_t h)
{
  return hton64(h);
}

int isspace(int c)
{
  switch (c) {
  case ' ':
  case '\t':
  case '\n':
  case '\v':
  case '\f':
  case '\r':
    return true;
  }
  return false;
}

int isdigit(int c)
{
  return ('0' <= c && c <= '9');
}

int isxdigit(int c)
{
  return ('0' <= c && c <= '9') || 
         ('a' <= c && c <= 'f') ||
         ('A' <= c && c <= 'F');
}

int isupper(int c)
{
  return ('A' <= c && c <= 'F');
}

int32_t atoi32(const char *s)
{
  if (!s)
    panic(ERR_CONVERSION);

  int is_neg = 0;
  if (*s != '\0' && *s == '-')
    is_neg = 1, s++;

  int32_t ret = 0;
  for (; *s != '\0'; ++s) {
    if (!isdigit(*s))
      panic(ERR_CONVERSION);
    ret *= 10, ret += *s - '0';
  }

  return is_neg ? -ret : ret;
}

uint32_t atoh32(const char *s)
{
  if (!s)
    panic(ERR_CONVERSION);

  uint32_t ret = 0;
  for (int i = 8; i-- && *s != '\0'; ++s) {
    if (!isxdigit(*s))
      panic(ERR_CONVERSION);
    ret <<= 4, ret |= (isdigit(*s) ? *s - '0' : 
                      (isupper(*s) ? *s - 'A' + 10 : 
                                     *s - 'a' + 10));
  }

  return ret;
}

void panic(int exitcode)
{
  static int print = 0;
  if (!print) {
    uart_puts(
      "OOPS! YOUR KERNEL DEAD" ENDL
      "ERROR CODE: "
    );
    uart_hex(exitcode);
    uart_puts(ENDL);
    print = 1;
  }

  asm volatile(
    "wfe\n"
    "b panic"
  );
  __builtin_unreachable();
}