6 files changed, 538 insertions, 2 deletions

diff --git a/include/litmus/reservations/ext_reservation.h b/include/litmus/reservations/ext_reservation.h
index c9ec924fc55b..432eba0785e2 100644
--- a/include/litmus/reservations/ext_reservation.h
+++ b/include/litmus/reservations/ext_reservation.h
@@ -6,6 +6,10 @@
 #include <litmus/debug_trace.h>
 #include <litmus/reservations/budget-notifier.h>
 
+#ifdef CONFIG_LITMUS_LOCKING
+#include <litmus/locking.h>
+#endif
+
 struct reservation_environment;
 struct reservation;
 
@@ -91,6 +95,17 @@ struct reservation {
         struct bheap_node* heap_node;
         struct release_heap* rel_heap;
         struct list_head ln;
+
+#ifdef CONFIG_LITMUS_LOCKING
+
+        /* reservation representing the current "inherited" reservation
+         * priority, assigned by the scheduler plugins.
+         * could point to self if PI does not result in
+         * an increased task priority.
+         */
+         struct reservation* inh_res;
+
+#endif
 };
 
 void init_ext_reservation(
@@ -141,6 +156,16 @@ typedef int (*env_is_np_t) (
 typedef void (*env_shutdown_t) (
         struct reservation_environment* env);
 
+#ifdef CONFIG_LITMUS_LOCKING
+/* Called when the current task attempts to create a new lock of a given
+ * protocol type. */
+typedef long (*env_allocate_lock_t) (
+        struct reservation_environment* env,
+        struct litmus_lock **lock,
+        int type,
+        void* __user config);
+#endif
+
 struct reservation_environment_ops {
         env_update_time_t       update_time;
         env_dispatch_t          dispatch;
@@ -151,6 +176,11 @@ struct reservation_environment_ops {
         env_find_res_t          find_res_by_id;
         env_is_np_t                     is_np;
         env_shutdown_t          shutdown;
+
+#ifdef CONFIG_LITMUS_LOCKING
+        /*      locking protocols       */
+        env_allocate_lock_t     allocate_lock;
+#endif
 };
 
 struct reservation_environment {
diff --git a/include/litmus/rt_domain.h b/include/litmus/rt_domain.h
index 16f6b6cd6357..8dc8efb24d98 100644
--- a/include/litmus/rt_domain.h
+++ b/include/litmus/rt_domain.h
@@ -69,6 +69,11 @@ struct release_heap {
 void domain_suspend_releases(rt_domain_t* rt);
 void domain_resume_releases(rt_domain_t* rt);
 
+/* caller must hold release lock */
+struct release_heap* get_release_heap_res(rt_domain_t *rt,
+                                                struct reservation* res,
+                                                int use_task_heap);
+
 static inline struct task_struct* __next_ready(rt_domain_t* rt)
 {
         struct bheap_node *hn = bheap_peek(rt->order, &rt->ready_queue);
diff --git a/litmus/reservations/ext_reservation.c b/litmus/reservations/ext_reservation.c
index 3bedb0fbee04..e6685fca20ea 100644
--- a/litmus/reservations/ext_reservation.c
+++ b/litmus/reservations/ext_reservation.c
@@ -22,6 +22,18 @@ int higher_res_prio(struct reservation* first,
         if (!first || !second)
                 return first && !second;
 
+#ifdef CONFIG_LITMUS_LOCKING
+
+        /* Check for inherited priorities. Change reservation
+         * used for comparison in such a case.
+         */
+        if (unlikely(first->inh_res))
+                first_task = first->inh_res;
+        if (unlikely(second->inh_res))
+                second_task = second->inh_res;
+
+#endif
+
         if (first_task->priority > second_task->priority) {
                 return 1;
         }
@@ -30,6 +42,16 @@ int higher_res_prio(struct reservation* first,
                 if (first_task->id < second_task->id) {
                         return 1;
                 }
+#ifdef CONFIG_LITMUS_LOCKING
+                else if (first_task->id == second_task->id) {
+                        /* If the PIDs are the same then the task with the
+                                * inherited priority wins.
+                                */
+                        if (!second->inh_res) {
+                                return 1;
+                        }
+                }
+#endif
         }
         return 0; /* fall-through. prio(second_task) > prio(first_task) */
 }
diff --git a/litmus/reservations/gedf_reservation.c b/litmus/reservations/gedf_reservation.c
index 771971d0af1b..44bb13c551a0 100644
--- a/litmus/reservations/gedf_reservation.c
+++ b/litmus/reservations/gedf_reservation.c
@@ -11,6 +11,7 @@
 #include <litmus/np.h>
 #include <litmus/sched_trace.h>
 #include <litmus/debug_trace.h>
+#include <litmus/trace.h>
 #include <litmus/reservations/gedf_reservation.h>
 
 // Needed to store context during cross-CPU function calls
@@ -705,6 +706,457 @@ static void gedf_env_release_jobs(rt_domain_t* rt, struct bheap* res)
         raw_spin_unlock_irqrestore(&rt->ready_lock, flags);
 }
 
+#ifdef CONFIG_LITMUS_LOCKING
+
+#include <litmus/fdso.h>
+#include <litmus/wait.h>
+
+/* called with IRQs off */
+static void set_priority_inheritance(struct task_struct* t, struct task_struct* prio_inh)
+{
+        struct gedf_cpu_entry* linked_on;
+        int check_preempt = 0;
+
+        struct reservation *t_res = (struct reservation *) tsk_rt(t)->plugin_state;
+        struct reservation *prio_inh_res = (struct reservation *) tsk_rt(prio_inh)->plugin_state;
+
+        struct reservation_environment *env = t_res->par_env;
+
+        struct gedf_reservation_environment *gedf_env;
+        gedf_env = container_of(env, struct gedf_reservation_environment, env);
+
+        struct gedf_reservation *gedf_res;
+        gedf_res = container_of(t_res, struct gedf_reservation, res);
+
+        raw_spin_lock(&gedf_env->domain.ready_lock);
+
+        TRACE_TASK(t, "inherits priority from %s/%d\n", prio_inh->comm, prio_inh->pid);
+        t_res->inh_res = prio_inh_res;
+
+        linked_on = gedf_res->linked_on;
+
+        /* If it is scheduled, then we need to reorder the CPU heap. */
+        if (linked_on) {
+                TRACE_TASK(t, "%s: linked  on %d\n",
+                           __FUNCTION__, linked_on->id);
+                /* holder is scheduled; need to re-order CPUs. */
+                update_cpu_position(gedf_res->linked_on, &gedf_env->cpu_heap);
+        } else {
+                /* holder may be queued: first stop queue changes */
+                raw_spin_lock(&gedf_env->domain.release_lock);
+                if (is_queued_res(t_res)) {
+                        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,
+                                               t_res->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__);
+                }
+                raw_spin_unlock(&gedf_env->domain.release_lock);
+
+                /* If holder was enqueued in a release heap, then the following
+                 * preemption check is pointless, but we can't easily detect
+                 * that case. If you want to fix this, then consider that
+                 * simply adding a state flag requires O(n) time to update when
+                 * releasing n tasks, which conflicts with the goal to have
+                 * O(log n) merges. */
+                if (check_preempt) {
+                        /* 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,
+                                         &gedf_env->domain.ready_queue);
+                        check_for_preemptions(gedf_env);
+                }
+        }
+
+        raw_spin_unlock(&gedf_env->domain.ready_lock);
+}
+
+/* called with IRQs off */
+static void clear_priority_inheritance(struct task_struct* t)
+{
+        struct reservation *t_res = (struct reservation *) tsk_rt(t)->plugin_state;
+
+        struct reservation_environment *env = t_res->par_env;
+
+        struct gedf_reservation_environment *gedf_env;
+        gedf_env = container_of(env, struct gedf_reservation_environment, env);
+
+        struct gedf_reservation *gedf_res;
+        gedf_res = container_of(t_res, struct gedf_reservation, res);
+
+        raw_spin_lock(&gedf_env->domain.ready_lock);
+
+        /* A job only stops inheriting a priority when it releases a
+         * resource. Thus we can make the following assumption.*/
+        int cpu = smp_processor_id();
+        struct gedf_cpu_entry *entry = &gedf_env->cpu_entries[cpu];
+        BUG_ON(entry->scheduled != gedf_res);
+
+        TRACE_TASK(t, "priority restored\n");
+        t_res->inh_res = NULL;
+
+        BUG_ON(!gedf_res->linked_on && !bheap_node_in_heap(t_res->heap_node));
+
+        /* Check if rescheduling is necessary. We can't use heap_decrease()
+         * since the priority was effectively lowered. Instead, we
+         * update the position of the CPU on which it is linked, or remove
+         * and re-add to to the appropriate heap if it is not linked. */
+        if (gedf_res->linked_on) {
+                update_cpu_position(gedf_res->linked_on, &gedf_env->cpu_heap);
+        }
+        else {
+                struct bheap *heap;
+                if (t_res->replenishment_time > litmus_clock()) {
+                        raw_spin_lock(&gedf_env->domain.release_lock);
+                        heap = &(get_release_heap_res(&gedf_env->domain, t_res, 0)->heap);
+                        raw_spin_unlock(&gedf_env->domain.release_lock);
+                }
+                else {
+                        heap = &(gedf_env->domain.ready_queue);
+                }
+
+                bheap_delete(edf_ready_order, heap, t_res->heap_node);
+                bheap_insert(edf_ready_order, heap, t_res->heap_node);
+        }
+
+        check_for_preemptions(gedf_env);
+
+        raw_spin_unlock(&gedf_env->domain.ready_lock);
+}
+
+/* ******************** OMLP support ********************** */
+
+/* struct for semaphore with priority inheritance */
+struct omlp_semaphore {
+        struct litmus_lock litmus_lock;
+
+        /* current resource holder */
+        struct task_struct *owner;
+
+        /* highest-priority waiter */
+        struct task_struct *hp_waiter;
+        struct reservation *hp_waiter_res;
+
+        /* FIFO queue of waiting tasks */
+        wait_queue_head_t fifo_wait;
+        /* Priority queue of waiting tasks */
+        wait_queue_head_t prio_wait;
+
+        /* How many slots remaining in FIFO queue? */
+        unsigned int num_free;
+};
+
+static inline struct omlp_semaphore* omlp_from_lock(struct litmus_lock* lock)
+{
+        return container_of(lock, struct omlp_semaphore, litmus_lock);
+}
+
+/* already locked */
+static void omlp_enqueue(struct omlp_semaphore *sem, prio_wait_queue_t* wait)
+{
+        if (sem->num_free) {
+                /* there is space in the FIFO queue */
+                sem->num_free--;
+                __add_wait_queue_tail_exclusive(&sem->fifo_wait, &wait->wq);
+        } else {
+                /* nope, gotta go to the priority queue */
+                __add_wait_queue_prio_exclusive(&sem->prio_wait, wait);
+        }
+}
+
+/* already locked */
+static int omlp_move(struct omlp_semaphore *sem)
+{
+        struct list_head* first;
+
+        if (waitqueue_active(&sem->prio_wait)) {
+                first = sem->prio_wait.task_list.next;
+                list_move_tail(first, &sem->fifo_wait.task_list);
+                return 1;
+        }
+        else
+                return 0;
+}
+
+static struct task_struct* omlp_dequeue(struct omlp_semaphore *sem)
+{
+        struct task_struct* first = __waitqueue_remove_first(&sem->fifo_wait);
+
+        if (first && !omlp_move(sem))
+                sem->num_free++;
+
+        return first;
+}
+
+/* caller is responsible for locking */
+static struct task_struct* omlp_find_hp_waiter(struct omlp_semaphore *sem,
+                                        struct task_struct* skip)
+{
+        struct list_head        *pos;
+        struct task_struct      *queued, *found = NULL;
+        struct reservation      *q_res, *f_res = NULL;
+
+        /* check FIFO queue first */
+        list_for_each(pos, &sem->fifo_wait.task_list) {
+                queued  = (struct task_struct*) list_entry(pos, wait_queue_t,
+                                                           task_list)->private;
+
+                /* Compare task prios, find high prio task. */
+                q_res = (struct reservation *) tsk_rt(queued)->plugin_state;
+                if (queued != skip && higher_res_prio(q_res, f_res)) {
+                        f_res = q_res;
+                        found = queued;
+                }
+        }
+
+        /* check priority queue next */
+        if (waitqueue_active(&sem->prio_wait)) {
+                /* first has highest priority */
+                pos = sem->prio_wait.task_list.next;
+                queued  = (struct task_struct*) list_entry(pos, wait_queue_t,
+                                                           task_list)->private;
+                q_res = (struct reservation *) tsk_rt(queued)->plugin_state;
+                if (higher_res_prio(q_res, f_res)) {
+                        f_res = q_res;
+                        found = queued;
+                }
+        }
+
+        return found;
+}
+
+int gedf_env_omlp_lock(struct litmus_lock* l)
+{
+        struct task_struct* t = current;
+        struct omlp_semaphore *sem = omlp_from_lock(l);
+        prio_wait_queue_t wait;
+        unsigned long flags;
+
+        if (!is_realtime(t))
+                return -EPERM;
+
+        /* prevent nested lock acquisition --- not supported by global OMLP
+           by default */
+        if (tsk_rt(t)->num_locks_held)
+                return -EBUSY;
+
+        struct reservation *t_res = (struct reservation *) tsk_rt(t)->plugin_state;
+
+        struct gedf_reservation *gedf_res = container_of(t_res, struct gedf_reservation, res);
+
+        spin_lock_irqsave(&sem->fifo_wait.lock, flags);
+
+        if (sem->owner) {
+                /* resource is not free => must suspend and wait */
+
+                /* the priority queue needs the deadline, not the "priority" */
+                init_prio_waitqueue_entry(&wait, t, ULLONG_MAX - t_res->priority);
+
+                set_task_state(t, TASK_UNINTERRUPTIBLE);
+
+                omlp_enqueue(sem, &wait);
+
+                /* check if we need to activate priority inheritance */
+                if (higher_res_prio(t_res, sem->hp_waiter_res)) {
+                        sem->hp_waiter = t;
+                        sem->hp_waiter_res = t_res;
+                        struct reservation *o_res = (struct reservation *) tsk_rt(sem->owner)->plugin_state;
+                        if (higher_res_prio(t_res, o_res)) {
+                                set_priority_inheritance(sem->owner, sem->hp_waiter);
+                        }
+                }
+
+                TS_LOCK_SUSPEND;
+
+                /* release lock before sleeping */
+                spin_unlock_irqrestore(&sem->fifo_wait.lock, flags);
+
+                BUG_ON(!gedf_res->linked_on && !bheap_node_in_heap(t_res->heap_node));
+
+                schedule(); // will have issues if the reservation
+                            // is not linked or on the ready queue
+
+                TS_LOCK_RESUME;
+
+                /* Since we hold the lock, no other task will change
+                 * ->owner. We can thus check it without acquiring the spin
+                 * lock. */
+                BUG_ON(sem->owner != t);
+        } else {
+                /* it's ours now */
+                sem->owner = t;
+
+                spin_unlock_irqrestore(&sem->fifo_wait.lock, flags);
+        }
+
+        tsk_rt(t)->num_locks_held++;
+
+        return 0;
+}
+
+static int gedf_env_omlp_unlock(struct litmus_lock* l)
+{
+        struct task_struct *t = current, *next;
+        struct omlp_semaphore *sem = omlp_from_lock(l);
+        unsigned long flags;
+        int err = 0;
+
+        spin_lock_irqsave(&sem->fifo_wait.lock, flags);
+
+        if (sem->owner != t) {
+                err = -EINVAL;
+                goto out;
+        }
+
+        tsk_rt(t)->num_locks_held--;
+
+        /* check if there are jobs waiting for this resource */
+        next = omlp_dequeue(sem);
+        if (next) {
+                /* next becomes the resouce holder */
+                sem->owner = next;
+                TRACE_CUR("lock ownership passed to %s/%d\n", next->comm, next->pid);
+
+                struct reservation *n_res = (struct reservation *) tsk_rt(next)->plugin_state;
+
+                /* determine new hp_waiter if necessary */
+                if (next == sem->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. */
+                        sem->hp_waiter = omlp_find_hp_waiter(sem, next);
+                        if (sem->hp_waiter) {
+                                TRACE_TASK(sem->hp_waiter, "is new highest-prio waiter\n");
+                                sem->hp_waiter_res = (struct reservation *) tsk_rt(sem->hp_waiter)->plugin_state;
+                        }
+                        else {
+                                TRACE("no further waiters\n");
+                                sem->hp_waiter_res = NULL;
+                        }
+                } 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, sem->hp_waiter);
+                }
+
+                /* wake up next */
+                wake_up_process(next);
+        } else
+                /* becomes available */
+                sem->owner = NULL;
+
+        /* we lose the benefit of priority inheritance (if any) */
+        if (((struct reservation *)tsk_rt(t)->plugin_state)->inh_res)
+                clear_priority_inheritance(t);
+
+out:
+        spin_unlock_irqrestore(&sem->fifo_wait.lock, flags);
+
+        return err;
+}
+
+static int gedf_env_omlp_close(struct litmus_lock* l)
+{
+        struct task_struct *t = current;
+        struct omlp_semaphore *sem = omlp_from_lock(l);
+        unsigned long flags;
+
+        int owner;
+
+        spin_lock_irqsave(&sem->fifo_wait.lock, flags);
+
+        owner = sem->owner == t;
+
+        spin_unlock_irqrestore(&sem->fifo_wait.lock, flags);
+
+        if (owner)
+                gedf_env_omlp_unlock(l);
+
+        return 0;
+}
+
+static void gedf_env_omlp_free(struct litmus_lock* lock)
+{
+        kfree(omlp_from_lock(lock));
+}
+
+static struct litmus_lock_ops gedf_env_omlp_lock_ops = {
+        .close  = gedf_env_omlp_close,
+        .lock   = gedf_env_omlp_lock,
+        .unlock = gedf_env_omlp_unlock,
+        .deallocate = gedf_env_omlp_free,
+};
+
+static struct litmus_lock* gedf_env_new_omlp(void)
+{
+        struct omlp_semaphore* sem;
+        struct reservation *t_res;
+        struct gedf_reservation_environment *gedf_env;
+
+        sem = kmalloc(sizeof(*sem), GFP_KERNEL);
+        if (!sem)
+                return NULL;
+
+        t_res = (struct reservation *) tsk_rt(current)->plugin_state;
+        gedf_env = container_of(t_res->par_env, struct gedf_reservation_environment, env);
+
+        sem->owner   = NULL;
+        sem->hp_waiter = NULL;
+        sem->hp_waiter_res = NULL;
+        init_waitqueue_head(&sem->fifo_wait);
+        init_waitqueue_head(&sem->prio_wait);
+        sem->litmus_lock.ops = &gedf_env_omlp_lock_ops;
+        /* free = cpus-1 since ->owner is the head and also counted */
+        sem->num_free = gedf_env->num_cpus - 1;
+
+        return &sem->litmus_lock;
+}
+
+/* **** lock constructor **** */
+
+static long gedf_env_allocate_lock(
+        struct reservation_environment* env,
+        struct litmus_lock **lock,
+        int type,
+        void* __user unused)
+{
+        int err = -ENXIO;
+
+        /* EXT-RES currently only supports the OMLP within components
+           for global resources. */
+        switch (type) {
+
+                case OMLP_SEM:
+                        /* O(m) Multiprocessor Locking Protocol */
+                        *lock  = gedf_env_new_omlp();
+                        if (*lock)
+                                err = 0;
+                        else
+                                err = -ENOMEM;
+                        break;
+
+        };
+
+        return err;
+}
+
+#endif
+
 static struct reservation_environment_ops gedf_env_ops = {
         .update_time = gedf_env_update_time,
         .dispatch = gedf_env_dispatch,
@@ -714,7 +1166,10 @@ static struct reservation_environment_ops gedf_env_ops = {
         .remove_res = gedf_env_remove_res,
         .find_res_by_id = gedf_find_res_by_id,
         .is_np = gedf_env_is_np,
-        .shutdown = gedf_env_shutdown
+        .shutdown = gedf_env_shutdown,
+#ifdef CONFIG_LITMUS_LOCKING
+        .allocate_lock = gedf_env_allocate_lock,
+#endif
 };
 
 long alloc_gedf_reservation_environment(
diff --git a/litmus/rt_domain.c b/litmus/rt_domain.c
index 80e01719884e..03ad18007069 100644
--- a/litmus/rt_domain.c
+++ b/litmus/rt_domain.c
@@ -202,7 +202,7 @@ static struct release_heap* __get_release_heap(rt_domain_t *rt,
         return heap;
 }
 
-static struct release_heap* get_release_heap_res(rt_domain_t *rt,
+struct release_heap* get_release_heap_res(rt_domain_t *rt,
                                                  struct reservation* res,
                                                  int use_task_heap)
 {
diff --git a/litmus/sched_ext_res.c b/litmus/sched_ext_res.c
index 583a2ed9aef0..4bf81cd46c59 100644
--- a/litmus/sched_ext_res.c
+++ b/litmus/sched_ext_res.c
@@ -275,6 +275,27 @@ static bool ext_res_should_wait_for_stack(struct task_struct* t) {
 }
 */
 
+
+#ifdef CONFIG_LITMUS_LOCKING
+
+/* **** lock constructor **** */
+
+static long ext_res_allocate_lock(struct litmus_lock **lock, int type,
+                                 void* __user unused)
+{
+        struct reservation_environment *gedf_env;
+        int err = -ENXIO;
+
+        /* pass the allocate_lock call to the task's component */
+        gedf_env = ((struct reservation*) tsk_rt(current)->plugin_state)->par_env;
+        err = gedf_env->ops->allocate_lock(gedf_env, lock, type, unused);
+
+        return err;
+}
+
+#endif
+
+
 static struct domain_proc_info ext_res_domain_proc_info;
 
 static long ext_res_get_domain_proc_info(struct domain_proc_info **ret)
@@ -361,6 +382,9 @@ static struct sched_plugin ext_res_plugin = {
         .reservation_create     = ext_res_reservation_create,
         //.current_budget         = pres_current_budget,
         //.should_wait_for_stack = ext_res_should_wait_for_stack,
+#ifdef CONFIG_LITMUS_LOCKING
+        .allocate_lock          = ext_res_allocate_lock,
+#endif
 };
 
 static int __init init_ext_res(void)