privileged mode works

diff --git a/include/priv_gpio.h b/include/priv_gpio.h
index eee5e18a414ff2ad473abf86abe3a8513d321f40..a97e294e28a9ac56469062c8618c52e69057028c 100644 (file)
--- a/include/priv_gpio.h
+++ b/include/priv_gpio.h
@@ -7,20 +7,18 @@
 #define GPIO_SPEED 3
 #define GPIO_OUT 4
 
-void gpio(uint32_t call, uint32_t port, uint32_t pin, uint32_t value) 
+void gpio(uint32_t call, uint32_t pin, uint32_t value) 
 {
        register uint32_t r0 asm("r0") = call;
-       register uint32_t r1 asm("r1") = port;
-       register uint32_t r2 asm("r2") = pin;
-       register uint32_t r3 asm("r3") = value;
+       register uint32_t r1 asm("r1") = pin;
+       register uint32_t r2 asm("r2") = value;
 
        asm("\
                mov r0, %0; \
                mov r1, %1; \
                mov r2, %2; \
-               mov r3, %3; \
                svc 1; \
-       " :: "r" (r0), "r" (r1), "r" (r2), "r" (r3));
+       " :: "r" (r0), "r" (r1), "r" (r2));
 }
 
 #endif // PRIV_GPIO_H_

diff --git a/src/gpio.c b/src/gpio.c
index 29ed763b8868e9e4894a5540ac4e630a33e9dbba..27bfdd3833680c882ea13aefde9123beccc675a2 100644 (file)
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -20,23 +20,31 @@
 
 #include <gpio.h>
 
+static const GPIO_TypeDef *gpio_ports[8] = {
+       GPIOA, GPIOB, GPIOC, GPIOD,
+       GPIOE, GPIOF, GPIOG, GPIOH
+};
+
 void gpio_svc(uint32_t *args)
 {
+       GPIO_TypeDef *port = gpio_ports[args[1] / 16];
+       uint32_t pin = args[1] & 0xF;
+
        switch (args[0]) {
        case 0:
-               gpio_mode((GPIO_TypeDef *)args[1], args[2], args[3]);
+               gpio_mode(port, pin, args[2]);
                break;
        case 1:
-               gpio_type((GPIO_TypeDef *)args[1], args[2], args[3]);
+               gpio_type(port, pin, args[2]);
                break;
        case 2:
-               gpio_pupd((GPIO_TypeDef *)args[1], args[2], args[3]);
+               gpio_pupd(port, pin, args[2]);
                break;
        case 3:
-               gpio_speed((GPIO_TypeDef *)args[1], args[2], args[3]);
+               gpio_speed(port, pin, args[2]);
                break;
        case 4:
-               gpio_dout((GPIO_TypeDef *)args[1], args[2], args[3]);
+               gpio_dout(port, pin, args[2]);
                break;
        }
 }

diff --git a/src/main.c b/src/main.c
index c929d163c2a9106eab70f247f50fe8a158ff5439..bbcd09b421efd1b26364f1d6a8dceba61afda532 100644 (file)
--- a/src/main.c
+++ b/src/main.c
@@ -54,12 +54,11 @@ int main(void)
 void task2(void);\r
 void kmain(void)\r
 {\r
-       gpio(GPIO_MODE, (uint32_t)GPIOA, 5, OUTPUT);\r
-       //gpio_mode(GPIOA, 5, OUTPUT);\r
+       gpio(GPIO_MODE, 5, OUTPUT);\r
        task_start(task2, 512);\r
 \r
        for (int i = 0; i < 8; i++) {\r
-               gpio_dout(GPIOA, 5, !(i & 1));\r
+               gpio(GPIO_OUT, 5, !(i & 1));\r
                delay(200);\r
        }\r
 }\r
@@ -76,7 +75,7 @@ void task3(void)
        int state = 0;\r
        delay(2500);\r
        while (1) {\r
-               gpio_dout(GPIOA, 5, state ^= 1);\r
+               gpio(GPIO_OUT, 5, state ^= 1);\r
                delay(500);\r
        }\r
 }\r

diff --git a/src/svc.c b/src/svc.c
index 4342a950388037105b14061652d8f16726671e24..f1325717b678f26afbcd474f4f2e614ddc0a4899 100644 (file)
--- a/src/svc.c
+++ b/src/svc.c
@@ -22,14 +22,19 @@
 #include <stdint.h>
 #include <gpio.h>
 #include <clock.h>
+#include <task.h>
 
 extern void gpio_svc(uint32_t *);
 
 void svc_handler(uint32_t *args)
 {
-       uint32_t svc_number = ((char *)args[6])[-2];
+       /*uint32_t*/int svc_number = ((char *)args[6])[-2];
 
        switch (svc_number) {
+       case -1:
+       case 0:
+               _exit(args[0]);
+               break;
        case 1:
                gpio_svc(args);
                break;

diff --git a/src/task.c b/src/task.c
index d87f48d4c38ae1a2629856097ea38a4201d0b818..4da2be10b91a063019516fdc009625afc3fc6bfb 100644 (file)
--- a/src/task.c
+++ b/src/task.c
@@ -48,19 +48,25 @@ void _exit(int code)
        free(current);
 
        SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
-       while (1);
 }
 
 /**
  * 'Prepares' task for running.
  * Calls the task's main code, setting _exit() as the return point.
  */
+__attribute__ ((naked))
+void task_doexit(void)
+{
+       asm("eor r0, r0; svc 0");
+       while (1);
+}
+
 __attribute__ ((naked))
 void task_crt0(void)
 {
        asm("\
                mov r4, lr; \
-               ldr lr, =_exit; \
+               ldr lr, =task_doexit; \
                bx r4; \
        ");
 }
@@ -95,8 +101,11 @@ task_t *task_create(void (*code)(void), uint32_t stackSize)
 void task_init(void (*init)(void))
 {
        current = (task_t *)malloc(sizeof(task_t));
+       current->stack = 0;
+
        task_t *init_task = task_create(init, 4096);
 
+       prev = init_task;
        current->next = init_task;
        init_task->next = init_task;
 
@@ -108,13 +117,12 @@ void task_init(void (*init)(void))
                mov %0, r0; \
                msr psp, r0; \
                mrs r0, control; \
-               orr r0, r0, #2; \
+               orr r0, r0, #3; \
                cpsie i; \
                msr control, r0; \
        " : "=r" (current->sp));
 
-       SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
-       while (1);
+       task_doexit();
 }
 
 void task_start(void (*task)(void), uint16_t stackSize)