add kfmlp as separate file

2 files changed, 440 insertions, 0 deletions

diff --git a/include/litmus/kfmlp_lock.h b/include/litmus/kfmlp_lock.h
new file mode 100644
index 000000000000..49156a9ba4ea
--- /dev/null
+++ b/include/litmus/kfmlp_lock.h
@@ -0,0 +1,39 @@
+#ifndef LITMUS_KFMLP_H
+#define LITMUS_KFMLP_H
+
+#include <litmus/litmus.h>
+#include <litmus/locking.h>
+
+/* struct for semaphore with priority inheritance */
+struct kfmlp_queue
+{
+        wait_queue_head_t wait;
+        struct task_struct* owner;
+        struct task_struct* hp_waiter;
+        int count; /* number of waiters + holder */
+};
+
+struct kfmlp_semaphore
+{
+        struct litmus_lock litmus_lock;
+        
+        spinlock_t      lock;
+        
+        int num_resources; /* aka k */
+        
+        struct kfmlp_queue *queues; /* array */
+        struct kfmlp_queue *shortest_queue; /* pointer to shortest queue */
+};
+
+static inline struct kfmlp_semaphore* kfmlp_from_lock(struct litmus_lock* lock)
+{
+        return container_of(lock, struct kfmlp_semaphore, litmus_lock);
+}
+
+int kfmlp_lock(struct litmus_lock* l);
+int kfmlp_unlock(struct litmus_lock* l);
+int kfmlp_close(struct litmus_lock* l);
+void kfmlp_free(struct litmus_lock* l);
+struct litmus_lock* kfmlp_new(struct litmus_lock_ops*, void* __user arg);
+
+#endif
\ No newline at end of file
diff --git a/litmus/kfmlp_lock.c b/litmus/kfmlp_lock.c
new file mode 100644
index 000000000000..37302064bd8c
--- /dev/null
+++ b/litmus/kfmlp_lock.c
@@ -0,0 +1,401 @@
+#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;
+}