From 34684b28a463404717a86f25ee381bfdaefbace4 Mon Sep 17 00:00:00 2001
From: tcsullivan <tullivan99@gmail.com>
Date: Wed, 10 Oct 2018 23:31:10 -0400
Subject: unpriviledged sleep wip

---
 src/kernel/clock.c | 15 ++++++++++++++-
 src/kernel/clock.h |  2 ++
 src/kernel/init.c  | 10 +++++++++-
 src/kernel/task.c  | 46 ++++++++++++++++++++++------------------------
 src/kernel/task.h  |  3 ++-
 5 files changed, 49 insertions(+), 27 deletions(-)

(limited to 'src/kernel')

diff --git a/src/kernel/clock.c b/src/kernel/clock.c
index 44b7722..5bfc93a 100644
--- a/src/kernel/clock.c
+++ b/src/kernel/clock.c
@@ -19,6 +19,7 @@
  */
 
 #include "clock.h"
+#include "task.h"
 #include <arch/stm/stm32l476xx.h>
 
 // ticks since init
@@ -26,9 +27,16 @@ volatile uint32_t ticks = 0;
 
 volatile uint8_t tim2_finished = 1;
 
+extern task_t *current;
+
 void clock_svc(uint32_t *args)
 {
-	udelay(args[0]);
+	if (args[0] == 0) {
+		current->sleep = ticks + args[1];
+		SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
+	} else if (args[0] == 1) {
+		udelay(args[1]);
+	}
 }
 
 void clock_init(void)
@@ -71,6 +79,11 @@ void clock_init(void)
 	TIM2->CR1 |= TIM_CR1_OPM | TIM_CR1_CEN;
 }
 
+uint32_t millis(void)
+{
+	return ticks;
+}
+
 void delay(uint32_t count)
 {
 	uint32_t target = ticks + count;
diff --git a/src/kernel/clock.h b/src/kernel/clock.h
index c3554f6..3ec7857 100644
--- a/src/kernel/clock.h
+++ b/src/kernel/clock.h
@@ -28,6 +28,8 @@
  */
 extern void clock_init(void);
 
+uint32_t millis(void);
+
 /**
  * Sleeps for given amount of milliseconds.
  * @param ms Number of milliseconds to sleep for
diff --git a/src/kernel/init.c b/src/kernel/init.c
index e4ac115..ca60c3e 100644
--- a/src/kernel/init.c
+++ b/src/kernel/init.c
@@ -27,6 +27,7 @@
 extern uint8_t __bss_end__;
 
 extern void user_main(void);
+void init_idle(void);
 
 int main(void)
 {
@@ -46,7 +47,14 @@ int main(void)
 	// enable FPU
 	//SCB->CPACR |= (0xF << 20);
 
-	task_init(user_main);
+	task_init(init_idle, 512);
 	while (1);
 }
 
+void init_idle(void)
+{
+	task_start(user_main, 4096);
+
+	while (1)
+		delay(100);
+}
diff --git a/src/kernel/task.c b/src/kernel/task.c
index 1a1c16b..19fa5ea 100644
--- a/src/kernel/task.c
+++ b/src/kernel/task.c
@@ -18,8 +18,9 @@
  * along with this program.  If not, see <https://www.gnu.org/licenses/>.
  */
 
-#include "task.h"
+#include "clock.h"
 #include "heap.h"
+#include "task.h"
 #include <arch/stm/stm32l476xx.h>
 
 task_t *current, *prev;
@@ -71,7 +72,7 @@ void task_crt0(void)
 	");
 }
 
-task_t *task_create(void (*code)(void), uint32_t stackSize)
+task_t *task_create(void (*code)(void), uint16_t stackSize)
 {
 	task_t *t = (task_t *)malloc(sizeof(task_t));
 	t->next = 0;
@@ -95,15 +96,16 @@ task_t *task_create(void (*code)(void), uint32_t stackSize)
 	t->sp[14] = (uint32_t)code;
 	t->sp[15] = (uint32_t)task_crt0;
 	t->sp[16] = 0x01000000;
+	t->sleep = 0;
 	return t;
 }
 
-void task_init(void (*init)(void))
+void task_init(void (*init)(void), uint16_t stackSize)
 {
 	current = (task_t *)malloc(sizeof(task_t));
 	current->stack = 0;
 
-	task_t *init_task = task_create(init, 4096);
+	task_t *init_task = task_create(init, stackSize);
 
 	prev = init_task;
 	current->next = init_task;
@@ -166,33 +168,29 @@ void PendSV_Handler(void)
 	if (task_disable != 0)
 		asm("bx lr");
 
-	// TODO why, and what does this do
 	// TODO get back to c, implement task sleeping
+
+	// Save current stack pointer
 	asm("\
 		mrs r0, psp; \
 		isb; \
-		ldr r1, =current; \
-		ldr r2, [r1]; \
 		stmdb r0!, {r4-r11, r14}; \
-		str r0, [r2, #8]; \
-		ldr r0, [r2, #0]; \
-		ldr r3, =prev; \
-		str r2, [r3]; \
-		str r0, [r1]; \
-		ldr r2, [r1]; \
-		ldr r0, [r2, #8]; \
+		mov %0, r0; \
+	" : "=r" (current->sp));
+
+	// Load next task
+	uint32_t ticks = millis();
+	do {
+		current = current->next;
+	} while (current->sleep > ticks);
+	current->sleep = 0;
+
+	// Load stack pointer, return
+	asm("\
+		mov r0, %0; \
 		ldmia r0!, {r4-r11, r14}; \
 		msr psp, r0; \
 		bx lr; \
-	");
-	// r1 = current
-	// r2 = *current
-	// r0 = sp
-	// *current.sp = sp
-	// r0 = current->next
-	// current = r0
-	// r2 = *current
-	// r0 = *current.sp
-	// unpack
+	" :: "r" (current->sp));
 }
 
diff --git a/src/kernel/task.h b/src/kernel/task.h
index 3d331ae..472e909 100644
--- a/src/kernel/task.h
+++ b/src/kernel/task.h
@@ -30,6 +30,7 @@ typedef struct {
 	void *next;      /**< pointer to the next task_t instance */
 	uint32_t *stack; /**< pointer to the task's stack */
 	uint32_t *sp;    /**< pointer to the task's last sp register value */
+	uint32_t sleep;  /**< number of milliseconds task is sleeping for */
 } task_t;
 
 /**
@@ -37,7 +38,7 @@ typedef struct {
  * This task is given a 4kb stack.
  * @param init the initial thread to run
  */
-void task_init(void (*init)(void));
+void task_init(void (*init)(void), uint16_t stackSize);
 
 /**
  * Starts a new task.
-- 
cgit v1.2.3