heap; multitasking

10 files changed, 224 insertions, 22 deletions

diff --git a/Makefile b/Makefile
index 4a7a22c..48175ff 100644
--- a/Makefile
+++ b/Makefile
@@ -6,6 +6,8 @@ all:
 	arm-none-eabi-gcc $(CFLAGS) system_stm32l4xx.c -c -o out/system_stm32l4xx.o
 	arm-none-eabi-gcc $(CFLAGS) stm32l4xx_it.c -c -o out/stm32l4xx_it.o
 	arm-none-eabi-gcc $(CFLAGS) clock.c -c -o out/clock.o
+	arm-none-eabi-gcc $(CFLAGS) heap.c -c -o out/heap.o
+	arm-none-eabi-gcc $(CFLAGS) task.c -c -o out/task.o
 	arm-none-eabi-gcc $(CFLAGS) main.c -c -o out/main.o
 	arm-none-eabi-gcc $(CFLAGS) -T link.ld out/*.o -o out/main.elf
 	arm-none-eabi-objcopy -O ihex out/main.elf main.hex
diff --git a/clock.c b/clock.c
index 8041cb2..ab92386 100644
--- a/clock.c
+++ b/clock.c
@@ -45,15 +45,18 @@ void delay(uint32_t count)
 	while (ticks < target);
 }
 
-void PendSV_Handler(void) {
-}
-
+__attribute__ ((naked))
 void SysTick_Handler(void)
 {
+	uint32_t lr;
+	asm("mov %0, lr" : "=r" (lr));
+
 	// just keep counting
 	ticks++;
 
-	if (!(ticks % 500))
-		SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk; 
+	if (!(ticks % 10))
+		SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
+
+	asm("mov lr, %0; bx lr" :: "r" (lr));
 }
 
diff --git a/heap.c b/heap.c
new file mode 100644
index 0000000..0433a1f
--- /dev/null
+++ b/heap.c
@@ -0,0 +1,38 @@
+#include <heap.h>
+#include <stm32l476xx.h>
+
+#define RAM_END 0x20018000
+
+//extern void *end;
+//extern uint32_t _total_ram;
+static uint32_t offset = 0;
+
+__attribute__ ((section(".data")))
+uint8_t heap[8192];
+void *end = heap;
+
+void heap_init(void)
+{
+	// what to do...
+}
+
+uint32_t heap_available(void)
+{
+	return 0;//	return _total_ram - offset;
+}
+
+void *hmalloc(uint32_t size)
+{
+	void *alloc = end + offset;
+	offset += size;
+	return alloc;
+}
+
+void *hcalloc(uint32_t count, uint32_t size)
+{
+	uint32_t total = count * size;
+	void *alloc = hmalloc(total);
+	for (uint32_t i = 0; i < total; i++)
+		((uint8_t *)alloc)[i] = 0;
+	return alloc;
+}
diff --git a/include/heap.h b/include/heap.h
new file mode 100644
index 0000000..e04b15e
--- /dev/null
+++ b/include/heap.h
@@ -0,0 +1,11 @@
+#ifndef HEAP_H_
+#define HEAP_H_
+
+#include <stdint.h>
+
+uint32_t heap_available(void);
+
+void *hmalloc(uint32_t size);
+void *hcalloc(uint32_t count, uint32_t size);
+
+#endif // HEAP_H_
diff --git a/include/task.h b/include/task.h
new file mode 100644
index 0000000..c57b2c0
--- /dev/null
+++ b/include/task.h
@@ -0,0 +1,10 @@
+#ifndef TASK_H_
+#define TASK_H_
+
+#include <stdint.h>
+
+void task_init(void (*init)(void));
+
+void task_start(void (*task)(void), uint16_t stackSize);
+
+#endif // TASK_H_
diff --git a/link.ld b/link.ld
index e24ff34..8877898 100644
--- a/link.ld
+++ b/link.ld
@@ -34,15 +34,15 @@ ENTRY(Reset_Handler)
 

 /* Highest address of the user mode stack */

 _estack = 0x20018000;    /* end of RAM */

-/* Generate a link error if heap and stack don't fit into RAM */

-_Min_Heap_Size = 0x200;;      /* required amount of heap  */

-_Min_Stack_Size = 0x400;; /* required amount of stack */

+

+_Min_Stack_Size = 0x0400; /* 1k min. stack */

+_Min_Heap_Size = 0x0400; /* 1k min. heap */

 

 /* Specify the memory areas */

 MEMORY

 {

-FLASH (rx)      : ORIGIN = 0x8000000, LENGTH = 1024K

-RAM (xrw)      : ORIGIN = 0x20000000, LENGTH = 96K

+	FLASH (rx)	: ORIGIN = 0x8000000,	LENGTH = 1024K

+	RAM   (xrw)	: ORIGIN = 0x20000000,	LENGTH = 96K

 }

 

 /* Define output sections */

@@ -166,4 +166,4 @@ SECTIONS
   .ARM.attributes 0 : { *(.ARM.attributes) }

 }

 

-

+_total_ram = _estack - _end;

diff --git a/main.c b/main.c
index e640a6b..9a95465 100644
--- a/main.c
+++ b/main.c
@@ -1,21 +1,26 @@
 #include <stm32l476xx.h>

 #include <clock.h>

+#include <heap.h>

+#include <task.h>

 

 /**

  * Accomplishments:

  *   - GPIO in/out

  *   - got to 80MHz clock

+ *   - basic heap

+ *   - multitask, can exit

  */

 

 void pulse(uint8_t byte);

+void kmain(void);

 

 int main(void)

 {

+	asm("cpsid i");

+

 	// prepare flash latency for 40MHz operation

 	FLASH->ACR &= ~(FLASH_ACR_LATENCY);

-	FLASH->ACR |= FLASH_ACR_LATENCY_2WS;

-

-	clock_init();

+	FLASH->ACR |= FLASH_ACR_LATENCY_4WS;

 

 	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;	// A clk enable

 	GPIOA->MODER &= ~(GPIO_MODER_MODE5 | GPIO_MODER_MODE6);	// A5 -> output, A0 input

@@ -24,16 +29,37 @@ int main(void)
 	GPIOA->PUPDR |= GPIO_PUPDR_PUPD5_0 | GPIO_PUPDR_PUPD6_0; // pulldown for button (1)

 	//if (GPIOA->IDR & 0x01)

 

+	clock_init();

+	task_init(kmain);

+

+	while (1);

+}

+

+void task(void);

+void kmain(void)

+{

+	asm("cpsie i");

+

+	task_start(task, 1024);

 	while (1) {

+		GPIOA->BSRR |= 1 << 6;

 		delay(500);

-		GPIOA->BSRR |= 1 << 5;

 		GPIOA->BRR |= 1 << 6;

 		delay(500);

-		GPIOA->BSRR |= 1 << 6;

+	}

+}

+

+void task(void)

+{

+	while (1) {

+		GPIOA->BSRR |= 1 << 5;

+		delay(200);

 		GPIOA->BRR |= 1 << 5;

+		delay(200);

 	}

 }

 

+

 void _exit(int code)

 { for (;;); }

 

diff --git a/startup_stm32l476xx.s b/startup_stm32l476xx.s
index b93d40a..57b11cb 100644
--- a/startup_stm32l476xx.s
+++ b/startup_stm32l476xx.s
@@ -62,7 +62,7 @@ defined in linker script */
 /* end address for the .bss section. defined in linker script */

 .word	_ebss

 

-.equ  BootRAM,        0xF1E0F85F

+//.equ  BootRAM,        0xF1E0F85F

 /**

  * @brief  This is the code that gets called when the processor first

  *          starts execution following a reset event. Only the absolutely

diff --git a/stm32l4xx_it.c b/stm32l4xx_it.c
index c3bd859..dc6dc16 100644
--- a/stm32l4xx_it.c
+++ b/stm32l4xx_it.c
@@ -1,25 +1,29 @@
-#include <stdint.h>

+#include <stm32l476xx.h>

 

 void NMI_Handler(void) {}

 

 void HardFault_Handler(void)

 {

-  while (1);

+	GPIOA->BSRR |= (1 << 5) | (1 << 6);

+	while (1);

 }

 

 void MemManage_Handler(void)

 {

-  while (1);

+	GPIOA->BSRR |= (1 << 5) | (1 << 6);

+	while (1);

 }

 

 void BusFault_Handler(void)

 {

-  while (1);

+	GPIOA->BSRR |= (1 << 5) | (1 << 6);

+	while (1);

 }

 

 void UsageFault_Handler(void)

 {

-  while (1);

+	GPIOA->BSRR |= (1 << 5) | (1 << 6);

+	while (1);

 }

 

 void SVC_Handler(void) {}

diff --git a/task.c b/task.c
new file mode 100644
index 0000000..b77d4f5
--- /dev/null
+++ b/task.c
@@ -0,0 +1,108 @@
+#include <task.h>
+#include <heap.h>
+#include <stm32l476xx.h>
+
+typedef struct {
+	uint32_t *sp;
+	uint8_t use;
+	uint32_t *stack;
+	void (*code)(void);
+} task_t;
+
+#define MAX_TASKS	4
+
+static task_t tasks[MAX_TASKS];
+static int next_idx = 0;
+
+void task_init(void (*init)(void))
+{
+	for (int i = 0; i < MAX_TASKS; i++)
+		tasks[i].use = 0;
+
+	task_start(init, 1024);
+	asm("\
+		msr psp, %0; \
+		mrs r0, control; \
+		orr r0, r0, #2; \
+		msr control, r0; \
+		isb; \
+	" :: "r" (tasks[0].sp));
+
+	init();
+
+	// force switch to tasking, call pendsv
+	//SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
+}
+
+extern void _exit(int);
+void task_exit(void)
+{
+	// TODO free stack?
+	asm("cpsid i"); // hope to catch next_idx
+	tasks[next_idx].use = 0;
+	asm("cpsie i");
+	while (1); // bye
+	//_exit(0);
+}
+
+void task_start(void (*task)(void), uint16_t stackSize)
+{
+	asm("cpsid i"); // just to be safe
+
+	for (int i = 0; i < MAX_TASKS; i++) {
+		if (tasks[i].use == 0) {
+			tasks[i].stack = hmalloc(stackSize);
+			tasks[i].sp = tasks[i].stack + stackSize - 16;
+			tasks[i].sp[13] = (uint32_t)task_exit;
+			tasks[i].sp[14] = (uint32_t)task;
+			tasks[i].sp[15] = 0x01000000;
+			tasks[i].use = 1;
+			tasks[i].code = task;
+			asm("cpsie i");
+			return;
+		}
+	}
+
+	// TODO handle error
+}
+
+__attribute__ ((naked))
+void PendSV_Handler(void) 
+{
+	// save state
+	//stmdb r0!, {r4-r11};
+	asm("\
+		cpsid i; \
+		isb; \
+		dsb; \
+		mrs r0, psp; \
+		stmdb r0!, {r4-r11}; \
+		mov %0, r0; \
+	" : "=r" (tasks[next_idx].sp));
+
+	// find next task (round-robin style)
+	do {
+		if (++next_idx == MAX_TASKS) {
+			next_idx = 0;
+			break; // task 0 better exist
+		}
+	} while (tasks[next_idx].use == 0);
+
+	// restore
+	//ldmia r0!, {r4-r11};
+	asm("\
+		mov r0, %0; \
+		ldmia r0!, {r4-r11}; \
+		msr psp, r0; \
+		isb; \
+		dsb; \
+	" :: "r" (tasks[next_idx].sp));
+
+	// end
+	asm("\
+		mov r0, #0xFFFFFFFD; \
+		cpsie i; \
+		bx r0; \
+	");
+}
+