1 files changed, 333 insertions, 172 deletions

diff --git a/litmus/sched_gsn_edf.c b/litmus/sched_gsn_edf.c
index 0050d375ab6c..3d653bdca357 100644
--- a/litmus/sched_gsn_edf.c
+++ b/litmus/sched_gsn_edf.c
@@ -108,7 +108,6 @@ typedef struct  {
         int                     cpu;
         struct task_struct*     linked;         /* only RT tasks */
         struct task_struct*     scheduled;      /* only RT tasks */
-        int                             in_heap;                /* == 0/1: not in / in heap*/
         struct binheap_node hn;
 } cpu_entry_t;
 DEFINE_PER_CPU(cpu_entry_t, gsnedf_cpu_entries);
@@ -144,10 +143,10 @@ static int cpu_lower_prio(struct binheap_node *_a, struct binheap_node *_b)
  */
 static void update_cpu_position(cpu_entry_t *entry)
 {
-        if (likely(entry->in_heap))
+        if (likely(binheap_is_in_heap(&entry->hn))) {
                 binheap_delete(&entry->hn, &gsnedf_cpu_heap);
+        }
         binheap_add(&entry->hn, &gsnedf_cpu_heap, cpu_entry_t, hn);
-        entry->in_heap = 1;
 }
 
 /* caller must hold gsnedf lock */
@@ -664,7 +663,7 @@ static void increase_priority_inheritance(struct task_struct* t, struct task_str
         raw_spin_lock(&gsnedf_lock);
 
         /* this sanity check allows for weaker locking in protocols */
-        if(__edf_higher_prio(prio_inh, BASE, t, INHERITED)) {
+        if(__edf_higher_prio(prio_inh, BASE, t, EFFECTIVE)) {
                 TRACE_TASK(t, "inherits priority from %s/%d\n", prio_inh->comm, prio_inh->pid);
                 tsk_rt(t)->inh_task = prio_inh;
 
@@ -737,11 +736,11 @@ static void decrease_priority_inheritance(struct task_struct* t, struct task_str
 {
         raw_spin_lock(&gsnedf_lock);
         
-        if(__edf_higher_prio(t, INHERITED, prio_inh, BASE)) {
+        if(__edf_higher_prio(t, EFFECTIVE, prio_inh, BASE)) {
                 /* A job only stops inheriting a priority when it releases a
                  * resource. Thus we can make the following assumption.*/
                 if(prio_inh)
-                        TRACE_TASK(t, "inherited priority decreased to %s/%d\n", prio_inh->comm, prio_inh->pid);
+                        TRACE_TASK(t, "EFFECTIVE priority decreased to %s/%d\n", prio_inh->comm, prio_inh->pid);
                 else
                         TRACE_TASK(t, "base priority restored.\n");     
                 
@@ -1016,7 +1015,7 @@ int gsnedf_rsm_mutex_lock(struct litmus_lock* l)
                                 TRACE_TASK(t, "is new hp_waiter.\n");
                                 
                                 if ((effective_priority(owner) == old_max_eff_prio) ||
-                                        (__edf_higher_prio(new_max_eff_prio, BASE, owner, INHERITED))){
+                                        (__edf_higher_prio(new_max_eff_prio, BASE, owner, EFFECTIVE))){
                                         new_prio = new_max_eff_prio;
                                 }
                         }
@@ -1126,7 +1125,7 @@ int gsnedf_rsm_mutex_unlock(struct litmus_lock* l)
                 {
                         // old_max_eff_prio > new_max_eff_prio
                         
-                        if(__edf_higher_prio(new_max_eff_prio, BASE, t, INHERITED)) {
+                        if(__edf_higher_prio(new_max_eff_prio, BASE, t, EFFECTIVE)) {
                                 TRACE_TASK(t, "new_max_eff_prio > task's eff_prio-- new_max_eff_prio: %s/%d   task: %s/%d [%s/%d]\n",
                                                    new_max_eff_prio->comm, new_max_eff_prio->pid,
                                                    t->comm, t->pid, tsk_rt(t)->inh_task->comm, tsk_rt(t)->inh_task->pid);
@@ -1271,15 +1270,15 @@ void gsnedf_rsm_mutex_propagate_increase_inheritance(struct litmus_lock* l,
                 if(new_max_eff_prio != old_max_eff_prio) {
                         // new_max_eff_prio > old_max_eff_prio holds.
                         if ((effective_priority(owner) == old_max_eff_prio) ||
-                                (__edf_higher_prio(new_max_eff_prio, BASE, owner, INHERITED))) {
+                                (__edf_higher_prio(new_max_eff_prio, BASE, owner, EFFECTIVE))) {
                                 
-                                TRACE_TASK(new_max_eff_prio, "Propagating inheritance to holder of lock %d.\n", l->ident);
+                                TRACE_CUR("Propagating inheritance to holder of lock %d.\n", l->ident);
                                 
                                 // beware: recursion
                                 nested_increase_priority_inheritance(owner, new_max_eff_prio, &mutex->lock, irqflags);  // unlocks mutex->lock
                         }
                         else {
-                                TRACE_TASK(new_max_eff_prio, "Lower priority than holder %s/%d.  No propagation.\n", owner->comm, owner->pid);
+                                TRACE_CUR("Lower priority than holder %s/%d.  No propagation.\n", owner->comm, owner->pid);
                                 raw_spin_unlock(&tsk_rt(owner)->hp_blocked_tasks_lock);
                                 raw_spin_unlock_irqrestore(&mutex->lock, irqflags);
                         }
@@ -1350,14 +1349,26 @@ void gsnedf_rsm_mutex_propagate_decrease_inheritance(struct litmus_lock* l,
                                 
                                 struct task_struct *decreased_prio;
 
-                                TRACE_TASK(new_max_eff_prio, "Propagating decreased inheritance to holder of lock %d.\n", l->ident);
+                                TRACE_CUR("Propagating decreased inheritance to holder of lock %d.\n", l->ident);
                                 
                                 if(__edf_higher_prio(new_max_eff_prio, BASE, owner, BASE)) {
-                                        TRACE_TASK(new_max_eff_prio, "has greater base priority than base priority of owner of lock %d.\n", l->ident);
+                                        TRACE_CUR("%s/%d has greater base priority than base priority of owner (%s/%d) of lock %d.\n",
+                                                          (new_max_eff_prio) ? new_max_eff_prio->comm : "nil",
+                                                          (new_max_eff_prio) ? new_max_eff_prio->pid : -1,
+                                                          owner->comm,
+                                                          owner->pid,
+                                                          l->ident);
+                                        
                                         decreased_prio = new_max_eff_prio;
                                 }
                                 else {
-                                        TRACE_TASK(new_max_eff_prio, "has lesser base priority than base priority of owner of lock %d.\n", l->ident);
+                                        TRACE_CUR("%s/%d has lesser base priority than base priority of owner (%s/%d) of lock %d.\n",
+                                                          (new_max_eff_prio) ? new_max_eff_prio->comm : "nil",
+                                                          (new_max_eff_prio) ? new_max_eff_prio->pid : -1,
+                                                          owner->comm,
+                                                          owner->pid,
+                                                          l->ident);
+                                        
                                         decreased_prio = NULL;
                                 }
 
@@ -1485,6 +1496,9 @@ static int ikglp_edf_max_heap_base_priority_order(struct binheap_node *a, struct
         ikglp_heap_node_t *d_a = binheap_entry(a, ikglp_heap_node_t, node);
         ikglp_heap_node_t *d_b = binheap_entry(b, ikglp_heap_node_t, node);
         
+        BUG_ON(!d_a);
+        BUG_ON(!d_b);
+        
         return __edf_higher_prio(d_a->task, BASE, d_b->task, BASE);     
 }
 
@@ -1711,8 +1725,8 @@ static void ikglp_add_global_list(struct ikglp_semaphore *sem, struct task_struc
         
         if(sem->top_m_size < sem->m) {
                 TRACE_CUR("Trivially adding %s/%d to top-m global list.\n", t->comm, t->pid);
-                TRACE_CUR("Top-M Before (size = %d):\n", sem->top_m_size);
-                print_global_list(sem->top_m.root, 1);
+//              TRACE_CUR("Top-M Before (size = %d):\n", sem->top_m_size);
+//              print_global_list(sem->top_m.root, 1);
                 
                 binheap_add(&node->node, &sem->top_m, ikglp_heap_node_t, node);
                 ++(sem->top_m_size);
@@ -1725,12 +1739,12 @@ static void ikglp_add_global_list(struct ikglp_semaphore *sem, struct task_struc
                 
                 TRACE_CUR("Adding %s/%d to top-m and evicting %s/%d.\n",
                                   t->comm, t->pid,
-                                  evicted->comm, evicted->pid);
+                                  evicted->task->comm, evicted->task->pid);
                 
-                TRACE_CUR("Not-Top-M Before:\n");
-                print_global_list(sem->not_top_m.root, 1);              
-                TRACE_CUR("Top-M Before (size = %d):\n", sem->top_m_size);
-                print_global_list(sem->top_m.root, 1);
+//              TRACE_CUR("Not-Top-M Before:\n");
+//              print_global_list(sem->not_top_m.root, 1);              
+//              TRACE_CUR("Top-M Before (size = %d):\n", sem->top_m_size);
+//              print_global_list(sem->top_m.root, 1);
                 
                 
                 binheap_delete_root(&sem->top_m, ikglp_heap_node_t, node);
@@ -1746,8 +1760,8 @@ static void ikglp_add_global_list(struct ikglp_semaphore *sem, struct task_struc
         }
         else {
                 TRACE_CUR("Trivially adding %s/%d to not-top-m global list.\n", t->comm, t->pid);
-                TRACE_CUR("Not-Top-M Before:\n");
-                print_global_list(sem->not_top_m.root, 1);
+//              TRACE_CUR("Not-Top-M Before:\n");
+//              print_global_list(sem->not_top_m.root, 1);
                 
                 binheap_add(&node->node, &sem->not_top_m, ikglp_heap_node_t, node);
                 
@@ -1766,10 +1780,10 @@ static void ikglp_del_global_list(struct ikglp_semaphore *sem, struct task_struc
         if(binheap_is_in_this_heap(&node->node, &sem->top_m)) {
                 TRACE_CUR("%s/%d is in top-m\n", t->comm, t->pid);
                 
-                TRACE_CUR("Not-Top-M Before:\n");
-                print_global_list(sem->not_top_m.root, 1);              
-                TRACE_CUR("Top-M Before (size = %d):\n", sem->top_m_size);
-                print_global_list(sem->top_m.root, 1);
+//              TRACE_CUR("Not-Top-M Before:\n");
+//              print_global_list(sem->not_top_m.root, 1);              
+//              TRACE_CUR("Top-M Before (size = %d):\n", sem->top_m_size);
+//              print_global_list(sem->top_m.root, 1);
                 
                 
                 binheap_delete(&node->node, &sem->top_m);
@@ -1777,7 +1791,7 @@ static void ikglp_del_global_list(struct ikglp_semaphore *sem, struct task_struc
                 if(!binheap_empty(&sem->not_top_m)) {
                         ikglp_heap_node_t *promoted = binheap_top_entry(&sem->not_top_m, ikglp_heap_node_t, node);
                         
-                        TRACE_CUR("Promoting %s/%d to top-m\n", promoted->comm, promoted->pid);
+                        TRACE_CUR("Promoting %s/%d to top-m\n", promoted->task->comm, promoted->task->pid);
                         
                         binheap_delete_root(&sem->not_top_m, ikglp_heap_node_t, node);
                         INIT_BINHEAP_NODE(&promoted->node);
@@ -1795,9 +1809,9 @@ static void ikglp_del_global_list(struct ikglp_semaphore *sem, struct task_struc
                 print_global_list(sem->not_top_m.root, 1);              
         }
         else {
-                TRACE_CUR("%s/%d is in not-top-m\n", t->comm, t->pid);
-                TRACE_CUR("Not-Top-M Before:\n");
-                print_global_list(sem->not_top_m.root, 1);
+//              TRACE_CUR("%s/%d is in not-top-m\n", t->comm, t->pid);
+//              TRACE_CUR("Not-Top-M Before:\n");
+//              print_global_list(sem->not_top_m.root, 1);
                 
                 binheap_delete(&node->node, &sem->not_top_m);
                 
@@ -1842,9 +1856,9 @@ void print_donees(struct ikglp_semaphore *sem, struct binheap_node *n, int depth
 
 static void ikglp_add_donees(struct ikglp_semaphore *sem, struct fifo_queue *fq, struct task_struct *t, ikglp_donee_heap_node_t* node)
 {
-        TRACE_CUR("Adding %s/%d to donor list.\n", t->comm, t->pid);
-        TRACE_CUR("donees Before:\n");
-        print_donees(sem, sem->donees.root, 1);
+//      TRACE_CUR("Adding %s/%d to donee list.\n", t->comm, t->pid);
+//      TRACE_CUR("donees Before:\n");
+//      print_donees(sem, sem->donees.root, 1);
         
         node->task = t;
         node->donor_info = NULL;
@@ -1860,8 +1874,8 @@ static void ikglp_add_donees(struct ikglp_semaphore *sem, struct fifo_queue *fq,
 
 static void ikglp_refresh_owners_prio_increase(struct task_struct *t, struct fifo_queue *fq, struct ikglp_semaphore *sem, unsigned long flags)
 {
-        // hp_waiter has increased
-        if (edf_higher_prio(t, fq->hp_waiter)) {
+        // priority of 't' has increased (note: 't' might already be hp_waiter).
+        if ((t == fq->hp_waiter) || edf_higher_prio(t, fq->hp_waiter)) {
                 struct task_struct *old_max_eff_prio;
                 struct task_struct *new_max_eff_prio;
                 struct task_struct *new_prio = NULL;
@@ -1895,7 +1909,7 @@ static void ikglp_refresh_owners_prio_increase(struct task_struct *t, struct fif
                                 TRACE_TASK(t, "is new hp_waiter.\n");
                                 
                                 if ((effective_priority(owner) == old_max_eff_prio) ||
-                                        (__edf_higher_prio(new_max_eff_prio, BASE, owner, INHERITED))){
+                                        (__edf_higher_prio(new_max_eff_prio, BASE, owner, EFFECTIVE))){
                                         new_prio = new_max_eff_prio;
                                 }
                         }
@@ -1926,7 +1940,7 @@ static void ikglp_refresh_owners_prio_increase(struct task_struct *t, struct fif
                 }
         }
         else {
-                TRACE_TASK(t, "no change in hp_waiter.\n");
+                TRACE_TASK(t, "hp_waiter is unaffected.\n");
                 raw_spin_unlock_irqrestore(&sem->lock, flags);
         }               
 }
@@ -1940,6 +1954,7 @@ static void ikglp_refresh_owners_prio_decrease(struct fifo_queue *fq, struct ikg
         struct task_struct *new_max_eff_prio;                   
 
         if(!owner) {
+                TRACE_CUR("No owner.  Returning.\n");
                 raw_spin_unlock_irqrestore(&sem->lock, flags);
                 return;
         }
@@ -1960,14 +1975,26 @@ static void ikglp_refresh_owners_prio_decrease(struct fifo_queue *fq, struct ikg
                 // Need to set new effective_priority for owner
                 struct task_struct *decreased_prio;
                 
-                TRACE_TASK(new_max_eff_prio, "Propagating decreased inheritance to holder of lock %d.\n", l->ident);
+                TRACE_CUR("Propagating decreased inheritance to holder of fq %d.\n", ikglp_get_idx(sem, fq));
                 
                 if(__edf_higher_prio(new_max_eff_prio, BASE, owner, BASE)) {
-                        TRACE_TASK(new_max_eff_prio, "has greater base priority than base priority of owner of lock %d.\n", l->ident);
+                        TRACE_CUR("%s/%d has greater base priority than base priority of owner (%s/%d) of fq %d.\n",
+                                          (new_max_eff_prio) ? new_max_eff_prio->comm : "nil",
+                                          (new_max_eff_prio) ? new_max_eff_prio->pid : -1,
+                                          owner->comm,
+                                          owner->pid,
+                                          ikglp_get_idx(sem, fq));
+                        
                         decreased_prio = new_max_eff_prio;
                 }
                 else {
-                        TRACE_TASK(new_max_eff_prio, "has lesser base priority than base priority of owner of lock %d.\n", l->ident);
+                        TRACE_CUR("%s/%d has lesser base priority than base priority of owner (%s/%d) of fq %d.\n",
+                                          (new_max_eff_prio) ? new_max_eff_prio->comm : "nil",
+                                          (new_max_eff_prio) ? new_max_eff_prio->pid : -1,
+                                          owner->comm,
+                                          owner->pid,
+                                          ikglp_get_idx(sem, fq));
+                        
                         decreased_prio = NULL;
                 }
                 
@@ -1975,6 +2002,7 @@ static void ikglp_refresh_owners_prio_decrease(struct fifo_queue *fq, struct ikg
                 nested_decrease_priority_inheritance(owner, decreased_prio, &sem->lock, flags); // will unlock mutex->lock
         }
         else {
+                TRACE_TASK(owner, "No need to propagate priority decrease forward.\n");
                 raw_spin_unlock(&tsk_rt(owner)->hp_blocked_tasks_lock);
                 raw_spin_unlock_irqrestore(&sem->lock, flags);
         }
@@ -2004,14 +2032,14 @@ static void ikglp_remove_donation_from_owner(struct binheap_node *n, struct fifo
                 // Need to set new effective_priority for owner
                 struct task_struct *decreased_prio;
                 
-                TRACE_TASK(new_max_eff_prio, "Propagating decreased inheritance to holder of lock %d.\n", l->ident);
+                TRACE_CUR("Propagating decreased inheritance to holder of fq %d.\n", ikglp_get_idx(sem, fq));
                 
                 if(__edf_higher_prio(new_max_eff_prio, BASE, owner, BASE)) {
-                        TRACE_TASK(new_max_eff_prio, "has greater base priority than base priority of owner of lock %d.\n", l->ident);
+                        TRACE_CUR("has greater base priority than base priority of owner of fq %d.\n", ikglp_get_idx(sem, fq));
                         decreased_prio = new_max_eff_prio;
                 }
                 else {
-                        TRACE_TASK(new_max_eff_prio, "has lesser base priority than base priority of owner of lock %d.\n", l->ident);
+                        TRACE_CUR("has lesser base priority than base priority of owner of fq %d.\n", ikglp_get_idx(sem, fq));
                         decreased_prio = NULL;
                 }
                 
@@ -2019,6 +2047,7 @@ static void ikglp_remove_donation_from_owner(struct binheap_node *n, struct fifo
                 nested_decrease_priority_inheritance(owner, decreased_prio, &sem->lock, flags); // will unlock mutex->lock
         }
         else {
+                TRACE_TASK(owner, "No need to propagate priority decrease forward.\n");
                 raw_spin_unlock(&tsk_rt(owner)->hp_blocked_tasks_lock);
                 raw_spin_unlock_irqrestore(&sem->lock, flags);
         }
@@ -2099,10 +2128,17 @@ static void __ikglp_enqueue_on_fq(
         
         // update global list.
         if(likely(global_heap_node)) {
+                if(binheap_is_in_heap(&global_heap_node->node)) {
+                        WARN_ON(1);
+                        ikglp_del_global_list(sem, t, global_heap_node);
+                }
                 ikglp_add_global_list(sem, t, global_heap_node);
         }
         // update donor eligiblity list.
         if(likely(donee_heap_node)) {
+                if(binheap_is_in_heap(&donee_heap_node->node)) {
+                        WARN_ON(1);
+                }
                 ikglp_add_donees(sem, fq, t, donee_heap_node);
         }
         
@@ -2121,9 +2157,12 @@ static void ikglp_enqueue_on_fq(
         unsigned long flags)
 {       
         /* resource is not free => must suspend and wait */
-        TRACE_TASK(t, "queue %d: Resource is not free => must suspend and wait.\n",
+        TRACE_TASK(wait->task, "queue %d: Resource is not free => must suspend and wait.\n",
                           ikglp_get_idx(sem, fq));
         
+        INIT_BINHEAP_NODE(&wait->global_heap_node.node);
+        INIT_BINHEAP_NODE(&wait->donee_heap_node.node);
+        
         __ikglp_enqueue_on_fq(sem, fq, wait->task, &wait->fq_node, &wait->global_heap_node, &wait->donee_heap_node);
         
         ikglp_refresh_owners_prio_increase(wait->task, fq, sem, flags);  // unlocks sem->lock   
@@ -2133,12 +2172,20 @@ static void ikglp_enqueue_on_fq(
 static void __ikglp_enqueue_on_pq(struct ikglp_semaphore *sem, ikglp_wait_state_t *wait)
 {
         TRACE_TASK(wait->task, "goes to PQ.\n");
-        
-        INIT_BINHEAP_NODE(&wait->pq_node.node);
+
+        wait->pq_node.task = wait->task; // copy over task (little redundant...)
         
         binheap_add(&wait->pq_node.node, &sem->priority_queue, ikglp_heap_node_t, node);
 }
 
+static void ikglp_enqueue_on_pq(struct ikglp_semaphore *sem, ikglp_wait_state_t *wait)
+{       
+        INIT_BINHEAP_NODE(&wait->global_heap_node.node);
+        INIT_BINHEAP_NODE(&wait->donee_heap_node.node); 
+        INIT_BINHEAP_NODE(&wait->pq_node.node);
+        
+        __ikglp_enqueue_on_pq(sem, wait);
+}
 
 void print_donors(struct binheap_node *n, int depth)
 {
@@ -2178,26 +2225,27 @@ static void ikglp_enqueue_on_donor(struct ikglp_semaphore *sem, ikglp_wait_state
         struct task_struct *new_max_eff_prio;
         struct task_struct *new_prio = NULL;
         
-        TRACE_CUR("Adding %s/%d as donor.\n", t->comm, t->pid);
-        TRACE_CUR("donors Before:\n");
-        print_donors(sem->donors.root, 1);      
-        
+        INIT_BINHEAP_NODE(&wait->global_heap_node.node);
+        INIT_BINHEAP_NODE(&wait->donee_heap_node.node); 
+        INIT_BINHEAP_NODE(&wait->pq_node.node); 
+        INIT_BINHEAP_NODE(&wait->node);
         
+//      TRACE_CUR("Adding %s/%d as donor.\n", t->comm, t->pid);
+//      TRACE_CUR("donors Before:\n");
+//      print_donors(sem->donors.root, 1);      
         
         // Add donor to the global list.
         ikglp_add_global_list(sem, t, &wait->global_heap_node);
         
-        INIT_BINHEAP_NODE(&wait->node);
-        
         // Select a donee
         donee_node = binheap_top_entry(&sem->donees, ikglp_donee_heap_node_t, node);
         donee = donee_node->task;
         
         TRACE_TASK(t, "Donee selected: %s/%d\n", donee->comm, donee->pid);
         
-        TRACE_CUR("Temporarily removing %s/%d to donor list.\n", donee->comm, donee->pid);
-        TRACE_CUR("donees Before:\n");
-        print_donees(sem, sem->donees.root, 1);
+        TRACE_CUR("Temporarily removing %s/%d to donee list.\n", donee->comm, donee->pid);
+//      TRACE_CUR("donees Before:\n");
+//      print_donees(sem, sem->donees.root, 1);
         
         binheap_delete_root(&sem->donees, ikglp_donee_heap_node_t, node);  // will re-add it shortly
         
@@ -2245,8 +2293,8 @@ static void ikglp_enqueue_on_donor(struct ikglp_semaphore *sem, ikglp_wait_state
         INIT_BINHEAP_NODE(&donee_node->node);
         
         
-        TRACE_CUR("Adding %s/%d back to donor list.\n", donee->comm, donee->pid);
-        TRACE_CUR("donees Before:\n");
+        TRACE_CUR("Adding %s/%d back to donee list.\n", donee->comm, donee->pid);
+//      TRACE_CUR("donees Before:\n");
         print_donees(sem, sem->donees.root, 1); 
         
         binheap_add(&donee_node->node, &sem->donees, ikglp_donee_heap_node_t, node);    
@@ -2268,20 +2316,20 @@ static void ikglp_enqueue_on_donor(struct ikglp_semaphore *sem, ikglp_wait_state
         
         if(new_max_eff_prio != old_max_eff_prio) {
                 if ((effective_priority(donee) == old_max_eff_prio) ||
-                        (__edf_higher_prio(new_max_eff_prio, BASE, donee, INHERITED))){
+                        (__edf_higher_prio(new_max_eff_prio, BASE, donee, EFFECTIVE))){
                         TRACE_TASK(t, "Donation increases %s/%d's effective priority\n", donee->comm, donee->pid);
                         new_prio = new_max_eff_prio;
                 }
-                else {
-                        // should be bug.  donor would not be in top-m.
-                        TRACE_TASK(t, "Donation is not greater than base prio of %s/%d?\n", donee->comm, donee->pid);
-                        WARN_ON(1);
-                }
-        }
-        else {
-                // should be bug.  donor would not be in top-m.
-                TRACE_TASK(t, "No change in %s/%d's inheritance heap?\n", donee->comm, donee->pid);
-                WARN_ON(1);
+//              else {
+//                      // should be bug.  donor would not be in top-m.
+//                      TRACE_TASK(t, "Donation is not greater than base prio of %s/%d?\n", donee->comm, donee->pid);
+//                      WARN_ON(1);
+//              }
+//      }
+//      else {
+//              // should be bug.  donor would not be in top-m.
+//              TRACE_TASK(t, "No change in %s/%d's inheritance heap?\n", donee->comm, donee->pid);
+//              WARN_ON(1);
         }
         
         if(new_prio) {
@@ -2320,6 +2368,7 @@ static int gsnedf_ikglp_lock(struct litmus_lock* l)
         struct ikglp_semaphore *sem = ikglp_from_lock(l);
         unsigned long flags, real_flags;        
         struct fifo_queue *fq = NULL;
+        int replica = -EINVAL;
         
         ikglp_wait_state_t wait;
         
@@ -2331,54 +2380,62 @@ static int gsnedf_ikglp_lock(struct litmus_lock* l)
 
         if(sem->shortest_fifo_queue->count == 0) {
                 // take available resource
+                replica = ikglp_get_idx(sem, sem->shortest_fifo_queue);
+                
                 ikglp_get_immediate(t, sem->shortest_fifo_queue, sem, flags);  // unlocks sem->lock
                 raw_spin_unlock_irqrestore(&sem->real_lock, real_flags);
-                return 0;
         }
-        
-        // we have to suspend.
-        
-        wait.task = t;   // THIS IS CRITICALLY IMPORTANT!!!
-        
-        tsk_rt(t)->blocked_lock = (struct litmus_lock*)sem;  // record where we are blocked
-        mb();
-        
-        /* FIXME: interruptible would be nice some day */
-        set_task_state(t, TASK_UNINTERRUPTIBLE);        
-        
-        if(sem->shortest_fifo_queue->count < sem->max_fifo_len) {
-                // enqueue on fq                
-                ikglp_enqueue_on_fq(sem, sem->shortest_fifo_queue, &wait, flags);  // unlocks sem->lock         
-        }
-        else {
-                // no room in fifos.  Go to PQ or donors.
+        else
+        {
+                // we have to suspend.
                 
-                if(__edf_higher_prio(ikglp_mth_highest(sem), BASE, t, BASE)) {
-                        // enqueue on PQ
-                        __ikglp_enqueue_on_pq(sem, &wait);
-                        raw_spin_unlock_irqrestore(&sem->lock, flags);
+                wait.task = t;   // THIS IS CRITICALLY IMPORTANT!!!
+                
+                tsk_rt(t)->blocked_lock = (struct litmus_lock*)sem;  // record where we are blocked
+                mb();
+                
+                /* FIXME: interruptible would be nice some day */
+                set_task_state(t, TASK_UNINTERRUPTIBLE);        
+                
+                if(sem->shortest_fifo_queue->count < sem->max_fifo_len) {
+                        // enqueue on fq                
+                        ikglp_enqueue_on_fq(sem, sem->shortest_fifo_queue, &wait, flags);  // unlocks sem->lock         
                 }
                 else {
-                        // enqueue as donor
-                        ikglp_enqueue_on_donor(sem, &wait, flags);       // unlocks sem->lock
+                        
+                        TRACE_CUR("IKGLP fifo queues are full.\n");
+                        
+                        // no room in fifos.  Go to PQ or donors.
+                        
+                        if(__edf_higher_prio(ikglp_mth_highest(sem), BASE, t, BASE)) {
+                                // enqueue on PQ
+                                ikglp_enqueue_on_pq(sem, &wait);
+                                raw_spin_unlock_irqrestore(&sem->lock, flags);
+                        }
+                        else {
+                                // enqueue as donor
+                                ikglp_enqueue_on_donor(sem, &wait, flags);       // unlocks sem->lock
+                        }
                 }
+                
+                raw_spin_unlock_irqrestore(&sem->real_lock, real_flags);
+                
+                TS_LOCK_SUSPEND;
+                
+                schedule();
+                
+                TS_LOCK_RESUME;
+                
+                fq = ikglp_get_queue(sem, t);
+                BUG_ON(!fq);            
+                
+                replica = ikglp_get_idx(sem, fq);
         }
         
-        raw_spin_unlock_irqrestore(&sem->real_lock, real_flags);
-        
-        TS_LOCK_SUSPEND;
-        
-        schedule();
-        
-        TS_LOCK_RESUME;
-        
-        fq = ikglp_get_queue(sem, t);
-        BUG_ON(!fq);
-        
         TRACE_CUR("Acquired lock %d, queue %d\n",
-                          l->ident, ikglp_get_idx(sem, fq));
+                          l->ident, replica);
         
-        return 0;
+        return replica;
 }
 
 static void ikglp_move_donor_to_fq(struct ikglp_semaphore *sem, struct fifo_queue *fq, ikglp_wait_state_t *donor_info)
@@ -2387,14 +2444,15 @@ static void ikglp_move_donor_to_fq(struct ikglp_semaphore *sem, struct fifo_queu
         
         TRACE_CUR("Donor %s/%d being moved to fq %d\n",
                           t->comm,
-                          t->pid;
+                          t->pid,
                           ikglp_get_idx(sem, fq));
         
         binheap_delete(&donor_info->node, &sem->donors);
 
         __ikglp_enqueue_on_fq(sem, fq, t,
                 &donor_info->fq_node,
-                &donor_info->global_heap_node,
+                NULL, // already in global_list, so pass null to prevent adding 2nd time.
+                //&donor_info->global_heap_node,
                 &donor_info->donee_heap_node);  
         
         // warning:
@@ -2407,7 +2465,7 @@ static void ikglp_move_pq_to_fq(struct ikglp_semaphore *sem, struct fifo_queue *
         
         TRACE_CUR("PQ request %s/%d being moved to fq %d\n",
                           t->comm,
-                          t->pid;
+                          t->pid,
                           ikglp_get_idx(sem, fq));
         
         binheap_delete(&wait->pq_node.node, &sem->priority_queue);
@@ -2425,35 +2483,69 @@ static ikglp_wait_state_t* ikglp_find_hp_waiter_to_steal(struct ikglp_semaphore*
         /* must hold sem->lock */
         
         struct fifo_queue *fq = NULL;
-        struct task_struct *max_hp = NULL;
-        
         struct list_head        *pos;
         struct task_struct      *queued;
         int i;
         
-        for(i = 0; i < sem->nr_replicas; ++i)
-        {
+        for(i = 0; i < sem->nr_replicas; ++i) {
                 if( (sem->fifo_queues[i].count > 1) &&
-                   ((fq == NULL) ||
-                        (edf_higher_prio(sem->fifo_queues[i].hp_waiter, fq->hp_waiter))) )
-                {
+                   (!fq || edf_higher_prio(sem->fifo_queues[i].hp_waiter, fq->hp_waiter)) ) {
+                        
+                        TRACE_CUR("hp_waiter on fq %d (%s/%d) has higher prio than hp_waiter on fq %d (%s/%d)\n",
+                                          ikglp_get_idx(sem, &sem->fifo_queues[i]),
+                                          sem->fifo_queues[i].hp_waiter->comm,
+                                          sem->fifo_queues[i].hp_waiter->pid,
+                                          (fq) ? ikglp_get_idx(sem, fq) : -1,
+                                          (fq) ? ((fq->hp_waiter) ? fq->hp_waiter->comm : "nil") : "nilXX",
+                                          (fq) ? ((fq->hp_waiter) ? fq->hp_waiter->pid : -1) : -2);
+                        
                         fq = &sem->fifo_queues[i];
+                           
+                        WARN_ON(!(fq->hp_waiter));
                 }
         }
         
         if(fq) {
-                list_for_each(pos, &fq->wait.task_list)
-                {
+                struct task_struct *max_hp = fq->hp_waiter;
+                ikglp_wait_state_t* ret = NULL;
+                
+                TRACE_CUR("Searching for %s/%d on fq %d\n",
+                                  max_hp->comm,
+                                  max_hp->pid,
+                                  ikglp_get_idx(sem, fq));
+                
+                BUG_ON(!max_hp);
+                
+                list_for_each(pos, &fq->wait.task_list) {
                         wait_queue_t *wait = list_entry(pos, wait_queue_t, task_list);
                         
                         queued  = (struct task_struct*) wait->private;
+                        
+                        TRACE_CUR("fq %d entry: %s/%d\n", ikglp_get_idx(sem, fq), queued->comm, queued->pid);
+                        
                         /* Compare task prios, find high prio task. */
-                        if (queued == max_hp)
-                        {
-                                return container_of(wait, ikglp_wait_state_t, fq_node);
+                        if (queued == max_hp) {
+                                TRACE_CUR("Found it!\n");
+                                ret = container_of(wait, ikglp_wait_state_t, fq_node);
                         }
+                }               
+                
+//              list_for_each(pos, &fq->wait.task_list) {
+//                      wait_queue_t *wait = list_entry(pos, wait_queue_t, task_list);
+//                      
+//                      queued  = (struct task_struct*) wait->private;
+//                      /* Compare task prios, find high prio task. */
+//                      if (queued == max_hp) {
+//                              TRACE_CUR("Found it!\n");
+//                              return container_of(wait, ikglp_wait_state_t, fq_node);
+//                      }
+//              }
+                
+                if(!ret) {
+                        WARN_ON(1);
                 }
-                BUG();
+                
+                return ret;             
         }
         
         return(NULL);
@@ -2468,14 +2560,21 @@ static void ikglp_steal_to_fq(struct ikglp_semaphore *sem, struct fifo_queue *fq
         
         TRACE_CUR("FQ request %s/%d being moved to fq %d\n",
                           t->comm,
-                          t->pid;
+                          t->pid,
                           ikglp_get_idx(sem, fq));      
         
         fq_wait->donee_heap_node.fq = fq;  // just to be safe
         
 
         __remove_wait_queue(&fq_steal->wait, &fq_wait->fq_node);
-        --fq_steal->count;
+        --(fq_steal->count);
+        
+        fq_steal->hp_waiter = ikglp_find_hp_waiter(fq_steal, NULL);     
+        TRACE_TASK(t, "New hp_waiter for fq %d is %s/%d!\n",
+                           ikglp_get_idx(sem, fq_steal),
+                           (fq_steal->hp_waiter) ? fq_steal->hp_waiter->comm : "nil",
+                           (fq_steal->hp_waiter) ? fq_steal->hp_waiter->pid : -1);
+        
         
         // Update shortest.
         if(fq_steal->count < sem->shortest_fifo_queue->count) {
@@ -2497,6 +2596,8 @@ static void ikglp_migrate_fq_to_owner_heap_nodes(struct ikglp_semaphore *sem, st
         
         BUG_ON(old_wait->donee_heap_node.fq != fq);
         
+        TRACE_TASK(t, "Migrating wait_state to memory of queue %d.\n", ikglp_get_idx(sem, fq));
+        
         // need to migrate global_heap_node and donee_heap_node off of the stack
         // to the nodes allocated for the owner of this fq.
         
@@ -2524,6 +2625,8 @@ static int gsnedf_ikglp_unlock(struct litmus_lock* l)
         struct task_struct *t = current;
         struct task_struct *donee = NULL;
         struct task_struct *next = NULL;
+        struct task_struct *new_on_fq = NULL;
+        
         ikglp_wait_state_t *other_donor_info = NULL;
         struct fifo_queue *to_steal = NULL;
         struct fifo_queue *fq;
@@ -2542,14 +2645,23 @@ static int gsnedf_ikglp_unlock(struct litmus_lock* l)
                 goto out;
         }
         
-        TRACE_TASK(t, "Freeing replica %d.\n", ikglp_to_idx(sem, fq));  
+        TRACE_TASK(t, "Freeing replica %d.\n", ikglp_get_idx(sem, fq));
+        
+        
+        // Remove 't' from the heaps, but data in nodes will still be good.
+        ikglp_del_global_list(sem, t, &fq->global_heap_node);
+        binheap_delete(&fq->donee_heap_node.node, &sem->donees);        
         
+        // Move the next request into the FQ and update heaps as needed.
+        // We defer re-evaluation of priorities to later in the function.
         if(fq->donee_heap_node.donor_info) {  // move my doner to FQ
                 ikglp_wait_state_t *donor_info = fq->donee_heap_node.donor_info;
                 
+                new_on_fq = donor_info->task;
+                
                 TRACE_TASK(t, "Moving MY donor (%s/%d) to fq %d.\n",
-                                   donor_info->task->comm, donor_info->task->pid,
-                                   ikglp_to_idx(sem, fq));
+                                   new_on_fq->comm, new_on_fq->pid,
+                                   ikglp_get_idx(sem, fq));
                 // donor moved to FQ
                 donee = t;
                 ikglp_move_donor_to_fq(sem, fq, donor_info);
@@ -2559,11 +2671,17 @@ static int gsnedf_ikglp_unlock(struct litmus_lock* l)
                 // move other donor to FQ
                 other_donor_info = binheap_top_entry(&sem->donors, ikglp_wait_state_t, node);
                 
+                new_on_fq = other_donor_info->task;
                 donee = other_donor_info->donee_info->task;
+                
+                // update the donee's heap position.
+                other_donor_info->donee_info->donor_info = NULL;  // clear the cross-link
+                binheap_decrease(&other_donor_info->donee_info->node, &sem->donees);
 
+                
                 TRACE_TASK(t, "Moving a donor (%s/%d) to fq %d.\n",
-                                   other_donor_info->task->comm, other_donor_info->task->pid,
-                                   ikglp_to_idx(sem, fq));
+                                   new_on_fq->comm, new_on_fq->pid,
+                                   ikglp_get_idx(sem, fq));
                 
                 ikglp_move_donor_to_fq(sem, fq, other_donor_info);
                 
@@ -2573,9 +2691,11 @@ static int gsnedf_ikglp_unlock(struct litmus_lock* l)
                 ikglp_heap_node_t *pq_node = binheap_top_entry(&sem->priority_queue, ikglp_heap_node_t, node);
                 ikglp_wait_state_t *pq_wait = container_of(pq_node, ikglp_wait_state_t, pq_node);
 
+                new_on_fq = pq_wait->task;
+                
                 TRACE_TASK(t, "Moving a pq waiter (%s/%d) to fq %d.\n",
-                                   pq_wait->task->comm, pq_wait->task->pid,
-                                   ikglp_to_idx(sem, fq));              
+                                   new_on_fq->comm, new_on_fq->pid,
+                                   ikglp_get_idx(sem, fq));             
                 
                 ikglp_move_pq_to_fq(sem, fq, pq_wait);
         }
@@ -2584,16 +2704,18 @@ static int gsnedf_ikglp_unlock(struct litmus_lock* l)
                 ikglp_wait_state_t *fq_wait;
                 
                 TRACE_TASK(t, "Looking to steal a request for fq %d...\n",
-                                   ikglp_to_idx(sem, fq));              
+                                   ikglp_get_idx(sem, fq));             
                 
                 fq_wait = ikglp_find_hp_waiter_to_steal(sem);
                 if(fq_wait) {
                         to_steal = fq_wait->donee_heap_node.fq;
                 
+                        new_on_fq = fq_wait->task;
+                        
                         TRACE_TASK(t, "Found %s/%d of fq %d to steal for fq %d...\n",
-                                           fq_wait->task->comm, fq_wait->task->pid,
-                                           ikglp_to_idx(sem, to_steal)
-                                           ikglp_to_idx(sem, fq));                              
+                                           new_on_fq->comm, new_on_fq->pid,
+                                           ikglp_get_idx(sem, to_steal),
+                                           ikglp_get_idx(sem, fq));                             
                         
                         ikglp_steal_to_fq(sem, fq, fq_wait);
                 
@@ -2601,7 +2723,7 @@ static int gsnedf_ikglp_unlock(struct litmus_lock* l)
                 }
                 else {
                         TRACE_TASK(t, "Found nothing to steal for fq %d.\n",
-                                           ikglp_to_idx(sem, fq));
+                                           ikglp_get_idx(sem, fq));
                 }
         }
         else { // move no one
@@ -2622,9 +2744,16 @@ static int gsnedf_ikglp_unlock(struct litmus_lock* l)
         }
         raw_spin_unlock(&tsk_rt(t)->hp_blocked_tasks_lock);     
         
-        ikglp_del_global_list(sem, t, &fq->global_heap_node);
-        binheap_delete(&fq->donee_heap_node.node, &sem->donees);
+        
+        // Updating the owner and updating sem->shortest_fifo_queue
+        // could have been done sooner, but it is deffered, hoping
+        // that it will reduce thrashing of sem->shortest_fifo_queue
+        // assignment.
         fq->owner = NULL;  // no longer owned!!
+        --(fq->count);
+        if(fq->count < sem->shortest_fifo_queue->count) {
+                sem->shortest_fifo_queue = fq;
+        }       
         
         // Now patch up other priorities.
         //
@@ -2649,7 +2778,7 @@ static int gsnedf_ikglp_unlock(struct litmus_lock* l)
                 if(donee == other_fq->owner) {
                         TRACE_TASK(t, "Donee %s/%d is an owner of fq %d.\n",
                                            donee->comm, donee->pid,
-                                           ikglp_to_idx(sem, other_fq));
+                                           ikglp_get_idx(sem, other_fq));
                         
                         ikglp_remove_donation_from_owner(&other_donor_info->prio_donation.hp_binheap_node, other_fq, sem, flags);
                         raw_spin_lock_irqsave(&sem->lock, flags);  // there should be no contention!!!!
@@ -2657,15 +2786,20 @@ static int gsnedf_ikglp_unlock(struct litmus_lock* l)
                 else {
                         TRACE_TASK(t, "Donee %s/%d is an blocked in of fq %d.\n",
                                            donee->comm, donee->pid,
-                                           ikglp_to_idx(sem, other_fq));                        
+                                           ikglp_get_idx(sem, other_fq));                       
                         
                         ikglp_remove_donation_from_fq_waiter(donee, &other_donor_info->prio_donation.hp_binheap_node);
                         if(donee == other_fq->hp_waiter) {
                                 TRACE_TASK(t, "Donee %s/%d was an hp_waiter of fq %d. Rechecking hp_waiter.\n",
                                                    donee->comm, donee->pid,
-                                                   ikglp_to_idx(sem, other_fq));                                
+                                                   ikglp_get_idx(sem, other_fq));                               
                                 
                                 other_fq->hp_waiter = ikglp_find_hp_waiter(other_fq, NULL);
+                                TRACE_TASK(t, "New hp_waiter for fq %d is %s/%d!\n",
+                                                   ikglp_get_idx(sem, other_fq),
+                                                   (other_fq->hp_waiter) ? other_fq->hp_waiter->comm : "nil",
+                                                   (other_fq->hp_waiter) ? other_fq->hp_waiter->pid : -1);                              
+                                
                                 ikglp_refresh_owners_prio_decrease(other_fq, sem, flags); // unlocks sem->lock.  reacquire it.
                                 raw_spin_lock_irqsave(&sem->lock, flags);  // there should be no contention!!!!                         
                         }
@@ -2673,38 +2807,57 @@ static int gsnedf_ikglp_unlock(struct litmus_lock* l)
         }
         else if(to_steal) {
                 TRACE_TASK(t, "Rechecking priority inheritance of fq %d, triggered by stealing.\n",
-                                   ikglp_to_idx(sem, to_steal));                
+                                   ikglp_get_idx(sem, to_steal));               
                 
                 ikglp_refresh_owners_prio_decrease(to_steal, sem, flags); // unlocks sem->lock.  reacquire it.
                 raw_spin_lock_irqsave(&sem->lock, flags);  // there should be no contention!!!!
         }
         
+        // check for new HP waiter.
+        if(new_on_fq) {
+                // fq->owner is null, so just update the hp_waiter without locking.
+                
+                if(new_on_fq == fq->hp_waiter) {
+                        TRACE_TASK(t, "new_on_fq is already hp_waiter.\n", fq->hp_waiter->comm, fq->hp_waiter->pid);
+                        fq->nest.hp_waiter_eff_prio = effective_priority(fq->hp_waiter);  // set this just to be sure...
+                }
+                else if(edf_higher_prio(new_on_fq, fq->hp_waiter)) {
+                        if(fq->hp_waiter)
+                                TRACE_TASK(t, "has higher prio than hp_waiter (%s/%d).\n", fq->hp_waiter->comm, fq->hp_waiter->pid);
+                        else
+                                TRACE_TASK(t, "has higher prio than hp_waiter (NIL).\n");
+                        
+                        fq->hp_waiter = new_on_fq;
+                        fq->nest.hp_waiter_eff_prio = effective_priority(fq->hp_waiter);
+                        
+                        TRACE_TASK(t, "New hp_waiter for fq %d is %s/%d!\n",
+                                           ikglp_get_idx(sem, fq),
+                                           (fq->hp_waiter) ? fq->hp_waiter->comm : "nil",
+                                           (fq->hp_waiter) ? fq->hp_waiter->pid : -1);
+                }
+        }
+        
 
         if(waitqueue_active(&fq->wait))
         {
                 wait_queue_t *wait = list_entry(fq->wait.task_list.next, wait_queue_t, task_list);
                 ikglp_wait_state_t *fq_wait = container_of(wait, ikglp_wait_state_t, fq_node);
+                next = (struct task_struct*) wait->private;
                 
-                next = __waitqueue_remove_first(&fq->wait);
+                __remove_wait_queue(&fq->wait, wait);
                 
                 TRACE_CUR("queue %d: ASSIGNING %s/%d as owner - next\n",
                                   ikglp_get_idx(sem, fq),
                                   next->comm, next->pid);
 
+                // migrate wait-state to fifo-memory.
                 ikglp_migrate_fq_to_owner_heap_nodes(sem, fq, fq_wait);
                 
                 /* next becomes the resouce holder */
                 fq->owner = next;
-                --fq->count;
                 tsk_rt(next)->blocked_lock = NULL;
-                
-                // TODO: UPDATE TO USE NEW HEAP NODES.
 
-                if(fq->count < sem->shortest_fifo_queue->count) {
-                        sem->shortest_fifo_queue = fq;
-                }       
 
-                
                 /* determine new hp_waiter if necessary */
                 if (next == fq->hp_waiter) {
                         
@@ -2713,11 +2866,16 @@ static int gsnedf_ikglp_unlock(struct litmus_lock* l)
                          * inherit.  However, we need to make sure that the
                          * next-highest priority in the queue is reflected in
                          * hp_waiter. */
-                        fq->hp_waiter = ikglp_find_hp_waiter(fq, next);
+                        fq->hp_waiter = ikglp_find_hp_waiter(fq, NULL);
+                        TRACE_TASK(next, "New hp_waiter for fq %d is %s/%d!\n",
+                                           ikglp_get_idx(sem, fq),
+                                           (fq->hp_waiter) ? fq->hp_waiter->comm : "nil",
+                                           (fq->hp_waiter) ? fq->hp_waiter->pid : -1);
+                        
                         fq->nest.hp_waiter_eff_prio = (fq->hp_waiter) ? effective_priority(fq->hp_waiter) : NULL;
                         
                         if (fq->hp_waiter)
-                                TRACE_TASK(mutex->hp_waiter, "is new highest-prio waiter\n");
+                                TRACE_TASK(fq->hp_waiter, "is new highest-prio waiter\n");
                         else
                                 TRACE("no further waiters\n");
                         
@@ -2737,7 +2895,10 @@ static int gsnedf_ikglp_unlock(struct litmus_lock* l)
                         /* Well, if 'next' is not the highest-priority waiter,
                          * then it (probably) ought to inherit the highest-priority
                          * waiter's priority. */
-                        TRACE_TASK(next, "is not hp_waiter of lock %d.\n", l->ident);
+                        TRACE_TASK(next, "is not hp_waiter of replica %d. hp_waiter is %s/%d\n",
+                                           ikglp_get_idx(sem, fq),
+                                           (fq->hp_waiter) ? fq->hp_waiter->comm : "nil",
+                                           (fq->hp_waiter) ? fq->hp_waiter->pid : -1);
                         
                         raw_spin_lock(&tsk_rt(next)->hp_blocked_tasks_lock);                            
                         
@@ -2748,23 +2909,26 @@ static int gsnedf_ikglp_unlock(struct litmus_lock* l)
                      * because that update hasn't yet executed (update operation is
                          * probably blocked on mutex->lock). So only inherit if the top of
                          * 'next's top heap node is indeed the effective prio. of hp_waiter. 
-                         * (We use l->hp_waiter_eff_prio instead of effective_priority(hp_waiter)
+                         * (We use fq->hp_waiter_eff_prio instead of effective_priority(hp_waiter)
                          * since the effective priority of hp_waiter can change (and the
                          * update has not made it to this lock).)
                          */
-                        if(likely(top_priority(&tsk_rt(next)->hp_blocked_tasks) == l->nest.hp_waiter_eff_prio))
+                        if(likely(top_priority(&tsk_rt(next)->hp_blocked_tasks) == fq->nest.hp_waiter_eff_prio))
                         {
-                                increase_priority_inheritance(next, l->nest.hp_waiter_eff_prio);
+                                if(fq->nest.hp_waiter_eff_prio)
+                                        increase_priority_inheritance(next, fq->nest.hp_waiter_eff_prio);
+                                else
+                                        WARN_ON(1);
                         }
                         
                         raw_spin_unlock(&tsk_rt(next)->hp_blocked_tasks_lock);
                 }
+
+
+                // wake up the new resource holder!
+                wake_up_process(next);
         }
 
-        
-        /* wake up next */
-        wake_up_process(next);
-        
 out:
         raw_spin_unlock_irqrestore(&sem->lock, flags);
         
@@ -2820,7 +2984,7 @@ static struct litmus_lock_ops gsnedf_ikglp_lock_ops = {
         .deallocate = gsnedf_ikglp_free,
 };
 
-static struct litmus_lock* gsnedf_new_ikglp(void* __user arg, int* ret_code)
+static struct litmus_lock* gsnedf_new_ikglp(void* __user arg)
 {
         struct ikglp_semaphore* sem;
         int nr_replicas = 0;
@@ -2828,24 +2992,20 @@ static struct litmus_lock* gsnedf_new_ikglp(void* __user arg, int* ret_code)
         
         if(!access_ok(VERIFY_READ, arg, sizeof(nr_replicas)))
         {
-                *ret_code = -EINVAL;
                 return(NULL);
         }
         if(__copy_from_user(&nr_replicas, arg, sizeof(nr_replicas)))
         {
-                *ret_code = -EINVAL;
                 return(NULL);
         }
         if(nr_replicas < 1)
         {
-                *ret_code = -EINVAL;
                 return(NULL);           
         }
         
         sem = kmalloc(sizeof(*sem), GFP_KERNEL);
         if(!sem)
         {
-                *ret_code = -ENOMEM;
                 return NULL;
         }
         
@@ -2853,7 +3013,6 @@ static struct litmus_lock* gsnedf_new_ikglp(void* __user arg, int* ret_code)
         if(!sem->fifo_queues)
         {
                 kfree(sem);
-                *ret_code = -ENOMEM;
                 return NULL;            
         }
 
@@ -2871,9 +3030,14 @@ static struct litmus_lock* gsnedf_new_ikglp(void* __user arg, int* ret_code)
         raw_spin_lock_init(&sem->real_lock);
 
         sem->nr_replicas = nr_replicas;
-        sem->m = num_online_cpus();
+        sem->m = num_online_cpus();  // default 'm' to number of CPUs.
         sem->max_fifo_len = (sem->m/nr_replicas) + ((sem->m%nr_replicas) != 0);
         
+        TRACE("New IKGLP Sem: m = %d, k = %d, max fifo_len = %d\n",
+                  sem->m,
+                  sem->nr_replicas,
+                  sem->max_fifo_len);
+        
         for(i = 0; i < nr_replicas; ++i)
         {
                 struct fifo_queue* q = &(sem->fifo_queues[i]);
@@ -2908,7 +3072,6 @@ static struct litmus_lock* gsnedf_new_ikglp(void* __user arg, int* ret_code)
         INIT_BINHEAP_HANDLE(&sem->priority_queue, ikglp_edf_max_heap_base_priority_order);
         INIT_BINHEAP_HANDLE(&sem->donors, ikglp_donor_edf_max_heap_base_priority_order);        
 
-        *ret_code = 0;
         return &sem->litmus_lock;
 }
 
@@ -3155,7 +3318,7 @@ static long gsnedf_allocate_lock(struct litmus_lock **lock, int type,
                 break;
                         
         case IKGLP_SEM:
-                *lock = gsnedf_new_ikglp(args, &err);
+                *lock = gsnedf_new_ikglp(args);
                 break;
 #endif
         
@@ -3189,7 +3352,6 @@ static long gsnedf_activate_plugin(void)
         for_each_online_cpu(cpu) {
                 entry = &per_cpu(gsnedf_cpu_entries, cpu);
                 INIT_BINHEAP_NODE(&entry->hn);
-                entry->in_heap   = 0;
                 entry->linked    = NULL;
                 entry->scheduled = NULL;
 #ifdef CONFIG_RELEASE_MASTER
@@ -3232,13 +3394,12 @@ static int __init init_gsn_edf(void)
 
         INIT_BINHEAP_HANDLE(&gsnedf_cpu_heap, cpu_lower_prio);
         /* initialize CPU state */
-        for (cpu = 0; cpu < NR_CPUS; cpu++)  {
+        for (cpu = 0; cpu < NR_CPUS; ++cpu)  {
                 entry = &per_cpu(gsnedf_cpu_entries, cpu);
                 gsnedf_cpus[cpu] = entry;
                 entry->cpu       = cpu;
 
                 INIT_BINHEAP_NODE(&entry->hn);
-                entry->in_heap   = 0;
         }
         edf_domain_init(&gsnedf, NULL, gsnedf_release_jobs);
         return register_sched_plugin(&gsn_edf_plugin);