/*
 * FMLP implementation.
 * Much of the code here is borrowed from include/asm-i386/semaphore.h
 */

#include <asm/atomic.h>

#include <linux/semaphore.h>
#include <linux/sched.h>
#include <linux/wait.h>
#include <linux/spinlock.h>

#include <litmus/litmus.h>
#include <litmus/sched_plugin.h>
#include <litmus/edf_common.h>

#include <litmus/fdso.h>

#include <litmus/trace.h>

#ifdef CONFIG_FMLP

static  void* create_fmlp_semaphore(void)
{
	struct pi_semaphore* sem;
	int i;

	sem = kmalloc(sizeof(*sem), GFP_KERNEL);
	if (!sem)
		return NULL;
	atomic_set(&sem->count, 1);
	sem->sleepers = 0;
	init_waitqueue_head(&sem->wait);
	sem->hp.task = NULL;
	sem->holder = NULL;
	for (i = 0; i < NR_CPUS; i++)
		sem->hp.cpu_task[i] = NULL;
	return sem;
}

static int open_fmlp_semaphore(struct od_table_entry* entry, void* __user arg)
{
	if (!fmlp_active())
		return -EBUSY;
	return 0;
}

static void destroy_fmlp_semaphore(void* sem)
{
	/* XXX assert invariants */
	kfree(sem);
}

struct fdso_ops fmlp_sem_ops = {
	.create  = create_fmlp_semaphore,
	.open    = open_fmlp_semaphore,
	.destroy = destroy_fmlp_semaphore
};

struct wq_pair {
	struct task_struct*  tsk;
	struct pi_semaphore* sem;
};

static int rt_pi_wake_up(wait_queue_t *wait, unsigned mode, int sync,
			   void *key)
{
	struct wq_pair* wqp   = (struct wq_pair*) wait->private;
	set_rt_flags(wqp->tsk, RT_F_EXIT_SEM);
	litmus->inherit_priority(wqp->sem, wqp->tsk);
	TRACE_TASK(wqp->tsk,
		   "woken up by rt_pi_wake_up() (RT_F_SEM_EXIT, PI)\n");
	/* point to task for default_wake_function() */
	wait->private = wqp->tsk;
	default_wake_function(wait, mode, sync, key);

	/* Always return true since we know that if we encountered a task
	 * that was already running the wake_up raced with the schedule in
	 * rt_pi_down(). In that case the task in rt_pi_down() will be scheduled
	 * immediately and own the lock. We must not wake up another task in
	 * any case.
	 */
	return 1;
}

/* caller is responsible for locking */
int edf_set_hp_task(struct pi_semaphore *sem)
{
	struct list_head	*tmp, *next;
	struct task_struct 	*queued;
	int ret = 0;

	sem->hp.task = NULL;
	list_for_each_safe(tmp, next, &sem->wait.task_list) {
		queued  = ((struct wq_pair*)
			list_entry(tmp, wait_queue_t,
				   task_list)->private)->tsk;

		/* Compare task prios, find high prio task. */
		if (edf_higher_prio(queued, sem->hp.task)) {
			sem->hp.task = queued;
			ret = 1;
		}
	}
	return ret;
}

/* caller is responsible for locking */
int edf_set_hp_cpu_task(struct pi_semaphore *sem, int cpu)
{
	struct list_head	*tmp, *next;
	struct task_struct 	*queued;
	int ret = 0;

	sem->hp.cpu_task[cpu] = NULL;
	list_for_each_safe(tmp, next, &sem->wait.task_list) {
		queued  = ((struct wq_pair*)
			list_entry(tmp, wait_queue_t,
				   task_list)->private)->tsk;

		/* Compare task prios, find high prio task. */
		if (get_partition(queued) == cpu &&
		    edf_higher_prio(queued, sem->hp.cpu_task[cpu])) {
			sem->hp.cpu_task[cpu] = queued;
			ret = 1;
		}
	}
	return ret;
}

static int do_fmlp_down(struct pi_semaphore* sem)
{
	unsigned long flags;
	struct task_struct *tsk = current;
	struct wq_pair pair;
	int suspended = 1;
	wait_queue_t wait = {
		.private = &pair,
		.func    = rt_pi_wake_up,
		.task_list = {NULL, NULL}
	};

	pair.tsk = tsk;
	pair.sem = sem;
	spin_lock_irqsave(&sem->wait.lock, flags);

	if (atomic_dec_return(&sem->count) < 0 ||
	    waitqueue_active(&sem->wait)) {
		/* we need to suspend */
		tsk->state = TASK_UNINTERRUPTIBLE;
		add_wait_queue_exclusive_locked(&sem->wait, &wait);

		TRACE_CUR("suspends on PI lock %p\n", sem);
		litmus->pi_block(sem, tsk);

		/* release lock before sleeping */
		spin_unlock_irqrestore(&sem->wait.lock, flags);

		TS_PI_DOWN_END;
		preempt_enable_no_resched();


		/* we depend on the FIFO order
		 * Thus, we don't need to recheck when we wake up, we
		 * are guaranteed to have the lock since there is only one
		 * wake up per release
		 */
		schedule();

		TRACE_CUR("woke up, now owns PI lock %p\n", sem);

		/* try_to_wake_up() set our state to TASK_RUNNING,
		 * all we need to do is to remove our wait queue entry
		 */
		remove_wait_queue(&sem->wait, &wait);
	} else {
		/* no priority inheritance necessary, since there are no queued
		 * tasks.
		 */
		suspended = 0;
		TRACE_CUR("acquired PI lock %p, no contention\n", sem);
		sem->holder  = tsk;
		sem->hp.task = tsk;
		litmus->inherit_priority(sem, tsk);
		spin_unlock_irqrestore(&sem->wait.lock, flags);
	}
	return suspended;
}

static void do_fmlp_up(struct pi_semaphore* sem)
{
	unsigned long flags;

	spin_lock_irqsave(&sem->wait.lock, flags);

	TRACE_CUR("releases PI lock %p\n", sem);
	litmus->return_priority(sem);
	sem->holder = NULL;
	if (atomic_inc_return(&sem->count) < 1)
		/* there is a task queued */
		wake_up_locked(&sem->wait);

	spin_unlock_irqrestore(&sem->wait.lock, flags);
}

asmlinkage long sys_fmlp_down(int sem_od)
{
	long ret = 0;
	struct pi_semaphore * sem;
	int suspended = 0;

	preempt_disable();
	TS_PI_DOWN_START;

	sem = lookup_fmlp_sem(sem_od);
	if (sem)
		suspended = do_fmlp_down(sem);
	else
		ret = -EINVAL;

	if (!suspended) {
		TS_PI_DOWN_END;
		preempt_enable();
	}

	return ret;
}

asmlinkage long sys_fmlp_up(int sem_od)
{
	long ret = 0;
	struct pi_semaphore * sem;

	preempt_disable();
	TS_PI_UP_START;

	sem = lookup_fmlp_sem(sem_od);
	if (sem)
		do_fmlp_up(sem);
	else
		ret = -EINVAL;


	TS_PI_UP_END;
	preempt_enable();

	return ret;
}

#else

struct fdso_ops fmlp_sem_ops = {};

asmlinkage long sys_fmlp_down(int sem_od)
{
	return -ENOSYS;
}

asmlinkage long sys_fmlp_up(int sem_od)
{
	return -ENOSYS;
}

#endif