Added kernel SRP "semaphore" support, to be included in PSN-EDF, and fixed

a small bug in semaphore free methods for all user-level semaphores.

3 files changed, 44 insertions, 20 deletions

diff --git a/arch/i386/kernel/pi_sem_syscalls.c b/arch/i386/kernel/pi_sem_syscalls.c
index 6aa66d2081..7c4758c42a 100644
--- a/arch/i386/kernel/pi_sem_syscalls.c
+++ b/arch/i386/kernel/pi_sem_syscalls.c
@@ -126,6 +126,9 @@ asmlinkage long sys_pi_sema_free(pi_sema_id sem_id)
         struct list_head *tmp, *next;
         unsigned long flags;
 
+        if (sem_id < 0 || sem_id >= MAX_PI_SEMAPHORES)
+                return -EINVAL;
+
         spin_lock_irqsave(&pi_sems[sem_id].wait.lock, flags);
         list_for_each_safe(tmp, next, &pi_sems[sem_id].wait.task_list) {                    
                 wait_queue_t *curr = list_entry(tmp, wait_queue_t, task_list);
diff --git a/arch/i386/kernel/sem_syscalls.c b/arch/i386/kernel/sem_syscalls.c
index 1a70e0dbbc..bc7c162e0b 100644
--- a/arch/i386/kernel/sem_syscalls.c
+++ b/arch/i386/kernel/sem_syscalls.c
@@ -63,7 +63,10 @@ asmlinkage long sys_sema_free(sema_id sem_id)
         struct list_head *tmp, *next;
         unsigned long flags;
 
-        spin_lock_irqsave(&sems[sem_id].wait.lock, flags);
+        if (sem_id < 0 || sem_id >= MAX_SEMAPHORES)
+                return -EINVAL;
+
+        spin_lock_irqsave(&sems[sem_id].wait.lock, flags);
         list_for_each_safe(tmp, next, &sems[sem_id].wait.task_list) {
                 wait_queue_t *curr = list_entry(tmp, wait_queue_t, task_list);
                 list_del(tmp);
diff --git a/arch/i386/kernel/srp_sem_syscalls.c b/arch/i386/kernel/srp_sem_syscalls.c
index 838df95202..639f5821eb 100644
--- a/arch/i386/kernel/srp_sem_syscalls.c
+++ b/arch/i386/kernel/srp_sem_syscalls.c
@@ -1,7 +1,5 @@
 #ifdef __KERNEL__
 
-// comparison by period and pid!
-
 /*
  * Uniprocessor SRP "semaphores".
  */
@@ -34,8 +32,10 @@ static int __init srp_sema_boot_init(void)
         srp_sema_id sem_id;
 
         printk("Initializing SRP semaphores...");
-        for (sem_id = 0; sem_id < MAX_SRP_SEMAPHORES; sem_id++)
+        for (sem_id = 0; sem_id < MAX_SRP_SEMAPHORES; sem_id++) {
                 srp_sems[sem_id].used = 0;
+                srp_sems[sem_id].claimed = 0;
+        }
         queue_lock_init(&srp_lock);
         printk(" complete!\n");
 
@@ -51,13 +51,23 @@ asmlinkage long sys_srp_sema_init (void)
         for (sem_id = 0; sem_id < MAX_SRP_SEMAPHORES; sem_id++) {
                 if (!cmpxchg(&srp_sems[sem_id].used, 0, 1)) {
                         srp_sems[sem_id].pc_task = NULL;
-                        srp_sems[sem_id].claimed = 0;
                         return sem_id;
                 }
         }
         return -ENOMEM;
 }
 
+/* SRP task priority comparison function. Smaller periods have highest
+ * priority, tie-break is PID.
+ */
+int srp_higher_prio(struct task_struct* first, struct task_struct* second)
+{
+        return !second || !is_realtime(second) ||
+               get_rt_period(first) < get_rt_period(second) ||
+               (get_rt_period(first) == get_rt_period(second) &&
+                first->pid < second->pid);
+}
+
 /* Adjust the system-wide priority ceiling if resource is claimed. */
 asmlinkage long sys_srp_down(srp_sema_id sem_id)
 {
@@ -68,7 +78,7 @@ asmlinkage long sys_srp_down(srp_sema_id sem_id)
 
         queue_lock_irqsave(&srp_lock, flags);
         srp_sems[sem_id].claimed = 1;
-        if (/* srp_sems[sem_id].pc_task higher prio than spc_task */1) {
+        if (srp_higher_prio(srp_sems[sem_id].pc_task, spc_task)) {
                 spc_task = srp_sems[sem_id].pc_task;
         }
         queue_unlock_irqrestore(&srp_lock, flags);                      
@@ -88,12 +98,10 @@ asmlinkage long sys_srp_up(srp_sema_id sem_id)
         queue_lock_irqsave(&srp_lock, flags);
         srp_sems[sem_id].claimed = 0;
         spc_task = NULL;
-        for (sem_ctr = 0; sem_ctr < MAX_SRP_SEMAPHORES; sem_ctr++) {                            
-                if (srp_sems[sem_ctr].used && srp_sems[sem_ctr].claimed /* && */
-                    /* srp_sems[sem_ctr].pc_task higher prio than spc_task */) {
+        for (sem_ctr = 0; sem_ctr < MAX_SRP_SEMAPHORES; sem_ctr++)
+                if (srp_sems[sem_ctr].used && srp_sems[sem_ctr].claimed &&
+                    srp_higher_prio(srp_sems[sem_ctr].pc_task, spc_task))
                         spc_task = srp_sems[sem_ctr].pc_task;
-                }
-        }
         queue_unlock_irqrestore(&srp_lock, flags);
 
         return 0;
@@ -114,9 +122,8 @@ asmlinkage long sys_reg_task_srp_sem(srp_sema_id sem_id,
                 return -EINVAL;
 
         queue_lock_irqsave(&srp_lock, flags);
-        if (/* t higher prio than srp_sems[sem_id].pc_task */1) {
+        if (srp_higher_prio(t, srp_sems[sem_id].pc_task))
                 srp_sems[sem_id].pc_task = t;
-        }
         queue_unlock_irqrestore(&srp_lock, flags);
 
         return 0;
@@ -130,21 +137,32 @@ void wait_until_exceed_spc(void)
         unsigned long flags;
 
         queue_lock_irqsave(&srp_lock, flags);
-        for (;;) { /* use callbacks here... */
-                if (/* current higher prio than spc_task */1) {
-                        break;
-                }
-                queue_unlock_irqrestore(&srp_lock, flags);                              
-                schedule();
+        while(!srp_higher_prio(current, spc_task)) {
+                queue_unlock_irqrestore(&srp_lock, flags);                                      schedule();
                 queue_lock_irqsave(&srp_lock, flags);
         }
         queue_unlock_irqrestore(&srp_lock, flags);
 }
 
-/* Free semaphore. */
+/* Free semaphore, adjusting the system-wide priority ceiling if necessary. */
 asmlinkage long sys_srp_sema_free(srp_sema_id sem_id)
 {
+        srp_sema_id sem_ctr;
+        unsigned long flags;
+
+        if (sem_id < 0 || sem_id >= MAX_SRP_SEMAPHORES)
+                return -EINVAL;
+
+        queue_lock_irqsave(&srp_lock, flags);
+        srp_sems[sem_id].claimed = 0;
         srp_sems[sem_id].used = 0;
+        spc_task = NULL;
+        for (sem_ctr = 0; sem_ctr < MAX_SRP_SEMAPHORES; sem_ctr++)
+                if (srp_sems[sem_ctr].used && srp_sems[sem_ctr].claimed &&
+                    srp_higher_prio(srp_sems[sem_ctr].pc_task, spc_task))
+                        spc_task = srp_sems[sem_ctr].pc_task;
+        queue_unlock_irqrestore(&srp_lock, flags);
+
         return 0;
 }