heap; multitasking

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

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))
+	if (!(ticks % 10))
 		SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
+
+	asm("mov lr, %0; bx lr" :: "r" (lr));
 }
 

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;
+}

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_

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_

link.ld

@@ -34,9 +34,9 @@ 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
@@ -166,4 +166,4 @@ SECTIONS
   .ARM.attributes 0 : { *(.ARM.attributes) }
 }
 
-
+_total_ram = _estack - _end;

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 (;;); }
 

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

stm32l4xx_it.c

@@ -1,24 +1,28 @@
-#include <stdint.h>
+#include <stm32l476xx.h>
 
 void NMI_Handler(void) {}
 
 void HardFault_Handler(void)
 {
+	GPIOA->BSRR |= (1 << 5) | (1 << 6);
 	while (1);
 }
 
 void MemManage_Handler(void)
 {
+	GPIOA->BSRR |= (1 << 5) | (1 << 6);
 	while (1);
 }
 
 void BusFault_Handler(void)
 {
+	GPIOA->BSRR |= (1 << 5) | (1 << 6);
 	while (1);
 }
 
 void UsageFault_Handler(void)
 {
+	GPIOA->BSRR |= (1 << 5) | (1 << 6);
 	while (1);
 }
 

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; \
+	");
+}
+