Scheduler selecting task logic

hal/stm32f401/include/fault_indicator.h

@@ -5,6 +5,6 @@
 
 void delay(volatile uint32_t count);
 void warning_light_init(void);
-void warning_light_mutex(void);
+void warning_light_scheduler(void);
 
 #endif

hal/stm32f401/src/fault_indicator.c

@@ -28,7 +28,7 @@ void warning_light_init(void) {
 }
 
 
-void warning_light_mutex(void) {
+void warning_light_scheduler(void) {
     while (1) {
         for (int i = 0; i < 6; i++) {
             gpio_toggle_pc13();

hal/stm32f401/src/fpu.c

@@ -1,6 +1,5 @@
-#include "stm32f401xc.h"
 #include "fpu.h"
-
+#include "stm32f401xc.h"
 
 /*  Enables FPU hardware 
     https://deepbluembedded.com/stm32-fpu-floating-point-unit-enable-disable/ */

hal/stm32f401/src/gpio.c

@@ -1,6 +1,6 @@
+#include "gpio.h"
 #include "stm32f401xc.h"
 #include <stdint.h>
-#include "gpio.h"
 
 /*  Page 158 of Reference manual 
     Moder configs whether pin is input, output, ect.

hal/stm32f401/src/rcc.c

@@ -1,8 +1,9 @@
 /* Page 91 of https://www.st.com/resource/en/reference_manual/dm00096844-stm32f401xb-c-and-stm32f401xd-e-advanced-arm-based-32-bit-mcus-stmicroelectronics.pdf*/
 
+#include "rcc.h"
+#include "gpio.h"
+#include "stm32f401xc.h"
 #include <stdint.h>
-#include <gpio.h>
-#include <stm32f401xc.h>
 
 void rcc_init(void) {
     /* Turn on HSI */

kernel/include/scheduler.h

@@ -8,7 +8,6 @@ extern Task *current_task;
 void scheduler_init(void);
 void scheduler_start(void);
 void scheduler_tick(void);
-Task *scheduler_get_current_task(void);
 void scheduler_select_next_task(void);
 
 #endif

kernel/src/scheduler.c

@@ -1,28 +1,71 @@
 #include "scheduler.h"
 #include "systick.h"
-#include "stddef.h"
+#include "fault_indicator.h"
+#include "port.h"
+#include "nvic.h"
+#include <stddef.h>
 
 /* Only scheduler.c needs tick count */
-//static uint32_t tick_count = 0;
+static uint32_t tick_count = 0;
 Task *current_task = NULL;
 
 void scheduler_start(void) {
     scheduler_init();
-
+    port_start_first_task();
 }
 
 void scheduler_init(void) {
+    /* Turn on SysTick */
     systick_init();
+    /* Set its priority */
+    nvic_init();
 }
 
-// void scheduler_tick(void) {
+/*  Called every 1ms to unblock 
+    sleeping tasks, setup by 
+    SysTick_Handler */
+void scheduler_tick(void) {
+    tick_count += 1;
 
-// }
+    for (int i = 0; i < MAX_TASKS; i++) {
+        Task *cur_task = &task_pool[i];
 
-// Task *scheduler_get_current_task(void) {
-
-// }
+        if (cur_task->state == BLOCKED && tick_count >= cur_task->delay_until) {
+            cur_task->state = READY;
+        }
+    }
+
+    port_trigger_context_switch();
+}
 
-// void scheduler_select_next_task(void) {
+/*  Used in PendSV after saving the
+    cur task context, PendSV Handler
+    will then select next task */
+void scheduler_select_next_task(void) {
+    Task *next_task = NULL;
+    for (int i = 0; i < MAX_TASKS; i++) {
+        Task *cur_task = &task_pool[i];
+        if (cur_task->state == READY) {
+            if (next_task == NULL) {
+                next_task = cur_task;
+            } else if (cur_task->priority > next_task->priority) {
+                next_task = cur_task;
+            }
+        }
+    }
 
-// }
+    /*  Should always have a READY Task,
+        so shouldn't have to worry about this */
+    if (next_task == NULL) {
+        return;
+    }
+
+    /*  Mark current task as READY and 
+        next task as RUNNING */
+    if (current_task != NULL && current_task->state == RUNNING) {
+        current_task->state = READY;
+    }
+
+    next_task->state = RUNNING;
+    current_task = next_task;
+}

kernel/src/task.c

@@ -1,8 +1,7 @@
 #include "task.h"
 #include "port.h"
-
-#include "systick.h"
 #include "fault_indicator.h"
+#include "systick.h"
 
 Task task_pool[MAX_TASKS];
 uint32_t stack_pool[MAX_TASKS][TASK_STACK_SIZE];

port/cortex_m4/CMakeLists.txt

@@ -2,6 +2,7 @@ add_library(port_cortex_m4 STATIC
     src/port.c
     src/port.s
     src/nvic.c
+    src/systick.c
 )
 
 target_link_libraries(port_cortex_m4 PUBLIC 

port/cortex_m4/include/nvic.h

@@ -1,8 +1,6 @@
 #ifndef NVIC_H
 #define NVIC_H
 
-#include "stm32f401xc.h"
-
 void nvic_init(void);
 
 #endif

port/cortex_m4/include/port.h

@@ -1,15 +1,18 @@
 #ifndef PORT_H 
 #define PORT_H
 
-#include <stm32f401xc.h>
 #include <stdint.h>
-#include "rtos_config.h"
 #include "task.h"
 
 #define PENDSVSET (1U << 28)
 
-void port_init_task(Task *task, uint32_t priority, uint32_t slot, const char* name);
+/* In port.c */
+void port_init_task(Task *task, uint8_t priority, uint32_t slot, const char* name);
 void port_init_task_stack(Task *task, void (*function)(void));
 void port_trigger_context_switch(void);
 
+/* In port.s */
+void PendSV_Handler(void);
+void port_start_first_task(void);
+
 #endif

port/cortex_m4/include/systick.h

@@ -1,9 +1,6 @@
 #ifndef SYSTICK_H
 #define SYSTICK_H
 
-#include "stm32f401xc.h"
-#include "scheduler.h"
-
 void systick_init(void);
 void SysTick_Handler(void);
 

port/cortex_m4/src/nvic.c

@@ -1,4 +1,5 @@
 #include "nvic.h"
+#include "stm32f401xc.h"
 
 /*  NVICs main role is to handle interrupts
     https://microcontrollerslab.com/nested-vectored-interrupt-controller-nvic-arm-cortex-m/ */

port/cortex_m4/src/port.c

@@ -1,6 +1,8 @@
 #include "port.h"
+#include "rtos_config.h"
+#include "stm32f401xc.h"
 
-void port_init_task(Task *task, uint32_t priority, uint32_t slot, const char *name) {
+void port_init_task(Task *task, uint8_t priority, uint32_t slot, const char *name) {
     task->stack_base = stack_pool[slot];
     *task->stack_base = 0xDEADBEEF;
 
@@ -55,6 +57,9 @@ void port_init_task_stack(Task *task, void (*function)(void)) {
     task->stack_ptr = sp;
 }
 
+/*  Turn on PendSV exception, so
+    we can save the current task's
+    context to memory */
 void port_trigger_context_switch(void) {
     SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
 }

port/cortex_m4/src/port.s

@@ -1 +1,3 @@
-PendSV_Handler:
+PendSV_Handler:
+
+port_start_first_task:

port/cortex_m4/src/systick.c

@@ -1,4 +1,6 @@
 #include "systick.h"
+#include "scheduler.h"
+#include "stm32f401xc.h"
 
 void systick_init(void) {
     /* Page 247 in cortex manual*/
@@ -19,11 +21,11 @@ void systick_init(void) {
     /* We get 84,000 cycles / 1 ms */
     /* Standard according to Claude is 1ms */
     /* Subtract 1 for LOAD - 83,999*/
-    SysTick->LOAD |= 83999;
+    SysTick->LOAD = 83999;
 
     /* Page 249 of cortex*/
     /* Just clear it */
-    SysTick->VAL |= 0x00;
+    SysTick->VAL = 0x00;
 
 }