FPU done, ready to flash

examples/rtos_demo/main.c

@@ -1,5 +1,10 @@
 #include "hal.h"
-#include "stdint.h"
+#include "task.h"
+#include "scheduler.h"
+#include <stdint.h>
+
+void task_one(void);
+void task_two(void);
 
 void delay(volatile uint32_t count) {
     while (count--);
@@ -10,12 +15,43 @@ int main(void) {
     rcc_init();
     rcc_enable_gpioc();
     gpio_init_pc13();
-    nvic_init();
 
+    task_create(&task_one, 1, "task one");
+    task_create(&task_two, 2, "task two");
+
+    scheduler_start();
+
+    return 0;
+}
+
+void task_one(void) {
     while (1) {
         gpio_toggle_pc13();
+
         delay(5000000);
+
+        gpio_toggle_pc13();
+
+        delay(5000000);
+
+        gpio_toggle_pc13();
+
+        task_delay(500);
     }
+}
 
-    return 0;
+void task_two(void) {
+    while (1) {
+        gpio_toggle_pc13();
+
+        delay(1000000);
+
+        gpio_toggle_pc13();
+
+        delay(1000000);
+
+        gpio_toggle_pc13();
+
+        task_delay(500);
+    }
 }

src/kernel/include/scheduler.h

@@ -9,5 +9,7 @@ void scheduler_init(void);
 void scheduler_start(void);
 void scheduler_tick(void);
 void scheduler_select_next_task(void);
+void task_delay(uint32_t ticks);
+
 
 #endif

src/kernel/include/task.h

@@ -25,7 +25,6 @@ typedef struct {
 } Task;
 
 Task *task_create(void (*function)(void), uint8_t priority, const char *name);
-void task_delay(uint32_t ticks);
 bool task_state_overflow(Task task);
 
 extern Task task_pool[MAX_TASKS];

src/kernel/src/scheduler.c

@@ -69,3 +69,10 @@ void scheduler_select_next_task(void) {
     next_task->state = RUNNING;
     current_task = next_task;
 }
+
+void task_delay(uint32_t ticks) {
+    current_task->delay_until = tick_count + ticks;
+    current_task->state = BLOCKED;
+
+    port_trigger_context_switch();
+}

src/kernel/src/task.c

@@ -1,11 +1,11 @@
 #include "task.h"
 #include "port.h"
 #include "fault_indicator.h"
+#include "scheduler.h"
 
 Task task_pool[MAX_TASKS];
 uint32_t stack_pool[MAX_TASKS][TASK_STACK_SIZE];
 
-
 Task *task_create(void (*function)(void), uint8_t priority, const char *name) {
     int new_task_slot = -1;
     for (int i = 0; i < MAX_TASKS; i++) {
@@ -26,11 +26,6 @@ Task *task_create(void (*function)(void), uint8_t priority, const char *name) {
     return new_task;
 }
 
-void task_delay(uint32_t ticks) {
-    ticks += ticks; /* Just using ticks so it compiles */
-    //TODO after scheduler
-}
-
 bool task_state_overflow(Task task) {
     if (*task.stack_base != 0xDEADBEEF) {
         return true;

src/linker/stm32f401.ld

@@ -1,6 +1,6 @@
 MEMORY
 {
-    flash   (RX) : ORIGIN = 0x08000000, LENGTH = 256K
+    flash   (RX) : ORIGIN = 0x08010000, LENGTH = 64K
     sram    (RW) : ORIGIN = 0x20000000, LENGTH = 64K
 }
 

src/port/cortex_m4/src/port.c

@@ -16,6 +16,7 @@ void port_init_task_stack(Task *task, void (*function)(void)) {
     uint32_t *sp = task->stack_base + (TASK_STACK_SIZE);
 
     /* Hardware Fake Frame */
+
 
     *(--sp) = 0x0;  /* FPSCR */
     *(--sp) = 0x0;  /* S15 */
@@ -43,6 +44,23 @@ void port_init_task_stack(Task *task, void (*function)(void)) {
     *(--sp) = 0x0;  /* R1 */
     *(--sp) = 0x0;  /* R0 */
 
+    *(--sp) = 0x0;  /* S31 */
+    *(--sp) = 0x0;  /* S30 */
+    *(--sp) = 0x0;  /* S29 */
+    *(--sp) = 0x0;  /* S28 */
+    *(--sp) = 0x0;  /* S27 */
+    *(--sp) = 0x0;  /* S26 */
+    *(--sp) = 0x0;  /* S25 */
+    *(--sp) = 0x0;  /* S24 */
+    *(--sp) = 0x0;  /* S23 */
+    *(--sp) = 0x0;  /* S22 */
+    *(--sp) = 0x0;  /* S21 */
+    *(--sp) = 0x0;  /* S20 */
+    *(--sp) = 0x0;  /* S19 */
+    *(--sp) = 0x0;  /* S18 */
+    *(--sp) = 0x0;  /* S17 */
+    *(--sp) = 0x0;  /* S16 */
+
     /* Software Fake Frame*/
 
     *(--sp) = 0x0;  /* R11 */

src/port/cortex_m4/src/port.s

@@ -1,3 +1,5 @@
+/*  Tell CPU thumb instrs.
+    are 32 bits not 16 */
 .syntax unified
 .text
 
@@ -8,6 +10,7 @@ PendSV_Handler:
         special register to a 
         general purpose register */
     MRS R0, PSP
+    VSTMDB R0!, {S16-S31}
     /*  ARM assembly is beautiful,
         wdym I can do R4-R11 at once,
         also this is STM-DB, where
@@ -38,9 +41,10 @@ PendSV_Handler:
         PSP to thread mode,
         0xFFFFFFFD returns 
         to Thread from PSP */
+    VLDMIA R0!, {S16-S31}
     MSR PSP, R0
 
-    MOV LR, #0xFFFFFFFD
+    MOV LR, #0xFFFFFFED
     BX LR
 
 /*  Function to manually bootstrap task
@@ -67,6 +71,8 @@ SVC_Handler:
 
     /*  Load all registers of fake frame*/
     LDMIA R0!, {R4-R11}
+    /*  Pop FPU registers */
+    VLDMIA R0!, {S16-S31}
     /*  Store updated PSP back */
     MSR PSP, R0
 
@@ -81,5 +87,5 @@ SVC_Handler:
 
     /* Return to Thread Mode */
 
-    MOV LR, #0xFFFFFFFD
+    MOV LR, #0xFFFFFFED
     BX LR

src/port/cortex_m4/src/systick.c

@@ -26,7 +26,6 @@ void systick_init(void) {
     /* Page 249 of cortex*/
     /* Just clear it */
     SysTick->VAL = 0x00;
-
 }
 
 void SysTick_Handler(void) {