sched_color: added support for group locks.

This does not add support for telling the plugin _which_ resources you will
acquire. Group locks were tested by setting random resource requests in
task_new().

10 files changed, 479 insertions, 103 deletions

diff --git a/include/litmus/dgl.h b/include/litmus/dgl.h
new file mode 100644
index 000000000000..989f6c7d5e8e
--- /dev/null
+++ b/include/litmus/dgl.h
@@ -0,0 +1,53 @@
+#ifndef __DGL_H_
+#define __DGL_H_
+
+#include <linux/list.h>
+
+#define NUM_REPLICAS  4
+#define NUM_RESOURCES 4
+#define MASK_SIZE     (sizeof(unsigned long) * 8)
+#define MASK_WORDS    (NUM_RESOURCES/MASK_SIZE + (NUM_RESOURCES%MASK_SIZE != 0))
+
+/*
+ * A request for @replica amount of a single resource.
+ */
+struct dgl_req {
+        unsigned short          replicas;
+        struct list_head        list;
+};
+
+/*
+ * Simultaneous @requests for multiple resources.
+ */
+struct dgl_group_req {
+        int                     cpu;
+        unsigned long           requested[MASK_WORDS];
+        unsigned long           waiting[MASK_WORDS];
+        struct dgl_req          requests[NUM_RESOURCES];
+};
+
+/*
+ * A single resource.
+ */
+struct dgl_resource {
+        unsigned int            free_replicas;
+        struct list_head        waiting;
+};
+
+/*
+ * A group of resources.
+ */
+struct dgl {
+        struct dgl_resource     resources[NUM_RESOURCES];
+        struct dgl_group_req*   acquired[NR_CPUS];
+};
+
+void dgl_init(struct dgl *dgl);
+void dgl_group_req_init(struct dgl_group_req *greq);
+
+void set_req(struct dgl_group_req *greq, int resource, int replicas);
+
+void add_group_req(struct dgl *dgl, struct dgl_group_req *greq, int cpu);
+void remove_group_req(struct dgl *dgl, struct dgl_group_req *greq);
+
+#endif
diff --git a/include/litmus/litmus.h b/include/litmus/litmus.h
index 8398a10171e0..7188edd3a6cf 100644
--- a/include/litmus/litmus.h
+++ b/include/litmus/litmus.h
@@ -60,6 +60,7 @@ void litmus_exit_task(struct task_struct *tsk);
 #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)->is_server)
+#define get_task_server(task)      (tsk_rt(task)->server)
 
 #define is_priority_boosted(t)  (tsk_rt(t)->priority_boosted)
 #define get_boost_start(t)      (tsk_rt(t)->boost_start_time)
@@ -216,10 +217,6 @@ static inline int is_np(struct task_struct *t)
         int kernel, user;
         kernel = is_kernel_np(t);
         user   = is_user_np(t);
-        if (kernel || user)
-                TRACE_TASK(t, " is non-preemptive: kernel=%d user=%d\n",
-
-                           kernel, user);
         return kernel || user;
 #else
         return unlikely(is_kernel_np(t) || is_user_np(t));
diff --git a/include/litmus/rt_param.h b/include/litmus/rt_param.h
index 4d3a14992e0c..e56aab006cc1 100644
--- a/include/litmus/rt_param.h
+++ b/include/litmus/rt_param.h
@@ -5,6 +5,8 @@
 #ifndef _LINUX_RT_PARAM_H_
 #define _LINUX_RT_PARAM_H_
 
+#include <litmus/dgl.h>
+
 /* Litmus time type. */
 typedef unsigned long long lt_t;
 
@@ -66,7 +68,7 @@ struct control_page {
         /* locking overhead tracing: time stamp prior to system call */
         uint64_t ts_syscall_start; /* Feather-Trace cycles */
 
-        /* to be extended */
+        int requests[NUM_RESOURCES];
 };
 
 /* don't export internal data structures to user space (liblitmus) */
@@ -77,6 +79,7 @@ struct bheap_node;
 struct release_heap;
 struct domain;
 struct rt_server;
+struct dgl_group_req;
 
 struct rt_job {
         /* Time instant the the job was or will be released.  */
@@ -218,6 +221,8 @@ struct rt_param {
 
         /* Pointer to the page shared between userspace and kernel. */
         struct control_page * ctrl_page;
+
+        struct dgl_group_req *req;
 };
 
 /*      Possible RT flags       */
diff --git a/include/litmus/rt_server.h b/include/litmus/rt_server.h
index 17517790a104..0f3147707a3b 100644
--- a/include/litmus/rt_server.h
+++ b/include/litmus/rt_server.h
@@ -14,11 +14,12 @@ typedef struct task_struct* (*server_take_t)(struct rt_server *srv);
 
 struct rt_server {
         int sid;
+        int cpu;
         struct task_struct*     linked;
         rt_domain_t*            domain;
         int running;
 
-        /* Does this server have a higher-priority task */
+        /* Does this server have a higher-priority task? */
         need_preempt_t          need_preempt;
         /* System state has changed, so should server */
         server_update_t         update;
@@ -29,11 +30,10 @@ struct rt_server {
 };
 
 void init_rt_server(struct rt_server *server,
-                    int sid, rt_domain_t *domain,
+                    int sid, int cpu, 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 9e58a9c78691..f1f92fa8d6d3 100644
--- a/litmus/Makefile
+++ b/litmus/Makefile
@@ -22,6 +22,7 @@ obj-y     = sched_plugin.o litmus.o \
             color_proc.o \
             sched_color.o \
             rt_server.o \
+            dgl.o \
             fifo_common.o
 
 
diff --git a/litmus/budget.c b/litmus/budget.c
index 0a7bd665f814..142ccd9e0b52 100644
--- a/litmus/budget.c
+++ b/litmus/budget.c
@@ -61,7 +61,7 @@ void arm_enforcement_timer(struct enforcement_timer* et,
         /* __hrtimer_start_range_ns() cancels the timer
          * anyway, so we don't have to check whether it is still armed */
 
-        if (likely(!is_np(t))) {
+        if (likely(!is_user_np(t))) {
                 when_to_fire = litmus_clock() + budget_remaining(t);
                 TRACE_TASK(t, "arming enforcement timer for %llu.\n",
                            when_to_fire);
diff --git a/litmus/color_proc.c b/litmus/color_proc.c
index 25915e714821..9a104a803cac 100644
--- a/litmus/color_proc.c
+++ b/litmus/color_proc.c
@@ -2,13 +2,13 @@
 #include <linux/sysctl.h>
 #include <linux/slab.h>
 
+#include <litmus/color.h>
 #include <litmus/sched_trace.h>
 
-
-#define SPERIOD_LEN 7
+#define SPERIOD_LEN  7
 #define SPERIOD_FILE "period"
-#define SWCET_LEN 5
-#define SWCET_FILE "wcet"
+#define SWCET_LEN    5
+#define SWCET_FILE   "wcet"
 
 static unsigned long *server_wcet;
 static unsigned long *server_period;
diff --git a/litmus/dgl.c b/litmus/dgl.c
new file mode 100644
index 000000000000..9749597aa769
--- /dev/null
+++ b/litmus/dgl.c
@@ -0,0 +1,197 @@
+#include <linux/sched.h>
+#include <litmus/litmus.h>
+#include <litmus/dgl.h>
+#include <litmus/sched_trace.h>
+
+/* Word, bit -> resource id */
+#define ri(w, b) (w * MASK_SIZE + b)
+
+ /* For loop, where @i iterates over each set bit in @bit_arr */
+#define for_each_resource(bit_arr, w, b, i)                                    \
+         for(w = 0; w < MASK_WORDS; ++w)                                       \
+                 for(b = find_first_bit(&bit_arr[w],MASK_SIZE), i = ri(w, b);  \
+                     b < MASK_SIZE;                                            \
+                     b = find_next_bit(&bit_arr[w],MASK_SIZE,b+1), i = ri(w, b))
+
+/* Return resource id in dgl @d for resource @r */
+#define resource_id(d, r) ((((void*)r) - (void*)(&(d)->resources))/ sizeof(*r))
+
+/* Return request group of req @r for resource @i */
+#define req_group(r, i) (container_of(((void*)r) - sizeof(*r)*(i),      \
+                                      struct dgl_group_req, requests))
+
+/* Resource id -> word, bit */
+static inline void mask_idx(int resource, int *word, int *bit)
+{
+        *word = resource / MASK_SIZE;
+        *bit  = resource % MASK_SIZE;
+}
+
+void dgl_init(struct dgl *dgl)
+{
+        int i;
+        struct dgl_resource *resource;
+
+        for (i = 0; i < NR_CPUS; i++)
+                dgl->acquired[i] = NULL;
+
+        for (i = 0; i < NUM_RESOURCES; i++) {
+                resource = &dgl->resources[i];
+
+                INIT_LIST_HEAD(&resource->waiting);
+                resource->free_replicas = NUM_REPLICAS;
+        }
+}
+
+void dgl_group_req_init(struct dgl_group_req *greq)
+{
+        int i;
+        greq->cpu = NO_CPU;
+        for (i = 0; i < MASK_WORDS; i++) {
+                greq->requested[i] = 0;
+                greq->waiting[i]   = 0;
+        }
+}
+
+/**
+ * set_req - create request for @replicas of @resource.
+ */
+void set_req(struct dgl_group_req *greq, int resource, int replicas)
+{
+        int word, bit;
+        struct dgl_req *req;
+
+        BUG_ON(replicas > NUM_REPLICAS);
+
+        mask_idx(resource, &word, &bit);
+        TRACE("0x%p will request resource %d, word %d, bit %d\n",
+              greq, resource, word, bit);
+
+        __set_bit(bit, &greq->requested[word]);
+
+        req = &greq->requests[resource];
+        INIT_LIST_HEAD(&req->list);
+        req->replicas = replicas;
+}
+
+/*
+ * Attempt to fulfill request @req for @resource.
+ * Return 1 if successful. If the matching group request has acquired all of
+ * its needed resources, this will then set that req as dgl->acquired[cpu].
+ */
+static unsigned long try_acquire(struct dgl *dgl, struct dgl_resource *resource,
+                                 struct dgl_req *req)
+{
+        int word, bit, rid;
+        unsigned long waiting;
+        struct dgl_group_req *greq;
+
+        if (resource->free_replicas < req->replicas) {
+                TRACE("0x%p cannot acquire %d replicas, only %d free\n",
+                      greq, req->replicas, resource->free_replicas);
+                return 0;
+        }
+
+        resource->free_replicas -= req->replicas;
+
+        rid  = resource_id(dgl, resource);
+        greq = req_group(req, rid);
+        mask_idx(rid, &word, &bit);
+
+        TRACE("0x%p acquired rid %d, word %d, bit %d\n",
+              greq, rid, word, bit);
+
+        clear_bit(bit, &greq->waiting[word]);
+
+        waiting = 0;
+        for (word = 0; word < MASK_WORDS; word++) {
+                waiting |= greq->waiting[word];
+                if (waiting)
+                        break;
+        }
+
+        if (!waiting) {
+                TRACE("0x%p acquired all resources\n", greq);
+                BUG_ON(dgl->acquired[greq->cpu]);
+                dgl->acquired[greq->cpu] = greq;
+                litmus_reschedule(greq->cpu);
+        }
+
+        return 1;
+}
+
+/**
+ * add_group_req - initiate group request.
+ */
+void add_group_req(struct dgl *dgl, struct dgl_group_req *greq, int cpu)
+{
+        int b, w, i, succ, all_succ = 1;
+        struct dgl_req *req;
+        struct dgl_resource *resource;
+
+        greq->cpu = cpu;
+
+        TRACE("0x%p group request added for CPU %d\n", greq, cpu);
+        BUG_ON(dgl->acquired[cpu] == greq);
+
+        for_each_resource(greq->requested, w, b, i) {
+                __set_bit(b, &greq->waiting[w]);
+        }
+
+        for_each_resource(greq->requested, w, b, i) {
+                req = &greq->requests[i];
+                resource = &dgl->resources[i];
+
+                succ = try_acquire(dgl, resource, req);
+                all_succ &= succ;
+
+                if (!succ) {
+                        TRACE("0x%p waiting on resource %d\n", greq, i);
+                        list_add_tail(&req->list, &resource->waiting);
+                }
+        }
+}
+
+/**
+ * remove_group_req - abandon group request.
+ *
+ * This will also progress the waiting queues of resources acquired by @greq.
+ */
+void remove_group_req(struct dgl *dgl, struct dgl_group_req *greq)
+{
+        int b, w, i;
+        struct dgl_req *req, *next;
+        struct dgl_resource *resource;
+
+        TRACE("0x%p removing group request for CPU %d\n", greq, greq->cpu);
+
+        if (dgl->acquired[greq->cpu] == greq) {
+                TRACE("0x%p no longer acquired on CPU %d\n", greq, greq->cpu);
+                dgl->acquired[greq->cpu] = NULL;
+        }
+
+        for_each_resource(greq->requested, w, b, i) {
+                req = &greq->requests[i];
+                resource = &dgl->resources[i];
+
+                if (!list_empty(&req->list)) {
+                        /* Waiting on resource */
+                        clear_bit(b, &greq->waiting[w]);
+                        list_del_init(&req->list);
+                        TRACE("0x%p quit waiting for resource %d\n", greq, i);
+                } else {
+                        /* Have resource */
+                        resource->free_replicas += req->replicas;
+                        TRACE("0x%p releasing resource %d\n", greq, i);
+
+                        if (!list_empty(&resource->waiting)) {
+                                /* Give it to the next guy */
+                                next = list_first_entry(&resource->waiting,
+                                                        struct dgl_req,
+                                                        list);
+                                if (try_acquire(dgl, resource, next))
+                                        list_del_init(&next->list);
+                        }
+                }
+        }
+}
diff --git a/litmus/rt_server.c b/litmus/rt_server.c
index 35f7d4dea079..818588a3d317 100644
--- a/litmus/rt_server.c
+++ b/litmus/rt_server.c
@@ -11,7 +11,7 @@ static void default_server_update(struct rt_server *srv)
 }
 
 void init_rt_server(struct rt_server *server,
-                    int sid, rt_domain_t *domain,
+                    int sid, int cpu, rt_domain_t *domain,
                     need_preempt_t need_preempt,
                     server_requeue_t requeue,
                     server_update_t update,
@@ -27,6 +27,7 @@ void init_rt_server(struct rt_server *server,
         server->take = (take) ? take : default_server_take;
 
         server->sid = sid;
+        server->cpu = cpu;
         server->linked = NULL;
         server->domain = domain;
         server->running = 0;
diff --git a/litmus/sched_color.c b/litmus/sched_color.c
index 02325db677ab..61a28da1ef6c 100644
--- a/litmus/sched_color.c
+++ b/litmus/sched_color.c
@@ -15,7 +15,14 @@
 #include <litmus/fifo_common.h>
 #include <litmus/budget.h>
 #include <litmus/rt_server.h>
+#include <litmus/dgl.h>
 
+/**
+ * @rt_server    Common server functionality.
+ * @task         Task used to schedule server.
+ * @timer        Budget enforcement for @task
+ * @start_time   If set, time at which server began running.
+ */
 struct fifo_server {
         struct rt_server                server;
         struct task_struct*             task;
@@ -23,39 +30,48 @@ struct fifo_server {
         lt_t                            start_time;
 };
 
+/**
+ * @server        Common server functionality.
+ * @edf_domain    PEDF domain.
+ * @scheduled     Task physically running on CPU.
+ * @fifo_server   Server partitioned to this CPU.
+ */
 struct cpu_entry {
-        int                     cpu;
         struct rt_server        server;
         rt_domain_t             edf_domain;
-        struct task_struct*     scheduled; /* Actually running, EDF or FIFO */
+        struct task_struct*     scheduled;
         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_entry           (&__get_cpu_var(color_cpus))
-#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 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))
+static struct dgl group_lock;
+static raw_spinlock_t dgl_lock;
+
+#define local_entry            (&__get_cpu_var(color_cpus))
+#define remote_entry(cpu)      (&per_cpu(color_cpus, cpu))
+#define task_entry(task)       remote_entry(get_partition(task))
+#define task_fserver(task)     (&task_entry(task)->fifo_server.server)
+#define entry_lock(entry)      (&entry->edf_domain.ready_lock)
+
+#define has_resources(t, c)    (tsk_rt(t)->req == group_lock.acquired[c])
+#define task_dom(entry, task)  (is_be(task) ? &fifo_domain : &entry->edf_domain)
+#define task_lock(entry, task) (is_be(task) ? &fifo_lock   : entry_lock(entry))
 
 /*
- * Requeue onto domains release or ready queue based on task state.
+ * Requeue onto domain's release or ready queue based on task state.
  */
 static void requeue(rt_domain_t *dom, struct task_struct* t)
 {
         if (is_server(t) && !tsk_rt(t)->present)
+                /* Remove stopped server from the system */
                 return;
 
+        TRACE_TASK(t, "Requeueing\n");
         BUG_ON(is_queued(t));
-        TRACE_TASK(t, "FIFO requeueing\n");
 
         set_rt_flags(t, RT_F_RUNNING);
         if (is_released(t, litmus_clock()))
@@ -65,25 +81,89 @@ static void requeue(rt_domain_t *dom, struct task_struct* t)
 }
 
 /*
+ * Relinquish resources held by @t (or its children).
+ */
+static void release_resources(struct task_struct *t)
+{
+        struct task_struct *sched;
+
+        TRACE_TASK(t, "Releasing resources\n");
+
+        if (is_server(t)) {
+                sched = task_fserver(t)->linked;
+                if (sched)
+                        release_resources(sched);
+        } else if (is_kernel_np(t))
+                remove_group_req(&group_lock, tsk_rt(t)->req);
+        tsk_rt(t)->kernel_np = 0;
+}
+
+/*
+ * Put in requests for resources needed by @t. If @t is a server, this will
+ * set @t's np flag to reflect resources held by @t's children.
+ */
+static void acquire_resources(struct task_struct *t)
+{
+        int cpu;
+        struct rt_server *server;
+        struct task_struct *sched;
+
+        /* Can't acquire resources if t is not running */
+        BUG_ON(!get_task_server(t));
+
+        if (is_kernel_np(t)) {
+                TRACE_TASK(t, "Already contending for resources\n");
+                return;
+        }
+        cpu = get_task_server(t)->cpu;
+
+        if (is_server(t)) {
+                server = task_fserver(t);
+                sched  = server->linked;
+
+                /* Happens when server is booted off on completion or
+                 * has just completed executing a task.
+                 */
+                if (sched && !is_kernel_np(sched))
+                        acquire_resources(sched);
+
+                /* Become np if there is a running task */
+                if (sched && has_resources(sched, cpu)) {
+                        TRACE_TASK(t, "Running task with resource\n");
+                        tsk_rt(t)->kernel_np = 1;
+                } else {
+                        TRACE_TASK(t, "Running no resources\n");
+                        tsk_rt(t)->kernel_np = 0;
+                }
+        } else {
+                TRACE_TASK(t, "Acquiring resources\n");
+                if (!has_resources(t, cpu))
+                        add_group_req(&group_lock, tsk_rt(t)->req, cpu);
+                tsk_rt(t)->kernel_np = 1;
+        }
+}
+
+/*
  * Stop logically running the currently linked task.
  */
 static void unlink(struct rt_server *server)
 {
-        struct rt_server *tserv;
-
         BUG_ON(!server->linked);
 
-        if (is_server(server->linked)) {
-                tserv = task_server(server->linked);
-                tserv->running = 0;
-        }
+        if (is_server(server->linked))
+                task_fserver(server->linked)->running = 0;
+
 
         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);
 
-        run_server(server->linked) = NULL;
+        raw_spin_lock(&dgl_lock);
+        release_resources(server->linked);
+        raw_spin_unlock(&dgl_lock);
+
+        get_task_server(server->linked) = NULL;
         server->linked = NULL;
 }
 
@@ -98,19 +178,21 @@ static void link(struct rt_server *server, struct task_struct *task)
 
         BUG_ON(server->linked);
         BUG_ON(!server->running);
+        BUG_ON(is_kernel_np(task));
+
+        TRACE_TASK(task, "Run by server %d\n", server->sid);
 
         if (is_server(task)) {
-                tserv = task_server(task);
+                tserv = task_fserver(task);
                 tserv->running = 1;
                 schedule_server(tserv);
         }
 
         server->linked = task;
-        run_server(task) = server;
+        get_task_server(task) = server;
 
         sched_trace_server_switch_to(server->sid, 0,
                                      task->pid, get_rt_job(task));
-        TRACE_TASK(task, "Run by server %d\n", server->sid);
 }
 
 /*
@@ -126,14 +208,13 @@ static void job_completion(struct rt_server *server)
         else
                 sched_trace_task_completion(t, 0);
 
-
         unlink(server);
         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));
+                                           get_release(t), get_deadline(t));
         else
                 sched_trace_task_release(t);
 
@@ -142,7 +223,7 @@ static void job_completion(struct rt_server *server)
 }
 
 /*
- * Update server state to reflect task state.
+ * Update @server state to reflect task's state.
  */
 static void update_task(struct rt_server *server)
 {
@@ -151,18 +232,16 @@ static void update_task(struct rt_server *server)
 
         block = !is_running(t);
         oot   = budget_enforced(t) && budget_exhausted(t);
-        np    = is_np(t);
+        np    = is_kernel_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)
                 unlink(server);
-        else if ((oot || sleep) && !np)
+        else if (oot || sleep)
                 job_completion(server);
-
 }
 
 /*
@@ -177,15 +256,15 @@ static struct task_struct* schedule_server(struct rt_server *server)
 
         if (server->linked) {
                 if (is_server(server->linked)) {
-                        lserver = task_server(server->linked);
+                        lserver = task_fserver(server->linked);
                         lserver->update(lserver);
                 }
-
                 update_task(server);
         }
 
         next = server->linked;
-        if (server->need_preempt(server->domain, next)) {
+        if ((!next || !is_np(next)) &&
+             server->need_preempt(server->domain, next)) {
                 if (next) {
                         TRACE_TASK(next, "Preempted\n");
                         unlink(server);
@@ -199,10 +278,11 @@ static struct task_struct* schedule_server(struct rt_server *server)
 }
 
 /*
- * Dumb requeue for CPU servers.
+ * Dumb requeue for PEDF (CPU) servers.
  */
 static void edf_requeue(struct rt_server *server, struct task_struct *t)
 {
+        BUG_ON(is_be(t));
         requeue(server->domain, t);
 }
 
@@ -211,6 +291,7 @@ static void edf_requeue(struct rt_server *server, struct task_struct *t)
  */
 static void fifo_requeue(struct rt_server *server, struct task_struct *t)
 {
+        BUG_ON(!is_be(t));
         raw_spin_lock(&fifo_lock);
         requeue(server->domain, t);
         raw_spin_unlock(&fifo_lock);
@@ -243,16 +324,27 @@ static void fifo_update(struct rt_server *server)
         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;
+        if (!server->linked || has_resources(server->linked, server->cpu)) {
+                /* Running here means linked to a parent server */
+                BUG_ON(!server->running);
+
+                /* Stop executing */
+                if (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);
+                } else {
+                        /* Server is linked, but not executing */
+                        BUG_ON(fserver->timer.armed);
+                }
 
-        cancel_enforcement_timer(&fserver->timer);
+                /* Calculate next task */
+                schedule_server(&fserver->server);
 
-        schedule_server(&fserver->server);
+                /* Reserve needed resources */
+                acquire_resources(fserver->task);
+        }
 }
 
 /*
@@ -273,7 +365,7 @@ static void color_edf_release(rt_domain_t *edf, struct bheap *tasks)
 
         if (edf_preemption_needed(edf, entry->server.linked) &&
             (!entry->server.linked || !is_kernel_np(entry->server.linked))) {
-                litmus_reschedule(entry->cpu);
+                litmus_reschedule(entry->server.cpu);
         }
 
         raw_spin_unlock_irqrestore(entry_lock(entry), flags);
@@ -335,10 +427,11 @@ static void color_fifo_release(rt_domain_t *dom, struct bheap *tasks)
 static struct task_struct* color_schedule(struct task_struct *prev)
 {
         unsigned long flags;
+        int server_running;
         struct cpu_entry *entry = local_entry;
         struct task_struct *next, *plink = entry->server.linked;
 
-        TRACE("Reschedule on %d at %llu\n", entry->cpu, litmus_clock());
+        TRACE("Reschedule on %d at %llu\n", entry->server.cpu, litmus_clock());
         BUG_ON(entry->scheduled && entry->scheduled != prev);
         BUG_ON(entry->scheduled && !is_realtime(prev));
 
@@ -346,15 +439,38 @@ static struct task_struct* color_schedule(struct task_struct *prev)
 
         if (entry->scheduled && cpu_empty(entry, plink) && is_running(prev)) {
                 TRACE_TASK(prev, "Snuck in on new!\n");
-                requeue(task_domain(entry, prev), prev);
+                requeue(task_dom(entry, prev), prev);
         }
 
         /* Pick next top-level task */
         next = schedule_server(&entry->server);
+        /* Schedule hierarchically */
+        server_running = next && is_server(next);
+        if (server_running)
+                next = task_fserver(next)->linked;
+
+        /* Selected tasks must contend for group lock */
+        if (next) {
+                raw_spin_lock(&dgl_lock);
+                acquire_resources(next);
+                if (has_resources(next, entry->server.cpu)) {
+                        TRACE_TASK(next, "Has group lock\n");
+                        sched_trace_task_resume(next, 1);
+                } else {
+                        TRACE_TASK(next, "Does not have lock, 0x%p does\n",
+                                   group_lock.acquired[entry->server.cpu]);
+                        sched_trace_task_block(next, 1);
+                        next = NULL;
+                        server_running = 0;
+                }
+                raw_spin_unlock(&dgl_lock);
+        }
 
-        /* Arm timer if needed */
-        if (next && is_server(next)) {
-                next = task_server(next)->linked;
+        /* Server is blocked if its running task is blocked. Note that if the
+         * server has no running task, the server will now execute NULL.
+         */
+        if (server_running) {
+                TRACE_TASK(entry->server.linked, "Server running\n");
                 arm_enforcement_timer(&entry->fifo_server.timer,
                                       entry->fifo_server.task);
                 entry->fifo_server.start_time = litmus_clock();
@@ -363,7 +479,7 @@ static struct task_struct* color_schedule(struct task_struct *prev)
         if (prev)
                 tsk_rt(prev)->scheduled_on = NO_CPU;
         if (next)
-                tsk_rt(next)->scheduled_on = entry->cpu;
+                tsk_rt(next)->scheduled_on = entry->server.cpu;
 
         entry->scheduled = next;
         sched_state_task_picked();
@@ -375,9 +491,11 @@ static struct task_struct* color_schedule(struct task_struct *prev)
 
 static void color_task_new(struct task_struct *t, int on_rq, int running)
 {
-        struct cpu_entry* entry;
         unsigned long flags;
+        int i, replicas;
         raw_spinlock_t *lock;
+        struct cpu_entry *entry;
+        struct dgl_group_req *req;
 
         TRACE_TASK(t, "New colored task\n");
         local_irq_save(flags);
@@ -387,21 +505,36 @@ static void color_task_new(struct task_struct *t, int on_rq, int running)
 
         release_at(t, litmus_clock());
 
+        /* Create request for dynamic group locks */
+        req = kmalloc(sizeof(*req), GFP_ATOMIC);
+        dgl_group_req_init(req);
+        for (i = 0; i < NUM_RESOURCES; i++) {
+                /* /\* Testing *\/ */
+                /* set_req(req, i, 2); */
+                /* /\* Real *\/ */
+                /* replicas = get_control_page(t)->requests[i]; */
+                /* if (replicas) { */
+                /*      set_req(req, i, replicas); */
+                /* } */
+        }
+        tsk_rt(t)->req = req;
+
+        /* Join system */
         raw_spin_lock(lock);
         if (running) {
-                /* Already running on a CPU, update CPU state to match */
+                TRACE_TASK(t, "Already scheduled on %d\n", entry->cpu);
                 BUG_ON(entry->scheduled);
                 entry->scheduled = t;
-                tsk_rt(t)->scheduled_on = entry->cpu;
-        } else {
-                requeue(task_domain(entry, t), t);
-        }
+                tsk_rt(t)->scheduled_on = entry->server.cpu;
+        } else
+                requeue(task_dom(entry, t), t);
         raw_spin_unlock(lock);
 
+        /* Trigger preemptions */
         if (is_be(t))
                 check_for_fifo_preempt();
         else
-                litmus_reschedule(entry->cpu);
+                litmus_reschedule(entry->server.cpu);
 
         local_irq_restore(flags);
 }
@@ -423,19 +556,21 @@ static void color_task_wake_up(struct task_struct *task)
                 sched_trace_task_release(task);
         }
 
+        /* Re-enter system */
         if (entry->scheduled != task) {
                 raw_spin_lock(lock);
-                requeue(task_domain(entry, task), task);
+                requeue(task_dom(entry, task), task);
                 raw_spin_unlock(lock);
         } else {
                 TRACE_TASK(task, "Is already scheduled on %d!\n",
                            entry->scheduled);
         }
 
+        /* Trigger preemptions */
         if (is_be(task))
                 check_for_fifo_preempt();
         else
-                litmus_reschedule(entry->cpu);
+                litmus_reschedule(entry->server.cpu);
 
         local_irq_restore(flags);
 }
@@ -460,15 +595,15 @@ static void color_task_exit(struct task_struct * t)
         /* Remove from scheduler consideration */
         if (is_queued(t)) {
                 raw_spin_lock(lock);
-                remove(task_domain(entry, t), t);
+                remove(task_dom(entry, t), t);
                 raw_spin_unlock(lock);
         }
 
         /* Stop parent server */
-        if (run_server(t))
-                unlink(run_server(t));
+        if (get_task_server(t))
+                unlink(get_task_server(t));
 
-        /* Unschedule running CPU */
+        /* Unschedule running task */
         if (tsk_rt(t)->scheduled_on != NO_CPU) {
                 entry = remote_entry(tsk_rt(t)->scheduled_on);
 
@@ -476,11 +611,17 @@ static void color_task_exit(struct task_struct * t)
 
                 tsk_rt(t)->scheduled_on = NO_CPU;
                 entry->scheduled = NULL;
-                litmus_reschedule(entry->cpu);
+                litmus_reschedule(entry->server.cpu);
 
                 raw_spin_unlock(entry_lock(entry));
         }
 
+        /* Remove dgl request from system */
+        raw_spin_lock(&dgl_lock);
+        release_resources(t);
+        raw_spin_unlock(&dgl_lock);
+        kfree(tsk_rt(t)->req);
+
         local_irq_restore(flags);
 }
 
@@ -492,25 +633,6 @@ 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)
-{
-        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.
  */
@@ -532,7 +654,7 @@ static long color_activate_plugin(void)
                                                &tp.period);
                 if (ret) {
                         printk(KERN_WARNING "Uninitialized server for CPU %d\n",
-                               entry->cpu);
+                               entry->server.cpu);
                         goto loop_end;
                 }
 
@@ -547,7 +669,6 @@ static long color_activate_plugin(void)
                 /* Make runnable */
                 release_at(server_task, now);
                 requeue(&entry->edf_domain, server_task);
-
                 entry->fifo_server.start_time = 0;
                 entry->scheduled = NULL;
 
@@ -607,7 +728,6 @@ static void color_release_ts(lt_t time)
 
 static struct sched_plugin color_plugin __cacheline_aligned_in_smp = {
         .plugin_name            = "COLOR",
-        .tick                   = color_tick,
         .task_new               = color_task_new,
         .complete_job           = complete_job,
         .task_exit              = color_task_exit,
@@ -634,13 +754,13 @@ static int __init init_color(void)
                 entry = remote_entry(cpu);
                 edf_domain_init(&entry->edf_domain, NULL, color_edf_release);
 
-                entry->cpu = cpu;
                 entry->scheduled = NULL;
 
                 /* Create FIFO server */
                 fifo_server = &entry->fifo_server;
                 init_rt_server(&fifo_server->server,
                                cpu + num_online_cpus() + 1,
+                               cpu,
                                &fifo_domain,
                                fifo_preemption_needed,
                                fifo_requeue, fifo_update, fifo_take);
@@ -660,10 +780,9 @@ static int __init init_color(void)
                 bheap_node_init(&tsk_rt(server_task)->heap_node, server_task);
                 tsk_rt(server_task)->is_server = 1;
 
-
                 /* Create CPU server */
                 cpu_server = &entry->server;
-                init_rt_server(cpu_server, cpu + 1,
+                init_rt_server(cpu_server, cpu + 1, cpu,
                                &entry->edf_domain, edf_preemption_needed,
                                edf_requeue, NULL, NULL);
                 cpu_server->running = 1;
@@ -674,6 +793,9 @@ static int __init init_color(void)
         fifo_domain_init(&fifo_domain, NULL, color_fifo_release);
         raw_spin_lock_init(&fifo_lock);
 
+        dgl_init(&group_lock);
+        raw_spin_lock_init(&dgl_lock);
+
         return register_sched_plugin(&color_plugin);
 }