1 files changed, 48 insertions, 0 deletions
diff --git a/src/kern/systick/systick_manager.c b/src/kern/systick/systick_manager.c
new file mode 100644
index 0000000..83e37f5
--- /dev/null
+++ b/src/kern/systick/systick_manager.c
@@ -0,0 +1,48 @@
+#include "kern/systick/systick_manager.h"
+
+#include "arch/stm32l4xxx/peripherals/system.h"
+#include "kern/mem.h"
+#include "shared/linked_list.h"
+
+typedef struct {
+  void* arg;
+  void (*callback)(void*);
+} systick_listener_t;
+
+LINKED_LIST_DECL(systick_listener_t);
+LINKED_LIST_IMPL(systick_listener_t);
+
+static linked_list_t(systick_listener_t) listeners;
+
+void on_systick()
+{
+  linked_list_foreach(listeners, val) { val.callback(val.arg); }
+}
+
+void enable_systick(uint32_t systick_counter)
+{
+  SCB.strv_r = systick_counter;
+
+  /* Enable interrupts. */
+  regset(SCB.stcs_r, scb_tickint, 1);
+
+  /* Start the systick. */
+  regset(SCB.stcs_r, scb_enable, 1);
+}
+
+void disable_systick()
+{
+  /* Disable interrupts. */
+  regset(SCB.stcs_r, scb_tickint, 0);
+
+  /* Stop the systick. */
+  regset(SCB.stcs_r, scb_enable, 0);
+}
+
+void systick_add_callback(void (*callback)(void*), void* arg)
+{
+  systick_listener_t l;
+  l.arg = arg;
+  l.callback = callback;
+  linked_list_push_back(systick_listener_t)(&listeners, l);
+}