Added preemptive locking.

Currently is always preempting. This needs to be configurable via proc.

4 files changed, 550 insertions, 218 deletions

diff --git a/litmus/color_dev.c b/litmus/color_dev.c
index 6287d14405e8..df43c9f1e37d 100644
--- a/litmus/color_dev.c
+++ b/litmus/color_dev.c
@@ -7,7 +7,7 @@
 #include <linux/highmem.h>
 #include <asm/io.h> /* page_to_phys on SPARC */
 
-#include <asm/glue-cache.h>
+/* #include <asm/glue-cache.h> */
 
 #include <litmus/litmus.h>
 #include <litmus/color.h>
diff --git a/litmus/dgl.c b/litmus/dgl.c
index 7331855d43f7..065a747f7dce 100644
--- a/litmus/dgl.c
+++ b/litmus/dgl.c
@@ -28,10 +28,13 @@
 #define req_group(r, i) (container_of(((void*)r) - sizeof(*r)*(i),      \
                                       struct dgl_group_req, requests))
 
+/* Server ID for trace records */
+#define cpu_sid(dgl, cpu) (dgl->sid_start + cpu)
+
 #define TRACE(fmt, args...) STRACE(fmt, ## args)
 #define TRACE_GREQ(greq, fmt, args...)                                 \
-        TRACE("(greq-%s/%d) " fmt, (greq->task ? greq->task->comm : "greq"), \
-              (greq->task ? greq->task->pid : (int)greq), ## args)
+        TRACE("(greq-%s/%llu) " fmt, (greq->task ? greq->task->comm : "greq"), \
+              (greq->task ? greq->task->pid : (unsigned long long)greq), ## args)
 
 /* Resource id -> word, bit */
 static inline void mask_idx(int resource, int *word, int *bit)
@@ -40,6 +43,13 @@ static inline void mask_idx(int resource, int *word, int *bit)
         *bit  = resource % MASK_SIZE;
 }
 
+static int higher_priority(struct dgl_group_req *a, struct dgl_group_req *b)
+{
+        /* TODO: will break without tasks */
+        return (a->priority < b->priority ||
+                (a->priority == b->priority && (a->task->pid < b->task->pid)));
+}
+
 static void print_waiting(struct dgl *dgl, struct dgl_resource *resource)
 {
         struct dgl_req *pos;
@@ -49,13 +59,223 @@ static void print_waiting(struct dgl *dgl, struct dgl_resource *resource)
         TRACE("List for rid %d\n", resource_id(dgl, resource));
         list_for_each_entry(pos, &resource->waiting, list) {
                 greq = pos->greq;
-                TRACE_GREQ(greq, "with timestamp %llu\n", greq->ts);
-                BUG_ON(greq->ts < last);
-                last = greq->ts;
+                TRACE_GREQ(greq, "with priority %llu\n", greq->priority);
+                last = greq->priority;
+        }
+}
+
+/*
+ * Add @req to @list in priority order (higher numbers are lower priority).
+ */
+static void add_req(struct dgl_req *req, struct list_head *list)
+{
+        struct dgl_req *pos;
+        struct list_head *last = list;
+        int check = 0;
+
+        BUG_ON(in_list(&req->list));
+
+        list_for_each_entry(pos, list, list) {
+                BUG_ON(pos == req);
+                if (!higher_priority(pos->greq, req->greq)) {
+                        break;
+                }
+                last = &pos->list;
+
+                BUG_ON(check++ > 10);
+        }
+        list_add(&req->list, last);
+}
+
+static void release_cpu(struct dgl *dgl, struct dgl_group_req *greq)
+{
+        int cpu = greq->cpu;
+        struct task_struct *task = greq->task;
+
+        TRACE_GREQ(greq, "no longer acquired on CPU %d\n", cpu);
+        BUG_ON(dgl->acquired[cpu] != greq);
+
+        dgl->acquired[cpu] = NULL;
+        --dgl->running;
+
+        /* if (task) { */
+        /*      sched_trace_server_switch_away(cpu_sid(dgl, cpu), 0, */
+        /*                                     -task->pid, get_rt_job(task)); */
+        /* } */
+}
+
+static void take_cpu(struct dgl *dgl, struct dgl_group_req *greq)
+{
+        int cpu = greq->cpu;
+        struct task_struct *task = greq->task;
+
+        TRACE_GREQ(greq, "Taking cpu %d\n", cpu);
+        BUG_ON(dgl->acquired[cpu]);
+
+        dgl->acquired[cpu] = greq;
+        ++dgl->running;
+
+        /* if (task) { */
+        /*      sched_trace_server_switch_to(cpu_sid(dgl, cpu), 0, */
+        /*                                   -task->pid, get_rt_job(task)); */
+        /* } */
+}
+
+static int try_bump_lp(struct dgl *dgl, struct dgl_resource *resource,
+                       struct dgl_req *req)
+{
+        int lower = 0, word, bit, rid;
+        struct dgl_req *acquired, *tmp;
+        struct dgl_group_req *greq, *bumper = req->greq;
+
+        rid  = resource_id(dgl, resource);
+        mask_idx(rid, &word, &bit);
+
+        TRACE_GREQ(bumper, "Trying to bump with priority %llu on resource with %d free\n",
+                   bumper->priority, resource->free_replicas);
+
+        BUG_ON(resource->free_replicas > req->replicas);
+
+        /* Find lower-priority holders */
+        list_for_each_entry(acquired, &resource->acquired, list) {
+                BUG_ON(acquired == req);
+                greq = acquired->greq;
+
+                if (higher_priority(greq, bumper) ||
+                    lower + resource->free_replicas >= req->replicas) {
+                        TRACE_GREQ(greq, "Has higher priority %llu or lower good: %d\n",
+                                   greq->priority, lower);
+                        break;
+                } else {
+                        TRACE_GREQ(greq, "Getting bumped with prio %llu\n", greq->priority);
+                }
+                lower += acquired->replicas;
+        }
+
+        if (lower + resource->free_replicas < req->replicas) {
+                TRACE_GREQ(bumper, "Failed to acquire replicas\n");
+                return 0;
+        }
+
+        BUG_ON(!lower);
+
+        /* Bump 'em */
+        resource->free_replicas += lower;
+
+        BUG_ON(resource->free_replicas > dgl->num_replicas);
+        TRACE_GREQ(bumper, "Freeing %d replicas for total of %d free\n",
+                   lower, resource->free_replicas);
+
+        list_for_each_entry_safe(acquired, tmp, &resource->acquired, list) {
+                greq = acquired->greq;
+
+                TRACE_GREQ(greq, "Bumped\n");
+
+                /* Now in waiting list */
+                list_del_init(&acquired->list);
+                add_req(acquired, &resource->waiting);
+
+                BUG_ON(!acquired->satisfied);
+                acquired->satisfied = 0;
+
+                lower -= acquired->replicas;
+
+                __set_bit(bit, &greq->waiting[word]);
+                if (dgl->acquired[greq->cpu] == greq) {
+                        release_cpu(dgl, greq);
+                        dgl->cpu_preempted(greq);
+                }
+
+                if (!lower)
+                        break;
         }
+
+        return 1;
 }
 
-static void dummy_acquired(int cpu){}
+/*
+ * 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, room;
+        unsigned long waiting;
+        struct dgl_group_req *greq;
+
+        rid  = resource_id(dgl, resource);
+        greq = req->greq;
+
+        TRACE_GREQ(greq, "try acquire\n");
+
+        room = resource->free_replicas >= req->replicas;
+        if (!room) room = try_bump_lp(dgl, resource, req);
+
+        if (!room) {
+                TRACE_GREQ(greq, "cannot acquire %d replicas, %d free\n",
+                           req->replicas, resource->free_replicas);
+                return 0;
+        }
+
+        resource->free_replicas -= req->replicas;
+
+        BUG_ON(resource->free_replicas < 0 || resource->free_replicas > dgl->num_replicas);
+
+        /* Move from waiting to acquired */
+        req->satisfied = 1;
+        list_del_init(&req->list);
+        add_req(req, &resource->acquired);
+
+        TRACE_GREQ(greq, "acquired %d replicas of rid %d\n",
+                   req->replicas, rid);
+
+        mask_idx(rid, &word, &bit);
+        clear_bit(bit, &greq->waiting[word]);
+
+        /* Check for other waiting resources */
+        waiting = 0;
+        for (word = 0; word < MASK_WORDS(dgl); word++) {
+                waiting |= greq->waiting[word];
+                if (waiting)
+                        break;
+        }
+
+        if (!waiting) {
+                TRACE_GREQ(greq, "acquired all resources on cpu %d\n", greq->cpu);
+                BUG_ON(dgl->acquired[greq->cpu]);
+
+                take_cpu(dgl, greq);
+                dgl->cpu_acquired(greq);
+        }
+
+        return 1;
+}
+
+static void try_pull_hp(struct dgl *dgl, struct dgl_resource *resource)
+{
+        int succ;
+        struct dgl_req *waiting, *tmp;
+
+        TRACE("Pulling into resource\n");
+
+        list_for_each_entry_safe(waiting, tmp, &resource->waiting, list) {
+                if (!resource->free_replicas) {
+                        TRACE("Quitting with no more free replicas\n");
+                        break;
+                }
+                succ = try_acquire(dgl, resource, waiting);
+        }
+}
+
+static void dummy_acquired(struct dgl_group_req *greq){}
+static void dummy_preempted(struct dgl_group_req *greq){}
+
+static unsigned long long timestamp_prio (struct dgl *dgl, struct dgl_group_req *greq)
+{
+        return dgl->ts++;
+}
 
 void dgl_init(struct dgl *dgl, unsigned long num_resources,
               unsigned long num_replicas)
@@ -78,6 +298,7 @@ void dgl_init(struct dgl *dgl, unsigned long num_resources,
                 resource = &dgl->resources[i];
 
                 INIT_LIST_HEAD(&resource->waiting);
+                INIT_LIST_HEAD(&resource->acquired);
                 resource->free_replicas = dgl->num_replicas;
         }
 
@@ -85,6 +306,9 @@ void dgl_init(struct dgl *dgl, unsigned long num_resources,
         dgl->running  = 0;
         dgl->ts = 0;
         dgl->cpu_acquired = dummy_acquired;
+        dgl->cpu_preempted = dummy_preempted;
+        dgl->assign_priority = timestamp_prio;
+        dgl->sid_start = 0;
 }
 
 void dgl_free(struct dgl *dgl)
@@ -140,69 +364,13 @@ void set_req(struct dgl *dgl, struct dgl_group_req *greq,
         TRACE_GREQ(greq, "requesting %d of %d\n", replicas, resource);
 
         req = &greq->requests[resource];
+
         req->greq = greq;
-        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, head, empty, room;
-        unsigned long waiting;
-        struct dgl_group_req *greq;
+        req->satisfied = 0;
 
-        rid  = resource_id(dgl, resource);
-        greq = req->greq;
-
-        TRACE_GREQ(greq, "try acquire\n");
-
-        head  = resource->waiting.next == &req->list;
-        empty = list_empty(&resource->waiting);
-        room  = resource->free_replicas >= req->replicas;
-
-        if (! (room && (head || empty)) ) {
-                TRACE_GREQ(greq, "cannot acquire %d replicas, %d free\n",
-                      req->replicas, resource->free_replicas,
-                      room, head, empty);
-                return 0;
-        }
-
-        resource->free_replicas -= req->replicas;
-
-        TRACE_GREQ(greq, "0x%p acquired %d replicas of rid %d\n",
-                   req->replicas, rid);
-
-        mask_idx(rid, &word, &bit);
-
-        TRACE_GREQ(greq, "0x%p, %lu, 0x%p\n", greq->waiting, greq->waiting[word],
-              &greq->waiting[word]);
-
-        clear_bit(bit, &greq->waiting[word]);
-
-        waiting = 0;
-        for (word = 0; word < MASK_WORDS(dgl); word++) {
-                waiting |= greq->waiting[word];
-                if (waiting)
-                        break;
-        }
-
-        if (!waiting) {
-                TRACE_GREQ(greq, "acquired all resources on cpu %d\n", greq->cpu);
-                BUG_ON(dgl->acquired[greq->cpu]);
-                dgl->acquired[greq->cpu] = greq;
-                litmus_reschedule(greq->cpu);
-                dgl->running++;
-
-                dgl->cpu_acquired(greq->cpu);
-        }
-
-        return 1;
+        INIT_LIST_HEAD(&req->list);
+        BUG_ON(in_list(&req->list));
 }
 
 /**
@@ -215,7 +383,8 @@ void add_group_req(struct dgl *dgl, struct dgl_group_req *greq, int cpu)
         struct dgl_resource *resource;
 
         greq->cpu = cpu;
-        greq->ts = dgl->ts++;
+
+        greq->priority = dgl->assign_priority(dgl, greq);
 
         TRACE_GREQ(greq, "group request added for CPU %d\n", cpu);
         BUG_ON(dgl->acquired[cpu] == greq);
@@ -230,20 +399,27 @@ void add_group_req(struct dgl *dgl, struct dgl_group_req *greq, int cpu)
                 req = &greq->requests[i];
                 resource = &dgl->resources[i];
 
+                TRACE_GREQ(greq, "Request %d has list 0x%p\n", i, &req->list);
+
+                if (in_list(&req->list)) {
+                        TRACE_GREQ(greq, "0x%p -> Request %d -> 0x%p\n",
+                                   req->list.prev, i, req->list.next);
+                        list_del_init(&req->list);
+                }
+                BUG_ON(in_list(&req->list));
+
                 succ = try_acquire(dgl, resource, req);
                 all_succ &= succ;
 
                 if (!succ) {
                         TRACE_GREQ(greq, "waiting on rid %d\n", i);
-                        list_add_tail(&req->list, &resource->waiting);
+                        add_req(req, &resource->waiting);
                 }
         }
 
         /* Grant empty requests */
         if (all_succ && !dgl->acquired[cpu]) {
-                TRACE_GREQ(greq, "empty group request acquired cpu %d\n", cpu);
-                dgl->acquired[cpu] = greq;
-                ++dgl->running;
+                take_cpu(dgl, greq);
         }
 
         BUG_ON(dgl->requests && !dgl->running);
@@ -257,27 +433,27 @@ 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)
 {
         int b, w, i;
-        struct dgl_req *req, *next;
+        struct dgl_req *req;
         struct dgl_resource *resource;
 
-        TRACE_GREQ(greq, "removing group request for CPU %d\n", greq, greq->cpu);
+        TRACE_GREQ(greq, "removing group request for CPU %d\n", greq->cpu);
 
         --dgl->requests;
 
         if (dgl->acquired[greq->cpu] == greq) {
-                TRACE_GREQ(greq, "no longer acquired on CPU %d\n", greq->cpu);
-                dgl->acquired[greq->cpu] = NULL;
-                --dgl->running;
+                release_cpu(dgl, greq);
         }
 
         for_each_resource(greq->requested, dgl, w, b, i) {
                 req = &greq->requests[i];
                 resource = &dgl->resources[i];
 
-                if (!list_empty(&req->list)) {
+                /* Remove from waiting or acquired list */
+                list_del_init(&req->list);
+
+                if (!req->satisfied) {
                         /* Waiting on resource */
                         clear_bit(b, &greq->waiting[w]);
-                        list_del_init(&req->list);
                         TRACE("Quitting 0x%p from rid %d\n",
                               req, i);
                 } else {
@@ -286,19 +462,12 @@ void remove_group_req(struct dgl *dgl, struct dgl_group_req *greq)
                         BUG_ON(resource->free_replicas > dgl->num_replicas);
                         TRACE_GREQ(greq, "releasing %d of %d replicas, rid %d\n",
                               req->replicas, resource->free_replicas, i);
+                        req->satisfied = 0;
 
                         if (!list_empty(&resource->waiting)) {
-                                /* Give it to the next guy */
-                                next = list_first_entry(&resource->waiting,
-                                                        struct dgl_req,
-                                                        list);
-
-                                BUG_ON(next->greq->ts < greq->ts);
-
-                                if (try_acquire(dgl, resource, next)) {
-                                        list_del_init(&next->list);
-                                        print_waiting(dgl, resource);
-                                }
+                                try_pull_hp(dgl, resource);
+                        } else if (list_empty(&resource->acquired)) {
+                                BUG_ON(resource->free_replicas != dgl->num_replicas);
                         }
                 }
         }
diff --git a/litmus/sched_color.c b/litmus/sched_color.c
index 28a7cfa401b4..f7e4be700548 100644
--- a/litmus/sched_color.c
+++ b/litmus/sched_color.c
@@ -64,7 +64,8 @@ static raw_spinlock_t dgl_lock;
 #define is_fifo_server(s)      ((s)->sid > num_online_cpus())
 #define lock_if(lock, cond)    do { if (cond) raw_spin_lock(lock);} while(0)
 #define unlock_if(lock, cond)  do { if (cond) raw_spin_unlock(lock);} while(0)
-
+#define crit_cpu(ce) \
+        (container_of((void*)((ce) - (ce)->level), struct cpu_entry, crit_entries))
 #ifdef CONFIG_NP_SECTION
 #define has_resources(t, c)   (tsk_rt(t)->req == group_lock.acquired[c])
 #else
diff --git a/litmus/sched_mc.c b/litmus/sched_mc.c
index 0ee7be46f1ee..14d41701ce68 100644
--- a/litmus/sched_mc.c
+++ b/litmus/sched_mc.c
@@ -28,6 +28,15 @@
 #include <litmus/ce_domain.h>
 #include <litmus/dgl.h>
 
+
+/**
+ * Signal triggerred via lock event.
+ */
+struct lock_signal {
+        enum crit_level level;
+        enum { NONE=0, ACQUIRED, PREEMPTED} type;
+};
+
 /**
  * struct cpu_entry - State of a CPU for the entire MC system
  * @cpu           CPU id
@@ -41,11 +50,12 @@
  */
 struct cpu_entry {
         int                     cpu;
-        enum crit_level         crit_signal;
         struct task_struct*     scheduled;
         struct task_struct*     will_schedule;
         struct task_struct*     linked;
+        struct lock_signal      signal;
         raw_spinlock_t          lock;
+        raw_spinlock_t          signal_lock;
         struct crit_entry       crit_entries[NUM_CRIT_LEVELS];
 #ifdef CONFIG_PLUGIN_MC_REDIRECT
         struct list_head        redir;
@@ -74,16 +84,78 @@ static int interrupt_cpu;
 #define is_global(dom)    (domain_data(dom)->heap)
 #define is_global_task(t) (is_global(get_task_domain(t)))
 #define can_use(ce) \
-        ((ce)->state == CS_ACTIVE || (ce->state == CS_ACTIVATE))
+        ((ce)->state != CS_REMOVE && (ce)->state != CS_REMOVED)
 #define can_requeue(t)                                                  \
         ((t)->rt_param.linked_on == NO_CPU && /* Not linked anywhere */ \
          !is_queued(t) &&                     /* Not gonna be linked */ \
          (!is_global_task(t) || (t)->rt_param.scheduled_on == NO_CPU))
 #define entry_level(e) \
         (((e)->linked) ? tsk_mc_crit((e)->linked) : NUM_CRIT_LEVELS - 1)
+#define get_crit_entry_for(cpu, level) (&per_cpu(cpus, cpu).crit_entries[level])
 #define crit_cpu(ce) \
         (container_of((void*)((ce) - (ce)->level), struct cpu_entry, crit_entries))
-#define get_crit_entry_for(cpu, level) (&per_cpu(cpus, cpu).crit_entries[level])
+
+
+#ifdef CONFIG_PREEMPT_STATE_TRACE
+#define NCS(x) case x: return #x
+#define ce_partition(ce)    (crit_cpu(ce)->cpu + 1)
+#define ce_st(ce, cpu) sched_trace_server_switch_to(cpu, 0, ce_sid(ce), 0)
+#define ce_sa(ce, cpu) sched_trace_server_switch_away(cpu, 0, ce_sid(ce), 0)
+
+static inline const char* ce_state_name(enum crit_state state)
+{
+        switch(state) {
+                NCS(CS_ACTIVE);NCS(CS_ACTIVATE);
+                NCS(CS_REMOVE);NCS(CS_REMOVED);
+                NCS(CS_BLOCKED); default: return "UNKNOWN";
+        }
+}
+
+static inline void ce_state_change(struct crit_entry *ce, enum crit_state state)
+{
+        switch (ce->state) {
+        case CS_BLOCKED:
+                sched_trace_server_resume(ce_sid(ce));
+                break;
+        case CS_ACTIVE:
+                if (state == CS_ACTIVATE) return;
+                BUG_ON(state == CS_ACTIVATE);
+                ce_sa(ce, ce_partition(ce));
+                break;
+        case CS_ACTIVATE:
+        case CS_REMOVE:
+                if (state == CS_REMOVE || state == CS_ACTIVATE)
+                        sched_trace_server_completion(ce_sid(ce), 0);
+                else
+                        ce_sa(ce, 0);
+                break;
+        case CS_REMOVED:
+                if (state == CS_REMOVE) return;
+                break;
+        };
+
+        switch (state) {
+        case CS_BLOCKED:
+                sched_trace_server_block(ce_sid(ce));
+                break;
+        case CS_ACTIVE:
+                ce_st(ce, ce_partition(ce));
+                break;
+        case CS_ACTIVATE:
+        case CS_REMOVE:
+                if (ce->state != CS_REMOVE && ce->state != CS_ACTIVATE)
+                        ce_st(ce, 0);
+                break;
+        }
+
+        TRACE_CRIT_ENTRY((ce), "(%s)->(%s)\n",
+                         ce_state_name((ce)->state),
+                         ce_state_name(state));
+        (ce)->state = state;
+}
+#else
+#define ce_state_change(ce, next) do{(ce)->state = next}while(0)
+#endif
 
 /*
  * Put in requests for resources needed by @t.
@@ -96,10 +168,10 @@ static int acquire_resources(struct task_struct *t)
         BUG_ON(tsk_rt(t)->linked_on == NO_CPU);
 
         raw_spin_lock(&dgl_lock);
-        if (is_kernel_np(t)) {
-                TRACE_MC_TASK(t, "Already contending for resources\n");
-                return;
-        }
+        /* if (is_kernel_np(t)) { */
+                /* TRACE_MC_TASK(t, "Already contending for resources\n"); */
+                /* return; */
+        /* } */
 
         cpu = tsk_rt(t)->linked_on;
 
@@ -108,7 +180,7 @@ static int acquire_resources(struct task_struct *t)
                 sched_trace_task_block(t);
                 TRACE_MC_TASK(t, "Blocked at %llu\n", litmus_clock());
                 add_group_req(&group_lock, tsk_rt(t)->req, cpu);
-                make_np(t);
+                /* make_np(t); */
         }
 
         acquired = has_resources(t, cpu);
@@ -119,20 +191,23 @@ static int acquire_resources(struct task_struct *t)
 
 static void release_resources(struct task_struct *t)
 {
-        if (is_kernel_np(t)) {
+        /* if (is_kernel_np(t)) { */
                 TRACE_MC_TASK(t, "Releasing resources\n");
 
                 raw_spin_lock(&dgl_lock);
-                if (get_rt_flags(t) & RT_F_BLOCKED) {
-                        TRACE_MC_TASK(t, "Forced off before we got our stuff\n");
-                        clear_rt_flags(t, RT_F_BLOCKED);
-                }
+
+                if (get_rt_flags(t) & RT_F_BLOCKED) {
+                        TRACE_MC_TASK(t, "Forced off before we got our stuff\n");
+                        clear_rt_flags(t, RT_F_BLOCKED);
+                        sched_trace_task_resume(t);
+                }
+
                 remove_group_req(&group_lock, tsk_rt(t)->req);
                 raw_spin_unlock(&dgl_lock);
                 take_np(t);
-        } else {
-                TRACE_MC_TASK(t, "No resources to release!\n");
-        }
+        /* } else { */
+        /*      TRACE_MC_TASK(t, "No resources to release!\n"); */
+        /* } */
 }
 
 /*
@@ -203,10 +278,10 @@ static void fix_crit_position(struct crit_entry *ce)
 {
         if (is_global(ce->domain)) {
                 if (CS_ACTIVATE == ce->state) {
-                        ce->state = CS_ACTIVE;
+                        ce_state_change(ce, CS_ACTIVE);
                         update_crit_position(ce);
                 } else if (CS_REMOVE == ce->state) {
-                        ce->state = CS_REMOVED;
+                        ce_state_change(ce, CS_REMOVED);
                         update_crit_position(ce);
                 }
         }
@@ -249,9 +324,28 @@ static inline void cancel_ghost(struct crit_entry *ce)
 }
 
 /*
+ * Time accounting for ghost tasks.
+ * Must be called before a decision is made involving the task's budget.
+ */
+static void update_server_time(struct task_struct *p)
+{
+        u64 clock = litmus_clock();
+        u64 delta = clock - p->rt_param.last_exec_time;
+        if (unlikely ((s64)delta < 0)) {
+                delta = 0;
+        }
+        if (budget_remaining(p) <= delta) {
+                tsk_rt(p)->job_params.exec_time = get_exec_cost(p);
+        } else {
+                tsk_rt(p)->job_params.exec_time += delta;
+        }
+        p->rt_param.last_exec_time = clock;
+}
+
+/*
  * Arm ghost timer. Will merge timers if the option is specified.
  */
-static inline void start_crit(struct crit_entry *ce)
+static void start_crit(struct crit_entry *ce)
 {
         lt_t fire;
         struct task_struct *task = ce->linked;
@@ -281,23 +375,17 @@ static inline void start_crit(struct crit_entry *ce)
                                      get_rt_job(task));
 }
 
-/*
- * Time accounting for ghost tasks.
- * Must be called before a decision is made involving the task's budget.
- */
-static void update_server_time(struct task_struct *p)
+static void stop_crit(struct crit_entry *ce)
 {
-        u64 clock = litmus_clock();
-        u64 delta = clock - p->rt_param.last_exec_time;
-        if (unlikely ((s64)delta < 0)) {
-                delta = 0;
-        }
-        if (budget_remaining(p) <= delta) {
-                tsk_rt(p)->job_params.exec_time = get_exec_cost(p);
-        } else {
-                tsk_rt(p)->job_params.exec_time += delta;
+        if (is_ghost(ce->linked)) {
+                cancel_ghost(ce);
+                if (!budget_exhausted(ce->linked)) {
+                        /* Job isn't finished, so do accounting */
+                        update_server_time(ce->linked);
+                }
         }
-        p->rt_param.last_exec_time = clock;
+        sched_trace_server_switch_away(ce_sid(ce), 0, -ce->linked->pid,
+                                       get_rt_job(ce->linked));
 }
 
 /**
@@ -317,19 +405,12 @@ static void link_task_to_crit(struct crit_entry *ce,
         if (ce->linked) {
                 ce->domain->release_resources(ce->linked);
                 if (ce->state == CS_BLOCKED)
-                        ce->state == CS_ACTIVE;
+                        ce_state_change(ce, CS_ACTIVE);
 
                 TRACE_MC_TASK(ce->linked, "Unlinking\n");
+
                 ce->linked->rt_param.linked_on = NO_CPU;
-                if (is_ghost(ce->linked)) {
-                        cancel_ghost(ce);
-                        if (!budget_exhausted(ce->linked)) {
-                                /* Job isn't finished, so do accounting */
-                                update_server_time(ce->linked);
-                        }
-                }
-                sched_trace_server_switch_away(ce_sid(ce), 0, -ce->linked->pid,
-                                               get_rt_job(ce->linked));
+                stop_crit(ce);
         }
 
         /* Actually link task */
@@ -341,7 +422,7 @@ static void link_task_to_crit(struct crit_entry *ce,
                 if (ce->domain->acquire_resources(task))
                         start_crit(ce);
                 else
-                        ce->state = CS_BLOCKED;
+                        ce_state_change(ce, CS_BLOCKED);
         }
 }
 
@@ -445,7 +526,7 @@ static void link_task_to_cpu(struct cpu_entry *entry, struct task_struct *task)
         struct crit_entry *ce;
         TRACE_MC_TASK(task, "Linking to P%d\n", entry->cpu);
         BUG_ON(task && tsk_rt(task)->linked_on != entry->cpu);
-        BUG_ON(task && (is_ghost(task) || is_cblocked(task)));
+        BUG_ON(task && is_ghost(task));
 
         if (entry->linked) {
                 sched_trace_server_switch_away(-entry->linked->pid,
@@ -467,8 +548,9 @@ static void link_task_to_cpu(struct cpu_entry *entry, struct task_struct *task)
         /* Higher criticality crit entries are now usable */
         for (; i < entry_level(entry) + 1; i++) {
                 ce = &entry->crit_entries[i];
-                if (!can_use(ce))
-                        ce->state = CS_ACTIVATE;
+                if (!can_use(ce)) {
+                        ce_state_change(ce, CS_ACTIVATE);
+                }
         }
 }
 
@@ -544,10 +626,12 @@ static void update_crit_levels(struct cpu_entry *entry)
                  */
                 readmit[i] = (!global_preempted) ? ce->linked : NULL;
 
-                ce->state = CS_REMOVE;
-                TRACE_CRIT_ENTRY(ce, "(CS_REMOVE)\n");
-                if (ce->linked)
-                        link_task_to_crit(ce, NULL);
+                if (ce->state != CS_REMOVE) {
+                        ce_state_change(ce, CS_REMOVE);
+                        TRACE_CRIT_ENTRY(ce, "(CS_REMOVE)\n");
+                        if (ce->linked)
+                                link_task_to_crit(ce, NULL);
+                }
         }
         /* Need to unlock so we can access domains */
         raw_spin_unlock(&entry->lock);
@@ -569,7 +653,7 @@ static void update_crit_levels(struct cpu_entry *entry)
  */
 static void check_for_preempt(struct domain *dom)
 {
-        int recheck = 1, higher_prio, was_inelig, update = 0;
+        int recheck = 1;
         struct cpu_entry *entry;
         struct crit_entry *ce;
 
@@ -604,25 +688,12 @@ static void check_for_preempt(struct domain *dom)
 
                 raw_spin_lock(&entry->lock);
 
-                if (can_use(ce)) {
-                        was_inelig = ce->linked && !is_ghost(ce->linked) &&
-                                ce->linked != entry->linked && !is_cblocked(ce->linked);
-                        higher_prio = mc_preempt_needed(dom, ce->linked);
-
-                        if (was_inelig) {
-                                preempt_cpu(entry, ce->linked);
-                                start_crit(ce);
-                        } else if (higher_prio)
-                                preempt_crit(dom, ce);
-                        update = was_inelig || higher_prio;
-
-                } else {
-                        TRACE_CRIT_ENTRY(ce, "Can't use!\n");
-                }
-                if (update)
+                if (can_use(ce) && mc_preempt_needed(dom, ce->linked)) {
+                        preempt_crit(dom, ce);
                         update_crit_levels(entry);
-                else
+                } else {
                         raw_spin_unlock(&entry->lock);
+                }
         }
 }
 
@@ -679,7 +750,6 @@ static void remove_from_all(struct task_struct* task)
  */
 static void job_completion(struct task_struct *task, int forced)
 {
-        lt_t now;
         int release_server;
         struct cpu_entry *entry;
         struct crit_entry *ce;
@@ -712,8 +782,6 @@ static void job_completion(struct task_struct *task, int forced)
                 remove_from_all(task);
         }
 
-        BUG_ON(get_rt_flags(task) & RT_F_BLOCKED);
-
         if (lt_before(get_user_release(task), litmus_clock())) {
                 TRACE_TASK(task, "Executable task going back to running\n");
                 clear_rt_flags(task, RT_F_SLEEP);
@@ -729,7 +797,7 @@ static void job_completion(struct task_struct *task, int forced)
                 if (is_running(task)) {
                         job_arrival(task);
                 }
-        } else if (is_ghost(task)) {
+        } else if (is_ghost(task) || is_cblocked(task)) {
                 entry = &per_cpu(cpus, tsk_rt(task)->linked_on);
                 ce = &entry->crit_entries[tsk_mc_crit(task)];
 
@@ -739,7 +807,8 @@ static void job_completion(struct task_struct *task, int forced)
                         link_task_to_cpu(entry, NULL);
                         sched_trace_server_switch_away(ce_sid(ce),0,-task->pid,
                                                        get_rt_job(task));
-                        start_crit(ce);
+                        if (ce->state == CS_ACTIVE)
+                                start_crit(ce);
                 }
                 raw_spin_unlock(&entry->lock);
         }
@@ -759,7 +828,8 @@ static enum hrtimer_restart mc_ghost_exhausted(struct hrtimer *timer)
         struct cpu_entry *entry = crit_cpu(ce);
         struct task_struct *tmp = NULL;
 #endif
-        TRACE_CRIT_ENTRY(ce, "Firing here at %llu\n", litmus_clock());
+        TRACE("Firing here at %llu\n", litmus_clock());
+        TRACE_CRIT_ENTRY(ce, "For this\n");
 
         raw_spin_lock(&entry->lock);
 
@@ -1068,7 +1138,7 @@ void pick_next_task(struct cpu_entry *entry)
                 raw_spin_lock(&entry->lock);
 
                 ready_task = NULL;
-                if (!entry->linked && can_use(ce)) {
+                if (!entry->linked && can_use(ce) && ce->state != CS_BLOCKED) {
                         if (ce->linked) {
                                 ready_task = ce->linked;
                         } else if (dtask) {
@@ -1092,6 +1162,58 @@ void pick_next_task(struct cpu_entry *entry)
         }
 }
 
+/*
+ * Caller must hold no locks, when it returns CPU lock will be held.
+ */
+static void process_signals(struct cpu_entry *entry)
+{
+        int type;
+        enum crit_level level;
+        struct crit_entry *ce;
+
+        /* Read current signal */
+        raw_spin_lock(&entry->signal_lock);
+        level = entry->signal.level;
+        type  = entry->signal.type;
+        entry->signal.type = NONE;
+        raw_spin_unlock(&entry->signal_lock);
+
+        raw_spin_lock(&entry->lock);
+
+        if (type) {
+                /* Don't care what type of signal it was due to race conditions:
+                 * just use the signal as a hint to check CPU state
+                 */
+                ce = &entry->crit_entries[level];
+
+                if (ce->state == CS_BLOCKED && type == ACQUIRED) {
+                        TRACE_CRIT_ENTRY(ce, "Processing acquired signal\n");
+                        BUG_ON(!ce->linked);
+
+                        ce_state_change(ce, CS_ACTIVE);
+                        if (!is_ghost(ce->linked)) {
+                                link_task_to_cpu(entry, ce->linked);
+                                update_crit_levels(entry);
+                                raw_spin_lock(&entry->lock);
+                        }
+                        start_crit(ce);
+                } else if ((ce->state == CS_ACTIVE || ce->state == CS_ACTIVATE) &&
+                           ce->linked && type == PREEMPTED) {
+                        TRACE_CRIT_ENTRY(ce, "Processing preempted signal\n");
+
+                        stop_crit(ce);
+                        ce_state_change(ce, CS_BLOCKED);
+                        if (entry->linked == ce->linked) {
+                                link_task_to_cpu(entry, NULL);
+                        }
+
+                } else {
+                        TRACE_CRIT_ENTRY(ce, "Missed a signal, state: (%s)\n",
+                                         ce_state_name(ce->state));
+                }
+        }
+}
+
 /**
  * mc_schedule() - Return next task which should be scheduled.
  */
@@ -1101,7 +1223,6 @@ static struct task_struct* mc_schedule(struct task_struct* prev)
         int out_of_time, sleep, preempt, exists, blocks, global, lower;
         struct cpu_entry* entry = &__get_cpu_var(cpus);
         struct task_struct *next = NULL;
-        struct crit_entry *ce;
 
         local_irq_save(flags);
 
@@ -1164,22 +1285,10 @@ static struct task_struct* mc_schedule(struct task_struct* prev)
                         job_arrival(entry->scheduled);
         }
 
-        /* Call before processing signals so any subsequent signal will cause
-         * a reschedule.
-         */
         sched_state_task_picked();
 
-        /* A remote processor unblocked one of our crit levels */
-        if (entry->crit_signal != NUM_CRIT_LEVELS) {
-                ce = &entry->crit_entries[entry->crit_signal];
-                TRACE_CRIT_ENTRY(ce, "Processing signal for %d\n", entry->crit_signal);
-                raw_spin_lock(ce->domain->lock);
-                check_for_preempt(ce->domain);
-                raw_spin_unlock(ce->domain->lock);
-                entry->crit_signal = NUM_CRIT_LEVELS;
-        }
-
-        raw_spin_lock(&entry->lock);
+        /* Method will return with entry locked */
+        process_signals(entry);
 
         /* Pick next task if none is linked */
         if (!entry->linked)
@@ -1191,6 +1300,7 @@ static struct task_struct* mc_schedule(struct task_struct* prev)
                 next->rt_param.scheduled_on = entry->cpu;
         entry->will_schedule = next;
 
+
         raw_spin_unlock_irqrestore(&entry->lock, flags);
 
         if (next) {
@@ -1225,6 +1335,54 @@ long mc_deactivate_plugin(void)
         return mc_ce_deactivate_plugin_common();
 }
 
+static unsigned long long deadline_prio(struct dgl *dgl, struct dgl_group_req *greq)
+{
+        return get_deadline(greq->task);
+}
+
+/*
+ * Setup and send signal to CPU for resource acquisition. To avoid touching
+ * CPU locks, all CPU state modifications are delayed until the signal is
+ * processed.
+ */
+static void cpu_acquired(struct dgl_group_req *greq)
+{
+        struct cpu_entry *entry = &per_cpu(cpus, greq->cpu);
+
+        TRACE_MC_TASK(greq->task, "Acquired CPU %d\n", greq->cpu);
+
+        sched_trace_task_resume(greq->task);
+        clear_rt_flags(greq->task, RT_F_BLOCKED);
+
+        raw_spin_lock(&entry->signal_lock);
+
+        entry->signal.type  = ACQUIRED;
+        entry->signal.level = CRIT_LEVEL_B;
+
+        litmus_reschedule(greq->cpu);
+
+        raw_spin_unlock(&entry->signal_lock);
+}
+
+static void cpu_preempted(struct dgl_group_req *greq)
+{
+        struct cpu_entry *entry = &per_cpu(cpus, greq->cpu);
+
+        TRACE_MC_TASK(greq->task, "Dropping CPU %d\n", entry->cpu);
+
+        sched_trace_task_block(greq->task);
+        set_rt_flags(greq->task, RT_F_BLOCKED);
+
+        raw_spin_lock(&entry->signal_lock);
+
+        entry->signal.type  = PREEMPTED;
+        entry->signal.level = CRIT_LEVEL_B;
+
+        litmus_reschedule(greq->cpu);
+
+        raw_spin_unlock(&entry->signal_lock);
+}
+
 /* **************************************************************************
  * Initialization
  * ************************************************************************** */
@@ -1265,6 +1423,8 @@ static long mc_activate_plugin(void)
 #endif
 #endif
 
+        group_lock.assign_priority = deadline_prio;
+
         for_each_online_cpu(cpu) {
                 BUG_ON(NR_CPUS <= n);
                 dom = per_cpu(cpus, cpu).crit_entries[CRIT_LEVEL_A].domain;
@@ -1286,7 +1446,6 @@ out:
         return ret;
 }
 
-
 static void mc_release_ts(lt_t time)
 {
         int cpu, cont_id = -1;
@@ -1302,6 +1461,7 @@ static void mc_release_ts(lt_t time)
                 sched_trace_container_param(++cont_id, (const char*)&name);
                 ce = &entry->crit_entries[level];
                 sched_trace_server_param(ce_sid(ce), cont_id, 0, 0);
+                ce_st(ce, ce_partition(ce));
         }
 
         level = CRIT_LEVEL_B;
@@ -1311,6 +1471,7 @@ static void mc_release_ts(lt_t time)
                 sched_trace_container_param(++cont_id, (const char*)&name);
                 ce = &entry->crit_entries[level];
                 sched_trace_server_param(ce_sid(ce), cont_id, 0, 0);
+                ce_st(ce, ce_partition(ce));
         }
 
         level = CRIT_LEVEL_C;
@@ -1320,8 +1481,15 @@ static void mc_release_ts(lt_t time)
                 entry = &per_cpu(cpus, cpu);
                 ce = &entry->crit_entries[level];
                 sched_trace_server_param(ce_sid(ce), cont_id, 0, 0);
+                ce_st(ce, ce_partition(ce));
         }
 
+        /* strcpy(name, "DGL"); */
+        /* sched_trace_container_param(++cont_id, (const char*)&name); */
+        /* for_each_online_cpu(cpu) { */
+        /*      sched_trace_server_param(group_lock.sid_start + cpu, cont_id, 0, 0); */
+        /* } */
+
         mc_ce_release_at_common(NULL, time);
 }
 
@@ -1419,44 +1587,38 @@ static inline void init_edf_domain(struct domain *dom, rt_domain_t *rt,
 #endif
 }
 
-/*
- * Setup and send signal to CPU for resource acquisition. To avoid touching
- * CPU locks, all CPU state modifications are delayed until the signal is
- * processed.
- */
-static void cpu_acquired(int cpu)
+static void init_lock(raw_spinlock_t *lock, int cpu, char *name)
 {
-        struct cpu_entry *entry = &per_cpu(cpus, cpu);
-        struct crit_entry *ce = &entry->crit_entries[CRIT_LEVEL_B];
+        int name_size;
+        char *lock_name;
+        struct lock_class_key *lock_key;
 
-        TRACE_CRIT_ENTRY(ce, "Acquired lock\n");
+        /* Create unique name / key so that lockdep is not triggered */
+        name_size = sizeof(*lock_name) * LITMUS_LOCKDEP_NAME_MAX_LEN;
+        lock_name = kmalloc(name_size, GFP_ATOMIC);
+        lock_key  = kmalloc(sizeof(*lock_key), GFP_ATOMIC);
 
-        BUG_ON(!ce->linked);
+        raw_spin_lock_init(lock);
 
-        clear_rt_flags(ce->linked, RT_F_BLOCKED);
-        sched_trace_task_resume(ce->linked);
+        LOCKDEP_DYNAMIC_ALLOC(lock, lock_key, lock_name, "%s%d", name, cpu);
+}
 
-        if (ce->state == CS_BLOCKED) {
-                entry->crit_signal = CRIT_LEVEL_B;
-                /* Yes this is ok for race conditions, but only in the system
-                 * for the MC-COLOR paper
-                 */
-                ce->state = CS_ACTIVE;
-                litmus_reschedule(cpu);
-        }
+static char* domain_name(const char *name, int cpu)
+{
+        char *buf = kmalloc(LITMUS_LOCKDEP_NAME_MAX_LEN * sizeof(char), GFP_ATOMIC);
+        snprintf(buf, LITMUS_LOCKDEP_NAME_MAX_LEN, "%s%d", name, cpu);
+        return buf;
 }
 
 struct domain_data *ce_domain_for(int);
 static int __init init_mc(void)
 {
-        int cpu, name_size;
-        char *lock_name;
+        int cpu;
         rt_domain_t *rt;
         raw_spinlock_t *a_dom_lock, *b_dom_lock, *c_dom_lock; /* For lock debugger */
         struct cpu_entry *entry;
         struct domain_data *dom_data;
         struct ce_dom_data *ce_data;
-        struct lock_class_key *lock_key;
 
         for_each_online_cpu(cpu) {
                 entry = &per_cpu(cpus, cpu);
@@ -1465,15 +1627,11 @@ static int __init init_mc(void)
                 entry->cpu = cpu;
                 entry->scheduled = NULL;
                 entry->linked = NULL;
-                entry->crit_signal = NUM_CRIT_LEVELS;
+                entry->signal.level = CRIT_LEVEL_A;
+                entry->signal.type  = NONE;
 
-                /* Trick lockdep for CPU locks */
-                name_size = sizeof(*lock_name) * LITMUS_LOCKDEP_NAME_MAX_LEN;
-                lock_name = kmalloc(name_size, GFP_ATOMIC);
-                lock_key  = kmalloc(sizeof(*lock_key), GFP_ATOMIC);
                 raw_spin_lock_init(&entry->lock);
-                LOCKDEP_DYNAMIC_ALLOC(&entry->lock, lock_key, lock_name,
-                                      "entry%d", cpu);
+                raw_spin_lock_init(&entry->signal_lock);
 
 #ifdef CONFIG_PLUGIN_MC_REDIRECT
                 raw_spin_lock_init(&entry->redir_lock);
@@ -1490,7 +1648,7 @@ static int __init init_mc(void)
                                 ce_peek_and_take_ready, ce_higher_prio, ce_data, cpu,
                                 ce_timer_function);
                 init_local_domain(entry, dom_data, CRIT_LEVEL_A);
-                dom_data->domain.name = "LVL-A";
+                dom_data->domain.name = domain_name("LVL-A", cpu);
 
                 /* CRIT_LEVEL_B */
                 dom_data = &per_cpu(_mc_crit_b, cpu);
@@ -1501,7 +1659,8 @@ static int __init init_mc(void)
                 dom_data->domain.release_resources = release_resources;
                 b_dom_lock = dom_data->domain.lock;
                 raw_spin_lock_init(b_dom_lock);
-                dom_data->domain.name = "LVL-B";
+
+                dom_data->domain.name = domain_name("LVL-B", cpu);
         }
 
         /* CRIT_LEVEL_C */
@@ -1512,12 +1671,15 @@ static int __init init_mc(void)
         c_dom_lock = _mc_crit_c.domain.lock;
         raw_spin_lock_init(c_dom_lock);
         _mc_crit_c.domain.name = "LVL-C";
-
+        /* GROUP LOCK */
         dgl_init(&group_lock, color_cache_info.nr_colors,
                  color_cache_info.ways);
-        group_lock.cpu_acquired = cpu_acquired;
+        group_lock.cpu_acquired  = cpu_acquired;
+        group_lock.cpu_preempted = cpu_preempted;
         raw_spin_lock_init(&dgl_lock);
 
+        group_lock.sid_start = num_online_cpus() * NUM_CRIT_LEVELS + 1;
+
         return register_sched_plugin(&mc_plugin);
 }