kosmos/source/firmware/kernel/thread.c

/*
 * thread.c
 *
 *  Created on: Mar 16, 2015
 *      Author: tkl
 */
#include <stddef.h>
#include <stdbool.h>

#include "irq.h"
#include "stack.h"
#include "ctx.h"
#include "queue.h"
#include "kernel.h"
#include "thread.h"

extern volatile struct thread_context *current_thread;
volatile struct thread_list threads;

void thread_init(void)
{
	int i;
	for(i = 0; i < MAX_THREAD_COUNT; i++)
		threads.list[i] = NULL;
	threads.count = 0;
}

static void sort_thread_list_by_priority(void)
{
	unsigned int irq, in, out;
	struct thread_context *tmp;

	irq = disable_irq();

	for(out = 0; out < threads.count; out++) {
		for(in = 0; in < threads.count - out - 1; in++) {
			if(threads.list[in]->priority < threads.list[in + 1]->priority) {
				tmp = threads.list[in];
				threads.list[in] = threads.list[in + 1];
				threads.list[in + 1] = tmp;
			}
		}
	}

	restore_irq(irq);
}

static int thread_list_add_node(struct thread_context *node)
{
	unsigned int irq;
	int i = -1;
	if((NULL == node) || (threads.count >= MAX_THREAD_COUNT))
		return i;
	irq = disable_irq();
	for(i = 0; i < MAX_THREAD_COUNT; i++) {
		if(threads.list[i] == NULL) {
			threads.list[i] = node;
			threads.count++;
			sort_thread_list_by_priority();
			break;
		}
	}
	restore_irq(irq);
	return i;
}

static void thread_list_defragment(void)
{
	int i;
	unsigned int irq_state = disable_irq();
	for(i = 0; i < (MAX_THREAD_COUNT - 1); i++) {
		if((threads.list[i] == NULL) && (threads.list[i + 1] != NULL)) {
			threads.list[i] = threads.list[i + 1];
			threads.list[i + 1] = NULL;
		}
	}
	restore_irq(irq_state);
}

static int thread_list_remove_node(struct thread_context *node)
{
	int i = -1;
	unsigned int irq_state;
	if(NULL == node)
		return i;
	irq_state = disable_irq();
	for(i = 0; i < MAX_THREAD_COUNT; i++) {
		if(threads.list[i] == node) {
			threads.list[i] = NULL;
			threads.count--;
			thread_list_defragment();
			break;
		}
	}
	restore_irq(irq_state);
	return i;
}

struct thread_context *thread_create(
		struct thread_context *thread,
		stack_t *stack,
		unsigned int stack_size,
		void (*thread_func)(void *),
		void *arg,
		enum thread_priority priority)
{
	if((thread == NULL) || (stack == NULL) || (thread_func == NULL) ||
			(stack_size == 0))
		return NULL;

	thread->pid = threads.count;
	thread->stack = stack;
	thread->stack_size = stack_size;
	thread->sp = stack_init(thread_func, arg, stack, stack_size);
	thread->status = THREAD_STATUS_EXECUTING;
	thread->priority = priority;
	thread_list_add_node(thread);

	return thread;
}

void thread_switch_context(void) {
	int i;
	for(i = 0; i < threads.count; i++) {
		if(threads.list[i]->status == THREAD_STATUS_EXECUTING) {
			current_thread = threads.list[i];
			return;
		}
	}
}

void thread_exit(void)
{
	disable_irq();
	thread_list_remove_node((struct thread_context *)current_thread);
	thread_switch_context();
	restore_context();
}

void blocking_read_wakeup(const void *src)
{
	int i;
	for(i = 0; i < threads.count; i++) {
		if(	(threads.list[i]->status == THREAD_STATUS_BLOCKING) &&
			(threads.list[i]->wakeup_blocking_source == src))
				threads.list[i]->status = THREAD_STATUS_EXECUTING;
	}
}