Add internal support for the PCP in preparation of the DPCP.

5 files changed, 357 insertions, 33 deletions

diff --git a/include/litmus/fp_common.h b/include/litmus/fp_common.h
index 9171040e65f8..dd1f7bf1e347 100644
--- a/include/litmus/fp_common.h
+++ b/include/litmus/fp_common.h
@@ -64,6 +64,14 @@ static inline void fp_prio_add(struct fp_prio_queue* q, struct task_struct* t, u
         bheap_insert(fp_ready_order, &q->queue[index], tsk_rt(t)->heap_node);
 }
 
+static inline void fp_prio_remove(struct fp_prio_queue* q, struct task_struct* t, unsigned int index)
+{
+        BUG_ON(!is_queued(t));
+
+        bheap_delete(fp_ready_order, &q->queue[index], tsk_rt(t)->heap_node);
+        if (likely(bheap_empty(&q->queue[index])))
+                fpq_clear(q, index);
+}
 
 static inline struct task_struct* fp_prio_peek(struct fp_prio_queue* q)
 {
diff --git a/include/litmus/wait.h b/include/litmus/wait.h
index 62884f5eb9c8..ab3770904b1f 100644
--- a/include/litmus/wait.h
+++ b/include/litmus/wait.h
@@ -21,34 +21,24 @@ static inline void init_prio_waitqueue_entry(prio_wait_queue_t *pwq,
         pwq->priority = priority;
 }
 
-static inline unsigned int __add_wait_queue_prio_exclusive(
+unsigned int __add_wait_queue_prio_exclusive(
+        wait_queue_head_t* head,
+        prio_wait_queue_t *new);
+
+static inline unsigned int add_wait_queue_prio_exclusive(
         wait_queue_head_t* head,
         prio_wait_queue_t *new)
 {
-        struct list_head *pos;
-        unsigned int passed = 0;
-
-        new->wq.flags |= WQ_FLAG_EXCLUSIVE;
-
-        /* find a spot where the new entry is less than the next */
-        list_for_each(pos, &head->task_list) {
-                prio_wait_queue_t* queued = list_entry(pos, prio_wait_queue_t,
-                                                       wq.task_list);
-
-                if (unlikely(lt_before(new->priority, queued->priority))) {
-                        /* pos is not less than new, thus insert here */
-                        __list_add(&new->wq.task_list, pos->prev, pos);
-                        goto out;
-                }
-                passed++;
-        }
-
-        /* if we get to this point either the list is empty or every entry
-         * queued element is less than new.
-         * Let's add new to the end. */
-        list_add_tail(&new->wq.task_list, &head->task_list);
-out:
+        unsigned long flags;
+        unsigned int passed;
+
+        spin_lock_irqsave(&head->lock, flags);
+        passed = __add_wait_queue_prio_exclusive(head, new);
+
+        spin_unlock_irqrestore(&head->lock, flags);
+
         return passed;
 }
 
+
 #endif
diff --git a/litmus/fp_common.c b/litmus/fp_common.c
index 8536da9ca98a..31fc2db20adf 100644
--- a/litmus/fp_common.c
+++ b/litmus/fp_common.c
@@ -26,7 +26,7 @@ int fp_higher_prio(struct task_struct* first,
         struct task_struct *second_task = second;
 
         /* There is no point in comparing a task to itself. */
-        if (first && first == second) {
+        if (unlikely(first && first == second)) {
                 TRACE_TASK(first,
                            "WARNING: pointless FP priority comparison.\n");
                 return 0;
diff --git a/litmus/locking.c b/litmus/locking.c
index 2693f1aca859..cea3191f6ed4 100644
--- a/litmus/locking.c
+++ b/litmus/locking.c
@@ -4,6 +4,7 @@
 
 #include <litmus/sched_plugin.h>
 #include <litmus/trace.h>
+#include <litmus/wait.h>
 
 static int create_generic_lock(void** obj_ref, obj_type_t type, void* __user arg);
 static int open_generic_lock(struct od_table_entry* entry, void* __user arg);
@@ -121,6 +122,36 @@ struct task_struct* __waitqueue_remove_first(wait_queue_head_t *wq)
         return(t);
 }
 
+unsigned int __add_wait_queue_prio_exclusive(
+        wait_queue_head_t* head,
+        prio_wait_queue_t *new)
+{
+        struct list_head *pos;
+        unsigned int passed = 0;
+
+        new->wq.flags |= WQ_FLAG_EXCLUSIVE;
+
+        /* find a spot where the new entry is less than the next */
+        list_for_each(pos, &head->task_list) {
+                prio_wait_queue_t* queued = list_entry(pos, prio_wait_queue_t,
+                                                       wq.task_list);
+
+                if (unlikely(lt_before(new->priority, queued->priority))) {
+                        /* pos is not less than new, thus insert here */
+                        __list_add(&new->wq.task_list, pos->prev, pos);
+                        goto out;
+                }
+                passed++;
+        }
+
+        /* if we get to this point either the list is empty or every entry
+         * queued element is less than new.
+         * Let's add new to the end. */
+        list_add_tail(&new->wq.task_list, &head->task_list);
+out:
+        return passed;
+}
+
 
 #else
 
diff --git a/litmus/sched_pfp.c b/litmus/sched_pfp.c
index 2b755ecab76e..38fd3c7f5c7f 100644
--- a/litmus/sched_pfp.c
+++ b/litmus/sched_pfp.c
@@ -50,6 +50,10 @@ static void preempt(pfp_domain_t *pfp)
 static unsigned int priority_index(struct task_struct* t)
 {
 #ifdef CONFIG_LOCKING
+        if (unlikely(t->rt_param.inh_task))
+                /* use effective priority */
+                t = t->rt_param.inh_task;
+
         if (is_priority_boosted(t)) {
                 /* zero is reserved for priority-boosted tasks */
                 return 0;
@@ -308,16 +312,17 @@ static void pfp_task_exit(struct task_struct * t)
 
         raw_spin_lock_irqsave(&pfp->slock, flags);
         if (is_queued(t)) {
+                BUG(); /* This currently doesn't work. */
                 /* dequeue */
                 dom  = task_dom(t);
                 remove(dom, t);
         }
-        if (pfp->scheduled == t)
+        if (pfp->scheduled == t) {
                 pfp->scheduled = NULL;
-
+                preempt(pfp);
+        }
         TRACE_TASK(t, "RIP, now reschedule\n");
 
-        preempt(pfp);
         raw_spin_unlock_irqrestore(&pfp->slock, flags);
 }
 
@@ -326,6 +331,52 @@ static void pfp_task_exit(struct task_struct * t)
 #include <litmus/fdso.h>
 #include <litmus/srp.h>
 
+static void fp_dequeue(pfp_domain_t* pfp, struct task_struct* t)
+{
+        BUG_ON(pfp->scheduled == t && is_queued(t));
+        if (is_queued(t))
+                fp_prio_remove(&pfp->ready_queue, t, priority_index(t));
+}
+
+static void fp_set_prio_inh(pfp_domain_t* pfp, struct task_struct* t,
+                            struct task_struct* prio_inh)
+{
+        int requeue;
+
+        if (!t || t->rt_param.inh_task == prio_inh)
+                /* no update  required */
+                return;
+
+        requeue = is_queued(t);
+
+        if (requeue)
+                /* first remove */
+                fp_dequeue(pfp, t);
+
+        t->rt_param.inh_task = prio_inh;
+
+        if (requeue)
+                /* add again to the right queue */
+                fp_prio_add(&pfp->ready_queue, t, priority_index(t));
+}
+
+static int effective_agent_prio(int prio)
+{
+        /* make sure agents have higher priority */
+        return prio - LITMUS_MAX_PRIORITY;
+}
+
+static lt_t prio_point(int eprio)
+{
+        /* make sure we have non-negative prio points */
+        return eprio + LITMUS_MAX_PRIORITY;
+}
+
+static int prio_from_point(lt_t prio_point)
+{
+        return ((int) prio_point) - LITMUS_MAX_PRIORITY;
+}
+
 static void boost_priority(struct task_struct* t, lt_t priority_point)
 {
         unsigned long           flags;
@@ -391,7 +442,6 @@ static unsigned int pfp_get_srp_prio(struct task_struct* t)
 
 /* ******************** FMLP support ********************** */
 
-/* struct for semaphore with priority inheritance */
 struct fmlp_semaphore {
         struct litmus_lock litmus_lock;
 
@@ -548,7 +598,6 @@ static struct litmus_lock* pfp_new_fmlp(void)
 
 /* ******************** MPCP support ********************** */
 
-/* struct for semaphore with priority inheritance */
 struct mpcp_semaphore {
         struct litmus_lock litmus_lock;
 
@@ -588,7 +637,7 @@ static void mpcp_vspin_enter(void)
                 unsigned long flags;
 
                 /* ordered by regular priority */
-                init_prio_waitqueue_entry(&wait, t, get_priority(t));
+                init_prio_waitqueue_entry(&wait, t, prio_point(get_priority(t)));
 
                 spin_lock_irqsave(&vspin->lock, flags);
 
@@ -638,6 +687,7 @@ static inline struct mpcp_semaphore* mpcp_from_lock(struct litmus_lock* lock)
 {
         return container_of(lock, struct mpcp_semaphore, litmus_lock);
 }
+
 int pfp_mpcp_lock(struct litmus_lock* l)
 {
         struct task_struct* t = current;
@@ -666,7 +716,7 @@ int pfp_mpcp_lock(struct litmus_lock* l)
                 /* resource is not free => must suspend and wait */
 
                 /* ordered by regular priority */
-                init_prio_waitqueue_entry(&wait, t, get_priority(t));
+                init_prio_waitqueue_entry(&wait, t, prio_point(get_priority(t)));
 
                 /* FIXME: interruptible would be nice some day */
                 set_task_state(t, TASK_UNINTERRUPTIBLE);
@@ -734,7 +784,7 @@ int pfp_mpcp_unlock(struct litmus_lock* l)
 out:
         spin_unlock_irqrestore(&sem->wait.lock, flags);
 
-        if (sem->vspin) {
+        if (sem->vspin && err == 0) {
                 preempt_disable();
                 mpcp_vspin_exit();
                 preempt_enable();
@@ -827,6 +877,249 @@ static struct litmus_lock* pfp_new_mpcp(int vspin)
         return &sem->litmus_lock;
 }
 
+
+/* ******************** PCP support ********************** */
+
+
+struct pcp_semaphore {
+        struct list_head ceiling;
+
+        /* current resource holder */
+        struct task_struct *owner;
+
+        /* priority ceiling --- can be negative due to DPCP support */
+        int prio_ceiling;
+
+        /* on which processor is this PCP semaphore allocated? */
+        int on_cpu;
+};
+
+struct pcp_state {
+        struct list_head system_ceiling;
+
+        /* highest-priority waiting task */
+        struct task_struct* hp_waiter;
+
+        /* list of jobs waiting to get past the system ceiling */
+        wait_queue_head_t ceiling_blocked;
+};
+
+static void pcp_init_state(struct pcp_state* s)
+{
+        INIT_LIST_HEAD(&s->system_ceiling);
+        s->hp_waiter = NULL;
+        init_waitqueue_head(&s->ceiling_blocked);
+}
+
+static DEFINE_PER_CPU(struct pcp_state, pcp_state);
+
+/* assumes preemptions are off */
+static struct pcp_semaphore* pcp_get_ceiling(void)
+{
+        struct list_head* top = __get_cpu_var(pcp_state).system_ceiling.next;
+
+        if (top)
+                return list_entry(top, struct pcp_semaphore, ceiling);
+        else
+                return NULL;
+}
+
+/* assumes preempt off */
+static void pcp_add_ceiling(struct pcp_semaphore* sem)
+{
+        struct list_head *pos;
+        struct list_head *in_use = &__get_cpu_var(pcp_state).system_ceiling;
+        struct pcp_semaphore* held;
+
+        BUG_ON(sem->on_cpu != smp_processor_id());
+        BUG_ON(in_list(&sem->ceiling));
+
+        list_for_each(pos, in_use) {
+                held = list_entry(pos, struct pcp_semaphore, ceiling);
+                if (held->prio_ceiling >= sem->prio_ceiling) {
+                        __list_add(&sem->ceiling, pos->prev, pos);
+                        return;
+                }
+        }
+
+        /* we hit the end of the list */
+
+        list_add_tail(&sem->ceiling, in_use);
+}
+
+/* assumes preempt off */
+static int pcp_exceeds_ceiling(struct pcp_semaphore* ceiling,
+                              struct task_struct* task,
+                              int effective_prio)
+{
+        return ceiling == NULL ||
+                ceiling->prio_ceiling > effective_prio ||
+                ceiling->owner == task;
+}
+
+/* assumes preempt off */
+static void pcp_priority_inheritance(void)
+{
+        unsigned long   flags;
+        pfp_domain_t*   pfp = local_pfp;
+
+        struct pcp_semaphore* ceiling = pcp_get_ceiling();
+        struct task_struct *blocker, *blocked;
+
+        blocker = ceiling ?  ceiling->owner : NULL;
+        blocked = __get_cpu_var(pcp_state).hp_waiter;
+
+        raw_spin_lock_irqsave(&pfp->slock, flags);
+
+        /* Current is no longer inheriting anything by default.  This should be
+         * the currently scheduled job, and hence not currently queued. */
+        BUG_ON(current != pfp->scheduled);
+
+        fp_set_prio_inh(pfp, current, NULL);
+        fp_set_prio_inh(pfp, blocked, NULL);
+        fp_set_prio_inh(pfp, blocker, NULL);
+
+
+        /* Let blocking job inherit priority of blocked job, if required. */
+        if (blocker && blocked &&
+            fp_higher_prio(blocked, blocker))
+                fp_set_prio_inh(pfp, blocker, blocked);
+
+        /* check if anything changed */
+        if (fp_higher_prio(fp_prio_peek(&pfp->ready_queue), pfp->scheduled))
+                preempt(pfp);
+
+        raw_spin_unlock_irqrestore(&pfp->slock, flags);
+}
+
+static void pcp_raise_ceiling(struct pcp_semaphore* sem,
+                              int effective_prio)
+{
+        struct task_struct* t = current;
+        struct pcp_semaphore* ceiling;
+        prio_wait_queue_t wait;
+        unsigned int waiting_higher_prio;
+
+        preempt_disable();
+
+        do {
+                ceiling = pcp_get_ceiling();
+                if (pcp_exceeds_ceiling(ceiling, t, effective_prio))
+                        break;
+
+                /* we need to wait until the ceiling is lowered */
+
+                /* enqueue in priority order */
+                init_prio_waitqueue_entry(&wait, t, prio_point(effective_prio));
+                set_task_state(t, TASK_UNINTERRUPTIBLE);
+                waiting_higher_prio = add_wait_queue_prio_exclusive(
+                        &__get_cpu_var(pcp_state).ceiling_blocked, &wait);
+
+                if (waiting_higher_prio == 0) {
+                        /* we are the new highest-priority waiting job
+                         * => update inheritance */
+                        __get_cpu_var(pcp_state).hp_waiter = t;
+                        pcp_priority_inheritance();
+                }
+
+                TS_LOCK_SUSPEND;
+
+                preempt_enable_no_resched();
+                schedule();
+                preempt_disable();
+
+                /* pcp_resume_unblocked() removed us from wait queue */
+
+                TS_LOCK_RESUME;
+        } while(1);
+
+        /* We are good to go. The semaphore should be available. */
+        BUG_ON(sem->owner != NULL);
+
+        sem->owner = t;
+
+        pcp_add_ceiling(sem);
+
+        preempt_enable();
+}
+
+static void pcp_resume_unblocked(void)
+{
+        wait_queue_head_t *blocked =  &__get_cpu_var(pcp_state).ceiling_blocked;
+        unsigned long flags;
+        prio_wait_queue_t* q;
+        struct task_struct* t = NULL;
+
+        struct pcp_semaphore* ceiling = pcp_get_ceiling();
+
+        spin_lock_irqsave(&blocked->lock, flags);
+
+        while (waitqueue_active(blocked)) {
+                /* check first == highest-priority waiting job */
+                q = list_entry(blocked->task_list.next,
+                               prio_wait_queue_t, wq.task_list);
+                t = (struct task_struct*) q->wq.private;
+
+                /* can it proceed now? => let it go */
+                if (pcp_exceeds_ceiling(ceiling, t,
+                                        prio_from_point(q->priority))) {
+                    __remove_wait_queue(blocked, &q->wq);
+                    wake_up_process(t);
+                } else {
+                        /* We are done. Update highest-priority waiter. */
+                        __get_cpu_var(pcp_state).hp_waiter = t;
+                        goto out;
+                }
+        }
+        /* If we get here, then there are no more waiting
+         * jobs. */
+        __get_cpu_var(pcp_state).hp_waiter = NULL;
+out:
+        spin_unlock_irqrestore(&blocked->lock, flags);
+}
+
+/* assumes preempt off */
+static void pcp_lower_ceiling(struct pcp_semaphore* sem)
+{
+        BUG_ON(!in_list(&sem->ceiling));
+        BUG_ON(sem->owner != current);
+        BUG_ON(sem->on_cpu != smp_processor_id());
+
+        /* remove from ceiling list */
+        list_del(&sem->ceiling);
+
+        /* release */
+        sem->owner = NULL;
+
+        /* Wake up all ceiling-blocked jobs that now pass the ceiling. */
+        pcp_resume_unblocked();
+
+        pcp_priority_inheritance();
+}
+
+static void pcp_update_prio_ceiling(struct pcp_semaphore* sem,
+                                    int effective_prio)
+{
+        sem->prio_ceiling = min(sem->prio_ceiling, effective_prio);
+}
+
+static struct pcp_semaphore* pfp_new_pcp(int cpu)
+{
+        struct pcp_semaphore* sem;
+
+        sem = kmalloc(sizeof(*sem), GFP_KERNEL);
+        if (!sem)
+                return NULL;
+
+        sem->owner   = NULL;
+        INIT_LIST_HEAD(&sem->ceiling);
+        sem->prio_ceiling = INT_MAX;
+        sem->on_cpu = cpu;
+
+        return sem;
+}
+
+
 /* **** lock constructor **** */
 
 
@@ -911,6 +1204,8 @@ static long pfp_activate_plugin(void)
         for_each_online_cpu(cpu) {
                 init_waitqueue_head(&per_cpu(mpcpvs_vspin_wait, cpu));
                 per_cpu(mpcpvs_vspin, cpu) = NULL;
+
+                pcp_init_state(&per_cpu(pcp_state, cpu));
         }
 
 #endif