46 lines
982 B
C
46 lines
982 B
C
#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();
|
|
}
|