#include <timer.h>
#include <interrupt.h>
#include <logger.h>
#include <utils.h>
#include <kmalloc.h>
#include <thread.h>

#define CORE0_TIMER_IRQ_CTRL ((volatile uint32_t *)0x40000040)

MMIO_W_HELPER(_enable_timer_irq, CORE0_TIMER_IRQ_CTRL, 0x1, 1);

void timer_init()
{
  uint64_t cntkctl_el1, cntfrq_el0;
  R_SYSREG(cntkctl_el1, cntkctl_el1);
  W_SYSREG(cntkctl_el1, cntkctl_el1 | 1);
  W_SYSREG(cntp_ctl_el0, 1);
  _enable_timer_irq(true);

  R_SYSREG(cntfrq_el0, cntfrq_el0);
  W_SYSREG(cntp_tval_el0, cntfrq_el0 >> 5);
}

void sleep(uint64_t ms)
{
  uint64_t cntpct_el0, cntfrq_el0;
  R_SYSREG(cntpct_el0, cntpct_el0);
  R_SYSREG(cntfrq_el0, cntfrq_el0);

  uint64_t wake = cntpct_el0 + cntfrq_el0 * ms / 1000000;
  for (;;) {
    R_SYSREG(cntpct_el0, cntpct_el0);
    if (cntpct_el0 >= wake)
      break;
    yield();
  }
}

void timer_irq_handler(void)
{
  uint64_t cntfrq_el0;
  R_SYSREG(cntfrq_el0, cntfrq_el0);
  W_SYSREG(cntp_tval_el0, cntfrq_el0 >> 5);
  schedule();
}