118 lines
2.5 KiB
C
118 lines
2.5 KiB
C
#include <interrupt.h>
|
|
#include <kmalloc.h>
|
|
#include <uart.h>
|
|
#include <timer.h>
|
|
#include <utils.h>
|
|
#include <logger.h>
|
|
#include <errcode.h>
|
|
|
|
#define CORE0_TIMER_IRQ_CTRL ((volatile uint32_t *)0x40000040)
|
|
#define CORE0_IRQ_SOURCE ((volatile uint32_t *)0x40000060)
|
|
|
|
interrupt_t *global_interrupt_pool = (interrupt_t *)0x0;
|
|
|
|
static inline
|
|
void _enable_interrupt(void)
|
|
{ W_SYSREG_IMM(DAIFClr, 0xf); }
|
|
|
|
static inline
|
|
void _disable_interrupt(void)
|
|
{ W_SYSREG_IMM(DAIFSet, 0xf); }
|
|
|
|
MMIO_W_HELPER(_enable_timer_irq, CORE0_TIMER_IRQ_CTRL, 0x1, 1);
|
|
|
|
MMIO_R_HELPER(_irq_source_timer, CORE0_IRQ_SOURCE, 0x1, 1);
|
|
MMIO_R_HELPER(_irq_source_uart, CORE0_IRQ_SOURCE, 0x1, 8);
|
|
|
|
static inline
|
|
uint32_t _d(interrupt_t *interrupt)
|
|
{ return interrupt ? interrupt->_d : 0; }
|
|
|
|
static inline
|
|
void _swap(interrupt_t **a, interrupt_t **b)
|
|
{
|
|
interrupt_t *tmp = *a;
|
|
*a = *b, *b = tmp;
|
|
}
|
|
|
|
static inline
|
|
interrupt_t *_merge(interrupt_t *a, interrupt_t *b)
|
|
{
|
|
if (!a || !b)
|
|
return a ?: b;
|
|
|
|
if (a->priority < b->priority)
|
|
_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
|
|
interrupt_t *_pop(interrupt_t *interrupt)
|
|
{
|
|
if (!interrupt || (!interrupt->_l && !interrupt->_r))
|
|
return (interrupt_t *)0x0;
|
|
return _merge(interrupt->_l, interrupt->_r);
|
|
}
|
|
|
|
void add_interrupt_task(uint64_t priority,
|
|
interrupt_callback_func_t func,
|
|
uint64_t param)
|
|
{
|
|
// DEBUG_EXCEP("add interrupt");
|
|
interrupt_t *newint = kmalloc(sizeof(interrupt_t));
|
|
*newint = (interrupt_t){
|
|
._l = (interrupt_t *)0x0,
|
|
._r = (interrupt_t *)0x0,
|
|
._d = 0,
|
|
|
|
.priority = priority,
|
|
.func = func,
|
|
.param = param
|
|
};
|
|
|
|
global_interrupt_pool = _merge(global_interrupt_pool, newint);
|
|
}
|
|
|
|
void init_interrupt(void)
|
|
{
|
|
uint64_t cntfrq_el0;
|
|
W_SYSREG(cntp_ctl_el0, 1);
|
|
R_SYSREG(cntfrq_el0, cntfrq_el0);
|
|
W_SYSREG(cntp_tval_el0, cntfrq_el0);
|
|
|
|
_enable_timer_irq(true);
|
|
_enable_interrupt();
|
|
}
|
|
|
|
void irq_handler(void)
|
|
{
|
|
// uint64_t core0_irq_source = *CORE0_IRQ_SOURCE;
|
|
// DEBUG_EXCEP(core0_irq_source);
|
|
|
|
if (_irq_source_timer())
|
|
timer_irq_handler();
|
|
|
|
if (_irq_source_uart())
|
|
uart_irq_handler();
|
|
}
|
|
|
|
void wfe(void)
|
|
{
|
|
if (!global_interrupt_pool) {
|
|
asm volatile("wfe");
|
|
return;
|
|
}
|
|
|
|
interrupt_t *interrupt = global_interrupt_pool;
|
|
global_interrupt_pool = _merge(global_interrupt_pool->_l,
|
|
global_interrupt_pool->_r);
|
|
|
|
interrupt->func(interrupt->param);
|
|
}
|