sched_color: Refactored for locking

6 files changed, 359 insertions, 344 deletions

diff --git a/include/litmus/litmus.h b/include/litmus/litmus.h
index c587aa4b890c..8398a10171e0 100644
--- a/include/litmus/litmus.h
+++ b/include/litmus/litmus.h
@@ -59,7 +59,7 @@ void litmus_exit_task(struct task_struct *tsk);
 #define get_release(t)          (tsk_rt(t)->job_params.release)
 #define get_class(t)            (tsk_rt(t)->task_params.cls)
 #define get_task_domain(t)      (tsk_rt(t)->_domain)
-#define is_server(t)            (tsk_rt(t)->server)
+#define is_server(t)            (tsk_rt(t)->is_server)
 
 #define is_priority_boosted(t)  (tsk_rt(t)->priority_boosted)
 #define get_boost_start(t)      (tsk_rt(t)->boost_start_time)
diff --git a/include/litmus/rt_param.h b/include/litmus/rt_param.h
index 31e8b8fed2a1..4d3a14992e0c 100644
--- a/include/litmus/rt_param.h
+++ b/include/litmus/rt_param.h
@@ -76,6 +76,7 @@ struct _rt_domain;
 struct bheap_node;
 struct release_heap;
 struct domain;
+struct rt_server;
 
 struct rt_job {
         /* Time instant the the job was or will be released.  */
@@ -119,7 +120,7 @@ struct rt_param {
         /* is the task present? (true if it can be scheduled) */
         unsigned int            present:1;
 
-        unsigned int            server:1;
+        unsigned int            is_server:1;
 
 #ifdef CONFIG_LITMUS_LOCKING
         /* Is the task being priority-boosted by a locking protocol? */
@@ -136,6 +137,8 @@ struct rt_param {
         struct rt_event *event;
 #endif
 
+        struct rt_server *server;
+
         /* user controlled parameters */
         struct rt_task          task_params;
 
diff --git a/include/litmus/rt_server.h b/include/litmus/rt_server.h
new file mode 100644
index 000000000000..17517790a104
--- /dev/null
+++ b/include/litmus/rt_server.h
@@ -0,0 +1,39 @@
+#ifndef __RT_SERVER_H
+#define __RT_SERVER_H
+
+#include <linux/sched.h>
+#include <litmus/litmus.h>
+#include <litmus/rt_domain.h>
+
+struct rt_server;
+
+typedef int (*need_preempt_t)(rt_domain_t *rt, struct task_struct *t);
+typedef void (*server_update_t)(struct rt_server *srv);
+typedef void (*server_requeue_t)(struct rt_server *srv, struct task_struct *t);
+typedef struct task_struct* (*server_take_t)(struct rt_server *srv);
+
+struct rt_server {
+        int sid;
+        struct task_struct*     linked;
+        rt_domain_t*            domain;
+        int running;
+
+        /* Does this server have a higher-priority task */
+        need_preempt_t          need_preempt;
+        /* System state has changed, so should server */
+        server_update_t         update;
+        /* Requeue task in domain */
+        server_requeue_t        requeue;
+        /* Take next task from domain */
+        server_take_t           take;
+};
+
+void init_rt_server(struct rt_server *server,
+                    int sid, rt_domain_t *domain,
+                    need_preempt_t need_preempt,
+                    server_requeue_t requeue,
+                    server_update_t update,
+                    server_take_t take);
+
+
+#endif
diff --git a/litmus/Makefile b/litmus/Makefile
index 30345ad07aa7..9e58a9c78691 100644
--- a/litmus/Makefile
+++ b/litmus/Makefile
@@ -21,6 +21,7 @@ obj-y     = sched_plugin.o litmus.o \
             sched_gsn_edf.o \
             color_proc.o \
             sched_color.o \
+            rt_server.o \
             fifo_common.o
 
 
diff --git a/litmus/rt_server.c b/litmus/rt_server.c
new file mode 100644
index 000000000000..35f7d4dea079
--- /dev/null
+++ b/litmus/rt_server.c
@@ -0,0 +1,33 @@
+#include <litmus/rt_server.h>
+
+
+static struct task_struct* default_server_take(struct rt_server *srv)
+{
+        return __take_ready(srv->domain);
+}
+
+static void default_server_update(struct rt_server *srv)
+{
+}
+
+void init_rt_server(struct rt_server *server,
+                    int sid, rt_domain_t *domain,
+                    need_preempt_t need_preempt,
+                    server_requeue_t requeue,
+                    server_update_t update,
+                    server_take_t take)
+{
+        if (!need_preempt || !requeue)
+                BUG_ON(1);
+
+        server->need_preempt = need_preempt;
+        server->requeue = requeue;
+
+        server->update = (update) ? update : default_server_update;
+        server->take = (take) ? take : default_server_take;
+
+        server->sid = sid;
+        server->linked = NULL;
+        server->domain = domain;
+        server->running = 0;
+}
diff --git a/litmus/sched_color.c b/litmus/sched_color.c
index 697d57194d3a..02325db677ab 100644
--- a/litmus/sched_color.c
+++ b/litmus/sched_color.c
@@ -14,54 +14,48 @@
 #include <litmus/color.h>
 #include <litmus/fifo_common.h>
 #include <litmus/budget.h>
-
+#include <litmus/rt_server.h>
 
 struct fifo_server {
-        struct task_struct*        task;
-        struct task_struct*        linked;
-
-        struct enforcement_timer   timer;
-        lt_t                       start_time; /* Used for exec time */
-        int                        running; /* For tracing niceties */
+        struct rt_server                server;
+        struct task_struct*             task;
+        struct enforcement_timer        timer;
+        lt_t                            start_time;
 };
 
 struct cpu_entry {
-        int                     cpu;
+        int                     cpu;
+        struct rt_server        server;
         rt_domain_t             edf_domain;
-
-        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;
+        struct fifo_server      fifo_server;
 };
 
 DEFINE_PER_CPU(struct cpu_entry, color_cpus);
 static rt_domain_t fifo_domain;
 static raw_spinlock_t fifo_lock;
 
-#define local_edf             (&__get_cpu_var(color_cpus).edf_domain)
 #define local_entry           (&__get_cpu_var(color_cpus))
-#define remote_edf(cpu)       (&per_cpu(color_cpus, cpu).edf_domain)
+#define remote_edf(cpu)       (&per_cpu(psnedf_domains, cpu).domain)
 #define remote_entry(cpu)     (&per_cpu(color_cpus, cpu))
 #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)
+#define task_server(task)     (&task_entry(task)->fifo_server.server)
+#define run_server(task)      (tsk_rt(task)->server)
+#define entry_lock(entry)     (&entry->edf_domain.ready_lock)
+#define task_domain(entry, task) (is_be(task)? &fifo_domain :&entry->edf_domain)
+#define task_lock(entry, task) (is_be(task) ? &fifo_lock : entry_lock(entry))
 
 /*
- * Requeue task in appropriate domain
+ * Requeue onto domains release or ready queue based on task state.
  */
-static void requeue(struct task_struct* t)
+static void requeue(rt_domain_t *dom, struct task_struct* t)
 {
-        rt_domain_t *dom;
+        if (is_server(t) && !tsk_rt(t)->present)
+                return;
 
         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);
+        TRACE_TASK(t, "FIFO requeueing\n");
 
         set_rt_flags(t, RT_F_RUNNING);
         if (is_released(t, litmus_clock()))
@@ -71,115 +65,61 @@ static void requeue(struct task_struct* t)
 }
 
 /*
- * Logically begin server execution.
+ * Stop logically running the currently linked task.
  */
-static void stop_serving(struct fifo_server *server, struct task_struct *t)
+static void unlink(struct rt_server *server)
 {
-        int task_job, task_pid;
-        struct task_struct *stask = server->task;
+        struct rt_server *tserv;
 
-        BUG_ON(!server->running);
-
-        task_pid = (t) ? t->pid : 0;
-        task_job = (t) ? get_rt_job(t) : 0;
+        BUG_ON(!server->linked);
 
-        if (t)
-                tsk_rt(t)->linked_on = NO_CPU;
+        if (is_server(server->linked)) {
+                tserv = task_server(server->linked);
+                tserv->running = 0;
+        }
 
-        TRACE_TASK(stask, "No longer serving (%d:%d)\n", task_pid, task_job);
+        sched_trace_server_switch_away(server->sid, 0,
+                                       server->linked->pid,
+                                       get_rt_job(server->linked));
+        TRACE_TASK(server->linked, "No longer run by server %d\n", server->sid);
 
-        sched_trace_server_switch_away(stask->pid, get_rt_job(stask),
-                                       task_pid, task_job);
-        server->running = 0;
+        run_server(server->linked) = NULL;
+        server->linked = NULL;
 }
 
-/*
- * Logically stop server execution
- */
-static void start_serving(struct fifo_server *server, struct task_struct *t)
-{
-        int next_pid, next_job;
-        struct cpu_entry *entry;
-
-        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);
-
-        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);
-
-        server->running = 1;
-}
+static struct task_struct* schedule_server(struct rt_server *server);
 
 /*
- * Remove from "linked" fields and, if necessary, drop switch_away rcords.
+ * Logically run @task.
  */
-static void unlink(struct task_struct *t)
+static void link(struct rt_server *server, struct task_struct *task)
 {
-        struct cpu_entry *entry = task_entry(t);
-
-        TRACE_TASK(t, "Unlinking\n");
-
-        if (is_be(t) && tsk_rt(t)->linked_on != NO_CPU) {
-                entry = remote_entry(tsk_rt(t)->linked_on);
-                BUG_ON(entry->scheduled != t);
+        struct rt_server *tserv;
 
-                stop_serving(&entry->server, entry->server.linked);
+        BUG_ON(server->linked);
+        BUG_ON(!server->running);
 
-                entry->server.linked = NULL;
-                entry->scheduled = NULL;
+        if (is_server(task)) {
+                tserv = task_server(task);
+                tserv->running = 1;
+                schedule_server(tserv);
         }
 
-        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;
+        server->linked = task;
+        run_server(task) = server;
 
-        if (is_server(t) && entry->server.running) {
-                stop_serving(&entry->server, entry->server.linked);
-        }
+        sched_trace_server_switch_to(server->sid, 0,
+                                     task->pid, get_rt_job(task));
+        TRACE_TASK(task, "Run by server %d\n", server->sid);
 }
 
 /*
- * Unlink task. If task is a server running another task, unlink and requeue
- * the sub-task.
+ * Complete job for task linked to @server.
  */
-static void preempt(struct task_struct *t)
+static void job_completion(struct rt_server *server)
 {
-        struct fifo_server *server;
-
-        TRACE_TASK(t, "Preempting\n");
-
-        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");
-
-                        raw_spin_lock(&fifo_lock);
-
-                        requeue(server->linked);
-                        unlink(server->linked);
-
-                        raw_spin_unlock(&fifo_lock);
-                }
-        }
-
-        unlink(t);
-}
+        struct task_struct *t = server->linked;
 
-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));
@@ -187,7 +127,7 @@ static void job_completion(struct task_struct *t)
                 sched_trace_task_completion(t, 0);
 
 
-        preempt(t);
+        unlink(server);
         set_rt_flags(t, RT_F_SLEEP);
         prepare_for_next_period(t);
 
@@ -198,67 +138,121 @@ static void job_completion(struct task_struct *t)
                 sched_trace_task_release(t);
 
         if (is_running(t))
-                requeue(t);
+                server->requeue(server, t);
 }
 
 /*
- * Update state of task and entries to reflect blocks or completions.
+ * Update server state to reflect task state.
  */
-static void update_task(struct task_struct *t)
+static void update_task(struct rt_server *server)
 {
         int oot, sleep, block, np;
+        struct task_struct *t = server->linked;
 
         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);
+                unlink(server);
         else if ((oot || sleep) && !np)
-                job_completion(t);
+                job_completion(server);
+
 }
 
 /*
- * Update state of server and constituent tasks to reflect any blocking,
- * time elapsed, or job completions encountered.
+ * Link next task for @server.
  */
-static void update_server(struct fifo_server *server)
+static struct task_struct* schedule_server(struct rt_server *server)
 {
-        struct cpu_entry *entry;
-        struct task_struct *task;
-        unsigned long long delta;
+        struct task_struct *next;
+        struct rt_server *lserver;
 
-        entry = task_entry(server->task);
-        task  = server->task;
+        TRACE("Scheduling server %d\n", server->sid);
 
-        BUG_ON(!server->start_time);
-        BUG_ON(!server_running(entry));
+        if (server->linked) {
+                if (is_server(server->linked)) {
+                        lserver = task_server(server->linked);
+                        lserver->update(lserver);
+                }
 
-        cancel_enforcement_timer(&server->timer);
+                update_task(server);
+        }
 
-        delta = litmus_clock() - server->start_time;
-        tsk_rt(task)->job_params.exec_time += delta;
-        server->start_time = 0;
+        next = server->linked;
+        if (server->need_preempt(server->domain, next)) {
+                if (next) {
+                        TRACE_TASK(next, "Preempted\n");
+                        unlink(server);
+                        server->requeue(server, next);
+                }
+                next = __take_ready(server->domain);
+                link(server, next);
+        }
 
-        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 {
+        return next;
+}
 
-                TRACE_TASK(task, "Updating server, delta: %llu, exec: %llu\n",
-                           delta, get_exec_time(task));
+/*
+ * Dumb requeue for CPU servers.
+ */
+static void edf_requeue(struct rt_server *server, struct task_struct *t)
+{
+        requeue(server->domain, t);
+}
 
-                if (server->linked)
-                        update_task(server->linked);
-                update_task(server->task);
-        }
+/*
+ * Locking requeue for FIFO servers.
+ */
+static void fifo_requeue(struct rt_server *server, struct task_struct *t)
+{
+        raw_spin_lock(&fifo_lock);
+        requeue(server->domain, t);
+        raw_spin_unlock(&fifo_lock);
+}
+
+
+/*
+ * Locking take for FIFO servers.
+ */
+static struct task_struct* fifo_take(struct rt_server *server)
+{
+        struct task_struct *ret;
+
+        raw_spin_lock(&fifo_lock);
+        ret = __take_ready(server->domain);
+        raw_spin_unlock(&fifo_lock);
+
+        return ret;
+}
+
+/*
+ * Update server state, including picking next running task and incrementing
+ * server execution time.
+ */
+static void fifo_update(struct rt_server *server)
+{
+        lt_t delta;
+        struct fifo_server *fserver;
+
+        fserver = container_of(server, struct fifo_server, server);
+        TRACE_TASK(fserver->task, "Updating FIFO server\n");
+
+        BUG_ON(!server->running);
+        BUG_ON(!fserver->start_time);
+
+        delta = litmus_clock() - fserver->start_time;
+        tsk_rt(fserver->task)->job_params.exec_time += delta;
+        fserver->start_time = 0;
+
+        cancel_enforcement_timer(&fserver->timer);
+
+        schedule_server(&fserver->server);
 }
 
 /*
@@ -273,16 +267,16 @@ static void color_edf_release(rt_domain_t *edf, struct bheap *tasks)
                    "Released set of EDF tasks\n");
 
         entry = container_of(edf, struct cpu_entry, edf_domain);
-        raw_spin_lock_irqsave(entry->lock, flags);
+        raw_spin_lock_irqsave(entry_lock(entry), flags);
 
         __merge_ready(edf, tasks);
 
-        if (edf_preemption_needed(edf, entry->linked) &&
-            (!entry->linked || !is_kernel_np(entry->linked))) {
+        if (edf_preemption_needed(edf, entry->server.linked) &&
+            (!entry->server.linked || !is_kernel_np(entry->server.linked))) {
                 litmus_reschedule(entry->cpu);
         }
 
-        raw_spin_unlock_irqrestore(entry->lock, flags);
+        raw_spin_unlock_irqrestore(entry_lock(entry), flags);
 }
 
 /*
@@ -292,20 +286,26 @@ static void check_for_fifo_preempt(void)
 {
         int ret = 0, cpu;
         struct cpu_entry *entry;
+        struct rt_server *cpu_server, *fifo_server;
+
+        TRACE("Checking for FIFO preempt\n");
 
         for_each_online_cpu(cpu) {
                 entry = remote_entry(cpu);
+                cpu_server = &entry->server;
+                fifo_server = &entry->fifo_server.server;
 
-                raw_spin_lock(entry->lock);
+                raw_spin_lock(entry_lock(entry));
                 raw_spin_lock(&fifo_lock);
 
-                if (server_running(entry) && !entry->server.linked) {
+                if (cpu_server->linked && is_server(cpu_server->linked) &&
+                    !fifo_server->linked) {
                         litmus_reschedule(cpu);
                         ret = 1;
                 }
 
                 raw_spin_unlock(&fifo_lock);
-                raw_spin_unlock(entry->lock);
+                raw_spin_unlock(entry_lock(entry));
 
                 if (ret)
                         break;
@@ -318,7 +318,6 @@ static void color_fifo_release(rt_domain_t *dom, struct bheap *tasks)
 
         TRACE_TASK(bheap2task(bheap_peek(dom->order, tasks)),
                    "Released set of FIFO tasks\n");
-
         local_irq_save(flags);
 
         raw_spin_lock(&fifo_lock);
@@ -330,127 +329,46 @@ static void color_fifo_release(rt_domain_t *dom, struct bheap *tasks)
         local_irq_restore(flags);
 }
 
-/*
- * Run top-level EDF scheduler.
- */
-static struct task_struct* schedule_edf(struct cpu_entry *entry,
-                                        struct task_struct *prev)
-{
-        struct task_struct *next, *updated_link;
-
-        /* Update previously-running task */
-        if (prev) {
-                if (is_server(prev))
-                        update_server(task_server(prev));
-                else
-                        update_task(prev);
-        }
-
-        /* 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);
-        }
+#define cpu_empty(entry, run) \
+        (!(run) || (is_server(run) && !(entry)->fifo_server.server.linked))
 
-        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;
-}
-
-/*
- * Run bottom-level fifo scheduler on entry running a server.
- */
-static struct task_struct* schedule_fifo(struct cpu_entry *entry,
-                                         struct task_struct *prev_linked)
-{
-        struct task_struct *task, *prev_fifo, *next_fifo = NULL;
-        struct fifo_server *server = &entry->server;
-
-        BUG_ON(server->linked && prev_linked != server->task);
-        BUG_ON(!server_running(entry));
-
-        task = server->task;
-
-        /* Update previously-running task */
-        if (prev_linked == task && server->linked) {
-                update_task(server->linked);
-        }
-
-        /* 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;
-}
-
-/*
- * Schedule hierarchically.
- */
 static struct task_struct* color_schedule(struct task_struct *prev)
 {
         unsigned long flags;
         struct cpu_entry *entry = local_entry;
-        struct task_struct *next, *prev_linked = entry->linked;
+        struct task_struct *next, *plink = entry->server.linked;
 
         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));
 
-        /* Top level */
-        next = schedule_edf(entry, prev_linked);
+        raw_spin_lock_irqsave(entry_lock(entry), flags);
 
-        /* Bottom level */
-        if (next && is_server(next))
-                next = schedule_fifo(entry, prev_linked);
+        if (entry->scheduled && cpu_empty(entry, plink) && is_running(prev)) {
+                TRACE_TASK(prev, "Snuck in on new!\n");
+                requeue(task_domain(entry, prev), prev);
+        }
+
+        /* Pick next top-level task */
+        next = schedule_server(&entry->server);
+
+        /* Arm timer if needed */
+        if (next && is_server(next)) {
+                next = task_server(next)->linked;
+                arm_enforcement_timer(&entry->fifo_server.timer,
+                                      entry->fifo_server.task);
+                entry->fifo_server.start_time = litmus_clock();
+        }
+
+        if (prev)
+                tsk_rt(prev)->scheduled_on = NO_CPU;
+        if (next)
+                tsk_rt(next)->scheduled_on = entry->cpu;
 
         entry->scheduled = next;
         sched_state_task_picked();
 
-        raw_spin_unlock_irqrestore(entry->lock, flags);
+        raw_spin_unlock_irqrestore(entry_lock(entry), flags);
 
         return entry->scheduled;
 }
@@ -459,43 +377,31 @@ static void color_task_new(struct task_struct *t, int on_rq, int running)
 {
         struct cpu_entry* entry;
         unsigned long flags;
+        raw_spinlock_t *lock;
 
-        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));
-
+        TRACE_TASK(t, "New colored task\n");
         local_irq_save(flags);
 
-        release_at(t, litmus_clock());
-
-        if (is_be(t)) {
-                entry = local_entry;
-                raw_spin_lock(entry->lock);
-                raw_spin_lock(&fifo_lock);
+        entry = (is_be(t)) ? local_entry : task_entry(t);
+        lock  = task_lock(entry, t);
 
-                if (running) {
-                        BUG_ON(entry->scheduled);
-                        entry->scheduled = t;
-                } else {
-                        requeue(t);
-                }
+        release_at(t, litmus_clock());
 
-                raw_spin_unlock(&fifo_lock);
-                raw_spin_unlock(entry->lock);
+        raw_spin_lock(lock);
+        if (running) {
+                /* Already running on a CPU, update CPU state to match */
+                BUG_ON(entry->scheduled);
+                entry->scheduled = t;
+                tsk_rt(t)->scheduled_on = entry->cpu;
         } else {
-                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);
+                requeue(task_domain(entry, t), t);
         }
+        raw_spin_unlock(lock);
+
+        if (is_be(t))
+                check_for_fifo_preempt();
+        else
+                litmus_reschedule(entry->cpu);
 
         local_irq_restore(flags);
 }
@@ -504,15 +410,14 @@ static void color_task_wake_up(struct task_struct *task)
 {
         unsigned long flags;
         struct cpu_entry* entry = task_entry(task);
-        raw_spinlock_t *lock;
+        raw_spinlock_t *lock = task_lock(entry, task);
         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;
-
+        /* Abuse sporadic model */
         if (is_tardy(task, now)) {
                 release_at(task, now);
                 sched_trace_task_release(task);
@@ -520,12 +425,17 @@ static void color_task_wake_up(struct task_struct *task)
 
         if (entry->scheduled != task) {
                 raw_spin_lock(lock);
-                requeue(task);
+                requeue(task_domain(entry, task), task);
                 raw_spin_unlock(lock);
+        } else {
+                TRACE_TASK(task, "Is already scheduled on %d!\n",
+                           entry->scheduled);
         }
 
         if (is_be(task))
                 check_for_fifo_preempt();
+        else
+                litmus_reschedule(entry->cpu);
 
         local_irq_restore(flags);
 }
@@ -540,23 +450,38 @@ static void color_task_block(struct task_struct *t)
 static void color_task_exit(struct task_struct * t)
 {
         unsigned long flags;
-        struct cpu_entry* entry = task_entry(t);
-        rt_domain_t* dom;
+        struct cpu_entry *entry = task_entry(t);
+        raw_spinlock_t *lock = task_lock(entry, t);
+
+        TRACE_TASK(t, "RIP, now reschedule\n");
 
-        raw_spin_lock_irqsave(entry->lock, flags);
+        local_irq_save(flags);
+
+        /* Remove from scheduler consideration */
         if (is_queued(t)) {
-                if (is_be(t))
-                        dom = &fifo_domain;
-                else
-                        dom = task_edf(t);
-                remove(dom, t);
+                raw_spin_lock(lock);
+                remove(task_domain(entry, t), t);
+                raw_spin_unlock(lock);
         }
-        unlink(t);
 
-        TRACE_TASK(t, "RIP, now reschedule\n");
+        /* Stop parent server */
+        if (run_server(t))
+                unlink(run_server(t));
+
+        /* Unschedule running CPU */
+        if (tsk_rt(t)->scheduled_on != NO_CPU) {
+                entry = remote_entry(tsk_rt(t)->scheduled_on);
 
-        preempt_if_preemptable(entry->scheduled, entry->cpu);
-        raw_spin_unlock_irqrestore(entry->lock, flags);
+                raw_spin_lock(entry_lock(entry));
+
+                tsk_rt(t)->scheduled_on = NO_CPU;
+                entry->scheduled = NULL;
+                litmus_reschedule(entry->cpu);
+
+                raw_spin_unlock(entry_lock(entry));
+        }
+
+        local_irq_restore(flags);
 }
 
 /*
@@ -599,40 +524,45 @@ static long color_activate_plugin(void)
 
         for_each_online_cpu(cpu) {
                 entry = remote_entry(cpu);
-                entry->scheduled = NULL;
+                server_task = entry->fifo_server.task;
 
-                server_task = entry->server.task;
+                raw_spin_lock(entry_lock(entry));
 
                 ret = color_server_params(cpu, &tp.exec_cost,
                                                &tp.period);
-
-                if (ret)
-                        continue;
+                if (ret) {
+                        printk(KERN_WARNING "Uninitialized server for CPU %d\n",
+                               entry->cpu);
+                        goto loop_end;
+                }
 
                 /* Fill rt parameters */
                 tp.phase = 0;
                 tp.cpu = cpu;
                 tp.cls = RT_CLASS_SOFT;
                 tp.budget_policy = PRECISE_ENFORCEMENT;
-                server_task->rt_param.task_params = tp;
+                tsk_rt(server_task)->task_params = tp;
+                tsk_rt(server_task)->present = 1;
 
+                /* Make runnable */
                 release_at(server_task, now);
+                requeue(&entry->edf_domain, server_task);
 
-                entry->server.start_time = 0;
-                entry->server.running = 1;
+                entry->fifo_server.start_time = 0;
+                entry->scheduled = NULL;
 
-                TRACE_TASK(server_task,
-                           "Created server with wcet: %llu, period: %llu\n",
-                           tp.exec_cost, tp.period);
+                TRACE_TASK(server_task, "Created server with wcet: %llu, "
+                           "period: %llu\n", tp.exec_cost, tp.period);
 
-                requeue(server_task);
+        loop_end:
+                raw_spin_unlock(entry_lock(entry));
         }
- out:
+
         return ret;
 }
 
 /*
- * Mark servers for system exit.
+ * Mark servers as unused, making future calls to requeue fail.
  */
 static long color_deactivate_plugin(void)
 {
@@ -641,8 +571,8 @@ static long color_deactivate_plugin(void)
 
         for_each_online_cpu(cpu) {
                 entry = remote_entry(cpu);
-                if (entry->server.task) {
-                        tsk_rt(entry->server.task)->present = 0;
+                if (entry->fifo_server.task) {
+                        tsk_rt(entry->fifo_server.task)->present = 0;
                 }
         }
         return 0;
@@ -656,7 +586,7 @@ static void color_release_ts(lt_t time)
         int cpu, fifo_cid;
         char fifo_name[TASK_COMM_LEN], cpu_name[TASK_COMM_LEN];
         struct cpu_entry *entry;
-        struct task_struct *server;
+        struct task_struct *stask;
 
         strcpy(cpu_name, "CPU");
         strcpy(fifo_name, "BE");
@@ -667,18 +597,11 @@ 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 + 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));
+                trace_litmus_server_param(entry->server.sid, cpu, 0, 0);
+                stask = entry->fifo_server.task;
+                trace_litmus_server_param(stask->pid, fifo_cid,
+                                          get_exec_cost(stask),
+                                          get_rt_period(stask));
         }
 }
 
@@ -704,6 +627,8 @@ static int __init init_color(void)
         int cpu;
         struct cpu_entry *entry;
         struct task_struct *server_task;
+        struct fifo_server *fifo_server;
+        struct rt_server *cpu_server;
 
         for_each_online_cpu(cpu) {
                 entry = remote_entry(cpu);
@@ -711,25 +636,39 @@ static int __init init_color(void)
 
                 entry->cpu = cpu;
                 entry->scheduled = NULL;
-                entry->lock = &entry->edf_domain.ready_lock;
 
+                /* Create FIFO server */
+                fifo_server = &entry->fifo_server;
+                init_rt_server(&fifo_server->server,
+                               cpu + num_online_cpus() + 1,
+                               &fifo_domain,
+                               fifo_preemption_needed,
+                               fifo_requeue, fifo_update, fifo_take);
+
+
+                /* Create task struct for FIFO server */
                 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;
+                server_task->pid = fifo_server->server.sid;
+                fifo_server->task = server_task;
 
-                /* Initialize rt_param data */
+                /* Create rt_params for FIFO server */
                 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;
+                tsk_rt(server_task)->is_server = 1;
+
+
+                /* Create CPU server */
+                cpu_server = &entry->server;
+                init_rt_server(cpu_server, cpu + 1,
+                               &entry->edf_domain, edf_preemption_needed,
+                               edf_requeue, NULL, NULL);
+                cpu_server->running = 1;
 
-                init_enforcement_timer(&entry->server.timer);
+                init_enforcement_timer(&fifo_server->timer);
         }
 
         fifo_domain_init(&fifo_domain, NULL, color_fifo_release);