MourOS task context switching

cswitch.c

inline void push_prg_stack(void) __attribute__((always_inline))
{
	register int *new_psp asm("r0");
	
	asm("msr psp, r0\n\t"
	    "sub r0, #32\n\t"
	    "mov r1, r0\n\t"
	    "stm r1!, {r4-r7}\n\t"
	    "mov r4, r8\n\t"
	    "mov r5, r9\n\t"
	    "mov r6, r10\n\t"
	    "mov r7, r11\n\t"
	    "stm r1!, {r4-r7}\n\t"
	    : "=r" (new_psp)
	    :: "cc", "memory");
	    
	current_tcb->stack = new_psp;
}

inline void pop_prg_stack(void) __attribute__((always_inline))
{
	register int *old_psp asm("r0") = current_task->stack;

	asm("add r1, r0, #16\n\t"
	    "ldm r1!, {r4-r7}\n\t"
	    "mov r8, r4\n\t"
	    "mov r9, r5\n\t"
	    "mov r10, r6\n\t"
	    "mov r11, r7\n\t"
	    "ldm r0!, {r4-r7}\n\t"
	    "mrs r1, psp\n\t"
	    : "r" (old_psp)
	    : "=r" (new_psp)
	    : "cc");
}

enum task_state {
	WAITING_FOR_START = 0,
	INTERRUPTED,
	RUNNING,
	STOPPED
};

typedef struct tcb {
	LIST_ENTRY(tcb) el;
	uint8_t id;
	const uint8_t *name;
	
	void (*task_func)(*void);
	void *task_params;
	
	uint8_t priority;

	enum task_state state;

	int *stack_base;
	int *stack;
	uint32_t stack_size;
} task_t;


LIST_HEAD(tcb_list, tcb) tasks;

struct tcb *current_task;

inline struct tcb *get_next_task()
{
	struct tcb *next_task;
	while (true) {
	}
	return next_task;
}

void pend_sv_handler(void)
{
	push_prg_stack();
	
	if (current_task->state == STOPPED) {
		LIST_REMOVE(current_task, el);
	} else {
		current_task->state = PENDING;
	}
	
	current_task = LIST_NEXT(current_task, el);
	if (current_task == NULL) {
		current_task = LIST_FIRST(&tasks);
	}

	current_task->state = RUNNING;
	
	systick_clear();
	
	pop_prg_stack();
	
	asm("bx lr");
}

#define __init_task_with_stack(task, name, stack_size, stack_id, priority, task_func, task_params) \
	uint8_t __stack_##stack_id[(stack_size)]; \
	init_task((task), (name), __stack_##stack_id, (stack_size), (priority), (task_func), (task_params))

#define init_task_with_stack(task, name, stack_size, priority, task_func, task_params) \
	__init_task_with_stack((task), (name), (stack_size), __COUNTER__, (priority), (task_func), (task_params))

void init_task(task_t *task, const uint8_t *name, uint8_t *stack_base, uint32_t stack_size, uint8_t priority, void (*task_func)(*void), void *task params)
{
	static uint16_t task_id = 0;
	CM_ATOMIC_BLOCK() {
		task->id = task_id++;
	}

	task->priority = priority;
	task->task_func = task_func;
	task->task_params = task_params;
	task->state = PENDING;

	task->name = name;
	task->stack_base = stack_base;
	task->stack_size = stack_size;
	task->stack = stack_base + stack_size - 64;
	
	int *sp = (int *) task->stack;
	for (uint8_t i = 0; i < 16; i++) {
		sp[i] = 0;
	}
	sp[8] = task_func;
	sp[13] = blocking_handler;
	sp[14] = task_runner;
	sp[15] = 1 << 24;
}

void __os_task_runner(struct tcb *task)
{
	task->task_func(task->task_params);
	task->state = DONE;
	os_yield();
}

void os_add_task(task_t *task)
{
	
	CM_ATOMIC_BLOCK() {
		
	}
}

void __os_idle_task(void *params)
{
	while(true);
}

void os_init(void)
{
	tasks = LIST_HEAD_INITIALIZER(tasks);
	
	static uint8_t idle_task_stack[10]; // ?
}

void os_start_tasks(void)
{
	// systick conf
	// yield
}