3 files changed, 142 insertions, 40 deletions

diff --git a/include/litmus/fp_common.h b/include/litmus/fp_common.h
index f23727ba913d..170cdf6b0e0b 100644
--- a/include/litmus/fp_common.h
+++ b/include/litmus/fp_common.h
@@ -6,6 +6,9 @@
 
 #include <litmus/rt_domain.h>
 
+#include <asm/bitops.h>
+
+
 void fp_domain_init(rt_domain_t* rt, check_resched_needed_t resched,
                     release_jobs_t release);
 
@@ -16,4 +19,80 @@ int fp_ready_order(struct bheap_node* a, struct bheap_node* b);
 
 int fp_preemption_needed(rt_domain_t* rt, struct task_struct *t);
 
+
+
+#define FP_PRIO_BIT_WORDS (LITMUS_MAX_PRIORITY / BITS_PER_LONG)
+
+#if (LITMUS_MAX_PRIORITY % BITS_PER_LONG)
+#error LITMUS_MAX_PRIORITY must be a multiple of BITS_PER_LONG
+#endif
+
+/* bitmask-inexed priority queue */
+struct fp_prio_queue {
+        unsigned long   bitmask[FP_PRIO_BIT_WORDS];
+        struct bheap    queue[LITMUS_MAX_PRIORITY];
+};
+
+void fp_prio_queue_init(struct fp_prio_queue* q);
+
+static inline void fpq_set(struct fp_prio_queue* q, unsigned int index)
+{
+        unsigned long *word = q->bitmask + (index / BITS_PER_LONG);
+        __set_bit(index % BITS_PER_LONG, word);
+}
+
+static inline void fpq_clear(struct fp_prio_queue* q, unsigned int index)
+{
+        unsigned long *word = q->bitmask + (index / BITS_PER_LONG);
+        __clear_bit(index % BITS_PER_LONG, word);
+}
+
+static inline unsigned int fpq_find(struct fp_prio_queue* q)
+{
+        int i;
+
+        /* loop optimizer should unroll this */
+        for (i = 0; i < FP_PRIO_BIT_WORDS; i++)
+                if (q->bitmask[i])
+                        return __ffs(q->bitmask[i]) + i * BITS_PER_LONG;
+
+        return LITMUS_MAX_PRIORITY; /* nothing found */
+}
+
+static inline void fp_prio_add(struct fp_prio_queue* q, struct task_struct* t, unsigned int index)
+{
+
+        BUG_ON(bheap_node_in_heap(tsk_rt(t)->heap_node));
+
+        fpq_set(q, index);
+        bheap_insert(fp_ready_order, &q->queue[index], tsk_rt(t)->heap_node);
+}
+
+
+static inline struct task_struct* fp_prio_peek(struct fp_prio_queue* q)
+{
+        unsigned int idx = fpq_find(q);
+        struct bheap_node* hn;
+
+        if (idx < LITMUS_MAX_PRIORITY) {
+                hn = bheap_peek(fp_ready_order, &q->queue[idx]);
+                return bheap2task(hn);
+        } else
+                return NULL;
+}
+
+static inline struct task_struct* fp_prio_take(struct fp_prio_queue* q)
+{
+        unsigned int idx = fpq_find(q);
+        struct bheap_node* hn;
+
+        if (idx < LITMUS_MAX_PRIORITY) {
+                hn = bheap_take(fp_ready_order, &q->queue[idx]);
+                if (likely(bheap_empty(&q->queue[idx])))
+                        fpq_clear(q, idx);
+                return bheap2task(hn);
+        } else
+                return NULL;
+}
+
 #endif
diff --git a/litmus/fp_common.c b/litmus/fp_common.c
index fa36574d5992..7898955fd991 100644
--- a/litmus/fp_common.c
+++ b/litmus/fp_common.c
@@ -110,3 +110,13 @@ int fp_preemption_needed(rt_domain_t* rt, struct task_struct *t)
         /* make sure to get non-rt stuff out of the way */
         return !is_realtime(t) || fp_higher_prio(__next_ready(rt), t);
 }
+
+void fp_prio_queue_init(struct fp_prio_queue* q)
+{
+        int i;
+
+        for (i = 0; i < FP_PRIO_BIT_WORDS; i++)
+                q->bitmask[i] = 0;
+        for (i = 0; i < LITMUS_MAX_PRIORITY; i++)
+                bheap_init(&q->queue[i]);
+}
diff --git a/litmus/sched_pfp.c b/litmus/sched_pfp.c
index 183ceedf8308..fb7433dacf40 100644
--- a/litmus/sched_pfp.c
+++ b/litmus/sched_pfp.c
@@ -21,6 +21,7 @@
 
 typedef struct {
         rt_domain_t             domain;
+        struct fp_prio_queue    ready_queue;
         int                     cpu;
         struct task_struct*     scheduled; /* only RT tasks */
 /*
@@ -39,50 +40,65 @@ DEFINE_PER_CPU(pfp_domain_t, pfp_domains);
 #define task_dom(task)          remote_dom(get_partition(task))
 #define task_pfp(task)          remote_pfp(get_partition(task))
 
+/* we assume the lock is being held */
+static void preempt(pfp_domain_t *pfp)
+{
+        preempt_if_preemptable(pfp->scheduled, pfp->cpu);
+}
+
+static unsigned int priority_index(struct task_struct* t)
+{
+#ifdef CONFIG_LOCKING
+        if (is_priority_boosted(t)) {
+                /* zero is reserved for priority-boosted tasks */
+                return 0;
+        } else
+#endif
+                return get_priority(t);
+}
+
+
+static void pfp_release_jobs(rt_domain_t* rt, struct bheap* tasks)
+{
+        pfp_domain_t *pfp = container_of(rt, pfp_domain_t, domain);
+        unsigned long flags;
+        struct task_struct* t;
+        struct bheap_node* hn;
+
+        raw_spin_lock_irqsave(&pfp->slock, flags);
+
+        while (!bheap_empty(tasks)) {
+                hn = bheap_take(fp_ready_order, tasks);
+                t = bheap2task(hn);
+                fp_prio_add(&pfp->ready_queue, t, priority_index(t));
+        }
+
+        /* do we need to preempt? */
+        if (fp_higher_prio(fp_prio_peek(&pfp->ready_queue), pfp->scheduled))
+                preempt(pfp);
+
+        raw_spin_unlock_irqrestore(&pfp->slock, flags);
+}
 
 static void pfp_domain_init(pfp_domain_t* pfp,
-                               check_resched_needed_t check,
-                               release_jobs_t release,
                                int cpu)
 {
-        fp_domain_init(&pfp->domain, check, release);
+        fp_domain_init(&pfp->domain, NULL, pfp_release_jobs);
         pfp->cpu                = cpu;
         pfp->scheduled          = NULL;
+        fp_prio_queue_init(&pfp->ready_queue);
 }
 
-static void requeue(struct task_struct* t, rt_domain_t *dom)
+static void requeue(struct task_struct* t, pfp_domain_t *pfp)
 {
         if (t->state != TASK_RUNNING)
                 TRACE_TASK(t, "requeue: !TASK_RUNNING\n");
 
         set_rt_flags(t, RT_F_RUNNING);
         if (is_released(t, litmus_clock()))
-                __add_ready(dom, t);
+                fp_prio_add(&pfp->ready_queue, t, priority_index(t));
         else
-                add_release(dom, t); /* it has got to wait */
-}
-
-/* we assume the lock is being held */
-static void preempt(pfp_domain_t *pfp)
-{
-        preempt_if_preemptable(pfp->scheduled, pfp->cpu);
-}
-
-/* This check is trivial in partioned systems as we only have to consider
- * the CPU of the partition.
- */
-static int pfp_check_resched(rt_domain_t *dom)
-{
-        pfp_domain_t *pfp = container_of(dom, pfp_domain_t, domain);
-
-        /* because this is a callback from rt_domain_t we already hold
-         * the necessary lock for the ready queue
-         */
-        if (fp_preemption_needed(dom, pfp->scheduled)) {
-                preempt(pfp);
-                return 1;
-        } else
-                return 0;
+                add_release(&pfp->domain, t); /* it has got to wait */
 }
 
 static void job_completion(struct task_struct* t, int forced)
@@ -185,8 +201,8 @@ static struct task_struct* pfp_schedule(struct task_struct * prev)
                  * the appropriate queue.
                  */
                 if (pfp->scheduled && !blocks)
-                        requeue(pfp->scheduled, dom);
-                next = __take_ready(dom);
+                        requeue(pfp->scheduled, pfp);
+                next = fp_prio_take(&pfp->ready_queue);
         } else
                 /* Only override Linux scheduler if we have a real-time task
                  * scheduled that needs to continue.
@@ -213,7 +229,6 @@ static struct task_struct* pfp_schedule(struct task_struct * prev)
  */
 static void pfp_task_new(struct task_struct * t, int on_rq, int running)
 {
-        rt_domain_t*            dom  = task_dom(t);
         pfp_domain_t*   pfp = task_pfp(t);
         unsigned long           flags;
 
@@ -232,7 +247,7 @@ static void pfp_task_new(struct task_struct * t, int on_rq, int running)
                 BUG_ON(pfp->scheduled);
                 pfp->scheduled = t;
         } else {
-                requeue(t, dom);
+                requeue(t, pfp);
                 /* maybe we have to reschedule */
                 preempt(pfp);
         }
@@ -242,8 +257,7 @@ static void pfp_task_new(struct task_struct * t, int on_rq, int running)
 static void pfp_task_wake_up(struct task_struct *task)
 {
         unsigned long           flags;
-        pfp_domain_t*   pfp = task_pfp(task);
-        rt_domain_t*            dom  = task_dom(task);
+        pfp_domain_t*           pfp = task_pfp(task);
         lt_t                    now;
 
         TRACE_TASK(task, "wake_up at %llu\n", litmus_clock());
@@ -271,7 +285,7 @@ static void pfp_task_wake_up(struct task_struct *task)
          * and won.
          */
         if (pfp->scheduled != task)
-                requeue(task, dom);
+                requeue(task, pfp);
 
         raw_spin_unlock_irqrestore(&pfp->slock, flags);
         TRACE_TASK(task, "wake up done\n");
@@ -579,7 +593,8 @@ static long pfp_allocate_lock(struct litmus_lock **lock, int type,
 
 static long pfp_admit_task(struct task_struct* tsk)
 {
-        return task_cpu(tsk) == tsk->rt_param.task_params.cpu ? 0 : -EINVAL;
+        return task_cpu(tsk) == tsk->rt_param.task_params.cpu
+                && get_priority(tsk) > 0 ? 0 : -EINVAL;
 }
 
 /*      Plugin object   */
@@ -609,9 +624,7 @@ static int __init init_pfp(void)
          * we cannot use num_online_cpu()
          */
         for (i = 0; i < num_online_cpus(); i++) {
-                pfp_domain_init(remote_pfp(i),
-                                   pfp_check_resched,
-                                   NULL, i);
+                pfp_domain_init(remote_pfp(i), i);
         }
         return register_sched_plugin(&pfp_plugin);
 }