1 files changed, 319 insertions, 0 deletions

diff --git a/litmus/omlp.c b/litmus/omlp.c
new file mode 100644
index 000000000000..dbb39dc0c07c
--- /dev/null
+++ b/litmus/omlp.c
@@ -0,0 +1,319 @@
+/*
+ * OMLP implementation.
+ * Much of the code here is borrowed from include/asm-i386/semaphore.h
+ */
+
+#include <asm/atomic.h>
+
+#include <linux/semaphore.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/spinlock.h>
+
+#include <litmus/litmus.h>
+#include <litmus/sched_plugin.h>
+#include <litmus/edf_common.h>
+
+#include <litmus/fdso.h>
+
+#include <litmus/trace.h>
+
+#ifdef CONFIG_OMLP
+
+static  void* create_omlp_semaphore(void)
+{
+        struct omlp_semaphore* sem;
+        int i;
+
+        sem = kmalloc(sizeof(*sem), GFP_KERNEL);
+        if (!sem)
+                return NULL;
+        atomic_set(&sem->pi.count, 1);
+        sem->pi.sleepers = 0;
+        init_waitqueue_head(&sem->pi.wait);
+        sem->pi.hp.task = NULL;
+        sem->holder = NULL;
+        for (i = 0; i < NR_CPUS; i++)
+                sem->pi.hp.cpu_task[i] = NULL;
+
+        sem->pi.stack_node.inh_task = NULL;
+        INIT_LIST_HEAD(&sem->pi.stack_node.list);
+    
+    
+    INIT_LIST_HEAD(&sem->pq_task_list);
+
+    TRACE_CUR("number of online CPUs: %d", num_online_cpus());
+
+        return sem;
+}
+
+static int open_omlp_semaphore(struct od_table_entry* entry, void* __user arg)
+{
+        if (!omlp_active())
+                return -EBUSY;
+        return 0;
+}
+
+static void destroy_omlp_semaphore(void* sem)
+{
+        /* XXX assert invariants */
+        kfree(sem);
+}
+
+struct fdso_ops omlp_sem_ops = {
+        .create  = create_omlp_semaphore,
+        .open    = open_omlp_semaphore,
+        .destroy = destroy_omlp_semaphore
+};
+
+struct wq_pair {
+        struct task_struct*  tsk;
+        struct omlp_semaphore* sem;
+};
+
+static int rt_pi_wake_up(wait_queue_t *wait, unsigned mode, int sync,
+                           void *key)
+{
+        struct wq_pair* wqp   = (struct wq_pair*) wait->private;
+        set_rt_flags(wqp->tsk, RT_F_EXIT_SEM);
+        litmus->omlp_inherit_priority(to_pi(wqp->sem), wqp->tsk);
+        TRACE_TASK(wqp->tsk,
+                   "woken up by rt_pi_wake_up() (RT_F_SEM_EXIT, PI)\n");
+        /* point to task for default_wake_function() */
+        wait->private = wqp->tsk;
+        default_wake_function(wait, mode, sync, key);
+
+        /* Always return true since we know that if we encountered a task
+         * that was already running the wake_up raced with the schedule in
+         * rt_pi_down(). In that case the task in rt_pi_down() will be scheduled
+         * immediately and own the lock. We must not wake up another task in
+         * any case.
+         */
+        return 1;
+}
+
+typedef int (*list_order_t)(struct wq_pair*, struct wq_pair*);
+static int edf_order(struct wq_pair *a, struct wq_pair *b)
+{
+    return(edf_higher_prio(a->tsk, b->tsk));
+}
+
+static inline void add_list_ordered(
+    struct list_head* pq,
+    wait_queue_t *wait,
+    list_order_t cmp)
+{
+    struct wq_pair       *curr_queued;
+    wait_queue_t        *tmp;
+    struct list_head    *pos, *next;
+    
+    struct wq_pair       *to_insert =
+        (struct wq_pair*) list_entry(&wait->task_list, wait_queue_t, task_list)->private;
+    
+    /* list is empty.  belongs at tail. */
+    if(list_empty(pq)) {
+        list_add_tail(&wait->task_list, pq);
+        return;
+    }   
+    
+    /* new job has the least priority.  belongs at tail. */
+    tmp = list_entry(pq->prev, wait_queue_t, task_list);
+    curr_queued = (struct wq_pair*)tmp->private;
+    if(curr_queued && !cmp(to_insert, curr_queued))
+    {   
+        list_add_tail(&wait->task_list, pq);
+        return;
+    }   
+    
+    /* belongs between nodes.  this is the hardest case
+     * and hence left for last.
+     */
+    list_for_each_safe(pos, next, pq) {
+        tmp = list_entry(pos, wait_queue_t, task_list);
+        curr_queued = (struct wq_pair*)tmp->private;
+        if (curr_queued && cmp(to_insert, curr_queued)) {
+            list_add(&wait->task_list, pos->prev);
+            return;
+        }   
+    }   
+    
+    /* fail-safe?? */
+    list_add_tail(&wait->task_list, pq);
+    return;
+}
+
+static int do_omlp_down(struct omlp_semaphore* sem)
+{
+        unsigned long flags;
+        struct task_struct *tsk = current;
+        struct wq_pair pair;
+        int suspended = 1;
+        int count;
+
+        wait_queue_t wait = {
+        .flags = 0,
+                .private = &pair,
+                .func    = rt_pi_wake_up,
+                .task_list = {NULL, NULL}
+        };
+
+        pair.tsk = tsk;
+        pair.sem = sem;
+    
+        spin_lock_irqsave(&sem->pi.wait.lock, flags);
+
+    count = atomic_dec_return(&sem->pi.count);
+    
+        if (count < 0 || waitqueue_active(&sem->pi.wait)) {
+
+        tsk->state = TASK_UNINTERRUPTIBLE;
+
+        if(count <= -(num_online_cpus()) || !list_empty(&sem->pq_task_list)) {
+            wait.flags |= WQ_FLAG_EXCLUSIVE;
+            /* G-EDF only, so there's one PQ. */
+            add_list_ordered(&sem->pq_task_list, &wait, edf_order);
+            TRACE_CUR("count = %d, suspends (%p) on PI lock (PQ) %p\n", count, tsk, sem);
+        } else {
+            /* we need to suspend on the wait queue */
+            add_wait_queue_exclusive_locked(&sem->pi.wait, &wait);
+            
+            TRACE_CUR("count = %d, suspends (%p) on PI lock (FIFO) %p\n", count, tsk, sem);
+            litmus->omlp_pi_block(to_pi(sem), tsk);            
+        }
+
+        /* release lock before sleeping */
+        spin_unlock_irqrestore(&sem->pi.wait.lock, flags);
+            
+        TS_PI_DOWN_END;
+        preempt_enable_no_resched();
+            
+            
+        /* we depend on the FIFO order
+         * Thus, we don't need to recheck when we wake up, we
+         * are guaranteed to have the lock since there is only one
+         * wake up per release
+         */
+        schedule();
+        
+        TRACE_CUR("woke up (%p), now owns PI lock %p\n", tsk, sem);
+        
+        /* try_to_wake_up() set our state to TASK_RUNNING,
+         * all we need to do is to remove our wait queue entry
+         * We'll now be on pi.wait even if we enqueued on pqwait.
+         */
+        remove_wait_queue(&sem->pi.wait, &wait);
+        } else {
+                /* no priority inheritance necessary, since there are no queued
+                 * tasks.
+                 */
+                suspended = 0;
+                TRACE_CUR("(%p) acquired PI lock %p, no contention\n", tsk, sem);
+                sem->holder  = tsk;
+
+                /* don't know if we're global or partitioned. */
+                sem->pi.hp.task = tsk;
+                sem->pi.hp.cpu_task[get_partition(tsk)] = tsk;
+
+                litmus->omlp_inherit_priority(to_pi(sem), tsk);
+                spin_unlock_irqrestore(&sem->pi.wait.lock, flags);
+        }
+        return suspended;
+}
+
+static void do_omlp_up(struct omlp_semaphore* sem)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&sem->pi.wait.lock, flags);
+
+        TRACE_CUR("(%p) releases PI lock %p\n", current, sem);
+        litmus->omlp_return_priority(to_pi(sem));
+        sem->holder = NULL;
+    
+        if (atomic_inc_return(&sem->pi.count) < 1) {
+                if (!list_empty(&sem->pq_task_list)) {  
+            /* Move the head task from pq_task_list to pi.wait.
+             * Do this before waking up the next task so it gets
+             * the proper pi.
+             */
+            wait_queue_t *wait =
+                list_entry(sem->pq_task_list.next, wait_queue_t, task_list);
+            list_del(&wait->task_list);
+            INIT_LIST_HEAD(&wait->task_list);
+            
+            /* we need to suspend on the wait queue */
+            add_wait_queue_exclusive_locked(&sem->pi.wait, wait);
+            
+            TRACE_CUR("moving waiter (%p) to FIFO queue. %p\n", ((struct wq_pair*)(wait->private))->tsk, sem);
+            /* Safe since sem->holder == NULL and pi_block checks
+             * for the case.
+             */
+            litmus->omlp_pi_block(to_pi(sem), ((struct wq_pair*)(wait->private))->tsk);
+        }
+        
+                /* there is a task queued */
+                wake_up_locked(&sem->pi.wait);
+    }
+
+        spin_unlock_irqrestore(&sem->pi.wait.lock, flags);
+}
+
+asmlinkage long sys_omlp_down(int sem_od)
+{
+        long ret = 0;
+        struct omlp_semaphore * sem;
+        int suspended = 0;
+
+        preempt_disable();
+        TS_PI_DOWN_START;
+
+        sem = lookup_omlp_sem(sem_od);
+        if (sem)
+                suspended = do_omlp_down(sem);
+        else
+                ret = -EINVAL;
+
+        if (!suspended) {
+                TS_PI_DOWN_END;
+                preempt_enable();
+        }
+
+        return ret;
+}
+
+asmlinkage long sys_omlp_up(int sem_od)
+{
+        long ret = 0;
+        struct omlp_semaphore * sem;
+
+        preempt_disable();
+        TS_PI_UP_START;
+
+        sem = lookup_omlp_sem(sem_od);
+        if (sem)
+                do_omlp_up(sem);
+        else
+                ret = -EINVAL;
+
+
+        TS_PI_UP_END;
+        preempt_enable();
+
+        return ret;
+}
+
+#else
+
+struct fdso_ops omlp_sem_ops = {};
+
+asmlinkage long sys_omlp_down(int sem_od)
+{
+        return -ENOSYS;
+}
+
+asmlinkage long sys_omlp_up(int sem_od)
+{
+        return -ENOSYS;
+}
+
+#endif