109 lines
2.0 KiB
C
109 lines
2.0 KiB
C
#include <timer.h>
|
|
#include <interrupt.h>
|
|
#include <logger.h>
|
|
#include <utils.h>
|
|
#include <kmalloc.h>
|
|
|
|
#define CORE0_TIMER_IRQ_CTRL ((volatile uint32_t *)0x40000040)
|
|
|
|
timer_t *global_timer = (timer_t *)0x0;
|
|
|
|
MMIO_W_HELPER(_enable_timer_irq, CORE0_TIMER_IRQ_CTRL, 0x1, 1);
|
|
|
|
static inline
|
|
uint32_t _d(timer_t *t)
|
|
{ return t ? t->_d : 0; }
|
|
|
|
static inline
|
|
void _swap(timer_t **a, timer_t **b)
|
|
{
|
|
timer_t *tmp = *a;
|
|
*a = *b, *b = tmp;
|
|
}
|
|
|
|
static inline
|
|
timer_t *_merge(timer_t *a, timer_t *b)
|
|
{
|
|
if (!a || !b)
|
|
return a ?: b;
|
|
|
|
if (a->data.firing_tick > b->data.firing_tick)
|
|
_swap(&a, &b);
|
|
a->_r = _merge(a->_r, b);
|
|
|
|
if (_d(a->_l) < _d(a->_r))
|
|
_swap(&a->_l, &a->_r);
|
|
|
|
a->_d = _d(a->_r) + 1;
|
|
return a;
|
|
}
|
|
|
|
static inline
|
|
timer_t *_pop(timer_t *t)
|
|
{
|
|
if (!t || (!t->_l && !t->_r))
|
|
return (timer_t *)0x0;
|
|
return _merge(t->_l, t->_r);
|
|
}
|
|
|
|
static inline
|
|
void _set_timer_interrupt()
|
|
{
|
|
if (global_timer) {
|
|
uint64_t cntpct_el0;
|
|
R_SYSREG(cntpct_el0, cntpct_el0);
|
|
DEBUG_EXCEP(cntpct_el0);
|
|
|
|
if (global_timer->data.firing_tick < cntpct_el0)
|
|
W_SYSREG(cntp_tval_el0, 1);
|
|
else
|
|
W_SYSREG(cntp_cval_el0, global_timer->data.firing_tick);
|
|
|
|
_enable_timer_irq(true);
|
|
}
|
|
}
|
|
|
|
static inline
|
|
void _traverse(timer_t *t)
|
|
{
|
|
if (!t) return;
|
|
DEBUG_EXCEP(t->data.firing_tick);
|
|
t->data.func(t->data.param);
|
|
_traverse(t->_l);
|
|
_traverse(t->_r);
|
|
}
|
|
|
|
void add_timer_task(task_t task)
|
|
{
|
|
DEBUG_EXCEP("add timer task");
|
|
timer_t *newtimer = kmalloc(sizeof(timer_t));
|
|
*newtimer = (timer_t){
|
|
._l = (timer_t *)0x0,
|
|
._r = (timer_t *)0x0,
|
|
._d = 0,
|
|
|
|
.data = task,
|
|
};
|
|
|
|
global_timer = _merge(global_timer, newtimer);
|
|
|
|
_traverse(global_timer);
|
|
_set_timer_interrupt();
|
|
}
|
|
|
|
void timer_irq_handler(void)
|
|
{
|
|
_enable_timer_irq(false);
|
|
|
|
uint64_t cntpct_el0;
|
|
R_SYSREG(cntpct_el0, cntpct_el0);
|
|
LOG("timer irq"); DEBUG_EXCEP(cntpct_el0);
|
|
|
|
if (global_timer) {
|
|
add_interrupt_task(20, global_timer->data.func, global_timer->data.param);
|
|
global_timer = _pop(global_timer);
|
|
}
|
|
|
|
_set_timer_interrupt();
|
|
}
|