6 files changed, 340 insertions, 5 deletions

diff --git a/include/litmus/binheap.h b/include/litmus/binheap.h
index 412a4c7bd4bc..ce5539d72f4d 100644
--- a/include/litmus/binheap.h
+++ b/include/litmus/binheap.h
@@ -137,7 +137,7 @@ __binheap_decrease((orig_node), (handle))
 static inline void INIT_BINHEAP_NODE(struct binheap_node *n)
 {
         n->data = NULL;
-        n->parent = NULL;
+        n->parent = BINHEAP_POISON;
         n->left = NULL;
         n->right = NULL;
         n->ref = NULL;
@@ -160,6 +160,12 @@ static inline int binheap_empty(struct binheap_handle *handle)
         return(handle->root == NULL);
 }
 
+/* Returns true (1) if binheap node is in a heap. */
+static inline int binheap_is_in_heap(struct binheap_node *node)
+{
+        return (node->parent != BINHEAP_POISON);
+}
+
 
 /* Update the node reference pointers.  Same logic as Litmus binomial heap. */
 static inline void __update_ref(struct binheap_node *parent,
@@ -516,6 +522,7 @@ static inline void __binheap_delete(
         __binheap_delete_root(handle, node_to_delete);
 
         node_to_delete->data = temp_data;  /* restore node data pointer */
+        node_to_delete->parent = BINHEAP_POISON; /* poison the node */
 }
 
 /**
diff --git a/include/litmus/fdso.h b/include/litmus/fdso.h
index 4b4ae7796788..4e7a6bf06134 100644
--- a/include/litmus/fdso.h
+++ b/include/litmus/fdso.h
@@ -21,8 +21,9 @@ typedef enum  {
         SRP_SEM         = 1,
         
         RSM_MUTEX       = 2,
+        IKGLP_SEM       = 3,
 
-        MAX_OBJ_TYPE    = 2
+        MAX_OBJ_TYPE    = 3
 } obj_type_t;
 
 struct inode_obj_id {
diff --git a/include/litmus/rt_param.h b/include/litmus/rt_param.h
index 8947dd0c7eb2..3a961005e23b 100644
--- a/include/litmus/rt_param.h
+++ b/include/litmus/rt_param.h
@@ -135,8 +135,8 @@ struct rt_param {
          * could point to self if PI does not result in
          * an increased task priority.
          */
-         struct task_struct*    inh_task;
-    
+        struct task_struct*     inh_task;
+        
 #ifdef CONFIG_LITMUS_NESTED_LOCKING
         raw_spinlock_t                  hp_blocked_tasks_lock;
         struct binheap_handle   hp_blocked_tasks;
@@ -144,6 +144,9 @@ struct rt_param {
         
         /* pointer to lock upon which is currently blocked */
         struct litmus_lock* blocked_lock;
+        
+        
+        struct task_struct* doner;
 #endif
     
     
diff --git a/litmus/fdso.c b/litmus/fdso.c
index f192787b577d..2000ba8a92f5 100644
--- a/litmus/fdso.c
+++ b/litmus/fdso.c
@@ -24,6 +24,7 @@ static const struct fdso_ops* fdso_ops[] = {
         &generic_lock_ops, /* FMLP_SEM */
         &generic_lock_ops, /* SRP_SEM */
         &generic_lock_ops, /* RSM_MUTEX */
+        &generic_lock_ops, /* IKGLP_SEM */
 };
 
 static int fdso_create(void** obj_ref, obj_type_t type, void* __user config)
diff --git a/litmus/litmus.c b/litmus/litmus.c
index 9d4fbd2f8a65..3dcefe7a08d6 100644
--- a/litmus/litmus.c
+++ b/litmus/litmus.c
@@ -316,6 +316,8 @@ static void reinit_litmus_state(struct task_struct* p, int restore)
 #ifdef CONFIG_LITMUS_NESTED_LOCKING
         WARN_ON(p->rt_param.blocked_lock);
     WARN_ON(!binheap_empty(&p->rt_param.hp_blocked_tasks));
+        
+        WARN_ON(p->rt_param.doner);
 #endif
 
         /* Cleanup everything else. */
@@ -491,6 +493,7 @@ void litmus_exec(void)
 
         if (is_realtime(p)) {
                 WARN_ON(p->rt_param.inh_task);
+                WARN_ON(p->rt_param.doner);
                 if (tsk_rt(p)->ctrl_page) {
                         free_page((unsigned long) tsk_rt(p)->ctrl_page);
                         tsk_rt(p)->ctrl_page = NULL;
diff --git a/litmus/sched_gsn_edf.c b/litmus/sched_gsn_edf.c
index 99c93ae65e06..10e14613655b 100644
--- a/litmus/sched_gsn_edf.c
+++ b/litmus/sched_gsn_edf.c
@@ -13,6 +13,10 @@
 #include <linux/sched.h>
 #include <linux/slab.h>
 
+#ifdef CONFIG_LITMUS_NESTED_LOCKING
+#include <linux/uaccess.h>
+#endif
+
 #include <litmus/litmus.h>
 #include <litmus/jobs.h>
 #include <litmus/sched_plugin.h>
@@ -1457,6 +1461,318 @@ static struct litmus_lock* gsnedf_new_rsm_mutex(void)
 
 /* **** lock constructor **** */
 
+
+
+
+
+
+
+/* ******************** IKGLP ********************** */
+
+/* struct for semaphore with priority inheritance */
+struct fifo_queue
+{
+        wait_queue_head_t wait;
+        struct task_struct* owner;
+        struct task_struct* hp_waiter;
+        int count; /* number of waiters + holder */
+};
+
+struct ikglp_donor_node
+{
+        struct task_struct *donor;
+        struct task_struct *donee;
+        struct binheap_node node;
+};
+
+static int ikglp_doner_edf_max_heap_base_priority_order(struct binheap_node *a, struct binheap_node *b)
+{
+        struct ikglp_donor_node *d_a = (struct ikglp_donor_node*)binheap_entry(a, struct donor_node, node);
+        struct ikglp_donor_node *d_b = (struct ikglp_donor_node*)binheap_entry(b, struct donor_node, node);
+        
+        return __edf_higher_prio(d_a->donor, BASE, d_b->donor, BASE);   
+}
+
+
+struct ikglp_heap_node
+{
+        struct task_struct *task;
+        struct binheap_node node;
+};
+
+static int ikglp_edf_max_heap_base_priority_order(struct binheap_node *a, struct binheap_node *b)
+{
+        struct ikglp_heap_node *d_a = (struct ikglp_heap_node*)binheap_entry(a, struct ikglp_heap_node, node);
+        struct ikglp_heap_node *d_b = (struct ikglp_heap_node*)binheap_entry(b, struct ikglp_heap_node, node);
+        
+        return __edf_higher_prio(d_a->task, BASE, d_b->task, BASE);     
+}
+
+static int ikglp_edf_min_heap_order(struct binheap_node *a, struct binheap_node *b)
+{
+        struct ikglp_heap_node *d_a = (struct ikglp_heap_node*)binheap_entry(a, struct ikglp_heap_node, node);
+        struct ikglp_heap_node *d_b = (struct ikglp_heap_node*)binheap_entry(b, struct ikglp_heap_node, node);
+        
+        // note reversed order
+        return edf_higher_prio(d_b->task, d_a->task);
+}
+
+
+struct ikglp_semaphore
+{
+        struct litmus_lock litmus_lock;
+        
+        raw_spinlock_t  lock;
+        
+        int nr_replicas; // AKA k
+        int nr_queued;  // num requests in fifos + holders
+        int max_nr_queued;  // capacity of fifos + holders
+        int max_fifo_len; // max len of a fifo queue
+        
+        struct binheap_handle global_heap;      // max-heap, base prio
+        struct ikglp_heap_node *global_holder_nodes;  // holders need their nodes allocated dynamically
+        
+        struct binheap_handle fifos_heap;       // min-heap, effective prio
+        struct ikglp_heap_node *fifos_holder_nodes;  // holders need their nodes allocated dynamically
+
+        struct fifo_queue *shortest_fifo_queue; // pointer to shortest fifo queue
+        
+        /* data structures for holding requests */
+        struct fifo_queue *fifo_queues; // array nr_replicas in length
+        struct binheap_handle priority_queue;   // max-heap, base prio
+        struct binheap_handle doners;   // max-heap, base prio
+};
+
+static inline struct ikglp_semaphore* ikglp_from_lock(struct litmus_lock* lock)
+{
+        return container_of(lock, struct ikglp_semaphore, litmus_lock);
+}
+
+
+static inline int ikglp_get_idx(struct ikglp_semaphore *sem,
+                                                                struct fifo_queue *queue)
+{
+        return (queue - &sem->fifo_queues[0]);
+}
+
+static inline struct fifo_queue* ikglp_get_queue(
+        struct ikglp_semaphore *sem,
+        struct task_struct *holder)
+{
+        int i;
+        for(i = 0; i < sem->nr_replicas; ++i)
+                if(sem->fifo_queues[i].owner == holder)
+                        return(&sem->fifo_queues[i]);
+        return(NULL);
+}
+
+static struct task_struct* ikglp_find_hp_waiter(
+        struct fifo_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 fifo_queue* ikglp_find_shortest(
+        struct ikglp_semaphore *sem,
+        struct fifo_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 fifo_queue* step = search_start;
+        struct fifo_queue* shortest = sem->shortest_fifo_queue;
+        
+        do {
+                step = (step+1 != &sem->fifo_queues[sem->nr_replicas]) ?
+                        step+1 : &sem->fifo_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 int gsnedf_ikglp_lock(struct litmus_lock* l)
+{
+        struct ikglp_semaphore *sem = ikglp_from_lock(l);
+        return 0;
+}
+
+static int gsnedf_ikglp_unlock(struct litmus_lock* l)
+{
+        struct ikglp_semaphore *sem = ikglp_from_lock(l);
+        return 0;
+}
+
+static int gsnedf_ikglp_close(struct litmus_lock* l)
+{
+        struct task_struct *t = current;
+        struct ikglp_semaphore *sem = ikglp_from_lock(l);
+        unsigned long flags;
+    
+        int owner = 0;
+        int i;
+    
+        raw_spin_lock_irqsave(&sem->lock, flags);
+    
+        for(i = 0; i < sem->nr_replicas; ++i) {
+                if(sem->fifo_queues[i].owner == t) {
+                        owner = 1;
+                        break;
+                }
+        }
+    
+        raw_spin_unlock_irqrestore(&sem->lock, flags);
+    
+        if (owner)
+                gsnedf_ikglp_unlock(l);
+    
+        return 0;
+}
+
+static void gsnedf_ikglp_free(struct litmus_lock* l)
+{
+        struct ikglp_semaphore *sem = ikglp_from_lock(l);
+        
+        kfree(sem->fifo_queues);
+        kfree(sem->fifos_holder_nodes);
+        kfree(sem->global_holder_nodes);
+        kfree(sem);
+}
+
+static struct litmus_lock_ops gsnedf_ikglp_lock_ops = {
+        .lock   = gsnedf_ikglp_lock,
+        .unlock = gsnedf_ikglp_unlock,
+        
+        // ikglp can only be an outer-most lock.
+        .propagate_increase_inheritance = NULL,
+        .propagate_decrease_inheritance = NULL, 
+        
+        .close  = gsnedf_ikglp_close,   
+        .deallocate = gsnedf_ikglp_free,
+};
+
+static struct litmus_lock* gsnedf_new_ikglp(void* __user arg, int* ret_code)
+{
+        struct ikglp_semaphore* sem;
+        int nr_replicas = 0;
+        int i;
+        
+        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;
+        }
+        
+        sem->global_holder_nodes = kmalloc(sizeof(struct ikglp_heap_node)*nr_replicas, GFP_KERNEL);
+        if(!sem->global_holder_nodes)
+        {
+                kfree(sem);
+                *ret_code = -ENOMEM;
+                return NULL;            
+        }
+        
+        sem->fifos_holder_nodes = kmalloc(sizeof(struct ikglp_heap_node)*nr_replicas, GFP_KERNEL);
+        if(!sem->fifos_holder_nodes)
+        {
+                kfree(sem->global_holder_nodes);
+                kfree(sem);
+                *ret_code = -ENOMEM;
+                return NULL;
+        }       
+        
+        sem->fifo_queues = kmalloc(sizeof(struct fifo_queue)*nr_replicas, GFP_KERNEL);
+        if(!sem->fifo_queues)
+        {
+                kfree(sem->fifos_holder_nodes);
+                kfree(sem->global_holder_nodes);
+                kfree(sem);
+                *ret_code = -ENOMEM;
+                return NULL;            
+        }
+        
+        sem->litmus_lock.ops = &gsnedf_ikglp_lock_ops;
+        
+#ifdef CONFIG_DEBUG_SPINLOCK
+        {
+                __raw_spin_lock_init(&sem->lock, ((struct litmus_lock*)sem)->cheat_lockdep, &((struct litmus_lock*)sem)->key);
+        }
+#else
+        raw_spin_lock_init(&sem->lock);
+#endif  
+
+        sem->nr_replicas = nr_replicas;
+        sem->nr_queued = 0;
+        
+        sem->max_fifo_len = (num_online_cpus()/nr_replicas) + ((num_online_cpus()%nr_replicas) != 0) - 1; // -1 to exclude holder
+        sem->max_nr_queued = (sem->max_fifo_len * nr_replicas) + nr_replicas; // capacity of fifos + holders
+        
+        for(i = 0; i < nr_replicas; ++i)
+        {
+                sem->fifo_queues[i].owner = NULL;
+                sem->fifo_queues[i].hp_waiter = NULL;
+                init_waitqueue_head(&sem->fifo_queues[i].wait);
+                sem->fifo_queues[i].count = 0;
+        }
+        
+        sem->shortest_fifo_queue = &sem->fifo_queues[0];
+        
+        
+        // init heaps
+        INIT_BINHEAP_HANDLE(&sem->global_heap, ikglp_edf_max_heap_base_priority_order);
+        INIT_BINHEAP_HANDLE(&sem->fifos_heap, ikglp_edf_min_heap_order);
+        INIT_BINHEAP_HANDLE(&sem->priority_queue, ikglp_edf_max_heap_base_priority_order);
+        INIT_BINHEAP_HANDLE(&sem->doners, ikglp_doner_edf_max_heap_base_priority_order);        
+        
+        // init heap nodes used by holders
+        for(i = 0; i < nr_replicas; ++i) {
+                INIT_BINHEAP_NODE(&(sem->global_holder_nodes[i].node));
+                INIT_BINHEAP_NODE(&(sem->fifos_holder_nodes[i].node));
+        }
+
+        *ret_code = 0;
+        return &sem->litmus_lock;
+}
+
+
+
+
+
 #endif
 
 
@@ -1678,7 +1994,7 @@ static struct litmus_lock* gsnedf_new_fmlp(void)
 
 
 static long gsnedf_allocate_lock(struct litmus_lock **lock, int type,
-                                 void* __user unused)
+                                 void* __user args)
 {
         int err;
 
@@ -1694,6 +2010,10 @@ static long gsnedf_allocate_lock(struct litmus_lock **lock, int type,
     case RSM_MUTEX:
                 *lock = gsnedf_new_rsm_mutex();
                 break;
+                        
+        case IKGLP_SEM:
+                *lock = gsnedf_new_ikglp(args, &err);
+                break;
 #endif
         
         default: