6 files changed, 233 insertions, 123 deletions

diff --git a/litmus/Kconfig b/litmus/Kconfig
index 91bf81ea9fae..5e80197c44db 100644
--- a/litmus/Kconfig
+++ b/litmus/Kconfig
@@ -81,6 +81,14 @@ config PLUGIN_MC_LEVEL_A_MAX_TASKS
        help
          The maximum number of level A tasks allowed (per-cpu) in level A.
 
+config PLUGIN_MC_LINUX_SLACK_STEALING
+       bool "Run background tasks in slack"
+       depends on PLUGIN
+       default n
+       help
+         Allow non-SCHED_LITMUS tasks to run in slack time generated by tasks
+         of the lowest criticality level.
+
 config PLUGIN_MC_RELEASE_MASTER
        bool "Release-master support for MC"
        depends on PLUGIN_MC && RELEASE_MASTER
diff --git a/litmus/budget.c b/litmus/budget.c
index b1c0a4b84c02..dd5a2b1e45d9 100644
--- a/litmus/budget.c
+++ b/litmus/budget.c
@@ -9,6 +9,8 @@
 
 DEFINE_PER_CPU(struct enforcement_timer, budget_timer);
 
+#define TRACE(fmt, args...) STRACE(fmt, ## args)
+
 enum hrtimer_restart on_enforcement_timeout(struct hrtimer *timer)
 {
         struct enforcement_timer* et = container_of(timer,
diff --git a/litmus/dgl.c b/litmus/dgl.c
index e2286b3e9239..cced7c259735 100644
--- a/litmus/dgl.c
+++ b/litmus/dgl.c
@@ -28,6 +28,11 @@
 #define req_group(r, i) (container_of(((void*)r) - sizeof(*r)*(i),      \
                                       struct dgl_group_req, requests))
 
+#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)
+
 /* Resource id -> word, bit */
 static inline void mask_idx(int resource, int *word, int *bit)
 {
@@ -35,7 +40,6 @@ static inline void mask_idx(int resource, int *word, int *bit)
         *bit  = resource % MASK_SIZE;
 }
 
-
 static void print_waiting(struct dgl *dgl, struct dgl_resource *resource)
 {
         struct dgl_req *pos;
@@ -45,12 +49,14 @@ 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("  0x%p with timestamp %llu\n", greq, greq->ts);
+                TRACE_GREQ(greq, "with timestamp %llu\n", greq->ts);
                 BUG_ON(greq->ts < last);
                 last = greq->ts;
         }
 }
 
+static void dummy_acquired(int cpu){}
+
 void dgl_init(struct dgl *dgl, unsigned long num_resources,
               unsigned long num_replicas)
 {
@@ -78,6 +84,7 @@ void dgl_init(struct dgl *dgl, unsigned long num_resources,
         dgl->requests = 0;
         dgl->running  = 0;
         dgl->ts = 0;
+        dgl->cpu_acquired = dummy_acquired;
 }
 
 void dgl_free(struct dgl *dgl)
@@ -130,7 +137,7 @@ void set_req(struct dgl *dgl, struct dgl_group_req *greq,
         mask_idx(resource, &word, &bit);
         __set_bit(bit, &greq->requested[word]);
 
-        TRACE("0x%p requesting %d of %d\n", greq, replicas, resource);
+        TRACE_GREQ(greq, "requesting %d of %d\n", replicas, resource);
 
         req = &greq->requests[resource];
         req->greq = greq;
@@ -153,27 +160,27 @@ static unsigned long try_acquire(struct dgl *dgl, struct dgl_resource *resource,
         rid  = resource_id(dgl, resource);
         greq = req->greq;
 
-        TRACE("0x%p greq\n", 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("0x%p cannot acquire %d replicas, %d free\n",
-                      greq, req->replicas, resource->free_replicas,
+                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("0x%p acquired %d replicas of rid %d\n",
-              greq, req->replicas, rid);
+        TRACE_GREQ(greq, "0x%p acquired %d replicas of rid %d\n",
+                   req->replicas, rid);
 
         mask_idx(rid, &word, &bit);
 
-        TRACE("0x%p, %lu, 0x%p\n", greq->waiting, greq->waiting[word],
+        TRACE_GREQ(greq, "0x%p, %lu, 0x%p\n", greq->waiting, greq->waiting[word],
               &greq->waiting[word]);
 
         clear_bit(bit, &greq->waiting[word]);
@@ -186,11 +193,19 @@ static unsigned long try_acquire(struct dgl *dgl, struct dgl_resource *resource,
         }
 
         if (!waiting) {
-                TRACE("0x%p acquired all resources\n", greq);
+                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++;
+
+                if (greq->task) {
+                        BUG_ON(tsk_rt(greq->task)->linked_on == NO_CPU);
+                        set_rt_flags(greq->task, RT_F_RUNNING);
+                        sched_trace_task_resume(greq->task);
+                }
+
+                dgl->cpu_acquired(greq->cpu);
         }
 
         return 1;
@@ -208,7 +223,7 @@ void add_group_req(struct dgl *dgl, struct dgl_group_req *greq, int cpu)
         greq->cpu = cpu;
         greq->ts = dgl->ts++;
 
-        TRACE("0x%p group request added for CPU %d\n", greq, cpu);
+        TRACE_GREQ(greq, "group request added for CPU %d\n", cpu);
         BUG_ON(dgl->acquired[cpu] == greq);
 
         ++dgl->requests;
@@ -225,14 +240,14 @@ void add_group_req(struct dgl *dgl, struct dgl_group_req *greq, int cpu)
                 all_succ &= succ;
 
                 if (!succ) {
-                        TRACE("0x%p waiting on rid %d\n", greq, i);
+                        TRACE_GREQ(greq, "waiting on rid %d\n", i);
                         list_add_tail(&req->list, &resource->waiting);
                 }
         }
 
         /* Grant empty requests */
         if (all_succ && !dgl->acquired[cpu]) {
-                TRACE("0x%p empty group request acquired cpu %d\n", greq, cpu);
+                TRACE_GREQ(greq, "empty group request acquired cpu %d\n", cpu);
                 dgl->acquired[cpu] = greq;
                 ++dgl->running;
         }
@@ -251,12 +266,12 @@ void remove_group_req(struct dgl *dgl, struct dgl_group_req *greq)
         struct dgl_req *req, *next;
         struct dgl_resource *resource;
 
-        TRACE("0x%p removing group request for CPU %d\n", greq, greq->cpu);
+        TRACE_GREQ(greq, "removing group request for CPU %d\n", greq, greq->cpu);
 
         --dgl->requests;
 
         if (dgl->acquired[greq->cpu] == greq) {
-                TRACE("0x%p no longer acquired on CPU %d\n", greq, greq->cpu);
+                TRACE_GREQ(greq, "no longer acquired on CPU %d\n", greq->cpu);
                 dgl->acquired[greq->cpu] = NULL;
                 --dgl->running;
         }
@@ -275,8 +290,8 @@ void remove_group_req(struct dgl *dgl, struct dgl_group_req *greq)
                         /* Have resource */
                         resource->free_replicas += req->replicas;
                         BUG_ON(resource->free_replicas > dgl->num_replicas);
-                        TRACE("0x%p releasing %d of %d replicas, rid %d\n",
-                              greq, req->replicas, resource->free_replicas, i);
+                        TRACE_GREQ(greq, "releasing %d of %d replicas, rid %d\n",
+                              req->replicas, resource->free_replicas, i);
 
                         if (!list_empty(&resource->waiting)) {
                                 /* Give it to the next guy */
diff --git a/litmus/domain.c b/litmus/domain.c
index 54c060d4c8af..0852f30b428e 100644
--- a/litmus/domain.c
+++ b/litmus/domain.c
@@ -3,6 +3,9 @@
 
 #include <litmus/domain.h>
 
+void dummy_acquire(struct task_struct *t){};
+void dummy_release(struct task_struct *t){};
+
 void domain_init(domain_t *dom,
                  raw_spinlock_t *lock,
                  requeue_t requeue,
@@ -16,4 +19,7 @@ void domain_init(domain_t *dom,
         dom->peek_ready = peek_ready;
         dom->take_ready = take_ready;
         dom->higher_prio = priority;
+
+        dom->acquire_resources = dummy_acquire;
+        dom->release_resources = dummy_release;
 }
diff --git a/litmus/rt_domain.c b/litmus/rt_domain.c
index b70c94695a58..687dc129bc2a 100644
--- a/litmus/rt_domain.c
+++ b/litmus/rt_domain.c
@@ -22,8 +22,10 @@
 #include <litmus/event_group.h>
 #endif
 
+#define TRACE(fmt, args...) STRACE(fmt, ## args)
+
 /* Uncomment when debugging timer races... */
-#if 1
+#if 0
 #define VTRACE_TASK TRACE_TASK
 #define VTRACE TRACE
 #else
diff --git a/litmus/sched_mc.c b/litmus/sched_mc.c
index 8c2238c6bf43..d0a56fc664c0 100644
--- a/litmus/sched_mc.c
+++ b/litmus/sched_mc.c
@@ -1,52 +1,8 @@
 /*
  * litmus/sched_mc.c
- *
  * Implementation of the Mixed Criticality scheduling algorithm.
  *
  * (Per Mollison, Erickson, Anderson, Baruah, Scoredos 2010)
- *
- * Absolute first: relative time spent doing different parts of release
- * and scheduling overhead needs to be measured and graphed.
- *
- * Domain locks should be more fine-grained. There is no reason to hold the
- * ready-queue lock when adding a task to the release-queue.
- *
- * The levels should be converted to linked-lists so that they are more
- * adaptable and need not be identical on all processors.
- *
- * The interaction between remove_from_all and other concurrent operations
- * should be re-examined. If a job_completion and a preemption happen
- * simultaneously, a task could be requeued, removed, then requeued again.
- *
- * Level-C tasks should be able to swap CPUs a-la GSN-EDF. They should also
- * try and swap with the last CPU they were on. This could be complicated for
- * ghost tasks.
- *
- * Locking for timer-merging could be infinitely more fine-grained. A second
- * hash could select a lock to use based on queue slot. This approach might
- * also help with add_release in rt_domains.
- *
- * It should be possible to reserve a CPU for ftdumping.
- *
- * The real_deadline business seems sloppy.
- *
- * The amount of data in the header file should be cut down. The use of the
- * header file in general needs to be re-examined.
- *
- * The plugin needs to be modified so that it doesn't freeze when it is
- * deactivated in a VM.
- *
- * The locking in check_for_preempt is not fine-grained enough.
- *
- * The size of the structures could be smaller. Debugging info might be
- * excessive as things currently stand.
- *
- * The macro can_requeue has been expanded too much. Anything beyond
- * scheduled_on is a hack!
- *
- * Domain names (rt_domain) are still clumsy.
- *
- * Should BE be moved into the kernel? This will require benchmarking.
  */
 #include <linux/spinlock.h>
 #include <linux/percpu.h>
@@ -67,9 +23,9 @@
 #include <litmus/bheap.h>
 #include <litmus/event_group.h>
 #include <litmus/budget.h>
-
 #include <litmus/sched_mc.h>
 #include <litmus/ce_domain.h>
+#include <litmus/dgl.h>
 
 /**
  * struct cpu_entry - State of a CPU for the entire MC system
@@ -84,6 +40,7 @@
  */
 struct cpu_entry {
         int                     cpu;
+        int                     lock_acquired;
         struct task_struct*     scheduled;
         struct task_struct*     will_schedule;
         struct task_struct*     linked;
@@ -98,11 +55,20 @@ struct cpu_entry {
 #endif
 };
 
+static struct dgl group_lock;
+static raw_spinlock_t dgl_lock;
+
 DEFINE_PER_CPU(struct cpu_entry, cpus);
 #ifdef CONFIG_RELEASE_MASTER
 static int interrupt_cpu;
 #endif
 
+#ifdef CONFIG_NP_SECTION
+#define has_resources(t, c)   (tsk_rt(t)->req == group_lock.acquired[c])
+#else
+#define has_resources(t, c)   (1)
+#endif
+
 #define domain_data(dom)  (container_of(dom, struct domain_data, domain))
 #define is_global(dom)    (domain_data(dom)->heap)
 #define is_global_task(t) (is_global(get_task_domain(t)))
@@ -117,16 +83,50 @@ static int interrupt_cpu;
 #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])
-#define TRACE_ENTRY(e, fmt, args...)                            \
-        STRACE("P%d, linked=" TS " " fmt, e->cpu, TA(e->linked), ##args)
-#define TRACE_CRIT_ENTRY(ce, fmt, args...)                      \
-        STRACE("%s P%d, linked=" TS " " fmt,                    \
-              (ce)->domain->name, crit_cpu(ce)->cpu, TA((ce)->linked), ##args)
 
-static int sid(struct crit_entry *ce)
+/*
+ * 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 task_struct *sched;
+
+        /* Can't contend for resources if not logically running */
+        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;
+        }
+
+        cpu = tsk_rt(t)->linked_on;
+
+        if (!has_resources(t, cpu)) {
+                set_rt_flags(t, RT_F_BLOCKED);
+                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);
+        }
+
+        raw_spin_unlock(&dgl_lock);
+}
+
+static void release_resources(struct task_struct *t)
 {
-        int level = ce->level * num_online_cpus() + crit_cpu(ce)->cpu + 1;
-        return -level;
+        if (is_kernel_np(t)) {
+                TRACE_MC_TASK(t, "Releasing resources\n");
+
+                raw_spin_lock(&dgl_lock);
+                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");
+        }
 }
 
 /*
@@ -145,13 +145,13 @@ static int cpu_lower_prio(struct bheap_node *a, struct bheap_node *b)
 
         if (first->state == CS_REMOVED || second->state == CS_REMOVED) {
                 /* Removed entries go at the back of the heap */
-                return first->state  != CS_REMOVED &&
-                       second->state != CS_REMOVED;
+                return second->state == CS_REMOVED &&
+                        first->state != CS_REMOVED;
         } else if (!first_link || !second_link) {
-                /* Entry with nothing scheduled is lowest priority */
+                /* Entry with nothing scheduled is lowest priority (front) */
                 return second_link && !first_link;
         } else {
-                /* Sort by deadlines of tasks */
+                /* Sort by deadlines of tasks (later deadlines first) */
                 domain = get_task_domain(first_link);
                 return domain->higher_prio(second_link, first_link);
         }
@@ -168,7 +168,8 @@ static int mc_preempt_needed(struct domain *dom, struct task_struct* curr)
                 return next && !curr;
         } else {
                 BUG_ON(tsk_mc_crit(next) != tsk_mc_crit(curr));
-                return get_task_domain(next)->higher_prio(next, curr);
+                return !is_np(curr) &&
+                        get_task_domain(next)->higher_prio(next, curr);
         }
 }
 
@@ -268,14 +269,11 @@ static void update_ghost_time(struct task_struct *p)
         BUG_ON(!is_ghost(p));
         if (unlikely ((s64)delta < 0)) {
                 delta = 0;
-                TRACE_MC_TASK(p, "WARNING: negative time delta\n");
         }
         if (budget_remaining(p) <= delta) {
-                TRACE_MC_TASK(p, "Ghost job could have ended\n");
                 tsk_rt(p)->job_params.exec_time = get_exec_cost(p);
                 p->se.exec_start = clock;
         } else {
-                TRACE_MC_TASK(p, "Ghost job updated, but didn't finish\n");
                 tsk_rt(p)->job_params.exec_time += delta;
                 p->se.exec_start = clock;
         }
@@ -298,6 +296,8 @@ static void link_task_to_crit(struct crit_entry *ce,
 
         /* Unlink last task */
         if (ce->linked) {
+                ce->domain->release_resources(ce->linked);
+
                 TRACE_MC_TASK(ce->linked, "Unlinking\n");
                 ce->linked->rt_param.linked_on = NO_CPU;
                 if (is_ghost(ce->linked)) {
@@ -307,7 +307,7 @@ static void link_task_to_crit(struct crit_entry *ce,
                                 update_ghost_time(ce->linked);
                         }
                 }
-                sched_trace_server_switch_away(sid(ce), 0, -ce->linked->pid,
+                sched_trace_server_switch_away(ce_sid(ce), 0, -ce->linked->pid,
                                                get_rt_job(ce->linked));
         }
 
@@ -318,14 +318,17 @@ static void link_task_to_crit(struct crit_entry *ce,
                 if (is_ghost(task) && CRIT_LEVEL_A != tsk_mc_crit(task)) {
                         /* There is a level-A timer that will force a
                          * preemption, so we don't set this for level-A
-                         * tasks. Otherwise reset the budget timer.
+                         * tasks. Otherwise reset the budget timer
                          */
                         task->se.exec_start = litmus_clock();
                         when_to_fire = task->se.exec_start + budget_remaining(task);
                         arm_ghost(ce, when_to_fire);
                 }
-                sched_trace_server_switch_to(sid(ce), 0, -task->pid,
+                sched_trace_server_switch_to(ce_sid(ce), 0, -task->pid,
                                              get_rt_job(ce->linked));
+
+                if (!is_ghost(task))
+                        ce->domain->acquire_resources(task);
         }
 }
 
@@ -448,12 +451,17 @@ 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)) {
+                if (!can_use(ce))
                         ce->state = CS_ACTIVATE;
-                }
         }
 }
 
+static void preempt_cpu(struct cpu_entry *entry, struct task_struct *t)
+{
+        link_task_to_cpu(entry, t);
+        litmus_reschedule(entry->cpu);
+}
+
 /**
  * preempt() - Preempt a logically running task with a higher priority one.
  * @dom Domain from which to draw higher priority task
@@ -461,7 +469,7 @@ static void link_task_to_cpu(struct cpu_entry *entry, struct task_struct *task)
  *
  * Caller must hold the lock for @dom and @ce's CPU lock.
  */
-static void preempt(struct domain *dom, struct crit_entry *ce)
+static void preempt_crit(struct domain *dom, struct crit_entry *ce)
 {
         struct task_struct *task = dom->take_ready(dom);
         struct cpu_entry *entry = crit_cpu(ce);
@@ -483,14 +491,12 @@ static void preempt(struct domain *dom, struct crit_entry *ce)
          * been disabled and a preemption could not have occurred
          */
         if (!is_ghost(task)) {
-                link_task_to_cpu(entry, task);
-                preempt_if_preemptable(entry->scheduled, entry->cpu);
+                preempt_cpu(entry, task);
         } else if (old && old == entry->linked) {
                 /* Preempted a running task with a ghost job. Null needs to be
                  * running.
                  */
-                link_task_to_cpu(entry, NULL);
-                preempt_if_preemptable(entry->scheduled, entry->cpu);
+                preempt_cpu(entry, NULL);
         }
 }
 
@@ -525,6 +531,7 @@ 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);
         }
@@ -548,13 +555,14 @@ static void update_crit_levels(struct cpu_entry *entry)
  */
 static void check_for_preempt(struct domain *dom)
 {
-        int recheck = 1;
+        int recheck = 1, higher_prio, was_ghost;
         struct cpu_entry *entry;
         struct crit_entry *ce;
 
+
         if (is_global(dom)) {
                 /* Loop until we find a non-preemptable CPU */
-                while ((ce = lowest_prio_cpu(dom)) && recheck) {
+                while (recheck &&(ce = lowest_prio_cpu(dom))) {
                         entry = crit_cpu(ce);
                         recheck = 1;
 
@@ -567,12 +575,10 @@ static void check_for_preempt(struct domain *dom)
                                 fix_crit_position(ce);
                         else if (mc_preempt_needed(dom, ce->linked))
                                 /* Success! Check for more preemptions */
-                                preempt(dom, ce);
-                        else {
+                                preempt_crit(dom, ce);
+                        else
                                 /* Failure! */
                                 recheck = 0;
-                                TRACE_CRIT_ENTRY(ce, "Stopped global check\n");
-                        }
                         raw_spin_unlock(&entry->lock);
                 }
         } else /* Partitioned */ {
@@ -583,12 +589,23 @@ static void check_for_preempt(struct domain *dom)
                 dom->peek_ready(dom);
 
                 raw_spin_lock(&entry->lock);
-                if (can_use(ce) && mc_preempt_needed(dom, ce->linked)) {
-                        preempt(dom, ce);
-                        update_crit_levels(entry);
-                } else {
-                        raw_spin_unlock(&entry->lock);
+
+                if (can_use(ce)) {
+                        was_ghost = ce->linked && !is_ghost(ce->linked) &&
+                                ce->linked != entry->linked;
+                        higher_prio = mc_preempt_needed(dom, ce->linked);
+
+                        if (was_ghost)
+                                preempt_cpu(entry, ce->linked);
+                        else if (higher_prio)
+                                preempt_crit(dom, ce);
+
+                        if (was_ghost || higher_prio) {
+                                update_crit_levels(entry);
+                                return;
+                        }
                 }
+                raw_spin_unlock(&entry->lock);
         }
 }
 
@@ -648,6 +665,8 @@ 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;
         TRACE_MC_TASK(task, "Completed\n");
 
         /* Logically stop the task execution */
@@ -655,20 +674,34 @@ static void job_completion(struct task_struct *task, int forced)
         remove_from_all(task);
 
         if (!forced) {
-                /* Userspace releases */
+                /* Userspace signaled job completion */
                 sched_trace_task_completion(current, 0);
                 setup_user_release(current, get_user_deadline(current));
+
         }
 
+        release_server = budget_exhausted(task);
+#ifndef CONFIG_PLUGIN_MC_LINUX_SLACK_STEALING
+        /* Release lowest-criticality task's servers with their userspace tasks,
+         * preventing them from getting behind userspace and forcing idleness
+         */
+        if (tsk_mc_crit(task) == NUM_CRIT_LEVELS - 1)
+                release_server = 1;
+#endif
 
         /* If server has run out of budget, wait until next release
          * TODO: Level A does this independently and should not.
          */
-        if (budget_exhausted(task) && CRIT_LEVEL_A != tsk_mc_crit(task)) {
+        if (release_server && CRIT_LEVEL_A != tsk_mc_crit(task)) {
                 sched_trace_server_completion(-task->pid, get_rt_job(task));
                 prepare_for_next_period(task);
         }
 
+        now = litmus_clock();
+        if (lt_before(get_user_release(task), now) || forced) {
+                set_rt_flags(task, RT_F_RUNNING);
+        }
+
         /* Requeue non-blocking tasks */
         if (is_running(task))
                 job_arrival(task);
@@ -691,7 +724,6 @@ static enum hrtimer_restart mc_ghost_exhausted(struct hrtimer *timer)
         struct task_struct *tmp = NULL;
 
         local_irq_save(flags);
-        TRACE("Ghost exhausted\n");
         TRACE_CRIT_ENTRY(ce, "Firing here at %llu\n", litmus_clock());
 
         /* Due to race conditions, we cannot just set the linked
@@ -796,7 +828,6 @@ static enum hrtimer_restart ce_timer_function(struct hrtimer *timer)
 }
 #endif /* CONFIG_MERGE_TIMERS */
 
-
 /**
  * mc_release_jobs() - Add heap of tasks to the system, check for preemptions.
  */
@@ -820,8 +851,12 @@ static void mc_release_jobs(rt_domain_t* rt, struct bheap* tasks)
 static void mc_task_new(struct task_struct *t, int on_rq, int running)
 {
         unsigned long flags;
+        int i;
         struct cpu_entry* entry;
         enum crit_level level = tsk_mc_crit(t);
+        struct dgl_group_req *req;
+        struct control_page  *cp = tsk_rt(t)->ctrl_page;
+        struct color_ctrl_page *ccp = tsk_rt(t)->color_ctrl_page;
 
         local_irq_save(flags);
         TRACE("New mixed criticality task %d\n", t->pid);
@@ -836,6 +871,18 @@ static void mc_task_new(struct task_struct *t, int on_rq, int running)
                 entry = &per_cpu(cpus, task_cpu(t));
         t->rt_param._domain = entry->crit_entries[level].domain;
 
+        /* Setup color request */
+        req = kmalloc(sizeof(*req), GFP_ATOMIC);
+        req->task = t;
+        tsk_rt(t)->req = req;
+        if (cp && ccp) {
+                TRACE_MC_TASK(t, "Initializing group request\n");
+                cp->colors_updated = 0;
+                dgl_group_req_init(&group_lock, req);
+                for (i = 0; ccp->pages[i]; ++i)
+                        set_req(&group_lock, req, ccp->colors[i], ccp->pages[i]);
+        }
+
         /* Userspace and kernelspace view of task state may differ.
          * Model kernel state as an additional container
          */
@@ -1019,6 +1066,16 @@ static struct task_struct* mc_schedule(struct task_struct* prev)
                         job_arrival(entry->scheduled);
         }
 
+        /* Signal sent by lock acquisition */
+        if (entry->lock_acquired < NUM_CRIT_LEVELS) {
+                STRACE("Lock acquired for %d\n", entry->lock_acquired);
+                dom = entry->crit_entries[entry->lock_acquired].domain;
+                raw_spin_lock(dom->lock);
+                check_for_preempt(dom);
+                raw_spin_unlock(dom->lock);
+                entry->lock_acquired = NUM_CRIT_LEVELS;
+        }
+
         /* Pick next task if none is linked */
         raw_spin_lock(&entry->lock);
         for (i = 0; i < NUM_CRIT_LEVELS && !entry->linked; i++) {
@@ -1037,23 +1094,26 @@ static struct task_struct* mc_schedule(struct task_struct* prev)
 
                 raw_spin_lock(&entry->lock);
 
-                if (!entry->linked && !ce->linked && dtask && can_use(ce)) {
-                        /* Pop dtask */
-                        dom->take_ready(dom);
-
-                        link_task_to_crit(ce, dtask);
-                        update_crit_position(ce);
-
-                        /* Actual running task found */
-                        ready_task = (is_ghost(dtask)) ? NULL : dtask;
-                        if (ready_task) {
-                                link_task_to_cpu(entry, ready_task);
-                                raw_spin_unlock(dom->lock);
-                                update_crit_levels(entry);
-                                raw_spin_lock(&entry->lock);
-                                continue;
+                ready_task = NULL;
+                if (!entry->linked && can_use(ce)) {
+                        if (ce->linked) {
+                                ready_task = ce->linked;
+                        } else if (dtask) {
+                                /* Need a new task */
+                                dom->take_ready(dom);
+                                ready_task = dtask;
+
+                                link_task_to_crit(ce, dtask);
+                                update_crit_position(ce);
                         }
                 }
+                if (ready_task && !is_ghost(ready_task)) {
+                        link_task_to_cpu(entry, ready_task);
+                        raw_spin_unlock(dom->lock);
+                        update_crit_levels(entry);
+                        raw_spin_lock(&entry->lock);
+                        continue;
+                }
                 raw_spin_unlock(dom->lock);
         }
 
@@ -1070,7 +1130,8 @@ static struct task_struct* mc_schedule(struct task_struct* prev)
                 BUG_ON(!get_rt_job(next));
                 TRACE_MC_TASK(next, "Picked this task\n");
         } else if (exists && !next)
-                TRACE_ENTRY(entry, "Becomes idle at %llu\n", litmus_clock());
+                STRACE("CPU %d becomes idle at %llu\n",
+                       entry->cpu, litmus_clock());
         return next;
 }
 
@@ -1173,7 +1234,7 @@ static void mc_release_ts(lt_t time)
                 entry = &per_cpu(cpus, cpu);
                 sched_trace_container_param(++cont_id, (const char*)&name);
                 ce = &entry->crit_entries[level];
-                sched_trace_server_param(sid(ce), cont_id, 0, 0);
+                sched_trace_server_param(ce_sid(ce), cont_id, 0, 0);
         }
 
         level = CRIT_LEVEL_B;
@@ -1182,7 +1243,7 @@ static void mc_release_ts(lt_t time)
                 entry = &per_cpu(cpus, cpu);
                 sched_trace_container_param(++cont_id, (const char*)&name);
                 ce = &entry->crit_entries[level];
-                sched_trace_server_param(sid(ce), cont_id, 0, 0);
+                sched_trace_server_param(ce_sid(ce), cont_id, 0, 0);
         }
 
         level = CRIT_LEVEL_C;
@@ -1191,7 +1252,7 @@ static void mc_release_ts(lt_t time)
         for_each_online_cpu(cpu) {
                 entry = &per_cpu(cpus, cpu);
                 ce = &entry->crit_entries[level];
-                sched_trace_server_param(sid(ce), cont_id, 0, 0);
+                sched_trace_server_param(ce_sid(ce), cont_id, 0, 0);
         }
 
         mc_ce_release_at_common(NULL, time);
@@ -1291,6 +1352,14 @@ static inline void init_edf_domain(struct domain *dom, rt_domain_t *rt,
 #endif
 }
 
+static void cpu_acquired(int cpu)
+{
+        struct cpu_entry *entry = &per_cpu(cpus, cpu);
+        STRACE("Lock acquired for cpu %d\n", cpu);
+        entry->lock_acquired = CRIT_LEVEL_B;
+        litmus_reschedule(entry->cpu);
+}
+
 struct domain_data *ce_domain_for(int);
 static int __init init_mc(void)
 {
@@ -1308,6 +1377,7 @@ static int __init init_mc(void)
                 entry->cpu = cpu;
                 entry->scheduled = NULL;
                 entry->linked = NULL;
+                entry->lock_acquired = NUM_CRIT_LEVELS;
 
                 raw_spin_lock_init(&entry->lock);
 
@@ -1333,6 +1403,8 @@ static int __init init_mc(void)
                 rt = &per_cpu(_mc_crit_b_rt, cpu);
                 init_local_domain(entry, dom_data, CRIT_LEVEL_B);
                 init_edf_domain(&dom_data->domain, rt, CRIT_LEVEL_B, 1, cpu);
+                dom_data->domain.acquire_resources = acquire_resources;
+                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";
@@ -1347,6 +1419,11 @@ static int __init init_mc(void)
         raw_spin_lock_init(c_dom_lock);
         _mc_crit_c.domain.name = "LVL-C";
 
+        dgl_init(&group_lock, color_cache_info.nr_colors,
+                 color_cache_info.ways);
+        group_lock.cpu_acquired = cpu_acquired;
+        raw_spin_lock_init(&dgl_lock);
+
         return register_sched_plugin(&mc_plugin);
 }