7 files changed, 265 insertions, 15 deletions

diff --git a/include/litmus/edf_common.h b/include/litmus/edf_common.h
index 80d4321cc87e..b6081553d901 100644
--- a/include/litmus/edf_common.h
+++ b/include/litmus/edf_common.h
@@ -22,6 +22,10 @@ int edf_ready_order(struct bheap_node* a, struct bheap_node* b);
 
 int edf_preemption_needed(rt_domain_t* rt, struct task_struct *t);
 
-int edf_set_hp_task(struct pi_semaphore *sem);
-int edf_set_hp_cpu_task(struct pi_semaphore *sem, int cpu);
+#ifdef CONFIG_FMLP
+int edf_set_hp_task(struct fmlp_semaphore *sem);
+int edf_set_hp_cpu_task(struct fmlp_semaphore *sem, int cpu);
 #endif
+
+#endif
+
diff --git a/include/litmus/prio_sem.h b/include/litmus/prio_sem.h
new file mode 100644
index 000000000000..eb54c670947a
--- /dev/null
+++ b/include/litmus/prio_sem.h
@@ -0,0 +1,31 @@
+/* Operations and structures for dealing with generic priority inheritance. */
+
+#ifndef _LINUX_PI_H_
+#include <linux/sched.h>
+
+/* forward declaration */
+struct pi_semaphore;
+
+/* stack operations */
+void push_pi_sem(
+        struct task_struct *tsk,
+        struct task_struct *inh,
+        struct pi_semaphore *sem);
+
+void pop_pi_sem(struct task_struct *tsk);
+
+pi_sem_record_t* peek_pi_sem(struct task_struct *tsk);
+
+/* non-stack operations */
+int update_pi_sem(
+        struct task_struct *tsk,
+        struct task_struct *inh,
+        struct pi_semaphore *sem);
+
+int remove_pi_sem(
+        struct task_struct *tsk,
+        struct pi_semaphore *sem);
+
+int has_pi_sem(struct task_struct *tsk);
+
+#endif
diff --git a/include/litmus/rt_param.h b/include/litmus/rt_param.h
index 6f40b52f0802..9e2f06951d4b 100644
--- a/include/litmus/rt_param.h
+++ b/include/litmus/rt_param.h
@@ -102,7 +102,7 @@ struct rt_job {
  * acquired.
  */
 struct pi_sem_record {
-        /* Pointer to inheritance-donating job.
+        /* Task from which task inherits.
          * May be set to "self" if there is no active
          * inheritance from this semaphore.
          */
@@ -136,13 +136,34 @@ struct rt_param {
         /* timing parameters */
         struct rt_job           job_params;
 
-        /* task representing the current "inherited" task
-         * priority, assigned by inherit_priority and
-         * return priority in the scheduler plugins.
-         * could point to self if PI does not result in
-         * an increased task priority.
+#ifdef CONFIG_PI_SEMAPHORES
+        /* Stack of tasks representing the inheritance from
+         * nested priority inheriting semaphores. Also provides
+         * a list of currently held pi-inheriting semaphores.
+         *
+         * End of List = Stack Top
+         *
+         * If a semaphore provides no inheritance, then record
+         * pointing to self should be pushed onto the stack (so
+         * inheritance record may be updated at a later time
+         * if a higher-priority job blocks).
+         *
+         * Other protocols may require additional information if
+         * a task can inherit multiple parameters from several other
+         * tasks.
          */
-         struct task_struct*    inh_task;
+        struct list_head pi_sem_stack;
+
+        /* It is useful to cache the highest priority inherited from
+         * all held locks (as recored by pi_sem_stack).  eff_priority
+         * is this cache.  Further, a protocol may not use traditional
+         * nesting, such as FMLP-Long.  Such protocols may forgo the
+         * the use of pi_sem_stack and use eff_priority to track inheritance
+         * alone.
+         */
+        struct task_struct*    eff_priority;
+#endif
+
 
 #ifdef CONFIG_NP_SECTION
         /* For the FMLP under PSN-EDF, it is required to make the task
diff --git a/litmus/Makefile b/litmus/Makefile
index 30369787ece2..1cf2b6267ae3 100644
--- a/litmus/Makefile
+++ b/litmus/Makefile
@@ -18,6 +18,8 @@ obj-y     = sched_plugin.o litmus.o \
 obj-$(CONFIG_PLUGIN_CEDF) += sched_cedf.o
 obj-$(CONFIG_PLUGIN_PFAIR) += sched_pfair.o
 
+obj-$(CONFIG_PI_SEMAPHORES) += prio_sem.o
+
 obj-$(CONFIG_FEATHER_TRACE) += ft_event.o ftdev.o
 obj-$(CONFIG_SCHED_TASK_TRACE) += sched_task_trace.o
 obj-$(CONFIG_SCHED_DEBUG_TRACE) += sched_trace.o
diff --git a/litmus/edf_common.c b/litmus/edf_common.c
index 06daec66c984..1e91848a71dd 100644
--- a/litmus/edf_common.c
+++ b/litmus/edf_common.c
@@ -18,6 +18,12 @@
  *                    than second. Deadline ties are broken by PID.
  *
  * both first and second may be NULL
+ *
+ * WARNING: Assumes rt_param.eff_priority holds the effective
+ * priority if PI semaphores are in use.  PI protocols should
+ * cache the highest priority received through all held locks
+ * in rt_param.eff_priority (or use a different edf_higher_prio()
+ * function).
  */
 int edf_higher_prio(struct task_struct* first,
                     struct task_struct* second)
@@ -32,14 +38,15 @@ int edf_higher_prio(struct task_struct* first,
                 return 0;
         }
 
-
+#ifdef CONFIG_PI_SEMAPHORES
         /* Check for inherited priorities. Change task
          * used for comparison in such a case.
          */
-        if (first && first->rt_param.inh_task)
-                first_task = first->rt_param.inh_task;
-        if (second && second->rt_param.inh_task)
-                second_task = second->rt_param.inh_task;
+        if (first && (first->rt_param.eff_priority != first))
+                first_task = first->rt_param.eff_priority;
+        if (second && (second->rt_param.eff_priority != second))
+                second_task = second->rt_param.eff_priority;
+#endif
 
         return
                 /* it has to exist in order to have higher priority */
@@ -65,7 +72,7 @@ int edf_higher_prio(struct task_struct* first,
                  * priority wins.
                  */
                 (first_task->pid == second_task->pid &&
-                 !second->rt_param.inh_task))));
+                 (second == second_task)))));
 }
 
 int edf_ready_order(struct bheap_node* a, struct bheap_node* b)
diff --git a/litmus/fmlp.c b/litmus/fmlp.c
index de46e626f43f..36965efe37e5 100644
--- a/litmus/fmlp.c
+++ b/litmus/fmlp.c
@@ -36,8 +36,10 @@ static  void* create_fmlp_semaphore(void)
         for (i = 0; i < NR_CPUS; i++)
                 sem->pi.hp.cpu_task[i] = NULL;
 
+        /* FMLP-Long does not use semaphore stack.
         sem->pi.stack_node.inh_task = NULL;
         INIT_LIST_HEAD(&sem->pi.stack_node.list);
+        */
 
         return sem;
 }
diff --git a/litmus/prio_sem.c b/litmus/prio_sem.c
new file mode 100644
index 000000000000..3c436656a8b4
--- /dev/null
+++ b/litmus/prio_sem.c
@@ -0,0 +1,183 @@
+/* Operations and structures for dealing with generic priority inheritance. */
+
+#include <litmus/litmus.h>
+#include <litmus/prio_sem.h>
+
+#include <litmus/sched_plugin.h>
+
+/* Add a record to the pi sem history stack.
+ *
+ * Recommend that inh == tsk for "no inheritance", though this
+ * is up to the pi sem protocol implementation.  inh could be
+ * NULL as long as the protocol implementation expects it.
+ */
+void push_pi_sem(
+        struct task_struct *tsk,
+        struct task_struct *inh,
+        struct pi_semaphore *sem)
+{
+        sem->stack_node.inh_task = inh;
+
+        /* list_add() needs node to be initialized properly.  Node may
+         * not be initialized yet.
+         */
+        INIT_LIST_HEAD(&sem->stack_node.list);
+
+        list_add_tail(&tsk->rt_param.pi_sem_stack, &sem->stack_node.list);
+
+        TRACE_TASK(tsk,
+                "Pushed semaphore %p with inheritance from task %d\n",
+                sem, inh->pid);
+}
+
+/* Removes the last semaphore record. */
+void pop_pi_sem(struct task_struct *tsk)
+{
+        int empty = list_empty(&tsk->rt_param.pi_sem_stack);
+
+        WARN_ON(empty);
+
+        if(!empty)
+        {
+                pi_sem_record_t *rec =
+                        list_entry(tsk->rt_param.pi_sem_stack.prev, pi_sem_record_t, list);
+                list_del(&rec->list);
+
+                TRACE_TASK(tsk, "Popped sem/inh record %p / %d.\n",
+                                container_of(rec, struct pi_semaphore, stack_node),
+                                rec->inh_task->pid);
+        }
+}
+
+/* Get a pointer to the last semaphore pushed on to the stack.
+ * Returns NULL if there is no record.
+ */
+pi_sem_record_t* peek_pi_sem(struct task_struct *tsk)
+{
+        if(!list_empty(&tsk->rt_param.pi_sem_stack))
+        {
+                pi_sem_record_t *rec =
+                        list_entry(tsk->rt_param.pi_sem_stack.prev,
+                                pi_sem_record_t, list);
+                return rec;
+        }
+        return NULL;
+}
+
+
+/* Update priority inheritance. */
+int update_pi_sem(
+        struct task_struct *tsk,
+        struct task_struct *inh,
+        struct pi_semaphore *sem)
+{
+        int success = 0;
+        struct list_head *pos, *next;
+        pi_sem_record_t *rec;
+
+        /* Though we have a reference to the semaphore (sem), we need
+         * to search through tsk's stack to validate that tsk actually
+         * holds sem.  If tsk holds sem, then sem will be in its stack.
+         *
+         * TODO: Add function pointer hooks to pi_semaphore for operations
+         * such as "int is_holder(struct task_struct *tsk)".  pi_semaphore
+         * does not track its holder(s), though concrete protocols may,
+         * providing more efficient validation methods than this stack search.
+         * Another function may be "pi_sem_record_t* get_record(...)" if the
+         * semaphore protocol supports multiple holders (though
+         * pi_semaphore.stack_node will probably go unused or replaced in
+         * such cases).
+         */
+
+        /* Assuming a sane locking order, the lock we're looking for
+         * is probably towards (at) the end.
+         */
+        list_for_each_prev_safe(pos, next, &tsk->rt_param.pi_sem_stack)
+        {
+                struct pi_semaphore* which_sem;
+                rec = list_entry(pos, pi_sem_record_t, list);
+
+                which_sem = container_of(rec, struct pi_semaphore, stack_node);
+                if(which_sem == sem)
+                {
+                        TRACE_TASK(tsk, "Updating inheritance from %d to %d on sem %p\n",
+                                rec->inh_task->pid, inh->pid, sem);
+
+                        rec->inh_task = inh;
+                        success = 1;
+                        break;
+                }
+        }
+
+        if(!success)
+        {
+                TRACE_TASK(tsk, "Could not find record for sem %p to update inheritance.\n",
+                        sem);
+        }
+
+        return success;
+}
+
+/* Removes a record from the semaphore stack if it exists.
+ *
+ * Note: pop_pi_sem() may be more appropriate if total ordering can be
+ * guaranteed.
+ */
+int remove_pi_sem(struct task_struct *tsk, struct pi_semaphore *sem)
+{
+        int success = 0;
+        struct list_head *pos, *next;
+        pi_sem_record_t *rec;
+
+        /* Though we have a reference to the semaphore (sem), we need
+         * to search through tsk's stack to validate that tsk actually
+         * holds sem.  If tsk holds sem, then sem will be in its stack.
+         *
+         * TODO: Add function pointer hooks to pi_semaphore for operations
+         * such as "int is_holder(struct task_struct *tsk)".  pi_semaphore
+         * does not track its holder(s), though concrete protocols may,
+         * providing more efficient validation methods than this stack search.
+         * Another function may be "pi_sem_record_t* get_record(...)" if the
+         * semaphore protocol supports multiple holders (though
+         * pi_semaphore.stack_node will probably go unused or replaced in
+         * such cases).
+         */
+
+        /* Assuming a sane locking order, the lock we're looking for
+         * is probably towards (at) the end.
+         */
+        list_for_each_prev_safe(pos, next, &tsk->rt_param.pi_sem_stack)
+        {   
+                struct pi_semaphore* which_sem;
+                rec = list_entry(pos, pi_sem_record_t, list);
+
+                which_sem = container_of(rec, struct pi_semaphore, stack_node);
+                if(which_sem == sem)
+                {
+                        TRACE_TASK(tsk, "Removing semaphore stack record from %d with sem %p\n",
+                                rec->inh_task->pid, sem);
+
+                        list_del(&rec->list);
+                        success = 1;
+                        break;
+                }   
+        }   
+
+        if(!success)
+        {   
+                TRACE_TASK(tsk, "Could not find record for sem %p to remove.\n",
+                        sem);
+        }
+
+        return success; 
+}
+
+/* Returns true of tsk holds ANY pi semaphores (as reportable by the stack).
+ * Function will return false if the protocol in use does not use the stack,
+ * such as FMLP-Long, which uses rt_params.eff_priority.
+ */
+int has_pi_sem(struct task_struct *tsk)
+{
+        return !list_empty(&tsk->rt_param.pi_sem_stack);
+}
+