/*
 * semaphore.c
 *
 *  Created on: Oct 25, 2015
 *      Author: tkl
 */
#include <stdbool.h>
#include <stddef.h>

#include "irq.h"
#include "queue.h"
#include "semaphore.h"
#include "stack.h"
#include "thread.h"
#include "schedule.h"
#include "kernel.h"

extern volatile struct thread_context *current_thread;

int semaphore_init(struct semaphore *sem, int value)
{
	if(NULL == sem)
		return -1;
	sem->cnt = value;
	queue_init(&sem->queue);
	return 0;
}

int semaphore_wait(struct semaphore *sem)
{
	unsigned int irq_state;
	if(NULL == sem)
		return -1;
	irq_state = disable_irq();
	if(sem->cnt > 0) {
		sem->cnt--;
		restore_irq(irq_state);
	}
	else {
		current_thread->sem_queue_node.data = (unsigned int) current_thread;
		queue_push(&sem->queue, (struct queue_node *)&current_thread->sem_queue_node);
		current_thread->status = THREAD_STATUS_WAITING;
		restore_irq(irq_state);
		schedule();
	}
	return 0;
}

int semaphore_post(struct semaphore *sem)
{
	unsigned int irq_state;
	if(NULL == sem)
		return -1;

	irq_state = disable_irq();
	if(queue_is_empty(&sem->queue))
		sem->cnt++;
	else {
		struct queue_node next;
		queue_pop(&sem->queue, &next);
		struct thread_context *ctx = (struct thread_context *) next.data;
		ctx->status = THREAD_STATUS_EXECUTING;
		restore_irq(irq_state);
		schedule();
	}
	restore_irq(irq_state);

	return 0;
}