Reimplemented LITMUS standard FIFO semaphores + misc fixes

This version is much more straightforward than the last one and actually
enforces FIFO order in all cases. Further, it removes the need for the
sem_wake_up() calls by providing a custom wake up function for the wait
queue.

6 files changed, 79 insertions, 59 deletions

diff --git a/include/linux/litmus.h b/include/linux/litmus.h
index 414d1d28a9..73ea6435f6 100644
--- a/include/linux/litmus.h
+++ b/include/linux/litmus.h
@@ -101,7 +101,7 @@ extern  spinlock_t litmus_task_set_lock;
         sched_trace_log_message("%d: " fmt, raw_smp_processor_id(), ## args)
 
 #define TRACE_TASK(t, fmt, args...) \
-        TRACE("(%s/%d)" fmt, (t)->comm, (t)->pid, ##args)
+        TRACE("(%s/%d) " fmt, (t)->comm, (t)->pid, ##args)
 
 #define TRACE_CUR(fmt, args...) \
         TRACE_TASK(current, fmt, ## args)
diff --git a/include/linux/wait.h b/include/linux/wait.h
index bcc8698da6..c7e96b6b59 100644
--- a/include/linux/wait.h
+++ b/include/linux/wait.h
@@ -161,8 +161,6 @@ wait_queue_head_t *FASTCALL(bit_waitqueue(void *, int));
 #define wake_up_locked(x)               __wake_up_locked((x), TASK_UNINTERRUPTIBLE | TASK_INTERRUPTIBLE)
 #define wake_up_interruptible_sync(x)   __wake_up_sync((x),TASK_INTERRUPTIBLE, 1)
 
-#define sem_wake_up(x)                  __sem_wake_up(x, TASK_UNINTERRUPTIBLE | TASK_INTERRUPTIBLE, 1, NULL)
-#define sem_wake_up_locked(x)           __sem_wake_up_locked((x), TASK_UNINTERRUPTIBLE | TASK_INTERRUPTIBLE)
 #define pi_wake_up(x)                   __pi_wake_up(x, TASK_UNINTERRUPTIBLE | TASK_INTERRUPTIBLE, 1, NULL)
 
 #define __wait_event(wq, condition)                                     \
diff --git a/kernel/litmus_sem.c b/kernel/litmus_sem.c
index 14de7e714b..cdcb6bd51f 100644
--- a/kernel/litmus_sem.c
+++ b/kernel/litmus_sem.c
@@ -13,7 +13,7 @@
 #include <linux/wait.h>
 #include <linux/sched_plugin.h>
 #include <linux/queuelock.h>
-
+#include <linux/litmus.h>
 
 /* ************************************************************************** */
 /*                        STANDARD FIFO SEMAPHORES                            */
@@ -24,6 +24,69 @@
 struct semaphore sems[MAX_SEMAPHORES]; /* all sems */
 typedef int sema_id; /* Userspace ID of a semaphore */
 
+static int rt_fifo_wake_up(wait_queue_t *wait, unsigned mode, int sync, 
+                           void *key)
+{
+        struct task_struct* t = (struct task_struct*) wait->private;
+        set_rt_flags(t, RT_F_EXIT_SEM);
+        TRACE_TASK(t, "woken up by rt_fifo_wake_up(), set RT_F_EXIT_SEM\n");
+        return default_wake_function(wait, mode, sync, key);    
+}
+
+static fastcall void rt_fifo_up(struct semaphore * sem)
+{
+        TRACE_CUR("releases lock %p\n");
+        if (atomic_inc_return(&sem->count) < 1)
+                /* there is a task queued */
+                wake_up(&sem->wait);    
+}
+
+/* not optimized like the Linux down() implementation, but then
+ * again we incur the cost of a syscall anyway, so this hardly matters
+ */
+static fastcall void rt_fifo_down(struct semaphore * sem)
+{
+        struct task_struct *tsk = current;
+        wait_queue_t wait = {
+                .private = tsk, 
+                .func    = rt_fifo_wake_up, 
+                .task_list = {NULL, NULL}
+        };
+        unsigned long flags;
+
+
+        spin_lock_irqsave(&sem->wait.lock, flags);
+        if (atomic_dec_return(&sem->count) < 0 || waitqueue_active(&sem->wait)) {
+                /* we need to suspend */
+                tsk->state = TASK_UNINTERRUPTIBLE;              
+                add_wait_queue_exclusive_locked(&sem->wait, &wait);             
+
+                TRACE_CUR("suspends on lock %p\n", sem);
+
+                /* release lock before sleeping */
+                spin_unlock_irqrestore(&sem->wait.lock, flags);                 
+                
+                /* 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, now owns lock %p\n", sem);
+
+                /* try_to_wake_up() set our state to TASK_RUNNING,
+                 * all we need to do is to remove our wait queue entry
+                 */
+                remove_wait_queue(&sem->wait, &wait);
+        } else {
+                TRACE_CUR("acquired lock %p, no contention\n", sem);
+                spin_unlock_irqrestore(&sem->wait.lock, flags);
+        }
+}
+
+
+
 /* Initialize semaphores at boot time. */
 static int __init sema_boot_init(void)
 {        
@@ -43,8 +106,8 @@ asmlinkage long sys_sema_init (void)
 {
         sema_id sem_id;
 
-        for (sem_id = 0; sem_id < MAX_SEMAPHORES; sem_id++) {                                 
-                if (!cmpxchg(&sems[sem_id].used, 0, 1)) {                                                        
+        for (sem_id = 0; sem_id < MAX_SEMAPHORES; sem_id++) {
+                if (!cmpxchg(&sems[sem_id].used, 0, 1)) {
                         sema_init(&sems[sem_id], 1);
                         return sem_id;
                 }
@@ -57,11 +120,10 @@ asmlinkage long sys_down(sema_id sem_id)
         if (sem_id < 0 || sem_id >= MAX_SEMAPHORES)
                 return -EINVAL;
 
+        if (!sems[sem_id].used)
+                return -EINVAL;
         /* This allows for FIFO sems and gives others a chance... */
-        if (waitqueue_active(&sems[sem_id].wait))
-                __down(&sems[sem_id]);
-        else
-                down(&sems[sem_id]);
+        rt_fifo_down(sems + sem_id);
         return 0;
 }
 
@@ -70,7 +132,9 @@ asmlinkage long sys_up(sema_id sem_id)
         if (sem_id < 0 || sem_id >= MAX_SEMAPHORES)
                 return -EINVAL;
 
-        up(&sems[sem_id]);
+        if (!sems[sem_id].used)
+                return -EINVAL;
+        rt_fifo_up(sems + sem_id);
         return 0;
 }
 
@@ -82,6 +146,9 @@ asmlinkage long sys_sema_free(sema_id sem_id)
         if (sem_id < 0 || sem_id >= MAX_SEMAPHORES)
                 return -EINVAL;
 
+        if (!sems[sem_id].used)
+                return -EINVAL;
+
         spin_lock_irqsave(&sems[sem_id].wait.lock, flags);
         if (waitqueue_active(&sems[sem_id].wait)) {
                 list_for_each_safe(tmp, next, &sems[sem_id].wait.task_list) {
@@ -105,8 +172,6 @@ asmlinkage long sys_sema_free(sema_id sem_id)
 
 
 
-
-
 /* ************************************************************************** */
 /*                          PRIORITY INHERITANCE                              */
 /* ************************************************************************** */
diff --git a/kernel/sched.c b/kernel/sched.c
index b04322ca10..74ad4500df 100644
--- a/kernel/sched.c
+++ b/kernel/sched.c
@@ -3809,25 +3809,6 @@ static void __wake_up_common(wait_queue_head_t *q, unsigned int mode,
         }
 }
 
-/* The core wakeup function, this time exclusively for semaphores. In LITMUS,
- * semaphores need to set a flag for tasks that they wakeup, and thus needed
- * their own wakeup functions.
- */
-static void __sem_wake_up_common(wait_queue_head_t *q, unsigned int mode,
-                                 int nr_exclusive, int sync, void *key)
-{        
-        struct list_head *tmp, *next;
-
-        list_for_each_safe(tmp, next, &q->task_list) {
-                wait_queue_t *curr = list_entry(tmp, wait_queue_t, task_list);
-                unsigned flags = curr->flags;
-
-                set_rt_flags((struct task_struct*)curr->private, RT_F_EXIT_SEM);
-                if (curr->func(curr, mode, sync, key) &&
-                                (flags & WQ_FLAG_EXCLUSIVE) && !--nr_exclusive)
-                        break;
-        }
-}
 
 /*
  * The core wakeup function, this time including priority inheritance. As a result,
@@ -3877,23 +3858,6 @@ void fastcall __wake_up(wait_queue_head_t *q, unsigned int mode,
 }
 EXPORT_SYMBOL(__wake_up);
 
-/**
- * __sem_wake_up - wake up threads blocked on a waitqueue,
- *                 and set RT_F_EXIT_SEM flag.
- * @q: the waitqueue
- * @mode: which threads
- * @nr_exclusive: how many wake-one or wake-many threads to wake up
- * @key: is directly passed to the wakeup function
- */
-void fastcall __sem_wake_up(wait_queue_head_t *q, unsigned int mode,
-                            int nr_exclusive, void *key)
-{
-        unsigned long flags;
-        
-        spin_lock_irqsave(&q->lock, flags);
-        __sem_wake_up_common(q, mode, nr_exclusive, 0, key);
-        spin_unlock_irqrestore(&q->lock, flags);
-}
 
 /**
  *  __pi_wake_up - wake up threads blocked on a waitqueue,
@@ -3924,13 +3888,6 @@ void fastcall __wake_up_locked(wait_queue_head_t *q, unsigned int mode)
         __wake_up_common(q, mode, 1, 0, NULL);
 }
 
-/*
- * Same as __sem_wake_up but called with the spinlock in wait_queue_head_t held.
- */
-void fastcall __sem_wake_up_locked(wait_queue_head_t *q, unsigned int mode)
-{
-        __sem_wake_up_common(q, mode, 1, 0, NULL);
-}
 
 /**
  * __wake_up_sync - wake up threads blocked on a waitqueue.
diff --git a/kernel/sched_gsn_edf.c b/kernel/sched_gsn_edf.c
index d007eb82c7..046fe41e09 100644
--- a/kernel/sched_gsn_edf.c
+++ b/kernel/sched_gsn_edf.c
@@ -267,7 +267,7 @@ static reschedule_check_t gsnedf_scheduler_tick(void)
          * no task  "runs away forever".
          */
         if (is_realtime(t))
-                TRACE_TASK(t, "scheduler tick");
+                TRACE_TASK(t, "scheduler tick\n");
 
         if (is_realtime(t) && t->time_slice && !--t->time_slice) {
                 if (!is_np(t)) { /* np tasks will be preempted when they become
diff --git a/lib/semaphore-sleepers.c b/lib/semaphore-sleepers.c
index 60e4526c5f..e500018f51 100644
--- a/lib/semaphore-sleepers.c
+++ b/lib/semaphore-sleepers.c
@@ -52,7 +52,7 @@
 
 fastcall void __up(struct semaphore *sem)
 {
-        sem_wake_up(&sem->wait);
+        wake_up(&sem->wait);
 }
 
 fastcall void __sched __down(struct semaphore * sem)
@@ -87,7 +87,7 @@ fastcall void __sched __down(struct semaphore * sem)
                 tsk->state = TASK_UNINTERRUPTIBLE;
         }
         remove_wait_queue_locked(&sem->wait, &wait);
-        sem_wake_up_locked(&sem->wait);
+        wake_up_locked(&sem->wait);
         spin_unlock_irqrestore(&sem->wait.lock, flags);
         tsk->state = TASK_RUNNING;
 }