1 files changed, 711 insertions, 45 deletions

diff --git a/litmus/sched_gsn_edf.c b/litmus/sched_gsn_edf.c
index 3092797480f8..d04e0703c154 100644
--- a/litmus/sched_gsn_edf.c
+++ b/litmus/sched_gsn_edf.c
@@ -12,6 +12,8 @@
 #include <linux/percpu.h>
 #include <linux/sched.h>
 #include <linux/slab.h>
+#include <linux/uaccess.h>
+
 
 #include <litmus/litmus.h>
 #include <litmus/jobs.h>
@@ -25,6 +27,19 @@
 
 #include <linux/module.h>
 
+#ifdef CONFIG_SCHED_CPU_AFFINITY
+#include <litmus/affinity.h>
+#endif
+
+#ifdef CONFIG_LITMUS_SOFTIRQD
+#include <litmus/litmus_softirq.h>
+#endif
+
+#ifdef CONFIG_LITMUS_NVIDIA
+#include <litmus/nvidia_info.h>
+#endif
+
+
 /* Overview of GSN-EDF operations.
  *
  * For a detailed explanation of GSN-EDF have a look at the FMLP paper. This
@@ -253,21 +268,52 @@ static noinline void requeue(struct task_struct* task)
         }
 }
 
+#ifdef CONFIG_SCHED_CPU_AFFINITY
+static cpu_entry_t* gsnedf_get_nearest_available_cpu(cpu_entry_t* start)
+{
+        cpu_entry_t* affinity;
+
+        get_nearest_available_cpu(affinity, start, gsnedf_cpu_entries,
+#ifdef CONFIG_RELEASE_MASTER
+                                        gsnedf.release_master
+#else
+                                        -1
+#endif
+                                        );
+
+        return(affinity);
+}
+#endif
+
 /* check for any necessary preemptions */
 static void check_for_preemptions(void)
 {
         struct task_struct *task;
-        cpu_entry_t* last;
+        cpu_entry_t *last;
 
         for(last = lowest_prio_cpu();
             edf_preemption_needed(&gsnedf, last->linked);
             last = lowest_prio_cpu()) {
                 /* preemption necessary */
                 task = __take_ready(&gsnedf);
-                TRACE("check_for_preemptions: attempting to link task %d to %d\n",
-                      task->pid, last->cpu);
+
+#ifdef CONFIG_SCHED_CPU_AFFINITY
+                {
+                        cpu_entry_t* affinity = gsnedf_get_nearest_available_cpu(
+                                                        &per_cpu(gsnedf_cpu_entries, task_cpu(task)));
+                        if(affinity)
+                                last = affinity;
+                        else if(last->linked)
+                                requeue(last->linked);
+                }
+#else
                 if (last->linked)
                         requeue(last->linked);
+#endif
+
+                TRACE("check_for_preemptions: attempting to link task %d to %d\n", 
+                                                task->pid, last->cpu);
+
                 link_task_to_cpu(task, last);
                 preempt(last);
         }
@@ -277,7 +323,7 @@ static void check_for_preemptions(void)
 static noinline void gsnedf_job_arrival(struct task_struct* task)
 {
         BUG_ON(!task);
-
+    
         requeue(task);
         check_for_preemptions();
 }
@@ -298,9 +344,13 @@ static void gsnedf_release_jobs(rt_domain_t* rt, struct bheap* tasks)
 static noinline void job_completion(struct task_struct *t, int forced)
 {
         BUG_ON(!t);
-
+        
         sched_trace_task_completion(t, forced);
 
+#ifdef CONFIG_LITMUS_NVIDIA
+        atomic_set(&tsk_rt(t)->nv_int_count, 0);
+#endif
+
         TRACE_TASK(t, "job_completion().\n");
 
         /* set flags */
@@ -401,17 +451,19 @@ static struct task_struct* gsnedf_schedule(struct task_struct * prev)
         TRACE_TASK(prev, "invoked gsnedf_schedule.\n");
 #endif
 
+        /*
         if (exists)
                 TRACE_TASK(prev,
                            "blocks:%d out_of_time:%d np:%d sleep:%d preempt:%d "
                            "state:%d sig:%d\n",
                            blocks, out_of_time, np, sleep, preempt,
                            prev->state, signal_pending(prev));
+         */
+        
         if (entry->linked && preempt)
                 TRACE_TASK(prev, "will be preempted by %s/%d\n",
                            entry->linked->comm, entry->linked->pid);
 
-
         /* If a task blocks we have no choice but to reschedule.
          */
         if (blocks)
@@ -456,12 +508,15 @@ static struct task_struct* gsnedf_schedule(struct task_struct * prev)
                         entry->scheduled->rt_param.scheduled_on = NO_CPU;
                         TRACE_TASK(entry->scheduled, "scheduled_on = NO_CPU\n");
                 }
-        } else
+        }
+        else
+        {
                 /* Only override Linux scheduler if we have a real-time task
                  * scheduled that needs to continue.
                  */
                 if (exists)
                         next = prev;
+        }
 
         sched_state_task_picked();
 
@@ -486,8 +541,9 @@ static struct task_struct* gsnedf_schedule(struct task_struct * prev)
 static void gsnedf_finish_switch(struct task_struct *prev)
 {
         cpu_entry_t*    entry = &__get_cpu_var(gsnedf_cpu_entries);
-
+        
         entry->scheduled = is_realtime(current) ? current : NULL;
+        
 #ifdef WANT_ALL_SCHED_EVENTS
         TRACE_TASK(prev, "switched away from\n");
 #endif
@@ -536,11 +592,14 @@ static void gsnedf_task_new(struct task_struct * t, int on_rq, int running)
 static void gsnedf_task_wake_up(struct task_struct *task)
 {
         unsigned long flags;
-        lt_t now;
-
+        lt_t now;       
+        
         TRACE_TASK(task, "wake_up at %llu\n", litmus_clock());
 
         raw_spin_lock_irqsave(&gsnedf_lock, flags);
+        
+        
+#if 0  // sporadic task model
         /* We need to take suspensions because of semaphores into
          * account! If a job resumes after being suspended due to acquiring
          * a semaphore, it should never be treated as a new job release.
@@ -562,19 +621,26 @@ static void gsnedf_task_wake_up(struct task_struct *task)
                         }
                 }
         }
+#else  // periodic task model
+        set_rt_flags(task, RT_F_RUNNING);
+#endif
+        
         gsnedf_job_arrival(task);
         raw_spin_unlock_irqrestore(&gsnedf_lock, flags);
 }
 
 static void gsnedf_task_block(struct task_struct *t)
 {
+        // TODO: is this called on preemption??
         unsigned long flags;
 
         TRACE_TASK(t, "block at %llu\n", litmus_clock());
 
         /* unlink if necessary */
         raw_spin_lock_irqsave(&gsnedf_lock, flags);
+        
         unlink(t);
+        
         raw_spin_unlock_irqrestore(&gsnedf_lock, flags);
 
         BUG_ON(!is_realtime(t));
@@ -608,51 +674,53 @@ static long gsnedf_admit_task(struct task_struct* tsk)
 
 #include <litmus/fdso.h>
 
-/* called with IRQs off */
-static void set_priority_inheritance(struct task_struct* t, struct task_struct* prio_inh)
+
+static void __set_priority_inheritance(struct task_struct* t, struct task_struct* prio_inh)
 {
         int linked_on;
-        int check_preempt = 0;
-
-        raw_spin_lock(&gsnedf_lock);
-
-        TRACE_TASK(t, "inherits priority from %s/%d\n", prio_inh->comm, prio_inh->pid);
+        int check_preempt = 0;  
+        
+        if(prio_inh != NULL)
+                TRACE_TASK(t, "inherits priority from %s/%d\n", prio_inh->comm, prio_inh->pid);
+        else
+                TRACE_TASK(t, "inherits priority from %p\n", prio_inh);
+        
+        sched_trace_eff_prio_change(t, prio_inh);
+        
         tsk_rt(t)->inh_task = prio_inh;
-
+        
         linked_on  = tsk_rt(t)->linked_on;
-
+        
         /* If it is scheduled, then we need to reorder the CPU heap. */
         if (linked_on != NO_CPU) {
                 TRACE_TASK(t, "%s: linked  on %d\n",
-                           __FUNCTION__, linked_on);
+                                   __FUNCTION__, linked_on);
                 /* Holder is scheduled; need to re-order CPUs.
                  * We can't use heap_decrease() here since
                  * the cpu_heap is ordered in reverse direction, so
                  * it is actually an increase. */
                 bheap_delete(cpu_lower_prio, &gsnedf_cpu_heap,
-                            gsnedf_cpus[linked_on]->hn);
+                     gsnedf_cpus[linked_on]->hn);
                 bheap_insert(cpu_lower_prio, &gsnedf_cpu_heap,
-                            gsnedf_cpus[linked_on]->hn);
+                     gsnedf_cpus[linked_on]->hn);
         } else {
                 /* holder may be queued: first stop queue changes */
                 raw_spin_lock(&gsnedf.release_lock);
                 if (is_queued(t)) {
-                        TRACE_TASK(t, "%s: is queued\n",
-                                   __FUNCTION__);
+                        TRACE_TASK(t, "%s: is queued\n", __FUNCTION__);
+
                         /* We need to update the position of holder in some
                          * heap. Note that this could be a release heap if we
                          * budget enforcement is used and this job overran. */
-                        check_preempt =
-                                !bheap_decrease(edf_ready_order,
-                                               tsk_rt(t)->heap_node);
+                        check_preempt = !bheap_decrease(edf_ready_order, tsk_rt(t)->heap_node);
+
                 } else {
                         /* Nothing to do: if it is not queued and not linked
                          * then it is either sleeping or currently being moved
                          * by other code (e.g., a timer interrupt handler) that
                          * will use the correct priority when enqueuing the
                          * task. */
-                        TRACE_TASK(t, "%s: is NOT queued => Done.\n",
-                                   __FUNCTION__);
+                        TRACE_TASK(t, "%s: is NOT queued => Done.\n", __FUNCTION__);
                 }
                 raw_spin_unlock(&gsnedf.release_lock);
 
@@ -666,34 +734,148 @@ static void set_priority_inheritance(struct task_struct* t, struct task_struct*
                         /* heap_decrease() hit the top level of the heap: make
                          * sure preemption checks get the right task, not the
                          * potentially stale cache. */
-                        bheap_uncache_min(edf_ready_order,
-                                         &gsnedf.ready_queue);
+                        bheap_uncache_min(edf_ready_order, &gsnedf.ready_queue);
                         check_for_preemptions();
                 }
         }
+}
 
+/* called with IRQs off */
+static void set_priority_inheritance(struct task_struct* t, struct task_struct* prio_inh)
+{
+        raw_spin_lock(&gsnedf_lock);
+
+        __set_priority_inheritance(t, prio_inh);
+
+#ifdef CONFIG_LITMUS_SOFTIRQD
+        if(tsk_rt(t)->cur_klitirqd != NULL)
+        {
+                TRACE_TASK(t, "%s/%d inherits a new priority!\n",
+                                tsk_rt(t)->cur_klitirqd->comm, tsk_rt(t)->cur_klitirqd->pid);
+
+                __set_priority_inheritance(tsk_rt(t)->cur_klitirqd, prio_inh);
+        }
+#endif
+        
         raw_spin_unlock(&gsnedf_lock);
 }
 
+
+/* called with IRQs off */
+static void __clear_priority_inheritance(struct task_struct* t)
+{
+    TRACE_TASK(t, "priority restored\n");
+        
+    if(tsk_rt(t)->scheduled_on != NO_CPU)
+    {
+                sched_trace_eff_prio_change(t, NULL);
+                
+        tsk_rt(t)->inh_task = NULL;
+        
+        /* Check if rescheduling is necessary. We can't use heap_decrease()
+         * since the priority was effectively lowered. */
+        unlink(t);
+        gsnedf_job_arrival(t);
+    }
+    else
+    {
+        __set_priority_inheritance(t, NULL);
+    }
+
+#ifdef CONFIG_LITMUS_SOFTIRQD
+        if(tsk_rt(t)->cur_klitirqd != NULL)
+        {
+                TRACE_TASK(t, "%s/%d inheritance set back to owner.\n",
+                                tsk_rt(t)->cur_klitirqd->comm, tsk_rt(t)->cur_klitirqd->pid);
+
+                if(tsk_rt(tsk_rt(t)->cur_klitirqd)->scheduled_on != NO_CPU)
+                {
+                        sched_trace_eff_prio_change(tsk_rt(t)->cur_klitirqd, t);
+                        
+                        tsk_rt(tsk_rt(t)->cur_klitirqd)->inh_task = t;
+                        
+                        /* Check if rescheduling is necessary. We can't use heap_decrease()
+                         * since the priority was effectively lowered. */
+                        unlink(tsk_rt(t)->cur_klitirqd);
+                        gsnedf_job_arrival(tsk_rt(t)->cur_klitirqd);
+                }
+                else
+                {
+                        __set_priority_inheritance(tsk_rt(t)->cur_klitirqd, t);
+                }
+        }
+#endif
+}
+
 /* called with IRQs off */
 static void clear_priority_inheritance(struct task_struct* t)
 {
         raw_spin_lock(&gsnedf_lock);
+        __clear_priority_inheritance(t);
+        raw_spin_unlock(&gsnedf_lock);
+}
 
-        /* A job only stops inheriting a priority when it releases a
-         * resource. Thus we can make the following assumption.*/
-        BUG_ON(tsk_rt(t)->scheduled_on == NO_CPU);
-
-        TRACE_TASK(t, "priority restored\n");
-        tsk_rt(t)->inh_task = NULL;
+#ifdef CONFIG_LITMUS_SOFTIRQD
+/* called with IRQs off */
+static void set_priority_inheritance_klitirqd(struct task_struct* klitirqd,
+                                                                                          struct task_struct* old_owner,
+                                                                                          struct task_struct* new_owner)
+{
+        BUG_ON(!(tsk_rt(klitirqd)->is_proxy_thread));
+        
+        raw_spin_lock(&gsnedf_lock);
+        
+        if(old_owner != new_owner)
+        {
+                if(old_owner)
+                {
+                        // unreachable?
+                        tsk_rt(old_owner)->cur_klitirqd = NULL;
+                }
+        
+                TRACE_TASK(klitirqd, "giving ownership to %s/%d.\n",
+                                   new_owner->comm, new_owner->pid);
 
-        /* Check if rescheduling is necessary. We can't use heap_decrease()
-         * since the priority was effectively lowered. */
-        unlink(t);
-        gsnedf_job_arrival(t);
+                tsk_rt(new_owner)->cur_klitirqd = klitirqd;
+        }
+        
+        __set_priority_inheritance(klitirqd,
+                        (tsk_rt(new_owner)->inh_task == NULL) ?
+                                new_owner :
+                                tsk_rt(new_owner)->inh_task);
+        
+        raw_spin_unlock(&gsnedf_lock);
+}
 
+/* called with IRQs off */
+static void clear_priority_inheritance_klitirqd(struct task_struct* klitirqd,
+                                                                                                struct task_struct* old_owner)
+{
+        BUG_ON(!(tsk_rt(klitirqd)->is_proxy_thread));
+        
+        raw_spin_lock(&gsnedf_lock);
+    
+    TRACE_TASK(klitirqd, "priority restored\n");
+        
+    if(tsk_rt(klitirqd)->scheduled_on != NO_CPU)
+    {
+        tsk_rt(klitirqd)->inh_task = NULL;
+        
+        /* Check if rescheduling is necessary. We can't use heap_decrease()
+         * since the priority was effectively lowered. */
+        unlink(klitirqd);
+        gsnedf_job_arrival(klitirqd);
+    }
+    else
+    {
+        __set_priority_inheritance(klitirqd, NULL);
+    }
+        
+        tsk_rt(old_owner)->cur_klitirqd = NULL;
+        
         raw_spin_unlock(&gsnedf_lock);
 }
+#endif
 
 
 /* ******************** FMLP support ********************** */
@@ -892,11 +1074,477 @@ static struct litmus_lock* gsnedf_new_fmlp(void)
         return &sem->litmus_lock;
 }
 
+
+
+
+
+
+
+/* ******************** KFMLP support ********************** */
+
+/* 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);
+}
+
+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(my_queue->hp_waiter)
+                        TRACE_CUR("queue %d: new hp_waiter is %s/%d\n",
+                                          kfmlp_get_idx(sem, my_queue),
+                                          my_queue->hp_waiter->comm,
+                                          my_queue->hp_waiter->pid);
+                else
+                        TRACE_CUR("queue %d: new hp_waiter is %p\n",
+                                          kfmlp_get_idx(sem, my_queue), NULL);
+                 */
+                
+                raw_spin_lock(&gsnedf_lock);
+                
+                /*
+                if(my_queue->owner)
+                        TRACE_CUR("queue %d: owner is %s/%d\n",
+                                          kfmlp_get_idx(sem, my_queue),
+                                          my_queue->owner->comm,
+                                          my_queue->owner->pid);
+                else
+                        TRACE_CUR("queue %d: owner is %p\n",
+                                          kfmlp_get_idx(sem, my_queue),
+                                          NULL);
+                 */
+                
+                if(tsk_rt(my_queue->owner)->inh_task == max_hp)
+                {
+                        __clear_priority_inheritance(my_queue->owner);
+                        if(my_queue->hp_waiter != NULL)
+                        {
+                                __set_priority_inheritance(my_queue->owner, my_queue->hp_waiter);
+                        }
+                }
+                raw_spin_unlock(&gsnedf_lock);
+                
+                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 gsnedf_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))
+                        {
+                                set_priority_inheritance(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 gsnedf_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);
+                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. */
+                        set_priority_inheritance(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);
+                        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)
+                clear_priority_inheritance(t);
+        
+out:
+        spin_unlock_irqrestore(&sem->lock, flags);
+        
+        return err;
+}
+
+int gsnedf_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)
+                gsnedf_kfmlp_unlock(l);
+        
+        return 0;
+}
+
+void gsnedf_kfmlp_free(struct litmus_lock* l)
+{
+        struct kfmlp_semaphore *sem = kfmlp_from_lock(l);
+        kfree(sem->queues);
+        kfree(sem);
+}
+
+static struct litmus_lock_ops gsnedf_kfmlp_lock_ops = {
+        .close  = gsnedf_kfmlp_close,
+        .lock   = gsnedf_kfmlp_lock,
+        .unlock = gsnedf_kfmlp_unlock,
+        .deallocate = gsnedf_kfmlp_free,
+};
+
+static struct litmus_lock* gsnedf_new_kfmlp(void* __user arg, int* ret_code)
+{
+        struct kfmlp_semaphore* sem;
+        int num_resources = 0;
+        int i;
+        
+        if(!access_ok(VERIFY_READ, arg, sizeof(num_resources)))
+        {
+                *ret_code = -EINVAL;
+                return(NULL);
+        }
+        if(__copy_from_user(&num_resources, arg, sizeof(num_resources)))
+        {
+                *ret_code = -EINVAL;
+                return(NULL);
+        }
+        if(num_resources < 1)
+        {
+                *ret_code = -EINVAL;
+                return(NULL);           
+        }
+        
+        sem = kmalloc(sizeof(*sem), GFP_KERNEL);
+        if(!sem)
+        {
+                *ret_code = -ENOMEM;
+                return NULL;
+        }
+        
+        sem->queues = kmalloc(sizeof(struct kfmlp_queue)*num_resources, GFP_KERNEL);
+        if(!sem->queues)
+        {
+                kfree(sem);
+                *ret_code = -ENOMEM;
+                return NULL;            
+        }
+        
+        sem->litmus_lock.ops = &gsnedf_kfmlp_lock_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];
+        
+        *ret_code = 0;
+        return &sem->litmus_lock;
+}
+
+
+
+
+
 /* **** lock constructor **** */
 
 
 static long gsnedf_allocate_lock(struct litmus_lock **lock, int type,
-                                 void* __user unused)
+                                 void* __user arg)
 {
         int err = -ENXIO;
 
@@ -911,7 +1559,10 @@ static long gsnedf_allocate_lock(struct litmus_lock **lock, int type,
                 else
                         err = -ENOMEM;
                 break;
-
+                        
+        case KFMLP_SEM:
+                *lock = gsnedf_new_kfmlp(arg, &err);
+                break;
         };
 
         return err;
@@ -919,7 +1570,6 @@ static long gsnedf_allocate_lock(struct litmus_lock **lock, int type,
 
 #endif
 
-
 static long gsnedf_activate_plugin(void)
 {
         int cpu;
@@ -946,6 +1596,15 @@ static long gsnedf_activate_plugin(void)
                 }
 #endif
         }
+    
+#ifdef CONFIG_LITMUS_SOFTIRQD
+    spawn_klitirqd(NULL);
+#endif
+
+#ifdef CONFIG_LITMUS_NVIDIA
+        init_nvidia_info();
+#endif
+        
         return 0;
 }
 
@@ -963,8 +1622,15 @@ static struct sched_plugin gsn_edf_plugin __cacheline_aligned_in_smp = {
         .admit_task             = gsnedf_admit_task,
         .activate_plugin        = gsnedf_activate_plugin,
 #ifdef CONFIG_LITMUS_LOCKING
-        .allocate_lock          = gsnedf_allocate_lock,
+        .allocate_lock  = gsnedf_allocate_lock,
+    .set_prio_inh   = set_priority_inheritance,
+    .clear_prio_inh = clear_priority_inheritance,       
+#endif
+#ifdef CONFIG_LITMUS_SOFTIRQD
+        .set_prio_inh_klitirqd = set_priority_inheritance_klitirqd,
+        .clear_prio_inh_klitirqd = clear_priority_inheritance_klitirqd,
 #endif
+
 };