sched_color: Scheduler working without locks

7 files changed, 460 insertions, 284 deletions

diff --git a/include/litmus/sched_trace.h b/include/litmus/sched_trace.h
index e193267a321f..0773aebcae16 100644
--- a/include/litmus/sched_trace.h
+++ b/include/litmus/sched_trace.h
@@ -187,8 +187,8 @@ feather_callback void do_sched_trace_sys_release(unsigned long id,
 
 #define trace_litmus_container_param(cid, name)
 #define trace_litmus_server_param(sid, cid, wcet, time)
-#define trace_litmus_server_switch_to(sid, job, tid)
-#define trace_litmus_server_switch_away(sid, job, tid)
+#define trace_litmus_server_switch_to(sid, job, tid, tjob)
+#define trace_litmus_server_switch_away(sid, job, tid, tjob)
 #define trace_litmus_server_release(sid, job, release, deadline)
 #define trace_litmus_server_completion(sid, job)
 
@@ -294,14 +294,14 @@ feather_callback void do_sched_trace_sys_release(unsigned long id,
                 trace_litmus_server_param(sid, cid, wcet, period);      \
         } while(0)
 
-#define sched_trace_server_switch_to(sid, job, tid)                     \
+#define sched_trace_server_switch_to(sid, job, tid, tjob)               \
         do {                                                            \
-                trace_litmus_server_switch_to(sid, job, tid);           \
+                trace_litmus_server_switch_to(sid, job, tid, tjob);     \
         } while(0)
 
-#define sched_trace_server_switch_away(sid, job, tid)                   \
+#define sched_trace_server_switch_away(sid, job, tid, tjob)             \
         do {                                                            \
-                trace_litmus_server_switch_away(sid, job, tid);         \
+                trace_litmus_server_switch_away(sid, job, tid, tjob);   \
         } while (0)
 
 #define sched_trace_server_release(sid, job, rel, dead)                 \
diff --git a/include/trace/events/litmus.h b/include/trace/events/litmus.h
index 7a34a3214ae6..136a80db54a4 100644
--- a/include/trace/events/litmus.h
+++ b/include/trace/events/litmus.h
@@ -331,43 +331,48 @@ TRACE_EVENT(litmus_server_param,
 
 TRACE_EVENT(litmus_server_switch_to,
 
-        TP_PROTO(int sid, unsigned int job, int tid),
+            TP_PROTO(int sid, unsigned int job, int tid, unsigned int tjob),
 
-        TP_ARGS(sid, job, tid),
+            TP_ARGS(sid, job, tid, tjob),
 
         TP_STRUCT__entry(
                 __field( int, sid)
                 __field( unsigned int, job)
                 __field( int, tid)
+                __field( unsigned int, tjob)
         ),
 
         TP_fast_assign(
                 __entry->sid = sid;
                 __entry->tid = tid;
                 __entry->job = job;
+                __entry->tjob = tjob;
         ),
 
-        TP_printk("switch_to(server(%d, %u)): %d\n", __entry->sid, __entry->job, __entry->tid)
+            TP_printk("switch_to(server(%d, %u)): (%d, %d)\n", __entry->sid, __entry->job, __entry->tid, __entry->tjob)
 );
 
 TRACE_EVENT(litmus_server_switch_away,
 
-        TP_PROTO(int sid, unsigned int job, int tid),
+            TP_PROTO(int sid, unsigned int job, int tid, unsigned int tjob),
 
-        TP_ARGS(sid, job, tid),
+            TP_ARGS(sid, job, tid, tjob),
 
         TP_STRUCT__entry(
                 __field( int, sid)
                 __field( unsigned int, job)
                 __field( int, tid)
+                __field( unsigned int, tjob)
         ),
 
         TP_fast_assign(
                 __entry->sid = sid;
                 __entry->tid = tid;
+                __entry->job = job;
+                __entry->tjob = tjob;
         ),
 
-        TP_printk("switch_away(server(%d, %u)): %d\n", __entry->sid, __entry->job, __entry->tid)
+            TP_printk("switch_away(server(%d, %u)): (%d, %d)\n", __entry->sid, __entry->job, __entry->tid, __entry->tjob)
 );
 
 TRACE_EVENT(litmus_server_release,
diff --git a/litmus/Makefile b/litmus/Makefile
index 1d68279af82d..30345ad07aa7 100644
--- a/litmus/Makefile
+++ b/litmus/Makefile
@@ -26,7 +26,7 @@ obj-y     = sched_plugin.o litmus.o \
 
 obj-$(CONFIG_PLUGIN_CEDF) += sched_cedf.o
 obj-$(CONFIG_PLUGIN_PFAIR) += sched_pfair.o
-obj-$(CONFIG_PLUGIN_MC) += sched_mc.o sched_mc_ce.o ce_domain.o
+# obj-$(CONFIG_PLUGIN_MC) += sched_mc.o sched_mc_ce.o ce_domain.o
 obj-$(CONFIG_MERGE_TIMERS) += event_group.o
 
 obj-$(CONFIG_FEATHER_TRACE) += ft_event.o ftdev.o
diff --git a/litmus/budget.c b/litmus/budget.c
index 7fe91815341c..0a7bd665f814 100644
--- a/litmus/budget.c
+++ b/litmus/budget.c
@@ -53,7 +53,6 @@ void arm_enforcement_timer(struct enforcement_timer* et,
                            struct task_struct* t)
 {
         lt_t when_to_fire;
-        TRACE_TASK(t, "arming enforcement timer.\n");
 
         /* Calling this when there is no budget left for the task
          * makes no sense, unless the task is non-preemptive. */
@@ -64,6 +63,9 @@ void arm_enforcement_timer(struct enforcement_timer* et,
 
         if (likely(!is_np(t))) {
                 when_to_fire = litmus_clock() + budget_remaining(t);
+                TRACE_TASK(t, "arming enforcement timer for %llu.\n",
+                           when_to_fire);
+
                 __hrtimer_start_range_ns(&et->timer,
                                          ns_to_ktime(when_to_fire),
                                          0 /* delta */,
diff --git a/litmus/color_proc.c b/litmus/color_proc.c
index 88c8c772dce7..25915e714821 100644
--- a/litmus/color_proc.c
+++ b/litmus/color_proc.c
@@ -2,6 +2,9 @@
 #include <linux/sysctl.h>
 #include <linux/slab.h>
 
+#include <litmus/sched_trace.h>
+
+
 #define SPERIOD_LEN 7
 #define SPERIOD_FILE "period"
 #define SWCET_LEN 5
@@ -41,7 +44,9 @@ int color_server_params(int cpu, unsigned long *wcet, unsigned long *period)
         }
 
         *wcet = server_wcet[cpu];
-        *server_period = server_period[cpu];
+        *period = server_period[cpu];
+
+        TRACE("For %d: %lu, %lu\n", cpu, server_wcet[cpu], server_period[cpu]);
 
         return 0;
 }
diff --git a/litmus/fifo_common.c b/litmus/fifo_common.c
index d4b3cb8cc721..84ae98e42ae4 100644
--- a/litmus/fifo_common.c
+++ b/litmus/fifo_common.c
@@ -21,6 +21,7 @@ int fifo_higher_prio(struct task_struct* first,
         if (first && first == second) {
                 TRACE_TASK(first,
                            "WARNING: pointless fifo priority comparison.\n");
+                BUG_ON(1);
                 return 0;
         }
 
diff --git a/litmus/sched_color.c b/litmus/sched_color.c
index 8f21b2f36f66..697d57194d3a 100644
--- a/litmus/sched_color.c
+++ b/litmus/sched_color.c
@@ -18,18 +18,22 @@
 
 struct fifo_server {
         struct task_struct*        task;
+        struct task_struct*        linked;
+
         struct enforcement_timer   timer;
-        lt_t                       start_time;
+        lt_t                       start_time; /* Used for exec time */
+        int                        running; /* For tracing niceties */
 };
 
 struct cpu_entry {
         int                     cpu;
         rt_domain_t             edf_domain;
 
-        struct task_struct*     linked;
-        struct task_struct*     scheduled;
+        struct task_struct*     linked;    /* Top-level EDF scheduler */
+        struct task_struct*     scheduled; /* Actually running, EDF or FIFO */
 
         raw_spinlock_t*         lock;
+
         struct fifo_server      server;
 };
 
@@ -44,13 +48,21 @@ static raw_spinlock_t fifo_lock;
 #define task_edf(task)        remote_edf(get_partition(task))
 #define task_entry(task)      remote_entry(get_partition(task))
 #define server_running(entry) (entry->linked && entry->linked == entry->server.task)
+#define task_server(task)     (&task_entry(task)->server)
 
-
+/*
+ * Requeue task in appropriate domain
+ */
 static void requeue(struct task_struct* t)
 {
-        rt_domain_t *dom = is_be(t) ? &fifo_domain : task_edf(t);
+        rt_domain_t *dom;
+
+        BUG_ON(is_queued(t));
+        BUG_ON(is_server(t) && !tsk_rt(t)->present);
         TRACE_TASK(t, "Requeueing\n");
 
+        dom = is_be(t) ? &fifo_domain : task_edf(t);
+
         set_rt_flags(t, RT_F_RUNNING);
         if (is_released(t, litmus_clock()))
                 __add_ready(dom, t);
@@ -59,362 +71,468 @@ static void requeue(struct task_struct* t)
 }
 
 /*
- * Preempts any preemptable task. If that preemptable task is a
- * server, this will also check that the running task is preemptable.
+ * Logically begin server execution.
  */
-static void color_edf_release(rt_domain_t *edf, struct bheap *tasks)
+static void stop_serving(struct fifo_server *server, struct task_struct *t)
 {
-        unsigned long flags;
-        struct cpu_entry *entry;
-        int lpre, spre;
+        int task_job, task_pid;
+        struct task_struct *stask = server->task;
 
-        entry = container_of(edf, struct cpu_entry, edf_domain);
+        BUG_ON(!server->running);
 
-        raw_spin_lock_irqsave(entry->lock, flags);
+        task_pid = (t) ? t->pid : 0;
+        task_job = (t) ? get_rt_job(t) : 0;
 
-        __merge_ready(edf, tasks);
+        if (t)
+                tsk_rt(t)->linked_on = NO_CPU;
 
-        if (edf_preemption_needed(edf, entry->linked)) {
-                lpre = entry->linked ? !is_kernel_np(entry->linked) : 0;
-                spre = entry->scheduled ? !is_kernel_np(entry->scheduled) : 0;
-                if (lpre && spre) {
-                        litmus_reschedule(entry->cpu);
-                }
-        }
+        TRACE_TASK(stask, "No longer serving (%d:%d)\n", task_pid, task_job);
 
-        raw_spin_unlock_irqrestore(entry->lock, flags);
+        sched_trace_server_switch_away(stask->pid, get_rt_job(stask),
+                                       task_pid, task_job);
+        server->running = 0;
 }
 
 /*
- * Trigger FIFO release on first server which is running nothing.
- * Note: switching to preempting based on fifo_preemption_needed is not
- * an easy thing! The cpus must be checked in reverse-fifo order, ala
- * gsn-edf, and the tasks to run must be chosen _here_.
+ * Logically stop server execution
  */
-static void color_fifo_release(rt_domain_t *dom, struct bheap *tasks)
+static void start_serving(struct fifo_server *server, struct task_struct *t)
 {
-        unsigned long flags;
-        int cpu, ret = 0;
+        int next_pid, next_job;
         struct cpu_entry *entry;
 
-        local_irq_save(flags);
+        next_pid = (t) ? t->pid : 0;
+        next_job = (t) ? get_rt_job(t) : 0;
+        entry = container_of(server, struct cpu_entry, server);
+        sched_trace_server_switch_to(server->task->pid,
+                                     get_rt_job(server->task),
+                                     next_pid, next_job);
 
-        raw_spin_lock(&fifo_lock);
-        __merge_ready(dom, tasks);
-        raw_spin_unlock(&fifo_lock);
+        if (t) {
+                tsk_rt(t)->linked_on = entry->cpu;
+                TRACE_TASK(t, "Run by server %d\n", server->task->pid);
+        } else
+                TRACE("(NULL) Run by server %d\n", server->task->pid);
 
-        for_each_online_cpu(cpu) {
-                entry = remote_entry(cpu);
+        server->running = 1;
+}
 
-                raw_spin_lock(entry->lock);
-                raw_spin_lock(&fifo_lock);
+/*
+ * Remove from "linked" fields and, if necessary, drop switch_away rcords.
+ */
+static void unlink(struct task_struct *t)
+{
+        struct cpu_entry *entry = task_entry(t);
 
-                if (server_running(entry) && !entry->scheduled) {
-                        litmus_reschedule(cpu);
-                        ret = 1;
-                }
+        TRACE_TASK(t, "Unlinking\n");
 
-                raw_spin_unlock(&fifo_lock);
-                raw_spin_unlock(entry->lock);
+        if (is_be(t) && tsk_rt(t)->linked_on != NO_CPU) {
+                entry = remote_entry(tsk_rt(t)->linked_on);
+                BUG_ON(entry->scheduled != t);
 
-                if (ret)
-                        goto out;
+                stop_serving(&entry->server, entry->server.linked);
+
+                entry->server.linked = NULL;
+                entry->scheduled = NULL;
+        }
+
+        if (entry->linked == t) {
+                entry->linked = NULL;
+                TRACE_TASK(t, "No longer running on %d\n", entry->cpu);
+                sched_trace_server_switch_away(entry->cpu + 1, 0,
+                                               t->pid, get_rt_job(t));
+        }
+        if (entry->scheduled == t)
+                entry->scheduled = NULL;
+
+        if (is_server(t) && entry->server.running) {
+                stop_serving(&entry->server, entry->server.linked);
         }
- out:
-        local_irq_restore(flags);
 }
 
 /*
- * Select second-level task to run and arm timer for server.
+ * Unlink task. If task is a server running another task, unlink and requeue
+ * the sub-task.
  */
-static void schedule_server(struct task_struct *server,
-                            struct task_struct *prev)
+static void preempt(struct task_struct *t)
 {
-        struct cpu_entry *entry = task_entry(server);
+        struct fifo_server *server;
 
-        BUG_ON(!is_server(server));
-        BUG_ON(entry->server.start_time);
+        TRACE_TASK(t, "Preempting\n");
 
-        raw_spin_lock(&fifo_lock);
-        if (prev != server ||
-            fifo_preemption_needed(&fifo_domain, entry->scheduled)) {
-                BUG_ON(!is_be(entry->scheduled));
-
-                if (is_be(entry->scheduled)) {
-                        requeue(entry->scheduled);
-
-                        TRACE_TASK(entry->scheduled, "Server %d stopped\n",
-                                   server->pid);
-                        sched_trace_server_switch_away(server->pid,
-                                                       get_rt_job(server),
-                                                       entry->scheduled->pid);
-                }
+        if (is_server(t)) {
+                server = task_server(t);
+                if (server->linked) {
+                        BUG_ON(tsk_rt(server->linked)->linked_on == NO_CPU);
+                        TRACE_TASK(server->linked, "Preempted with server\n");
 
-                entry->scheduled = __take_ready(&fifo_domain);
+                        raw_spin_lock(&fifo_lock);
 
-                if (entry->scheduled) {
-                        TRACE_TASK(entry->scheduled, "Running on server %d\n",
-                                   server->pid);
-                        sched_trace_server_switch_to(server->pid,
-                                                     get_rt_job(server),
-                                                     entry->scheduled->pid);
+                        requeue(server->linked);
+                        unlink(server->linked);
+
+                        raw_spin_unlock(&fifo_lock);
                 }
         }
-        raw_spin_unlock(&fifo_lock);
 
-        /* Trigger a reschedule when the server would be exhausted */
-        entry->server.start_time = litmus_clock();
-        arm_enforcement_timer(&entry->server.timer, server);
+        unlink(t);
+}
+
+static void job_completion(struct task_struct *t)
+{
+        TRACE_TASK(t, "Job completed\n");
+        if (is_server(t))
+                sched_trace_server_completion(t->pid, get_rt_job(t));
+        else
+                sched_trace_task_completion(t, 0);
+
+
+        preempt(t);
+        set_rt_flags(t, RT_F_SLEEP);
+        prepare_for_next_period(t);
+
+        if (is_server(t))
+                sched_trace_server_release(t->pid, get_rt_job(t),
+                                           get_release(t), get_rt_period(t));
+        else
+                sched_trace_task_release(t);
+
+        if (is_running(t))
+                requeue(t);
 }
 
 /*
- * Update server exec time since a server was started.
+ * Update state of task and entries to reflect blocks or completions.
  */
-static void update_server(struct task_struct *server)
+static void update_task(struct task_struct *t)
 {
-        struct cpu_entry *entry = task_entry(server);
+        int oot, sleep, block, np;
+
+        block = !is_running(t);
+        oot   = budget_enforced(t) && budget_exhausted(t);
+        np    = is_np(t);
+        sleep = get_rt_flags(t) == RT_F_SLEEP;
+
+        TRACE_TASK(t, "Updating task, block: %d, oot: %d, np: %d, sleep: %d\n",
+                   block, oot, np, sleep);
+
+        if (block)
+                preempt(t);
+        else if ((oot || sleep) && !np)
+                job_completion(t);
+}
+
+/*
+ * Update state of server and constituent tasks to reflect any blocking,
+ * time elapsed, or job completions encountered.
+ */
+static void update_server(struct fifo_server *server)
+{
+        struct cpu_entry *entry;
+        struct task_struct *task;
         unsigned long long delta;
 
-        BUG_ON(!is_server(server));
-        BUG_ON(!entry->server.start_time);
+        entry = task_entry(server->task);
+        task  = server->task;
+
+        BUG_ON(!server->start_time);
         BUG_ON(!server_running(entry));
 
-        delta = litmus_clock() - entry->server.start_time;
-        entry->server.start_time = 0;
+        cancel_enforcement_timer(&server->timer);
+
+        delta = litmus_clock() - server->start_time;
+        tsk_rt(task)->job_params.exec_time += delta;
+        server->start_time = 0;
+
+        if (!tsk_rt(server->task)->present) {
+                /* Deactivate uses present to trigger server exits */
+                TRACE_TASK(server->task, "RIP server - %llu\n", litmus_clock());
+                preempt(server->task);
+                BUG_ON(is_queued(server->task));
+                BUG_ON(1);
+        } else {
+
+                TRACE_TASK(task, "Updating server, delta: %llu, exec: %llu\n",
+                           delta, get_exec_time(task));
+
+                if (server->linked)
+                        update_task(server->linked);
+                update_task(server->task);
+        }
+}
+
+/*
+ * Triggers preemption on edf-scheduled "linked" field only.
+ */
+static void color_edf_release(rt_domain_t *edf, struct bheap *tasks)
+{
+        unsigned long flags;
+        struct cpu_entry *entry;
+
+        TRACE_TASK(bheap2task(bheap_peek(edf->order, tasks)),
+                   "Released set of EDF tasks\n");
+
+        entry = container_of(edf, struct cpu_entry, edf_domain);
+        raw_spin_lock_irqsave(entry->lock, flags);
 
-        tsk_rt(server)->job_params.exec_time += delta;
+        __merge_ready(edf, tasks);
 
-        cancel_enforcement_timer(&entry->server.timer);
+        if (edf_preemption_needed(edf, entry->linked) &&
+            (!entry->linked || !is_kernel_np(entry->linked))) {
+                litmus_reschedule(entry->cpu);
+        }
 
-        TRACE_TASK(server, "Delta: %llu, new exec: %llu\n", delta,
-                   get_exec_time(server));
+        raw_spin_unlock_irqrestore(entry->lock, flags);
 }
 
 /*
- * Requeue server and, if necessary, a task it is running.
+ * Triggers preemption on first FIFO server which is running NULL.
  */
-static void preempt_server(struct task_struct *server)
+static void check_for_fifo_preempt(void)
 {
-        struct cpu_entry *entry = task_entry(server);
+        int ret = 0, cpu;
+        struct cpu_entry *entry;
 
-        BUG_ON(!is_server(server));
+        for_each_online_cpu(cpu) {
+                entry = remote_entry(cpu);
 
-        requeue(server);
-        if (entry->scheduled) {
-                BUG_ON(!is_be(entry->scheduled));
+                raw_spin_lock(entry->lock);
                 raw_spin_lock(&fifo_lock);
-                requeue(entry->scheduled);
+
+                if (server_running(entry) && !entry->server.linked) {
+                        litmus_reschedule(cpu);
+                        ret = 1;
+                }
+
                 raw_spin_unlock(&fifo_lock);
+                raw_spin_unlock(entry->lock);
+
+                if (ret)
+                        break;
         }
 }
 
-static void unlink(struct task_struct *t)
+static void color_fifo_release(rt_domain_t *dom, struct bheap *tasks)
 {
-        struct cpu_entry *entry = task_entry(t);
+        unsigned long flags;
 
-        TRACE_TASK(t, "Unlinking\n");
-        if (entry->linked == t)
-                entry->linked = NULL;
-        if (entry->scheduled == t)
-                entry->scheduled = NULL;
+        TRACE_TASK(bheap2task(bheap_peek(dom->order, tasks)),
+                   "Released set of FIFO tasks\n");
+
+        local_irq_save(flags);
+
+        raw_spin_lock(&fifo_lock);
+        __merge_ready(dom, tasks);
+        raw_spin_unlock(&fifo_lock);
+
+        check_for_fifo_preempt();
+
+        local_irq_restore(flags);
 }
 
 /*
- * Complete a process's job.
+ * Run top-level EDF scheduler.
  */
-static void real_job_completion(struct task_struct* t, int forced)
+static struct task_struct* schedule_edf(struct cpu_entry *entry,
+                                        struct task_struct *prev)
 {
-        sched_trace_task_completion(t,forced);
-        TRACE_TASK(t, "Job completed\n");
+        struct task_struct *next, *updated_link;
 
-        unlink(t);
-        set_rt_flags(t, RT_F_SLEEP);
-        prepare_for_next_period(t);
+        /* Update previously-running task */
+        if (prev) {
+                if (is_server(prev))
+                        update_server(task_server(prev));
+                else
+                        update_task(prev);
+        }
 
-        sched_trace_task_release(t);
+        /* Select next task to run */
+        next = entry->linked;
+        updated_link = next;
+        if (edf_preemption_needed(&entry->edf_domain, entry->linked)) {
+                if (entry->linked) {
+                        requeue(entry->linked);
+                        preempt(entry->linked);
+                }
+                next = __take_ready(&entry->edf_domain);
+                BUG_ON(next == updated_link);
+        }
 
-        if (is_running(t))
-                requeue(t);
+        if (next != prev       || /* New task to run or */
+            next != updated_link  /* Same task, new job */) {
+                if (next) {
+                        sched_trace_server_switch_to(entry->cpu + 1, 0,
+                                                     next->pid,
+                                                     get_rt_job(next));
+                        TRACE_TASK(next, "Running on %d\n", entry->cpu);
+                } else {
+                        TRACE("CPU %d going idle\n", entry->cpu);
+                }
+        }
+
+        entry->linked = next;
+
+        return next;
 }
 
 /*
- * Complete a server's job,requeing where appropriate.
+ * Run bottom-level fifo scheduler on entry running a server.
  */
-static void server_job_completion(struct task_struct *t)
+static struct task_struct* schedule_fifo(struct cpu_entry *entry,
+                                         struct task_struct *prev_linked)
 {
-        sched_trace_server_completion(t->pid, get_rt_job(t));
-        TRACE_TASK(t, "Server job completed\n");
+        struct task_struct *task, *prev_fifo, *next_fifo = NULL;
+        struct fifo_server *server = &entry->server;
 
-        unlink(t);
-        set_rt_flags(t, RT_F_SLEEP);
-        prepare_for_next_period(t);
+        BUG_ON(server->linked && prev_linked != server->task);
+        BUG_ON(!server_running(entry));
 
-        sched_trace_server_release(t->pid, get_rt_job(t),
-                                   get_release(t),
-                                   get_deadline(t));
-        preempt_server(t);
+        task = server->task;
 
-}
+        /* Update previously-running task */
+        if (prev_linked == task && server->linked) {
+                update_task(server->linked);
+        }
 
-static void color_tick(struct task_struct *t)
-{
-        struct cpu_entry *entry = local_entry;
-        BUG_ON(is_realtime(t) && t != entry->scheduled);
-        if (is_realtime(t) && budget_enforced(t) && budget_exhausted(t)) {
-                if (!is_np(t)) {
-                        litmus_reschedule_local();
-                        TRACE("color_scheduler_tick: "
-                              "%d is preemptable "
-                              " => FORCE_RESCHED\n", t->pid);
-                } else if (is_user_np(t)) {
-                        TRACE("color_scheduler_tick: "
-                              "%d is non-preemptable, "
-                              "preemption delayed.\n", t->pid);
-                        request_exit_np(t);
+        /* Select next task to run */
+        next_fifo = server->linked;
+        prev_fifo = next_fifo;
+        if (fifo_preemption_needed(&fifo_domain, server->linked)) {
+                if (server->linked) {
+                        raw_spin_lock(&fifo_lock);
+                        requeue(server->linked);
+                        preempt(server->linked);
+                        raw_spin_unlock(&fifo_lock);
                 }
+                next_fifo = __take_ready(&fifo_domain);
+        }
+
+        /* Stop serving the NULL task */
+        if (server->running && !prev_fifo && prev_fifo != next_fifo) {
+                stop_serving(server, prev_fifo);
+        }
+
+        /* If the server was not running or wasn't running next_fifo */
+        if (!server->running || next_fifo != prev_fifo) {
+                start_serving(server, next_fifo);
         }
+
+        server->linked = next_fifo;
+        server->start_time = litmus_clock();
+        arm_enforcement_timer(&server->timer, server->task);
+
+        return next_fifo;
 }
 
-static struct task_struct* color_schedule(struct task_struct * prev)
+/*
+ * Schedule hierarchically.
+ */
+static struct task_struct* color_schedule(struct task_struct *prev)
 {
-        struct cpu_entry        *entry = local_entry;
-        rt_domain_t             *edf  = &entry->edf_domain;
-        struct task_struct      *prev_linked;
-        int                     out_of_time, sleep,
-                                np, exists, blocks, server_exhausted;
+        unsigned long flags;
+        struct cpu_entry *entry = local_entry;
+        struct task_struct *next, *prev_linked = entry->linked;
 
-        raw_spin_lock(entry->lock);
+        TRACE("Reschedule on %d at %llu\n", entry->cpu, litmus_clock());
+
+        raw_spin_lock_irqsave(entry->lock, flags);
 
         BUG_ON(entry->scheduled && entry->scheduled != prev);
         BUG_ON(entry->scheduled && !is_realtime(prev));
 
-        /* (0) Update state */
-        if (server_running(entry))
-                update_server(entry->linked);
-
-        /* (1) Determine state */
-        exists      = entry->scheduled != NULL;
-        blocks      = exists && !is_running(entry->scheduled);
-        out_of_time = exists && budget_enforced(entry->scheduled) &&
-                                budget_exhausted(entry->scheduled);
-        np          = exists && is_np(entry->scheduled);
-        sleep       = exists && get_rt_flags(entry->scheduled) == RT_F_SLEEP;
-        server_exhausted = server_running(entry) &&
-                           budget_exhausted(entry->server.task);
-        prev_linked = entry->linked;
-
-        TRACE("Schedule on %d, exists: %d, blocks: %d, out_of_time: %d, ",
-              "np: %d, sleep: %d, server_exhausted: %d, sched: %d, linked: %d "
-              "at %llu\n",
-              entry->cpu, exists, blocks, out_of_time, np, sleep,
-              server_exhausted, (exists) ? prev->pid : 0,
-              (entry->linked) ? entry->linked->pid : 0, litmus_clock());
-
-
-        /* (2) Move previous tasks to proper locations */
-        if (blocks)
-                unlink(entry->scheduled);
-        if (server_exhausted && !np)
-                server_job_completion(entry->linked);
-        else if (!np && !blocks && (out_of_time || sleep))
-                real_job_completion(entry->scheduled, !sleep);
-
-        /* (3) Figure out what should be running at the top level */
-        if (edf_preemption_needed(edf, entry->linked)) {
-                if (entry->linked) {
-                        if (is_server(entry->linked))
-                                preempt_server(entry->linked);
-                        else
-                                requeue(entry->linked);
-                }
-                entry->linked = __take_ready(edf);
-        }
+        /* Top level */
+        next = schedule_edf(entry, prev_linked);
 
-        /* (4) Schedule bottom level */
-        if (is_server(entry->linked))
-                schedule_server(entry->linked, prev_linked);
-        else
-                entry->scheduled = entry->linked;
-
-        /* (5) Do top level tracing */
-        if (entry->linked != prev_linked) {
-                if (prev_linked)
-                        sched_trace_server_switch_away(entry->cpu, 0,
-                                                       prev_linked->pid);
-                if (entry->linked) {
-                        TRACE_TASK(entry->linked, "Run on %d\n", entry->cpu);
-                        sched_trace_server_switch_to(entry->cpu, 0,
-                                                     entry->linked->pid);
-                } else
-                        TRACE("Running (NULL) on %d\n", entry->cpu);
-        } else
-                TRACE("Running same task on %d\n", entry->cpu);
+        /* Bottom level */
+        if (next && is_server(next))
+                next = schedule_fifo(entry, prev_linked);
 
+        entry->scheduled = next;
         sched_state_task_picked();
-        raw_spin_unlock(entry->lock);
+
+        raw_spin_unlock_irqrestore(entry->lock, flags);
 
         return entry->scheduled;
 }
 
-
-/*      Prepare a task for running in RT mode
- */
-static void color_task_new(struct task_struct * t, int on_rq, int running)
+static void color_task_new(struct task_struct *t, int on_rq, int running)
 {
-        struct cpu_entry*       entry = task_entry(t);
-        unsigned long           flags;
+        struct cpu_entry* entry;
+        unsigned long flags;
 
-        TRACE_TASK(t, "psn edf: task new, cpu = %d\n",
-                   t->rt_param.task_params.cpu);
+        TRACE_TASK(t, "New colored task, cpu = %d, wcet = %llu, period = %llu\n",
+                   t->rt_param.task_params.cpu,
+                   get_exec_cost(t), get_rt_period(t));
+
+        local_irq_save(flags);
 
-        /* setup job parameters */
         release_at(t, litmus_clock());
 
-        /* The task should be running in the queue, otherwise signal
-         * code will try to wake it up with fatal consequences.
-         */
-        raw_spin_lock_irqsave(entry->lock, flags);
-        if (running) {
-                /* there shouldn't be anything else running at the time */
-                BUG_ON(entry->scheduled);
-                entry->scheduled = t;
+        if (is_be(t)) {
+                entry = local_entry;
+                raw_spin_lock(entry->lock);
+                raw_spin_lock(&fifo_lock);
+
+                if (running) {
+                        BUG_ON(entry->scheduled);
+                        entry->scheduled = t;
+                } else {
+                        requeue(t);
+                }
+
+                raw_spin_unlock(&fifo_lock);
+                raw_spin_unlock(entry->lock);
         } else {
-                requeue(t);
-                /* maybe we have to reschedule */
-                if (!is_be(t))
+                entry = task_entry(t);
+                raw_spin_lock(entry->lock);
+
+                if (running) {
+                        BUG_ON(entry->scheduled);
+                        entry->scheduled = t;
+                } else {
+                        requeue(t);
                         preempt_if_preemptable(entry->scheduled, entry->cpu);
+                }
+
+                raw_spin_unlock(entry->lock);
         }
-        raw_spin_unlock_irqrestore(entry->lock, flags);
+
+        local_irq_restore(flags);
 }
 
 static void color_task_wake_up(struct task_struct *task)
 {
-        unsigned long           flags;
-        struct cpu_entry*       entry = task_entry(task);
-        lt_t                    now;
+        unsigned long flags;
+        struct cpu_entry* entry = task_entry(task);
+        raw_spinlock_t *lock;
+        lt_t now = litmus_clock();
+
+        TRACE_TASK(task, "Wake up at %llu\n", now);
+
+        local_irq_save(flags);
+
+        lock = is_be(task) ? &fifo_lock : entry->lock;
 
-        TRACE_TASK(task, "wake_up at %llu\n", litmus_clock());
-        raw_spin_lock_irqsave(entry->lock, flags);
-        BUG_ON(is_queued(task));
-        now = litmus_clock();
         if (is_tardy(task, now)) {
-                /* new sporadic release */
                 release_at(task, now);
                 sched_trace_task_release(task);
         }
 
-        if (entry->scheduled != task)
+        if (entry->scheduled != task) {
+                raw_spin_lock(lock);
                 requeue(task);
+                raw_spin_unlock(lock);
+        }
 
-        raw_spin_unlock_irqrestore(entry->lock, flags);
-        TRACE_TASK(task, "wake up done\n");
+        if (is_be(task))
+                check_for_fifo_preempt();
+
+        local_irq_restore(flags);
 }
 
 static void color_task_block(struct task_struct *t)
 {
-        /* only running tasks can block, thus t is in no queue */
-        TRACE_TASK(t, "block at %llu, state=%d\n", litmus_clock(), t->state);
-
+        TRACE_TASK(t, "Block at %llu, state=%d\n", litmus_clock(), t->state);
         BUG_ON(!is_realtime(t));
         BUG_ON(is_queued(t));
 }
@@ -433,8 +551,7 @@ static void color_task_exit(struct task_struct * t)
                         dom = task_edf(t);
                 remove(dom, t);
         }
-        if (entry->scheduled == t)
-                entry->scheduled = NULL;
+        unlink(t);
 
         TRACE_TASK(t, "RIP, now reschedule\n");
 
@@ -442,59 +559,81 @@ static void color_task_exit(struct task_struct * t)
         raw_spin_unlock_irqrestore(entry->lock, flags);
 }
 
-static long color_admit_task(struct task_struct* tsk)
+/*
+ * Non-be tasks must have migrated to the right CPU.
+ */
+static long color_admit_task(struct task_struct* t)
+{
+        return is_be(t) || task_cpu(t) == get_partition(t) ? 0 : -EINVAL;
+}
+
+static void color_tick(struct task_struct *t)
 {
-        return task_cpu(tsk) == tsk->rt_param.task_params.cpu ? 0 : -EINVAL;
+        struct cpu_entry *entry = local_entry;
+        BUG_ON(is_realtime(t) && t != entry->scheduled);
+        if (is_realtime(t) && budget_enforced(t) && budget_exhausted(t)) {
+                if (!is_np(t)) {
+                        litmus_reschedule_local();
+                        TRACE("color_scheduler_tick: "
+                              "%d is preemptable "
+                              " => FORCE_RESCHED\n", t->pid);
+                } else if (is_user_np(t)) {
+                        TRACE("color_scheduler_tick: "
+                              "%d is non-preemptable, "
+                              "preemption delayed.\n", t->pid);
+                        request_exit_np(t);
+                }
+        }
 }
 
+/*
+ * Load server parameters.
+ */
 static long color_activate_plugin(void)
 {
         int cpu, ret = 0;
-        char name[TASK_COMM_LEN];
         struct rt_task tp;
-        struct task_struct *server;
+        struct task_struct *server_task;
         struct cpu_entry *entry;
-
-        strcpy(name, "server");
+        lt_t now = litmus_clock();
 
         for_each_online_cpu(cpu) {
                 entry = remote_entry(cpu);
                 entry->scheduled = NULL;
 
-                server = kmalloc(sizeof(struct task_struct), GFP_ATOMIC);
-                memset(server, 0, sizeof(*server));
+                server_task = entry->server.task;
 
                 ret = color_server_params(cpu, &tp.exec_cost,
                                                &tp.period);
+
                 if (ret)
-                        goto out;
+                        continue;
 
                 /* Fill rt parameters */
                 tp.phase = 0;
                 tp.cpu = cpu;
                 tp.cls = RT_CLASS_SOFT;
                 tp.budget_policy = PRECISE_ENFORCEMENT;
-                server->rt_param.task_params = tp;
-
-                /* Fill linux parameters */
-                strcpy(server->comm, "server");
-                server->pid = cpu + num_online_cpus();
+                server_task->rt_param.task_params = tp;
 
-                /* Initialize rt_param data */
-                tsk_rt(server)->heap_node = bheap_node_alloc(GFP_ATOMIC);
-                tsk_rt(server)->rel_heap = release_heap_alloc(GFP_ATOMIC);
-                bheap_node_init(&tsk_rt(server)->heap_node, server);
-                tsk_rt(server)->server = 1;
+                release_at(server_task, now);
 
-                entry->server.task = server;
                 entry->server.start_time = 0;
+                entry->server.running = 1;
 
-                requeue(server);
+                TRACE_TASK(server_task,
+                           "Created server with wcet: %llu, period: %llu\n",
+                           tp.exec_cost, tp.period);
+
+                requeue(server_task);
         }
  out:
         return ret;
 }
 
+/*
+ * Mark servers for system exit.
+ */
 static long color_deactivate_plugin(void)
 {
         int cpu;
@@ -509,6 +648,9 @@ static long color_deactivate_plugin(void)
         return 0;
 }
 
+/*
+ * Dump container and server parameters for tracing.
+ */
 static void color_release_ts(lt_t time)
 {
         int cpu, fifo_cid;
@@ -525,10 +667,15 @@ static void color_release_ts(lt_t time)
         for_each_online_cpu(cpu) {
                 entry = remote_entry(cpu);
                 trace_litmus_container_param(cpu, cpu_name);
-                trace_litmus_server_param(cpu, cpu, 0, 0);
+                trace_litmus_server_param(cpu + 1, cpu, 0, 0);
 
                 server = entry->server.task;
                 BUG_ON(!server);
+
+                TRACE_TASK(server, "Server (%llu, %llu)\n",
+                           get_exec_cost(server),
+                           get_rt_period(server));
+
                 trace_litmus_server_param(server->pid, fifo_cid,
                                           get_exec_cost(server),
                                           get_rt_period(server));
@@ -556,6 +703,7 @@ static int __init init_color(void)
 {
         int cpu;
         struct cpu_entry *entry;
+        struct task_struct *server_task;
 
         for_each_online_cpu(cpu) {
                 entry = remote_entry(cpu);
@@ -565,7 +713,22 @@ static int __init init_color(void)
                 entry->scheduled = NULL;
                 entry->lock = &entry->edf_domain.ready_lock;
 
-                entry->server.task = NULL;
+                server_task = kmalloc(sizeof(struct task_struct), GFP_ATOMIC);
+                memset(server_task, 0, sizeof(*server_task));
+                entry->server.task = server_task;
+
+                /* Fill linux parameters */
+                server_task->policy = SCHED_LITMUS;
+                strcpy(server_task->comm, "server");
+                server_task->pid = cpu + num_online_cpus() + 1;
+
+                /* Initialize rt_param data */
+                tsk_rt(server_task)->heap_node = bheap_node_alloc(GFP_ATOMIC);
+                tsk_rt(server_task)->rel_heap = release_heap_alloc(GFP_ATOMIC);
+                bheap_node_init(&tsk_rt(server_task)->heap_node, server_task);
+                tsk_rt(server_task)->server = 1;
+                tsk_rt(server_task)->present = 1;
+
                 init_enforcement_timer(&entry->server.timer);
         }