1 files changed, 72 insertions, 3 deletions

diff --git a/litmus/sched_gsn_edf.c b/litmus/sched_gsn_edf.c
index cbde657ce7..53538e2eb2 100644
--- a/litmus/sched_gsn_edf.c
+++ b/litmus/sched_gsn_edf.c
@@ -600,6 +600,72 @@ static void gsnedf_task_exit(struct task_struct * t)
 }
 
 #ifdef CONFIG_FMLP
+
+/* Update the queue position of a task that got it's priority boosted via
+ * priority inheritance. */
+static void update_queue_position(struct task_struct *holder)
+{
+        /* We don't know whether holder is in the ready queue. It should, but
+         * on a budget overrun it may already be in a release queue.  Hence,
+         * calling unlink() is not possible since it assumes that the task is
+         * not in a release queue.  However, we can safely check whether
+         * sem->holder is currently in a queue or scheduled after locking both
+         * the release and the ready queue lock. */
+
+        /* Assumption: caller holds gsnedf_lock */
+
+        int check_preempt = 0;
+
+        if (tsk_rt(holder)->linked_on != NO_CPU) {
+                TRACE_TASK(holder, "%s: linked  on %d\n",
+                           __FUNCTION__, tsk_rt(holder)->linked_on);
+                /* Holder is scheduled; need to re-order CPUs.
+                 * We can't use heap_decrease() here since
+                 * the cpu_heap is ordered in reverse direction, so
+                 * it is actually an increase. */
+                heap_delete(cpu_lower_prio, &gsnedf_cpu_heap,
+                            gsnedf_cpus[tsk_rt(holder)->linked_on]->hn);
+                heap_insert(cpu_lower_prio, &gsnedf_cpu_heap,
+                            gsnedf_cpus[tsk_rt(holder)->linked_on]->hn);
+        } else {
+                /* holder may be queued: first stop queue changes */
+                spin_lock(&gsnedf.release_lock);
+                if (is_queued(holder)) {
+                        TRACE_TASK(holder, "%s: is queued\n",
+                                   __FUNCTION__);
+                        /* We need to update the position
+                         * of holder in some heap. Note that this
+                         * may be a release heap. */
+                        check_preempt =
+                                !heap_decrease(edf_ready_order,
+                                               tsk_rt(holder)->heap_node);
+                } else {
+                        /* Nothing to do: if it is not queued and not linked
+                         * then it is currently being moved by other code
+                         * (e.g., a timer interrupt handler) that will use the
+                         * correct priority when enqueuing the task. */
+                        TRACE_TASK(holder, "%s: is NOT queued => Done.\n",
+                                   __FUNCTION__);
+                }
+                spin_unlock(&gsnedf.release_lock);
+
+                /* If holder was enqueued in a release heap, then the following
+                 * preemption check is pointless, but we can't easily detect
+                 * that case. If you want to fix this, then consider that
+                 * simply adding a state flag requires O(n) time to update when
+                 * releasing n tasks, which conflicts with the goal to have
+                 * O(log n) merges. */
+                if (check_preempt) {
+                        /* heap_decrease() hit the top level of the heap: make
+                         * sure preemption checks get the right task, not the
+                         * potentially stale cache. */
+                        heap_uncache_min(edf_ready_order,
+                                         &gsnedf.ready_queue);
+                        check_for_preemptions();
+                }
+        }
+}
+
 static long gsnedf_pi_block(struct pi_semaphore *sem,
                             struct task_struct *new_waiter)
 {
@@ -613,16 +679,19 @@ static long gsnedf_pi_block(struct pi_semaphore *sem,
         BUG_ON(!new_waiter);
 
         if (edf_higher_prio(new_waiter, sem->hp.task)) {
-                TRACE_TASK(new_waiter, " boosts priority\n");
+                TRACE_TASK(new_waiter, " boosts priority via %p\n", sem);
                 /* called with IRQs disabled */
                 spin_lock(&gsnedf_lock);
                 /* store new highest-priority task */
                 sem->hp.task = new_waiter;
                 if (sem->holder) {
+                        TRACE_TASK(sem->holder,
+                                   " holds %p and will inherit from %s/%d\n",
+                                   sem,
+                                   new_waiter->comm, new_waiter->pid);
                         /* let holder inherit */
                         sem->holder->rt_param.inh_task = new_waiter;
-                        unlink(sem->holder);
-                        gsnedf_job_arrival(sem->holder);
+                        update_queue_position(sem->holder);
                 }
                 spin_unlock(&gsnedf_lock);
         }