Modified SRP semaphores so that waiting for prio to exceed SPC works

correctly, mostly by adding waitqueues and rewriting wait_until_exceed_spc.

1 files changed, 54 insertions, 6 deletions

diff --git a/arch/i386/kernel/srp_sem_syscalls.c b/arch/i386/kernel/srp_sem_syscalls.c
index cf9ef72a3f..a07c36c7bf 100644
--- a/arch/i386/kernel/srp_sem_syscalls.c
+++ b/arch/i386/kernel/srp_sem_syscalls.c
@@ -23,6 +23,7 @@ typedef int srp_sema_id; /* Userspace ID of a srp_semaphore */
 
 /* System-wide priority ceiling, represented as a pointer to a task. */
 DEFINE_PER_CPU(struct task_struct *, spc_tasks);
+DEFINE_PER_CPU(wait_queue_head_t, spc_waitqueues);
 
 /* Used to serialize access to SRP semaphores and system priority ceiling. */
 static queuelock_t srp_lock;
@@ -39,8 +40,10 @@ static int __init srp_sema_boot_init(void)
                 srp_sems[sem_id].claimed = 0;
                 srp_sems[sem_id].cpu = -1;
         }
-        for (i = 0; i < NR_CPUS; i++)
+        for (i = 0; i < NR_CPUS; i++) {
                 per_cpu(spc_tasks, i) = NULL;
+                init_waitqueue_head(&per_cpu(spc_waitqueues, i));
+        }
         queue_lock_init(&srp_lock);
         printk(" done!\n");
 
@@ -98,14 +101,17 @@ asmlinkage long sys_srp_down(srp_sema_id sem_id)
 asmlinkage long sys_srp_up(srp_sema_id sem_id)
 {
         srp_sema_id sem_ctr;
+        struct list_head *tmp, *next;
         unsigned long flags;
         int cpu = smp_processor_id();
 
         if (sem_id < 0 || sem_id >= MAX_SRP_SEMAPHORES ||
             srp_sems[sem_id].cpu != cpu)
                 return -EINVAL;
-        
+
         queue_lock_irqsave(&srp_lock, flags);
+        
+        /* Determine new system priority ceiling for this CPU. */
         srp_sems[sem_id].claimed = 0;
         __get_cpu_var(spc_tasks) = NULL;
         for (sem_ctr = 0; sem_ctr < MAX_SRP_SEMAPHORES; sem_ctr++)
@@ -114,8 +120,25 @@ asmlinkage long sys_srp_up(srp_sema_id sem_id)
                     srp_higher_prio(srp_sems[sem_ctr].pc_task,
                                     __get_cpu_var(spc_tasks)))
                         __get_cpu_var(spc_tasks) = srp_sems[sem_ctr].pc_task;
-        queue_unlock_irqrestore(&srp_lock, flags);
 
+        /* Wake tasks on this CPU, if they exceed current ceiling. */
+        if (waitqueue_active(&__get_cpu_var(spc_waitqueues))) {
+                list_for_each_safe(tmp, next,
+                                   &__get_cpu_var(spc_waitqueues).task_list) {
+                        wait_queue_t *curr = list_entry(tmp, wait_queue_t,
+                                                        task_list);
+                        set_rt_flags((struct task_struct*)curr->private,
+                                     RT_F_EXIT_SEM);
+                        
+                        if (srp_higher_prio((struct task_struct*)curr->private,
+                                            __get_cpu_var(spc_tasks)))
+                                curr->func(curr,
+                                           TASK_UNINTERRUPTIBLE |
+                                           TASK_INTERRUPTIBLE, 0, NULL);
+                }               
+        }
+
+        queue_unlock_irqrestore(&srp_lock, flags);
         return 0;
 }
 
@@ -161,14 +184,19 @@ asmlinkage long sys_reg_task_srp_sem(srp_sema_id sem_id, pid_t t_pid)
  */
 void wait_until_exceed_spc(void)
 {
+        struct task_struct *tsk = current;
+        DECLARE_WAITQUEUE(wait, tsk);
         unsigned long flags;
 
         queue_lock_irqsave(&srp_lock, flags);
-        while(!srp_higher_prio(current, __get_cpu_var(spc_tasks))) {
+        add_wait_queue_exclusive_locked(&__get_cpu_var(spc_waitqueues), &wait);
+        while(!srp_higher_prio(tsk, __get_cpu_var(spc_tasks))) {
+                tsk->state = TASK_UNINTERRUPTIBLE;
                 queue_unlock_irqrestore(&srp_lock, flags);
                 schedule();
                 queue_lock_irqsave(&srp_lock, flags);
         }
+        remove_wait_queue_locked(&__get_cpu_var(spc_waitqueues), &wait);
         queue_unlock_irqrestore(&srp_lock, flags);
 }
 
@@ -176,7 +204,8 @@ void wait_until_exceed_spc(void)
 asmlinkage long sys_srp_sema_free(srp_sema_id sem_id)
 {
         srp_sema_id sem_ctr;
-        unsigned long flags;
+        struct list_head *tmp, *next;
+        unsigned long flags;
         int cpu = smp_processor_id();
 
         if (sem_id < 0 || sem_id >= MAX_SRP_SEMAPHORES ||
@@ -186,6 +215,8 @@ asmlinkage long sys_srp_sema_free(srp_sema_id sem_id)
         queue_lock_irqsave(&srp_lock, flags);
         srp_sems[sem_id].claimed = 0;
         srp_sems[sem_id].used = 0;
+
+        /* Determine new system priority ceiling for this CPU. */
         __get_cpu_var(spc_tasks) = NULL;
         for (sem_ctr = 0; sem_ctr < MAX_SRP_SEMAPHORES; sem_ctr++)
                 if (srp_sems[sem_ctr].used && srp_sems[sem_ctr].claimed &&
@@ -194,8 +225,25 @@ asmlinkage long sys_srp_sema_free(srp_sema_id sem_id)
                                     __get_cpu_var(spc_tasks)))
                         __get_cpu_var(spc_tasks) = srp_sems[sem_ctr].pc_task;
         srp_sems[sem_id].cpu = -1;
-        queue_unlock_irqrestore(&srp_lock, flags);
 
+        /* Wake tasks on this CPU, if they exceed current ceiling. */
+        if (waitqueue_active(&__get_cpu_var(spc_waitqueues))) {
+                list_for_each_safe(tmp, next,
+                                   &__get_cpu_var(spc_waitqueues).task_list) {
+                        wait_queue_t *curr = list_entry(tmp, wait_queue_t,
+                                                        task_list);
+                        set_rt_flags((struct task_struct*)curr->private,
+                                     RT_F_EXIT_SEM);
+
+                        if (srp_higher_prio((struct task_struct*)curr->private,
+                                            __get_cpu_var(spc_tasks)))
+                                curr->func(curr,
+                                           TASK_UNINTERRUPTIBLE |
+                                           TASK_INTERRUPTIBLE, 0, NULL);
+                }
+        }
+
+        queue_unlock_irqrestore(&srp_lock, flags);
         return 0;
 }