Added priority inheritance within semaphores - compiles but has not been

tested.

1 files changed, 18 insertions, 9 deletions

diff --git a/kernel/sched.c b/kernel/sched.c
index 27dcab489b..4341c1881b 100644
--- a/kernel/sched.c
+++ b/kernel/sched.c
@@ -3811,26 +3811,35 @@ static void __pi_wake_up_common(struct pi_semaphore *s, unsigned int mode,
                                 int nr_exclusive, int sync, void *key)
 {
         struct list_head *tmp, *next;
+        jiffie_t task_prio;
         jiffie_t highest_prio = LOWEST_SEM_PRIO;
 
+        /* First pass: wakeup tasks. */
         list_for_each_safe(tmp, next, &s->wait.task_list) {
                 wait_queue_t *curr = list_entry(tmp, wait_queue_t, task_list);
                 unsigned flags = curr->flags;
         
-                /* TODO: Find highest priority task remaining in queue. */
-                /* task_priority = tmp_task->rt_param.times.deadline;
-                 * if (time_before(task_priority, highest_prio)) {
-                 *      highest_prio = task_priority;
-                 * }
-                 */
-
                 if (curr->func(curr, mode, sync, key) &&
                                 (flags & WQ_FLAG_EXCLUSIVE) && !--nr_exclusive)
                         break;
         }
 
-        /* TODO: Set new priority to highest priority task remaining in queue. */
-        /* s->sem_prio = highest_prio; */
+        /* Second pass: find highest-priority sleeping task. */
+        list_for_each_safe(tmp, next, &s->wait.task_list) {                 
+                wait_queue_t *curr = list_entry(tmp, wait_queue_t, task_list);
+                
+                /* Compare task priorities, find highest (earliest d/l). */
+                task_prio = ((struct task_struct *)curr->private)->rt_param.times.deadline;
+                if (time_before(task_prio, highest_prio)) {
+                        highest_prio = task_prio;
+                }
+        }
+
+        /*
+         * Set new priority to that of highest priority
+         * task remaining in waitqueue.
+         */
+        s->sem_prio = highest_prio;
 }
 
 /**