Feat: lab 3

kernel/lib/interrupt.c

@@ -1,4 +1,5 @@
 #include <interrupt.h>
+#include <kmalloc.h>
 #include <uart.h>
 #include <timer.h>
 #include <utils.h>
@@ -8,6 +9,8 @@
 #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); }
@@ -21,6 +24,61 @@ 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;
@@ -43,3 +101,17 @@ void irq_handler(void)
   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);
+}