path: root/litmus/kfmlp_lock.c

#include <linux/slab.h>
#include <linux/uaccess.h>

#include <litmus/trace.h>
#include <litmus/sched_plugin.h>
#include <litmus/kfmlp_lock.h>

#include <litmus/edf_common.h>

static inline int kfmlp_get_idx(struct kfmlp_semaphore* sem,
								struct kfmlp_queue* queue)
{
	return (queue - &sem->queues[0]);
}

static inline struct kfmlp_queue* kfmlp_get_queue(struct kfmlp_semaphore* sem,
												  struct task_struct* holder)
{
	int i;
	for(i = 0; i < sem->num_resources; ++i)
		if(sem->queues[i].owner == holder)
			return(&sem->queues[i]);
	return(NULL);
}

/* caller is responsible for locking */
static struct task_struct* kfmlp_find_hp_waiter(struct kfmlp_queue *kqueue,
												struct task_struct *skip)
{
	struct list_head	*pos;
	struct task_struct 	*queued, *found = NULL;
	
	list_for_each(pos, &kqueue->wait.task_list) {
		queued  = (struct task_struct*) list_entry(pos, wait_queue_t,
												   task_list)->private;
		
		/* Compare task prios, find high prio task. */
		if (queued != skip && edf_higher_prio(queued, found))
			found = queued;
	}
	return found;
}

static inline struct kfmlp_queue* kfmlp_find_shortest(struct kfmlp_semaphore* sem,
													  struct kfmlp_queue* search_start)
{
	// we start our search at search_start instead of at the beginning of the
	// queue list to load-balance across all resources.
	struct kfmlp_queue* step = search_start;
	struct kfmlp_queue* shortest = sem->shortest_queue;
	
	do
	{
		step = (step+1 != &sem->queues[sem->num_resources]) ?
		step+1 : &sem->queues[0];
		
		if(step->count < shortest->count)
		{
			shortest = step;
			if(step->count == 0)
				break; /* can't get any shorter */
		}
		
	}while(step != search_start);
	
	return(shortest);
}

static struct task_struct* kfmlp_remove_hp_waiter(struct kfmlp_semaphore* sem)
{
	/* must hold sem->lock */
	
	struct kfmlp_queue *my_queue = NULL;
	struct task_struct *max_hp = NULL;
	
	
	struct list_head	*pos;
	struct task_struct 	*queued;
	int i;
	
	for(i = 0; i < sem->num_resources; ++i)
	{
		if( (sem->queues[i].count > 1) &&
		   ((my_queue == NULL) ||
			(edf_higher_prio(sem->queues[i].hp_waiter, my_queue->hp_waiter))) )
		{
			my_queue = &sem->queues[i];
		}
	}
	
	if(my_queue)
	{		
		max_hp = my_queue->hp_waiter;
		
		BUG_ON(!max_hp);
		
		TRACE_CUR("queue %d: stealing %s/%d from queue %d\n",
				  kfmlp_get_idx(sem, my_queue),
				  max_hp->comm, max_hp->pid,
				  kfmlp_get_idx(sem, my_queue));
		
		my_queue->hp_waiter = kfmlp_find_hp_waiter(my_queue, max_hp);
		
		if(tsk_rt(my_queue->owner)->inh_task == max_hp)
		{
			litmus->decrease_prio(my_queue->owner, my_queue->hp_waiter);
		}

		list_for_each(pos, &my_queue->wait.task_list)
		{
			queued  = (struct task_struct*) list_entry(pos, wait_queue_t,
													   task_list)->private;
			/* Compare task prios, find high prio task. */
			if (queued == max_hp)
			{
				/*
				 TRACE_CUR("queue %d: found entry in wait queue.  REMOVING!\n",
				 kfmlp_get_idx(sem, my_queue));
				 */
				__remove_wait_queue(&my_queue->wait,
									list_entry(pos, wait_queue_t, task_list));
				break;
			}
		}
		--(my_queue->count);
	}
	
	return(max_hp);
}

int kfmlp_lock(struct litmus_lock* l)
{
	struct task_struct* t = current;
	struct kfmlp_semaphore *sem = kfmlp_from_lock(l);
	struct kfmlp_queue* my_queue;
	wait_queue_t wait;
	unsigned long flags;
	
	if (!is_realtime(t))
		return -EPERM;
	
	spin_lock_irqsave(&sem->lock, flags);
	
	my_queue = sem->shortest_queue;
	
	if (my_queue->owner) {
		/* resource is not free => must suspend and wait */
		TRACE_CUR("queue %d: Resource is not free => must suspend and wait.\n",
				  kfmlp_get_idx(sem, my_queue));
		
		init_waitqueue_entry(&wait, t);
		
		/* FIXME: interruptible would be nice some day */
		set_task_state(t, TASK_UNINTERRUPTIBLE);
		
		__add_wait_queue_tail_exclusive(&my_queue->wait, &wait);
		
		/* check if we need to activate priority inheritance */
		if (edf_higher_prio(t, my_queue->hp_waiter))
		{
			my_queue->hp_waiter = t;
			if (edf_higher_prio(t, my_queue->owner))
			{
				litmus->increase_prio(my_queue->owner, my_queue->hp_waiter);
			}
		}
		
		++(my_queue->count);
		sem->shortest_queue = kfmlp_find_shortest(sem, my_queue);
		
		/* release lock before sleeping */
		spin_unlock_irqrestore(&sem->lock, flags);
		
		/* 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 (or steal).
		 */
		schedule();
		
		
		if(my_queue->owner == t)
		{
			TRACE_CUR("queue %d: acquired through waiting\n",
					  kfmlp_get_idx(sem, my_queue));
		}
		else
		{
			/* this case may happen if our wait entry was stolen
			 between queues. record where we went. */
			my_queue = kfmlp_get_queue(sem, t);
			
			BUG_ON(!my_queue);
			TRACE_CUR("queue %d: acquired through stealing\n",
					  kfmlp_get_idx(sem, my_queue));
		}
	}
	else
	{
		TRACE_CUR("queue %d: acquired immediately\n",
				  kfmlp_get_idx(sem, my_queue));
		
		my_queue->owner = t;
		
		++(my_queue->count);
		sem->shortest_queue = kfmlp_find_shortest(sem, my_queue);		
		
		spin_unlock_irqrestore(&sem->lock, flags);
	}
	
	return kfmlp_get_idx(sem, my_queue);
}


int kfmlp_unlock(struct litmus_lock* l)
{
	struct task_struct *t = current, *next;
	struct kfmlp_semaphore *sem = kfmlp_from_lock(l);
	struct kfmlp_queue *my_queue;
	unsigned long flags;
	int err = 0;
	
	spin_lock_irqsave(&sem->lock, flags);
	
	my_queue = kfmlp_get_queue(sem, t);
	
	if (!my_queue) {
		err = -EINVAL;
		goto out;
	}
	
	/* check if there are jobs waiting for this resource */
	next = __waitqueue_remove_first(&my_queue->wait);
	if (next) {
		/*
		 TRACE_CUR("queue %d: ASSIGNING %s/%d as owner - next\n",
		 kfmlp_get_idx(sem, my_queue),
		 next->comm, next->pid);
		 */
		/* next becomes the resouce holder */
		my_queue->owner = next;
		
		--(my_queue->count);
		// the '=' of '<=' is a dumb method to attempt to build
		// affinity until tasks can tell us where they ran last...
		if(my_queue->count <= sem->shortest_queue->count)
		{
			sem->shortest_queue = my_queue;
		}	
		
		TRACE_CUR("queue %d: lock ownership passed to %s/%d\n",
				  kfmlp_get_idx(sem, my_queue), next->comm, next->pid);
		
		/* determine new hp_waiter if necessary */
		if (next == my_queue->hp_waiter) {
			TRACE_TASK(next, "was highest-prio waiter\n");
			/* next has the highest priority --- it doesn't need to
			 * inherit.  However, we need to make sure that the
			 * next-highest priority in the queue is reflected in
			 * hp_waiter. */
			my_queue->hp_waiter = kfmlp_find_hp_waiter(my_queue, next);
			if (my_queue->hp_waiter)
				TRACE_TASK(my_queue->hp_waiter, "queue %d: is new highest-prio waiter\n", kfmlp_get_idx(sem, my_queue));
			else
				TRACE("queue %d: no further waiters\n", kfmlp_get_idx(sem, my_queue));
		} else {
			/* Well, if next is not the highest-priority waiter,
			 * then it ought to inherit the highest-priority
			 * waiter's priority. */
			litmus->increase_prio(next, my_queue->hp_waiter);
		}
		
		/* wake up next */
		wake_up_process(next);
	}
	else
	{
		TRACE_CUR("queue %d: looking to steal someone...\n", kfmlp_get_idx(sem, my_queue));
		
		next = kfmlp_remove_hp_waiter(sem); /* returns NULL if nothing to steal */
		
		/*
		 if(next)
		 TRACE_CUR("queue %d: ASSIGNING %s/%d as owner - steal\n",
		 kfmlp_get_idx(sem, my_queue),
		 next->comm, next->pid);
		 */
		
		my_queue->owner = next;
		
		if(next)
		{
			TRACE_CUR("queue %d: lock ownership passed to %s/%d (which was stolen)\n",
					  kfmlp_get_idx(sem, my_queue),
					  next->comm, next->pid);
			
			/* wake up next */
			wake_up_process(next);			
		}
		else
		{
			TRACE_CUR("queue %d: no one to steal.\n", kfmlp_get_idx(sem, my_queue));
			
			--(my_queue->count);
			// the '=' of '<=' is a dumb method to attempt to build
			// affinity until tasks can tell us where they ran last...
			if(my_queue->count <= sem->shortest_queue->count)
			{
				sem->shortest_queue = my_queue;
			}
		}
	}
	
	/* we lose the benefit of priority inheritance (if any) */
	if (tsk_rt(t)->inh_task)
		litmus->decrease_prio(t, NULL);
	
out:
	spin_unlock_irqrestore(&sem->lock, flags);
	
	return err;
}

int kfmlp_close(struct litmus_lock* l)
{
	struct task_struct *t = current;
	struct kfmlp_semaphore *sem = kfmlp_from_lock(l);
	struct kfmlp_queue *my_queue;
	unsigned long flags;
	
	int owner;
	
	spin_lock_irqsave(&sem->lock, flags);
	
	my_queue = kfmlp_get_queue(sem, t);	
	owner = (my_queue) ? (my_queue->owner == t) : 0;
	
	spin_unlock_irqrestore(&sem->lock, flags);
	
	if (owner)
		kfmlp_unlock(l);
	
	return 0;
}

void kfmlp_free(struct litmus_lock* l)
{
	struct kfmlp_semaphore *sem = kfmlp_from_lock(l);
	kfree(sem->queues);
	kfree(sem);
}



struct litmus_lock* kfmlp_new(struct litmus_lock_ops* ops, void* __user args)
{
	struct kfmlp_semaphore* sem;
	int num_resources = 0;
	int i;
	
	if(!access_ok(VERIFY_READ, args, sizeof(num_resources)))
	{
		return(NULL);
	}
	if(__copy_from_user(&num_resources, args, sizeof(num_resources)))
	{
		return(NULL);
	}
	if(num_resources < 1)
	{
		return(NULL);		
	}
	
	sem = kmalloc(sizeof(*sem), GFP_KERNEL);
	if(!sem)
	{
		return(NULL);
	}
	
	sem->queues = kmalloc(sizeof(struct kfmlp_queue)*num_resources, GFP_KERNEL);
	if(!sem->queues)
	{
		kfree(sem);
		return(NULL);
	}
	
	sem->litmus_lock.ops = ops;
	spin_lock_init(&sem->lock);
	sem->num_resources = num_resources;
	
	for(i = 0; i < num_resources; ++i)
	{
		sem->queues[i].owner = NULL;
		sem->queues[i].hp_waiter = NULL;
		init_waitqueue_head(&sem->queues[i].wait);
		sem->queues[i].count = 0;
	}
	
	sem->shortest_queue = &sem->queues[0];

	return &sem->litmus_lock;
}