path: root/litmus/sched_mc2.c

/*
 * litmus/sched_mc2.c
 *
 * Implementation of the Mixed-Criticality on MultiCore scheduler
 *
 * Thus plugin implements a scheduling algorithm proposed in 
 * "Mixed-Criticality Real-Time Scheduling for Multicore System" paper.
 */ 
 
#include <linux/percpu.h>
#include <linux/slab.h>
#include <linux/rwlock.h>
#include <asm/uaccess.h>
#include <linux/delay.h>

#include <litmus/sched_plugin.h>
#include <litmus/preempt.h>
#include <litmus/debug_trace.h>

#include <litmus/litmus.h>
#include <litmus/jobs.h>
#include <litmus/budget.h>
#include <litmus/litmus_proc.h>
#include <litmus/sched_trace.h>
#include <litmus/cache_proc.h>
#include <litmus/trace.h>

#include <litmus/mc2_common.h>
#include <litmus/reservation.h>
#include <litmus/polling_reservations.h>

#ifdef CONFIG_PGMRT_SUPPORT
#include <litmus/pgm.h>
#endif

//#define TRACE(fmt, args...) do {} while (false)
//#define TRACE_TASK(fmt, args...) do {} while (false)

#define BUDGET_ENFORCEMENT_AT_C 0

extern atomic_t num_sync_released;
extern void do_partition(enum crit_level lv, int cpu);

/* _global_env - reservation container for level-C tasks*/
struct gmp_reservation_environment _global_env_modes[NR_MODES];
struct gmp_reservation_environment *_global_env;
raw_spinlock_t global_lock;

/* cpu_entry - keep track of a running task on a cpu
 * This state is used to decide the lowest priority cpu
 */
struct cpu_entry {
	struct task_struct *scheduled;
	lt_t deadline;
	int cpu;
	enum crit_level lv;
	/* if will_schedule is true, this cpu is already selected and
	   call mc2_schedule() soon. */
	bool will_schedule;
};

/* cpu_priority - a global state for choosing the lowest priority CPU */
struct cpu_priority {
	raw_spinlock_t lock;
	struct cpu_entry cpu_entries[NR_CPUS];
};

struct cpu_priority _lowest_prio_cpu;
	
/* mc2_task_state - a task state structure */
struct mc2_task_state {
	struct task_client res_info[NR_MODES];
	/* if cpu == -1, this task is a global task (level C) */
	int cpu;
	bool has_departed;
	struct mc2_task mc2_param;
};

/* mc2_cpu_state - maintain the scheduled state and ghost jobs
 * timer : timer for partitioned tasks (level A and B)
 * g_timer : timer for global tasks (level C)
 */
struct mc2_cpu_state {
	raw_spinlock_t lock;

	struct sup_reservation_environment sup_env_modes[NR_MODES];
	struct sup_reservation_environment *sup_env;
	struct hrtimer timer;

	int cpu;
	struct task_struct* scheduled;
	//struct crit_entry crit_entries[NUM_CRIT_LEVELS];
	//bool spin_flag; //not used on cpu 0
};

static void mc2_update_timer_and_unlock(struct mc2_cpu_state *state);

static int resched_cpu[NR_CPUS];
static DEFINE_PER_CPU(struct mc2_cpu_state, mc2_cpu_state);
//level_a_priorities unused
//static int level_a_priorities[NR_CPUS];

#define cpu_state_for(cpu_id)	(&per_cpu(mc2_cpu_state, cpu_id))
#define local_cpu_state()	(this_cpu_ptr(&mc2_cpu_state))


unsigned int mode; //currently executing mode, from 0 to NR_MODES-1
unsigned int requested_mode; //The pending mode
/* Prevent multiple requests from entering and prevent request from entering while old
 * is being enacted */
raw_spinlock_t mode_lock;

unsigned int mode_sizes[NR_MODES];
unsigned int res_reported;
bool cpu_0_spin_flag;
bool seen_once;
bool cpu_0_task_exist;
bool mode_changed;
bool mode_poll_exited;
static DEFINE_PER_CPU(unsigned long, mode_counter);
#define local_mode_counter()	(this_cpu_ptr(&mode_counter))
#define cpu_0_mode_counter()	(&per_cpu(mode_counter, 0))
#define in_mode(t, modenum)	(tsk_mc2_data(t)->mode_mask & (1 << modenum))
#define pending			(mode != requested_mode)
#define ready			(!res_reported)

/*
 * To be called from level A task's with period equal to
 * A and B hyperperiod
 */

asmlinkage long sys_enact_mode(void)
{
	struct mc2_cpu_state *state = local_cpu_state();
	struct reservation *res;
	struct list_head *pos;
	unsigned long flags;
	TRACE_TASK(current, "ENACTING SYSCALL\n");
	if (state->cpu == 0 && !mode_poll_exited){
		unsigned long *other_cpu_counter;
		unsigned long cpu0_val = this_cpu_read(mode_counter);
		int i;
		for(i = 1; i < NR_CPUS; i++){
			other_cpu_counter = &per_cpu(mode_counter, i);
			while(cpu0_val == *other_cpu_counter && !mode_poll_exited){
				udelay(1);
			}
		}
		mode_changed = false;
		if (pending){ //MCR has entered
			raw_spin_lock_irqsave(&state->lock,flags);
			//TRACE_CUR("LOCK_MSG: GLOBAL LOCK\n");
			raw_spin_lock(&global_lock);
			raw_spin_lock(&mode_lock);
		
			if (!seen_once){
				TRACE_TASK(current, "REQUEST\n");
				sched_trace_request_mode(current);
				//clean up jobs that are already done
				//after this jobs report themselves
				list_for_each(pos, &_global_env->active_reservations){
					res = list_entry(pos, struct reservation, list);
					if (tsk_rt(res->tsk)->completed && res->mode == mode && !res->reported){
						res->reported = 1;
						TRACE_TASK(res->tsk,"R%d RES_REPORTED_ACTIVE = %d mode %d\n", res->id, res_reported, res->mode);
						res_reported--;
					}
				}
				list_for_each(pos, &_global_env->depleted_reservations){
					res = list_entry(pos, struct reservation, list);
					if (tsk_rt(res->tsk)->completed && res->mode == mode && !res->reported){
						res->reported = 1;
						TRACE_TASK(res->tsk,"R%d RES_REPORTED_DEPLETED = %d mode %d\n",res->id, res_reported, res->mode);
						res_reported--;
					}

				}
				list_for_each(pos, &_global_env->inactive_reservations){
					res = list_entry(pos, struct reservation, list);
					if (tsk_rt(res->tsk)->completed && res->mode == mode && !res->reported){
						res->reported = 1;
						TRACE_TASK(res->tsk,"R%d RES_REPORTED_INACTIVE = %d mode %d\n", res->id, res_reported, res->mode);
						res_reported--;
					}
				}
				seen_once = true;
			}
			if (seen_once && !(ready)){
				TRACE("Mode change waiting on tasks\n");
				list_for_each(pos, &_global_env->active_reservations){
					res = list_entry(pos, struct reservation, list);
					if (!res->reported){
						TRACE_TASK(res->tsk, "R%d is ACTIVE LIST with state %d scheduled on %d and hasn't reported\n", res->id, res->state, res->scheduled_on);
					}
				}
				list_for_each(pos, &_global_env->depleted_reservations){
					res = list_entry(pos, struct reservation, list);
					if (!res->reported){
						TRACE_TASK(res->tsk, "R%d is DEPLETED LIST and hasn't reported\n", res->id);
					}
				}
				list_for_each(pos, &_global_env->inactive_reservations){
					res = list_entry(pos, struct reservation, list);
					if (!res->reported){
						TRACE_TASK(res->tsk, "R%d is INACTIVE LIST and hasn't reported\n", res->id);
					}
				}
			}
			if( ready ){ //C is throttled
				lt_t new_mode_basetime = get_release(current);
				//TRACE("Timer canceled\n");
				//hrtimer_cancel(&state->timer);//stop listening to old mode timers
				mode = requested_mode;
				TRACE("Mode has been changed.\n");
				mode_changed = true;
				_global_env = &_global_env_modes[mode];
				//set res->reported for new global tasks
				list_for_each(pos, &_global_env->active_reservations){
					res = list_entry(pos, struct reservation, list);
					release_at(res->tsk, new_mode_basetime);
					res->reported = 0;
				}
				list_for_each(pos, &_global_env->depleted_reservations){
					res = list_entry(pos, struct reservation, list);
					release_at(res->tsk, new_mode_basetime);
					res->reported = 0;
				}
				list_for_each(pos, &_global_env->inactive_reservations){
					res = list_entry(pos, struct reservation, list);
					release_at(res->tsk, new_mode_basetime);
					res->reported = 0;
				}
				gmp_update_time(_global_env, litmus_clock());
				//raw_spin_lock(&state->lock);
		
				state->sup_env = &state->sup_env_modes[mode];
				list_for_each(pos, &state->sup_env->active_reservations){
					res = list_entry(pos, struct reservation, list);
					release_at(res->tsk, new_mode_basetime);
				}
				list_for_each(pos, &state->sup_env->depleted_reservations){
					res = list_entry(pos, struct reservation, list);
					release_at(res->tsk, new_mode_basetime);
				}
				list_for_each(pos, &state->sup_env->inactive_reservations){
					res = list_entry(pos, struct reservation, list);
					release_at(res->tsk, new_mode_basetime);
				}
				sup_update_time(state->sup_env, litmus_clock());
				//raw_spin_unlock(&state->lock);
				sched_trace_enact_mode(current);
				TRACE("ENACT\n");
			}
			raw_spin_unlock(&mode_lock);
			//TRACE_CUR("LOCK_MSG: GLOBAL UNLOCK\n");
			raw_spin_unlock(&global_lock);
			//raw_spin_unlock(&state->lock);
			raw_spin_unlock_irqrestore(&state->lock, flags);
			
			raw_spin_lock(&state->lock);
			mc2_update_timer_and_unlock(state);
		}
		this_cpu_inc(mode_counter);
		//local_irq_restore(flags);
		//cpu_0_spin_flag = !cpu_0_spin_flag;
	}
	else if (!mode_poll_exited) {
		unsigned long *cpu0_counter = cpu_0_mode_counter();
		unsigned long my_val;
		//int timeout = 0;
		this_cpu_inc(mode_counter);
		my_val = this_cpu_read(mode_counter);
		//spin, wait for CPU 0 to stabilize mode decision
		//before scheduling next hyperperiod
		//TRACE("CPU%d start spinning. %d\n",state->cpu, mode_changed);
/*
		if (state->spin_flag) {
			while(cpu_0_spin_flag)
				udelay(1);
		}
		else {
			while(!cpu_0_spin_flag)
				udelay(1);
		}
		*/
		//TRACE("CPU%d flag check. %d\n",state->cpu, mode_changed);
		while (*cpu0_counter < my_val && !mode_poll_exited){
			udelay(1);
			//if (timeout++ > 1000){
			//	if (!cpu_0_task_exist){
			//		break;
			//	}
			//	timeout = 0;
			//}
		}
		TRACE("CPU%d counter check. %d\n",state->cpu, this_cpu_read(mode_counter));
		//local_irq_save(flags);
		if (mode_changed) {
			lt_t new_mode_basetime = get_release(current);
			//TRACE("CPU%d mode changed\n",state->cpu);
			hrtimer_cancel(&state->timer); //stop listening to old mode timers
			TRACE("Timer is cancelled at %llu. mode-change\n", litmus_clock());
			raw_spin_lock_irqsave(&state->lock, flags);
			state->sup_env = &state->sup_env_modes[mode];
			list_for_each(pos, &state->sup_env->active_reservations){
				res = list_entry(pos, struct reservation, list);
				release_at(res->tsk, new_mode_basetime);
			}
			list_for_each(pos, &state->sup_env->depleted_reservations){
				res = list_entry(pos, struct reservation, list);
				release_at(res->tsk, new_mode_basetime);
			}
			list_for_each(pos, &state->sup_env->inactive_reservations){
				res = list_entry(pos, struct reservation, list);
				release_at(res->tsk, new_mode_basetime);
			}
			sup_update_time(state->sup_env, litmus_clock());
			raw_spin_unlock_irqrestore(&state->lock, flags);
			
			raw_spin_lock(&state->lock);
			mc2_update_timer_and_unlock(state);
			//local_irq_restore(flags);
	
		}
		//state->spin_flag = !state->spin_flag;
	}
	else {
		//TRACE("CPU%d no cpu_0_task_exist.%d\n",state->cpu, mode_changed);
		return 0;
	}
	TRACE("CPU%d enact syscall ends m_c? %d new_mode %d\n",state->cpu, mode_changed, mode);
	//if mode didn't change this has no effect on what's being scheduled
	//raw_spin_lock(&state->lock);
	//state->sup_env = &state->sup_env_modes[mode];
	//raw_spin_unlock(&state->lock);
	//sup_update_time(state->sup_env, litmus_clock());
	//raw_spin_lock(&state->lock);
	//mc2_update_timer_and_unlock(state);
	TRACE("is timer active? %d remaining %llu\n",hrtimer_active(&state->timer), hrtimer_get_remaining(&state->timer));
	
	return 0;
}


/*
 * Called from non-real time program
 * Protect by exclusive lock to prevent from occuring while mode change is enacted
 */

#define GET_MODE -1
#define GET_REP -2
#define GET_SIZE -3

asmlinkage long sys_request_mode(int new_mode){
	TRACE("Requesting mode %d\n", new_mode);
	preempt_disable();
	raw_spin_lock(&mode_lock);
	if (new_mode == GET_MODE){
		int tmp_mode = mode;
		raw_spin_unlock(&mode_lock);
		preempt_enable();
		return tmp_mode;
	}
	if (new_mode == GET_REP){
		int tmp_rep = res_reported;
		raw_spin_unlock(&mode_lock);
		preempt_enable();
		return tmp_rep;
	}
	if (new_mode == GET_SIZE){
		int tmp_size = mode_sizes[mode];
		raw_spin_unlock(&mode_lock);
		preempt_enable();
		return tmp_size;
	}
	if (pending){
		TRACE("Request to %d denied due to pending to %d\n", new_mode, requested_mode);
		raw_spin_unlock(&mode_lock);
		preempt_enable();
		TRACE("MCR rejected because the previous MCR is pedning.\n");
		return -EAGAIN;
	}
	if (mode == new_mode){
		TRACE("Request to %d denied becuase I am in that mode already\n", new_mode);
		raw_spin_unlock(&mode_lock);
		preempt_enable();
		return -EINVAL;
	}
	requested_mode = new_mode;
	TRACE("MCR received: %d, old:%d\n",requested_mode,mode);
	res_reported = mode_sizes[mode];
	TRACE_CUR("RES_REPORTED = %d\n",res_reported);
	seen_once = false;
	raw_spin_unlock(&mode_lock);
	preempt_enable();
	return 0;
}
	

/* get_mc2_state - get the task's state */
static struct mc2_task_state* get_mc2_state(struct task_struct *tsk)
{
	struct mc2_task_state* tinfo;
	
	tinfo = (struct mc2_task_state*)tsk_rt(tsk)->plugin_state;
	
	if (tinfo)
		return tinfo;
	else
		return NULL;
}

/* get_task_crit_level - return the criticaility level of a task */
static enum crit_level get_task_crit_level(struct task_struct *tsk)
{
	struct mc2_task *mp;
	
	if (!tsk || !is_realtime(tsk))
		return NUM_CRIT_LEVELS;
	
	mp = tsk_rt(tsk)->mc2_data;
	
	if (!mp)
		return NUM_CRIT_LEVELS;
	else
		return mp->crit;
}

static int is_init_finished(struct task_struct *tsk)
{
	struct mc2_task *mp;
	
	if (!tsk || !is_realtime(tsk))
		return 0;
	
	mp = tsk_rt(tsk)->mc2_data;
	
	if (!mp)
		return 0;
	else
		return mp->init_finished;
}

/* task_depart - remove a task from its reservation
 *               If the job has remaining budget, convert it to a ghost job
 *               and update crit_entries[]
 *               
 * @job_complete	indicate whether job completes or not              
 */
static void task_departs(struct task_struct *tsk, int job_complete)
{
	struct mc2_task_state* tinfo = get_mc2_state(tsk);
	//struct mc2_cpu_state* state = local_cpu_state();
	struct reservation* res = NULL;
	struct reservation_client *client = NULL;
	int i;
	BUG_ON(!is_realtime(tsk));

	for(i = 0; i < NR_MODES; i++){
		if (! in_mode(tsk, i) && i != 0)
			continue;	
		res    = tinfo->res_info[i].client.reservation;
		client = &tinfo->res_info[i].client;
		BUG_ON(!res);
		BUG_ON(!client);

		if (job_complete)
			res->cur_budget = 0;
		
		res->ops->client_departs(res, client, job_complete);
	}

/* 9/18/2015 fix start - no ghost job handling, empty remaining budget */
/*
	if (job_complete) {
		//res->cur_budget = 0;
	}
*/
/* fix end */

	tinfo->has_departed = true;
	TRACE_TASK(tsk, "CLIENT DEPART with budget %llu at %llu\n", res->cur_budget, litmus_clock());
}

/* task_arrive - put a task into its reservation
 *               If the job was a ghost job, remove it from crit_entries[]
 */
static void task_arrives(struct mc2_cpu_state *state, struct task_struct *tsk)
{
	struct mc2_task_state* tinfo = get_mc2_state(tsk);
	struct reservation* res;
	struct reservation_client *client;
	enum crit_level lv = get_task_crit_level(tsk);
	int i;

	switch(lv) {
		case CRIT_LEVEL_A:
		case CRIT_LEVEL_B:
			TS_RELEASE_START;
			break;
		case CRIT_LEVEL_C:
			TS_RELEASE_C_START;
			break;
		default:
			break;
	}

	tinfo->has_departed = false;

	TRACE_TASK(tsk, "CLIENT ARRIVES at %llu\n", litmus_clock());
	
	for(i = 0; i < NR_MODES; i++){
		if (! in_mode(tsk, i) && i != 0)
			continue;	
		res    = tinfo->res_info[i].client.reservation;
		client = &tinfo->res_info[i].client;

		res->ops->client_arrives(res, client);
	}	

	switch(lv) {
		case CRIT_LEVEL_A:
		case CRIT_LEVEL_B:
			TS_RELEASE_END;
			break;
		case CRIT_LEVEL_C:
			TS_RELEASE_C_END;
			break;
		default:
			break;
	}
}

/* get_lowest_prio_cpu - return the lowest priority cpu
 *                       This will be used for scheduling level-C tasks.
 *                       If all CPUs are running tasks which has
 *                       higher priority than level C, return NO_CPU.
 */
static int get_lowest_prio_cpu(lt_t priority)
{
	struct cpu_entry *ce;
	int cpu, ret = NO_CPU;
	lt_t latest_deadline = 0;
	
	if (priority == LITMUS_NO_PRIORITY)
		return ret;

	ce = &_lowest_prio_cpu.cpu_entries[local_cpu_state()->cpu];
	if (!ce->will_schedule && !ce->scheduled) {
		TRACE("CPU %d (local) is the lowest (Idle)!\n", ce->cpu);
		return ce->cpu;
	} else {
		TRACE("Local CPU will_schedule=%d, scheduled=(%s/%d)\n", ce->will_schedule, ce->scheduled ? (ce->scheduled)->comm : "null", ce->scheduled ? (ce->scheduled)->pid : 0);
	}

	for_each_online_cpu(cpu) {
		ce = &_lowest_prio_cpu.cpu_entries[cpu];
		/* If a CPU will call schedule() in the near future, we don't
		   return that CPU. */
/*		
		TRACE("CPU %d will_schedule=%d, scheduled=(%s/%d:%d)\n", cpu, ce->will_schedule,
	      ce->scheduled ? (ce->scheduled)->comm : "null",
	      ce->scheduled ? (ce->scheduled)->pid : 0,
	      ce->scheduled ? (ce->scheduled)->rt_param.job_params.job_no : 0);
*/
		if (!ce->will_schedule) {
			if (!ce->scheduled) {
				/* Idle cpu, return this. */
				TRACE("CPU %d is the lowest (Idle)!\n", ce->cpu);
				return ce->cpu;
			} else if (ce->lv == CRIT_LEVEL_C && 
			           ce->deadline > latest_deadline) {
				latest_deadline = ce->deadline;
				ret = ce->cpu;
			}
		}
	}		
	
	TRACE("CPU %d is the lowest! deadline = %llu, my priority = %llu\n", ret, latest_deadline, priority);
	
	if (priority >= latest_deadline) {
		TRACE("CPU %d is running a higher-priority task. return NO_CPU\n", ret);
		ret = NO_CPU;
	}	

	return ret;
}

/* NOTE: drops state->lock */
/* mc2_update_timer_and_unlock - set a timer and unlock state->lock.
 *                               Whenever res_env.current_time is updated,
 *                               we check next_scheduler_update and set 
 *                               a timer.
 *                               If there exist a global event which is 
 *                               not armed on any CPU and g_timer is not
 *                               active, set a g_timer for that event.
 */
static void mc2_update_timer_and_unlock(struct mc2_cpu_state *state)
{
	int local, cpus;
	lt_t update, now;
	//enum crit_level lv = get_task_crit_level(state->scheduled);
	struct next_timer_event *event, *next;
	int reschedule[NR_CPUS];

	for (cpus = 0; cpus<NR_CPUS; cpus++)
		reschedule[cpus] = 0;
	
	update = state->sup_env->next_scheduler_update;
	now = state->sup_env->env.current_time;

	/* Be sure we're actually running on the right core,
	 * as pres_update_timer() is also called from pres_task_resume(),
	 * which might be called on any CPU when a thread resumes.
	 */
	local = local_cpu_state() == state;

	//TRACE_CUR("LOCK_MSG: GLOBAL LOCK\n");
	raw_spin_lock(&global_lock);
		
	list_for_each_entry_safe(event, next, &_global_env->next_events, list) {
		/* If the event time is already passed, we call schedule() on
		   the lowest priority cpu */
		if (event->next_update >= update) {
			break;
		}
		
		if (event->next_update < litmus_clock()) {
			if (event->timer_armed_on == NO_CPU) {
				struct reservation *res = gmp_find_by_id(_global_env, event->id);
				int cpu = get_lowest_prio_cpu(res?res->priority:LITMUS_NO_PRIORITY);
				//TRACE("GLOBAL EVENT PASSED!! poking CPU %d to reschedule\n", cpu);
				list_del(&event->list);
				kfree(event);
				if (cpu != NO_CPU) {
					//raw_spin_lock(&_lowest_prio_cpu.lock);
					_lowest_prio_cpu.cpu_entries[cpu].will_schedule = true;
					//raw_spin_unlock(&_lowest_prio_cpu.lock);
					
					//if (cpu == local_cpu_state()->cpu)
					//	litmus_reschedule_local();
					//else
						reschedule[cpu] = 1;
				}
			}
		} else if (event->next_update < update && (event->timer_armed_on == NO_CPU || event->timer_armed_on == state->cpu)) {
			event->timer_armed_on = state->cpu;
			update = event->next_update;
			break;
		}
	}
	
	/* Must drop state lock before calling into hrtimer_start(), which
	 * may raise a softirq, which in turn may wake ksoftirqd. */
	//TRACE_CUR("LOCK_MSG: GLOBAL UNLOCK\n");
	raw_spin_unlock(&global_lock);
	raw_spin_unlock(&state->lock);

	if ((update <= now) || reschedule[state->cpu]) {
		reschedule[state->cpu] = 0;
		litmus_reschedule(state->cpu);
		/*
		raw_spin_lock(&state->lock);
		preempt_if_preemptable(state->scheduled, state->cpu);
		raw_spin_unlock(&state->lock);
		*/
	} else if (likely(local && update != SUP_NO_SCHEDULER_UPDATE)) {
		/* Reprogram only if not already set correctly. */
		if (!hrtimer_active(&state->timer) ||
		    ktime_to_ns(hrtimer_get_expires(&state->timer)) != update) {
			TRACE("canceling timer...at %llu\n", 
			      ktime_to_ns(hrtimer_get_expires(&state->timer)));
			hrtimer_cancel(&state->timer);
			TRACE("setting scheduler timer for %llu\n", update);
			/* We cannot use hrtimer_start() here because the
			 * wakeup flag must be set to zero. */
			__hrtimer_start_range_ns(&state->timer,
					ns_to_ktime(update),
					0 /* timer coalescing slack */,
					HRTIMER_MODE_ABS_PINNED,
					0 /* wakeup */);
			if (update < litmus_clock()) {
				/* uh oh, timer expired while trying to set it */
				TRACE("timer expired during setting "
				      "update:%llu now:%llu actual:%llu\n",
				      update, now, litmus_clock());
	 			/* The timer HW may not have been reprogrammed
	 			 * correctly; force rescheduling now. */
				litmus_reschedule(state->cpu);
			}
		}
	} else if (unlikely(!local && update != SUP_NO_SCHEDULER_UPDATE)) {
		/* Poke remote core only if timer needs to be set earlier than
		 * it is currently set.
		 */
		TRACE("mc2_update_timer for remote CPU %d (update=%llu, "
		      "active:%d, set:%llu)\n",
			state->cpu, update, hrtimer_active(&state->timer),
			ktime_to_ns(hrtimer_get_expires(&state->timer)));
		if (!hrtimer_active(&state->timer) ||
		    ktime_to_ns(hrtimer_get_expires(&state->timer)) > update) {
			TRACE("poking CPU %d so that it can update its "
			      "scheduling timer (active:%d, set:%llu)\n",
			      state->cpu,
			      hrtimer_active(&state->timer),
			      ktime_to_ns(hrtimer_get_expires(&state->timer)));
			//litmus_reschedule(state->cpu);
/*
			raw_spin_lock(&state->lock);
			preempt_if_preemptable(state->scheduled, state->cpu);
			raw_spin_unlock(&state->lock);
			reschedule[state->cpu] = 0;
*/			
		}
	}
/*	
	for (cpus = 0; cpus<NR_CPUS; cpus++) {
		if (reschedule[cpus]) {
			litmus_reschedule(cpus);
		}
	}
*/
}

/* update_cpu_prio - Update cpu's priority
 *                   When a cpu picks a new task, call this function
 *                   to update cpu priorities.
 */
static void update_cpu_prio(struct mc2_cpu_state *state)
{
	struct cpu_entry *ce = &_lowest_prio_cpu.cpu_entries[state->cpu];
	enum crit_level lv = get_task_crit_level(state->scheduled);
	
	if (!state->scheduled) {
		/* cpu is idle. */
		ce->scheduled = NULL;
		ce->deadline = ULLONG_MAX;
		ce->lv = NUM_CRIT_LEVELS;
	} else if (lv == CRIT_LEVEL_C) {
		ce->scheduled = state->scheduled;
		ce->deadline = get_deadline(state->scheduled);
		ce->lv = lv;
	} else if (lv < CRIT_LEVEL_C) {
		/* If cpu is running level A or B tasks, it is not eligible
		   to run level-C tasks */
		ce->scheduled = state->scheduled;
		ce->deadline = 0;
		ce->lv = lv;
	}
};

/* on_scheduling_timer - timer event for partitioned tasks
 */                       
static enum hrtimer_restart on_scheduling_timer(struct hrtimer *timer)
{
	unsigned long flags;
	enum hrtimer_restart restart = HRTIMER_NORESTART;
	struct mc2_cpu_state *state;
	lt_t update, now;
	int global_schedule_now;
	//lt_t remain_budget; // no ghost jobs
	int reschedule[NR_CPUS];
	int cpus;
	
	if (mode_changed)
		return restart;
	
	for (cpus = 0; cpus<NR_CPUS; cpus++)
		reschedule[cpus] = 0;
	
	state = container_of(timer, struct mc2_cpu_state, timer);

	/* The scheduling timer should only fire on the local CPU, because
	 * otherwise deadlocks via timer_cancel() are possible.
	 * Note: this does not interfere with dedicated interrupt handling, as
	 * even under dedicated interrupt handling scheduling timers for
	 * budget enforcement must occur locally on each CPU.
	 */
	BUG_ON(state->cpu != raw_smp_processor_id());

	TS_ISR_START;
	
	TRACE("Timer fired at %llu\n", litmus_clock());
	raw_spin_lock_irqsave(&state->lock, flags);
	//raw_spin_lock(&state->lock);
	//local_irq_save(flags);
	now = litmus_clock();
	sup_update_time(state->sup_env, now);

/* 9/20/2015 fix - no ghost job 	
	remain_budget = mc2_update_ghost_state(state);
*/	
	update = state->sup_env->next_scheduler_update;
	now = state->sup_env->env.current_time;


	if (update <= now) {
		litmus_reschedule_local();
	} else if (update != SUP_NO_SCHEDULER_UPDATE) {
		hrtimer_set_expires(timer, ns_to_ktime(update));
		restart = HRTIMER_RESTART;
	}

	//TRACE_CUR("LOCK_MSG: GLOBAL LOCK\n");
	raw_spin_lock(&global_lock);
	global_schedule_now = gmp_update_time(_global_env, litmus_clock());
	BUG_ON(global_schedule_now < 0 || global_schedule_now > 4);
	
	/* Find the lowest cpu, and call reschedule */
	while (global_schedule_now--) {
		int cpu = get_lowest_prio_cpu(0);
		if (cpu != NO_CPU && _lowest_prio_cpu.cpu_entries[cpu].will_schedule == false) {
			//raw_spin_lock(&_lowest_prio_cpu.lock);
			_lowest_prio_cpu.cpu_entries[cpu].will_schedule = true;
			//raw_spin_unlock(&_lowest_prio_cpu.lock);
			TRACE("LOWEST CPU = P%d\n", cpu);
			if (cpu == state->cpu && update > now)
				;//litmus_reschedule_local();
			else
				reschedule[cpu] = 1;
		}
	} 
	//TRACE_CUR("LOCK_MSG: GLOBAL UNLOCK\n");
	raw_spin_unlock(&global_lock);
	raw_spin_unlock_irqrestore(&state->lock, flags);
	//raw_spin_unlock(&state->lock);
	//local_irq_restore(flags);
	
	TS_ISR_END;
	
	for (cpus = 0; cpus<NR_CPUS; cpus++) {
		if (reschedule[cpus]) {
			litmus_reschedule(cpus);
			/*
			struct mc2_cpu_state *remote_state;

			remote_state = cpu_state_for(cpus);
			raw_spin_lock(&remote_state->lock);
			preempt_if_preemptable(remote_state->scheduled, remote_state->cpu);
			raw_spin_unlock(&remote_state->lock);
			*/
		}
	}
	
	
	return restart;
}

#define INIT_PHASE_LENGTH_NS	(1000000000)

/* mc2_complete_job - syscall backend for job completions
 */
static long mc2_complete_job(void)
{
	ktime_t next_release;
	long err;
	
	enum crit_level lv = get_task_crit_level(current);
	
	raw_spin_lock(&mode_lock);
	tsk_rt(current)->completed = 1;
	raw_spin_unlock(&mode_lock);
	
	if (atomic_read(&num_sync_released) == 0 && mode != 0) {
			tsk_rt(current)->sporadic_release = 0;
			TRACE_CUR("num_sync_released is 0\n");
	}
	
	/* If this the first job instance, we need to reset replenish
	   time to the next release time */
	if (tsk_rt(current)->sporadic_release) {
		struct mc2_cpu_state *state;
		struct mc2_task_state *tinfo;
		struct reservation *res = NULL;
		unsigned long flags;

		local_irq_save(flags);
		
		tinfo = get_mc2_state(current);
		
		if (lv < CRIT_LEVEL_C) {
			int i;
			state = cpu_state_for(tinfo->cpu);
			raw_spin_lock(&state->lock);
			for (i = 0; i<NR_MODES; i++) {
				if (in_mode(current,i) || i == 0) {
					state->sup_env_modes[i].env.time_zero = tsk_rt(current)->sporadic_release_time + INIT_PHASE_LENGTH_NS*(tsk_rt(current)->job_params.job_no);
				}
			}
			res = sup_find_by_id(state->sup_env, tinfo->mc2_param.res_id);
			
		}
		else if (lv == CRIT_LEVEL_C) {
			int i;
			state = local_cpu_state();		
			raw_spin_lock(&state->lock);
			//TRACE_CUR("LOCK_MSG: GLOBAL LOCK\n");
			raw_spin_lock(&global_lock);
			for (i = 0; i < NR_MODES; i++) {
				if (in_mode(current,i) || i == 0) {
					_global_env_modes[i].env.time_zero = tsk_rt(current)->sporadic_release_time + INIT_PHASE_LENGTH_NS*(tsk_rt(current)->job_params.job_no);
				}
			}
			res = gmp_find_by_id(_global_env, tinfo->mc2_param.res_id);
		}
		else
			BUG();

		/* set next_replenish to synchronous release time */
		BUG_ON(!res);
		res->next_replenishment = tsk_rt(current)->sporadic_release_time + INIT_PHASE_LENGTH_NS*(tsk_rt(current)->job_params.job_no);
/*		
		if (get_task_crit_level(current) == CRIT_LEVEL_A) {
			struct table_driven_reservation *tdres;
			tdres = container_of(res, struct table_driven_reservation, res);
			tdres->next_interval = 0;
			tdres->major_cycle_start = tsk_rt(current)->sporadic_release_time;
			res->next_replenishment += tdres->intervals[0].start;			
		}
*/		
		res->cur_budget = 0;
		res->env->change_state(res->env, res, RESERVATION_DEPLETED);
		
		TRACE_CUR("CHANGE NEXT_REP = %llu NEXT_UPDATE = %llu\n", res->next_replenishment, state->sup_env->next_scheduler_update);
		if (lv == CRIT_LEVEL_C){
			raw_spin_unlock(&global_lock);
		}
		raw_spin_unlock(&state->lock);
		local_irq_restore(flags);
	}
	
	sched_trace_task_completion(current, 0);		
	/* update the next release time and deadline */
	prepare_for_next_period(current);
	sched_trace_task_release(current);
	next_release = ns_to_ktime(get_release(current));
	preempt_disable();
	TRACE_CUR("next_release=%llu\n", get_release(current));

	/*
	 * Changed logic for mode switch case
	 * In case of mode switch, do not want to release
	 * new job even if release time has passed
	 */

	if (lv == CRIT_LEVEL_C && pending) {
		struct reservation *res = NULL;

		raw_spin_lock(&mode_lock);
		res = gmp_find_by_id(_global_env, tsk_mc2_data(current)->res_id);
		if (res && !res->reported){
			res_reported--;
			TRACE_CUR("RES_REPORTED = %d\n", res_reported);
			res->reported = 1;
			//Current task doesn't exist in new mode
			//if ( !in_mode(current, requested_mode) ){
			//	litmus_reschedule_local();
			//}
		}
		raw_spin_unlock(&mode_lock);
	}
	

	if (get_release(current) > litmus_clock()) {
		/* sleep until next_release */
		set_current_state(TASK_INTERRUPTIBLE);
		preempt_enable_no_resched();
		TRACE_CUR("Sleep until %llu\n", next_release);
		err = schedule_hrtimeout(&next_release, HRTIMER_MODE_ABS);
	} else {
		/* release the next job immediately */
		err = 0;
		TRACE_CUR("TARDY: release=%llu now=%llu\n", get_release(current), litmus_clock());
		preempt_enable();
	}

	TRACE_CUR("mc2_complete_job returns at %llu\n", litmus_clock());

	raw_spin_lock(&mode_lock);
	tsk_rt(current)->completed = 0;
	raw_spin_unlock(&mode_lock);
	
	return err;
}

/* mc2_dispatch - Select the next task to schedule.
 */
struct task_struct* mc2_dispatch(struct sup_reservation_environment* sup_env, struct mc2_cpu_state* state)
{
	struct reservation *res, *next;
	struct task_struct *tsk = NULL;
	//struct crit_entry *ce;
	enum crit_level lv;
	lt_t time_slice;


	list_for_each_entry_safe(res, next, &sup_env->active_reservations, list) {
		if (res->state == RESERVATION_ACTIVE) {
			struct task_struct *t = res->ops->dispatch_client(res, &time_slice);
			TRACE_TASK(tsk, "CPU%d ACT_LIST R%d cur.mode = %d, res->mode = %d budget = %llu next_repl. = %llu, deadline = %llu\n", state->cpu, res->id, mode, res->mode, res->cur_budget, res->next_replenishment, get_deadline(t));
		}
	}
		
	list_for_each_entry_safe(res, next, &sup_env->active_reservations, list) {
		if (res->state == RESERVATION_ACTIVE) {
			tsk = res->ops->dispatch_client(res, &time_slice);
			if (likely(tsk)) {
				lv = get_task_crit_level(tsk);
				if (lv == NUM_CRIT_LEVELS) {
					sup_scheduler_update_after(sup_env, res->cur_budget);
					return tsk;
				} else {
					//if (!is_init_finished(tsk)) {
//					TRACE_TASK(tsk, "num_sync_released = %d, mode = %d\n", num_sync_released, mode);
//					if (num_sync_released != 0 && mode == 0) {
					//ce = &state->crit_entries[lv];
					sup_scheduler_update_after(sup_env, res->cur_budget);
					res->blocked_by_ghost = 0;
					res->is_ghost = NO_CPU;
					return tsk;
/*
					} else if (res->mode == mode) {
						sup_scheduler_update_after(sup_env, res->cur_budget);
						res->blocked_by_ghost = 0;
						res->is_ghost = NO_CPU;
						return tsk;
					}
*/					
				}
			}
		}
	}
	
	return NULL;
}

struct task_struct* mc2_global_dispatch(struct mc2_cpu_state* state)
{
	struct reservation *res, *next;
	struct task_struct *tsk = NULL;
	//struct crit_entry *ce;
	enum crit_level lv;
	lt_t time_slice;
	
	list_for_each_entry_safe(res, next, &_global_env->active_reservations, list) {
		if (res->state == RESERVATION_ACTIVE) {
			struct task_struct *t = res->ops->dispatch_client(res, &time_slice);
			TRACE_TASK(tsk, "GLOBAL ACT_LIST R%d cur.mode = %d, res->mode = %d budget = %llu next_repl. = %llu, deadline = %llu\n", res->id, mode, res->mode, res->cur_budget, res->next_replenishment, get_deadline(t));
		}
	}
	
	//raw_spin_lock(&mode_lock);
	list_for_each_entry_safe(res, next, &_global_env->active_reservations, list) {
		BUG_ON(!res);
		if (res->state == RESERVATION_ACTIVE && res->scheduled_on == NO_CPU) {
			tsk = res->ops->dispatch_client(res, &time_slice);
			if (pending && res->reported && !in_mode(tsk, requested_mode)){
				TRACE_TASK(tsk, "Rejected because task not in requested mode %d\n", requested_mode);
				continue;
			}
			if (likely(tsk)) {
				lv = get_task_crit_level(tsk);
				if (lv != CRIT_LEVEL_C)
					BUG();
#if BUDGET_ENFORCEMENT_AT_C			
				gmp_add_event_after(_global_env, res->cur_budget, res->id, EVENT_DRAIN);
#endif
				res->event_added = 1;
				res->blocked_by_ghost = 0;
				res->is_ghost = NO_CPU;	
				TRACE_TASK(res->tsk, "R%d global dispatched on %d\n", res->id, state->cpu);
				res->scheduled_on = state->cpu;
				//raw_spin_unlock(&mode_lock);
				return tsk;
			}
		}
	}
	//raw_spin_unlock(&mode_lock);
	return NULL;
}

static inline void pre_schedule(struct task_struct *prev, int cpu)
{
	TS_SCHED_A_START;
	TS_SCHED_C_START;
	
	if (!prev || !is_realtime(prev))
		return;
	
	do_partition(CRIT_LEVEL_C, cpu);
}

static inline void post_schedule(struct task_struct *next, int cpu)
{
	enum crit_level lev;
	if ((!next) || !is_realtime(next))
		return;

	lev = get_task_crit_level(next);
	if (is_mode_poll_task(next))  {
		lev = MODE_POLL_TASK;
	}

	do_partition(lev, cpu);

	switch(lev) {
		case CRIT_LEVEL_A:
		case CRIT_LEVEL_B:
		case MODE_POLL_TASK:
			TS_SCHED_A_END(next);
			break;
		case CRIT_LEVEL_C:
			TS_SCHED_C_END(next);
			break;
		default:
			break;
	}
	
}

/* mc2_schedule - main scheduler function. pick the next task to run
 */
static struct task_struct* mc2_schedule(struct task_struct * prev)
{
	int np, blocks, exists, to_schedule;
	/* next == NULL means "schedule background work". */
	lt_t now = litmus_clock();
	struct mc2_cpu_state *state = local_cpu_state();
	
	raw_spin_lock(&state->lock);
	
	pre_schedule(prev, state->cpu);

	BUG_ON(state->scheduled && state->scheduled != prev);
	BUG_ON(state->scheduled && !is_realtime(prev));
	
	//if (state->scheduled && state->scheduled != prev)
	//	printk(KERN_ALERT "BUG1!!!!!!!! %s %s\n", state->scheduled ? (state->scheduled)->comm : "null", prev ? (prev)->comm : "null");
	//if (state->scheduled && !is_realtime(prev))
	//	printk(KERN_ALERT "BUG2!!!!!!!! \n");

	/* (0) Determine state */
	exists = state->scheduled != NULL;
	blocks = exists && !is_current_running();
	np = exists && is_np(state->scheduled);

	/* update time */
	state->sup_env->will_schedule = true;
	sup_update_time(state->sup_env, now);
	/* 9/20/2015 fix */
	//raw_spin_lock(&_global_env.lock);
	//to_schedule = gmp_update_time(&_global_env, now);
	//raw_spin_unlock(&_global_env.lock);
	
	/* 9/20/2015 fix 
	mc2_update_ghost_state(state);	
	*/
	
	/* remove task from reservation if it blocks */
	/*
	if (is_realtime(prev) && !is_running(prev)) {
		if (get_task_crit_level(prev) == CRIT_LEVEL_C)
			raw_spin_lock(&_global_env.lock);
		task_departs(prev, is_completed(prev));
		if (get_task_crit_level(prev) == CRIT_LEVEL_C)
			raw_spin_unlock(&_global_env.lock);
	}*/
	if (is_realtime(current) && blocks) {
		if (get_task_crit_level(current) == CRIT_LEVEL_C){
			//TRACE_CUR("LOCK_MSG: GLOBAL LOCK\n");
			raw_spin_lock(&global_lock);
		}
		task_departs(current, is_completed(current));
		if (get_task_crit_level(current) == CRIT_LEVEL_C){
			//TRACE_CUR("LOCK_MSG: GLOBAL UNLOCK\n");
			raw_spin_unlock(&global_lock);
		}
	}
	
	/* figure out what to schedule next */
	if (!np)
		state->scheduled = mc2_dispatch(state->sup_env, state);

	if (!state->scheduled) {
		//TRACE_CUR("LOCK_MSG: GLOBAL LOCK\n");
		raw_spin_lock(&global_lock);
		if (is_realtime(prev))
			gmp_update_time(_global_env, now);
		state->scheduled = mc2_global_dispatch(state);
		_lowest_prio_cpu.cpu_entries[state->cpu].will_schedule = false;
		update_cpu_prio(state);
		raw_spin_unlock(&global_lock);
	} else {
		raw_spin_lock(&global_lock);
		_lowest_prio_cpu.cpu_entries[state->cpu].will_schedule = false;
		update_cpu_prio(state);
		raw_spin_unlock(&global_lock);
	}
	/*
	if (!state->scheduled) {
		TRACE_CUR("LOCK_MSG: GLOBAL LOCK\n");raw_spin_lock(&global_lock);
		//to_schedule = gmp_update_time(_global_env, now);
		state->scheduled = mc2_global_dispatch(state);
		_lowest_prio_cpu.cpu_entries[state->cpu].will_schedule = false;
		update_cpu_prio(state);
		TRACE_CUR("LOCK_MSG: GLOBAL UNLOCK\n");raw_spin_unlock(&global_lock);
	} else {
		TRACE_CUR("LOCK_MSG: GLOBAL LOCK\n");raw_spin_lock(&global_lock);
		_lowest_prio_cpu.cpu_entries[state->cpu].will_schedule = false;
		update_cpu_prio(state);
		TRACE_CUR("LOCK_MSG: GLOBAL UNLOCK\n");raw_spin_unlock(&global_lock);
	}
	*/
	
	//raw_spin_lock(&_lowest_prio_cpu.lock);
	//_lowest_prio_cpu.cpu_entries[state->cpu].will_schedule = false;
	//update_cpu_prio(state);
	//raw_spin_unlock(&_lowest_prio_cpu.lock);
	
	/* Notify LITMUS^RT core that we've arrived at a scheduling decision. */
	sched_state_task_picked();

	/* program scheduler timer */
	state->sup_env->will_schedule = false;
		
	/* NOTE: drops state->lock */
	mc2_update_timer_and_unlock(state);

	raw_spin_lock(&state->lock);
	if (prev != state->scheduled && is_realtime(prev)) {
		struct mc2_task_state* tinfo = get_mc2_state(prev);
		struct reservation* res = tinfo->res_info[mode].client.reservation;
		if (res) {
			TRACE_TASK(prev, "PREV JOB of mode %d was scheduled_on = P%d\n", mode, res->scheduled_on);
			res->scheduled_on = NO_CPU;
		}
		TRACE_TASK(prev, "descheduled at %llu.\n", litmus_clock());
		/* if prev is preempted and a global task, find the lowest cpu and reschedule */
		if (tinfo->has_departed == false && get_task_crit_level(prev) == CRIT_LEVEL_C) {
			int cpu;
			//TRACE_CUR("LOCK_MSG: GLOBAL LOCK\n");
			raw_spin_lock(&global_lock);
			cpu = get_lowest_prio_cpu(res?res->priority:LITMUS_NO_PRIORITY);
			if (cpu != NO_CPU && _lowest_prio_cpu.cpu_entries[cpu].will_schedule == false) {
				//raw_spin_lock(&_lowest_prio_cpu.lock);
				_lowest_prio_cpu.cpu_entries[cpu].will_schedule = true;
				resched_cpu[cpu] = 1;
				//raw_spin_unlock(&_lowest_prio_cpu.lock);
				TRACE("LEVEL-C TASK PREEMPTED!! poking CPU %d to reschedule\n", cpu);
			}
			//TRACE_CUR("LOCK_MSG: GLOBAL UNLOCK\n");
			raw_spin_unlock(&global_lock);
		}
	}

/*	
	if (to_schedule != 0) {
		TRACE_CUR("LOCK_MSG: GLOBAL LOCK\n");raw_spin_lock(&global_lock);
		while (to_schedule--) {
			int cpu = get_lowest_prio_cpu(0);
			if (cpu != NO_CPU && _lowest_prio_cpu.cpu_entries[cpu].will_schedule == false) {
				_lowest_prio_cpu.cpu_entries[cpu].will_schedule = true;
				resched_cpu[cpu] = 1;
			}
		}
		TRACE_CUR("LOCK_MSG: GLOBAL UNLOCK\n");raw_spin_unlock(&global_lock);	
	}
*/
	post_schedule(state->scheduled, state->cpu);

	raw_spin_unlock(&state->lock);
	if (state->scheduled) {
		TRACE_TASK(state->scheduled, "scheduled.\n");
	}
	
	
	return state->scheduled;
}

static void resume_legacy_task_model_updates(struct task_struct *tsk)
{
	lt_t now;
	if (is_sporadic(tsk)) {
		/* If this sporadic task was gone for a "long" time and woke up past
		 * its deadline, then give it a new budget by triggering a job
		 * release. This is purely cosmetic and has no effect on the
		 * MC2 scheduler. */

		now = litmus_clock();
		if (is_tardy(tsk, now)) {
			release_at(tsk, now);
		}
	}
}

/* mc2_task_resume - Called when the state of tsk changes back to 
 *                   TASK_RUNNING. We need to requeue the task.
 */
static void mc2_task_resume(struct task_struct  *tsk)
{
	unsigned long flags;
	struct mc2_task_state* tinfo;
	struct mc2_cpu_state *state;

	TRACE_TASK(tsk, "thread wakes up at %llu\n", litmus_clock());

	//local_irq_save(flags);
	preempt_disable();
	tinfo = get_mc2_state(tsk);
	if (tinfo->cpu != -1)
		state = cpu_state_for(tinfo->cpu);
	else
		state = local_cpu_state();
	preempt_enable();
	
	/* 9/20/2015 fix
	raw_spin_lock(&_global_env.lock);
	*/
	/* Requeue only if self-suspension was already processed. */
	if (tinfo->has_departed)
	{
		/* We don't want to consider jobs before synchronous releases */
		if (tsk_rt(tsk)->job_params.job_no == 2) {
/*
			switch(get_task_crit_level(tsk)) {
				case CRIT_LEVEL_A:
					TS_RELEASE_LATENCY_A(get_release(tsk));
					break;
				case CRIT_LEVEL_B:
					TS_RELEASE_LATENCY_B(get_release(tsk));
					break;
				case CRIT_LEVEL_C:
					TS_RELEASE_LATENCY_C(get_release(tsk));
					break;
				default:
					break;
			}
*/
			TRACE_TASK(tsk, "INIT_FINISHED is SET\n");
			tsk_mc2_data(tsk)->init_finished = 1;
			atomic_dec(&num_sync_released);

			if (atomic_read(&num_sync_released) == 0) {
				lt_t start = tsk_rt(tsk)->sporadic_release_time + INIT_PHASE_LENGTH_NS*(tsk_rt(tsk)->job_params.job_no);
				TRACE("INIT_PHASE FINISHED. CHANGE TO MODE 1\n");
				sys_request_mode(1);
				sched_trace_sys_start(&start);
			}
			TRACE_TASK(tsk, "INIT_FINISHED is SET, num_sync_released decreased to %d\n", atomic_read(&num_sync_released));			
		}
		raw_spin_lock_irqsave(&state->lock, flags);

		/* Assumption: litmus_clock() is synchronized across cores,
		 * since we might not actually be executing on tinfo->cpu
		 * at the moment. */
		if (tinfo->cpu != -1) {
			sup_update_time(state->sup_env, litmus_clock());
			task_arrives(state, tsk);
		} else {
			//TRACE_CUR("LOCK_MSG: GLOBAL LOCK\n");
			raw_spin_lock(&global_lock);
			gmp_update_time(_global_env, litmus_clock());
			task_arrives(state, tsk);
			//TRACE_CUR("LOCK_MSG: GLOBAL UNLOCK\n");
			raw_spin_unlock(&global_lock);
		}
			
		/* 9/20/2015 fix 
		mc2_update_ghost_state(state);
		*/
		//task_arrives(state, tsk);
		/* NOTE: drops state->lock */
		TRACE_TASK(tsk, "mc2_resume()\n");
		raw_spin_unlock_irqrestore(&state->lock, flags);

		raw_spin_lock(&state->lock);
		mc2_update_timer_and_unlock(state);	
	} else {
		TRACE_TASK(tsk, "resume event ignored, still scheduled\n");
		//raw_spin_unlock(&_global_env.lock);
	}

	//local_irq_restore(flags);
	
	//gmp_free_passed_event();
	resume_legacy_task_model_updates(tsk);
}


/* mc2_admit_task - Setup mc2 task parameters
 */
static long mc2_admit_task(struct task_struct *tsk)
{
	long err = 0;
	unsigned long flags;
	struct reservation *res;
	struct mc2_cpu_state *state;
	struct mc2_task_state *tinfo = kzalloc(sizeof(*tinfo), GFP_ATOMIC);
	struct mc2_task *mp = tsk_rt(tsk)->mc2_data;
	enum crit_level lv;
	int i;
		
	TRACE_TASK(tsk, "MC2 admitting task\n");
	if (!tinfo)
		return -ENOMEM;

	if (!mp) {
		TRACE("mc2_admit_task: criticality level has not been set\n");
		return -ESRCH;
	}
	
	lv = mp->crit;

	if (lv < CRIT_LEVEL_C) {
		state = cpu_state_for(task_cpu(tsk));
		//local_irq_save(flags);
		//raw_spin_lock(&state->lock);
		raw_spin_lock_irqsave(&state->lock, flags);
		
		tinfo->mc2_param.crit = mp->crit;
		tinfo->cpu = task_cpu(tsk);
		tinfo->has_departed = true;
		tinfo->mc2_param.res_id = mp->res_id;
		tinfo->mc2_param.mode_mask = mp->mode_mask;
		tinfo->mc2_param.init_finished = 0;
//		TRACE_TASK(tsk, "mode_mask = %x\n", mp->mode_mask);
		
//		TRACE_TASK(tsk, "Mode 0\n");
		res = sup_find_by_id(&(state->sup_env_modes[0]), mp->res_id);
	
		/* found the appropriate reservation */
		if (res) {
//			TRACE_TASK(tsk, "SUP FOUND RES ID in mode 0\n");			
			
			/* initial values */
			err = err? err:mc2_task_client_init(&tinfo->res_info[0], &tinfo->mc2_param, tsk, res);
		}
		else {
			//failed to find an expected reservation
			err = -ESRCH;
		}
			
		for(i = 1; i < NR_MODES; i++){
			if (!in_mode(tsk, i)){
				//task not present in mode
				continue;
			}
//			TRACE_TASK(tsk, "Mode %d\n",i);
			res = sup_find_by_id(&(state->sup_env_modes[i]), mp->res_id);
	
			/* found the appropriate reservation */
			if (res) {
	//			TRACE_TASK(tsk, "SUP FOUND RES ID in mode %d\n", i);			
				
				/* initial values */
				err = err? err:mc2_task_client_init(&tinfo->res_info[i], &tinfo->mc2_param, tsk, res);
			}
			else{
				//failed to find an expected reservation
				err = -ESRCH;
			}
		}

		if (!err){
			/* disable LITMUS^RT's per-thread budget enforcement */
			tsk_rt(tsk)->plugin_state = tinfo;
			tsk_rt(tsk)->task_params.budget_policy = NO_ENFORCEMENT;
		}

		if (is_mode_poll_task(tsk) && tinfo->cpu == 0) {
			cpu_0_task_exist = true;
		}
		atomic_inc(&num_sync_released);
		//raw_spin_unlock(&state->lock);
		//local_irq_restore(flags);
		raw_spin_unlock_irqrestore(&state->lock, flags);
	} else if (lv == CRIT_LEVEL_C) {
//		TRACE_TASK(tsk, "Task being admitted is Level C\n");
		state = local_cpu_state();
		raw_spin_lock_irqsave(&state->lock, flags);
		//local_irq_save(flags);
		//raw_spin_lock(&state->lock);
		//state = local_cpu_state();
		
		//raw_spin_lock(&state->lock);
	
		tinfo->mc2_param.crit = mp->crit;
		tinfo->cpu = -1;
		tinfo->has_departed = true;	
		tinfo->mc2_param.res_id = mp->res_id;
		tinfo->mc2_param.mode_mask = mp->mode_mask;
		tinfo->mc2_param.init_finished = 0;
		
	//	TRACE_TASK(tsk, "mode_mask = %x\n", mp->mode_mask);
		
//		TRACE_TASK(tsk, "Mode 0\n");

		//TRACE_CUR("LOCK_MSG: GLOBAL LOCK\n");
		raw_spin_lock(&global_lock);
		res = gmp_find_by_id(&(_global_env_modes[0]), mp->res_id);
	
		/* found the appropriate reservation */
		if (res) {
		//	TRACE_TASK(tsk, "GMP FOUND RES ID in mode 0\n");			
			
			/* initial values */
			err = err? err:mc2_task_client_init(&tinfo->res_info[0], &tinfo->mc2_param, tsk, res);
		}
		else {
			//failed to find an expected reservation
			err = -ESRCH;
		}
		
		for(i = 1; i < NR_MODES; i++){
			if (!in_mode(tsk, i)) 
				continue;
			res = gmp_find_by_id(&(_global_env_modes[i]), mp->res_id);

			/* found the appropriate reservation (or vCPU) */
			if (res) {
				TRACE_TASK(tsk, "GMP FOUND RES ID in mode %d\n", i);
				
				/* initial values */
				err = err? err:mc2_task_client_init(&tinfo->res_info[i], &tinfo->mc2_param, tsk, res);
	
			}
		}
		raw_spin_unlock(&global_lock);
		
		if (!err){
			/* disable LITMUS^RT's per-thread budget enforcement */
			tsk_rt(tsk)->plugin_state = tinfo;
			tsk_rt(tsk)->task_params.budget_policy = NO_ENFORCEMENT;
			raw_spin_lock(&mode_lock);
			for(i = 1; i < NR_MODES; i++){
				if (in_mode(tsk, i)){
					mode_sizes[i]++;
				}
			}
			mode_sizes[0]++;
			raw_spin_unlock(&mode_lock);

		}
		
		atomic_inc(&num_sync_released);
		raw_spin_unlock_irqrestore(&state->lock, flags);	
		//raw_spin_unlock(&state->lock);
		//local_irq_restore(flags);
	}
	

	if (err)
		kfree(tinfo);

	TRACE_TASK(tsk, "MC2 task admitted %d\n", err);
	return err;
}

/* mc2_task_new - A new real-time job is arrived. Release the next job
 *                at the next reservation replenish time
 */
static void mc2_task_new(struct task_struct *tsk, int on_runqueue,
			  int is_running)
{
	unsigned long flags;
	struct mc2_task_state* tinfo = get_mc2_state(tsk);
	struct mc2_cpu_state *state; // = cpu_state_for(tinfo->cpu);
	struct reservation *res;
	enum crit_level lv = get_task_crit_level(tsk);
	lt_t release = 0;

	BUG_ON(lv < CRIT_LEVEL_A || lv > CRIT_LEVEL_C);

	TRACE_TASK(tsk, "new RT task %llu (on_rq:%d, running:%d)\n",
		   litmus_clock(), on_runqueue, is_running);

	if (tinfo->cpu == -1)
		state = local_cpu_state();
	else 
		state = cpu_state_for(tinfo->cpu);
	
	
	/* acquire the lock protecting the state and disable interrupts */
	//raw_spin_lock(&_global_env.lock);
	//raw_spin_lock(&state->lock);
	if (is_running) {
		state->scheduled = tsk;
		/* make sure this task should actually be running */
		litmus_reschedule_local();
	}
	
	local_irq_save(flags);
	raw_spin_lock(&state->lock);
	
	if (lv == CRIT_LEVEL_C) {
		//TRACE_CUR("LOCK_MSG: GLOBAL LOCK\n");
		raw_spin_lock(&global_lock);
		res = gmp_find_by_id(_global_env, tinfo->mc2_param.res_id);
	}
	else {
		res = sup_find_by_id(state->sup_env, tinfo->mc2_param.res_id);
	}

	//BUG_ON(!res);
	// the current mode doesn't have this task.
	// do not update timer and set the next release time.

	//res = res_find_by_id(state, tinfo->mc2_param.res_id);
	BUG_ON(!res);
	
	if (on_runqueue || is_running) {
		/* Assumption: litmus_clock() is synchronized across cores
		 * [see comment in pres_task_resume()] */
		if (lv == CRIT_LEVEL_C) {
			gmp_update_time(_global_env, litmus_clock());
			//raw_spin_unlock(&_global_env.lock);
		}
		else
			sup_update_time(state->sup_env, litmus_clock());
		//mc2_update_time(lv, state, litmus_clock());
		/* 9/20/2015 fix 
		mc2_update_ghost_state(state);
		*/
		task_arrives(state, tsk);
		if (lv == CRIT_LEVEL_C){
			//TRACE_CUR("LOCK_MSG: GLOBAL UNLOCK\n");
			raw_spin_unlock(&global_lock);
		}
		/* NOTE: drops state->lock */
		TRACE("mc2_new()\n");
		raw_spin_unlock(&state->lock);
		local_irq_restore(flags);
		
		raw_spin_lock(&state->lock);
		mc2_update_timer_and_unlock(state);
	} else {
		if (lv == CRIT_LEVEL_C){
			//TRACE_CUR("LOCK_MSG: GLOBAL UNLOCK\n");
			raw_spin_unlock(&global_lock);
		}
		raw_spin_unlock(&state->lock);
		local_irq_restore(flags);
	}
	release = res->next_replenishment;
	
	//local_irq_restore(flags);
	
	if (!release) {
		/*TRACE_TASK(tsk, "mc2_task_new() next_release = NULL\n");
		release = res->next_replenishment;
		TRACE_TASK(tsk, "mc2_task_new() next_release SET! = %llu\n", release);
		release_at(tsk, release);
		*/
		BUG();
	}
	else
		TRACE_TASK(tsk, "mc2_task_new() next_release = %llu\n", release);
}

/* mc2_reservation_destroy - reservation_destroy system call backend
 */
static long mc2_reservation_destroy(unsigned int reservation_id, int cpu)
{
	long ret = -EINVAL;
	struct mc2_cpu_state *state;
	struct reservation *res = NULL, *next;
	struct sup_reservation_environment *sup_env;
	int found = 0;
	//enum crit_level lv = get_task_crit_level(current);
	unsigned long flags;
	int i;
	
	if (cpu == -1) {
		struct next_timer_event *event, *e_next;
		local_irq_save(flags);
		//TRACE_CUR("LOCK_MSG: GLOBAL LOCK\n");
		raw_spin_lock(&global_lock);

		/* if the reservation is global reservation */
		//state = local_cpu_state();
		//delete reservation id in all modes
		for(i = 0; i < NR_MODES; i++) {
			//raw_spin_lock(&state->lock);
		
			list_for_each_entry_safe(res, next, &_global_env_modes[i].depleted_reservations, list) {
				if (res->id == reservation_id) {
					list_del(&res->list);
					kfree(res);
					found = 1;
					ret = 0;
				}
			}
			if (!found) {
				list_for_each_entry_safe(res, next, &_global_env_modes[i].inactive_reservations, list) {
					if (res->id == reservation_id) {
						list_del(&res->list);
						kfree(res);
						found = 1;
						ret = 0;
					}
				}
			}
			if (!found) {
				list_for_each_entry_safe(res, next, &_global_env_modes[i].active_reservations, list) {
					if (res->id == reservation_id) {
						list_del(&res->list);
						kfree(res);
						found = 1;
						ret = 0;
					}
				}
			}

			//raw_spin_unlock(&state->lock);
			list_for_each_entry_safe(event, e_next, &_global_env_modes[i].next_events, list) {
				if (event->id == reservation_id) {
					list_del(&event->list);
					TRACE("EVENT id %d deleted\n", event->id);
					kfree(event);
				} 
			}
		}
		
		//TRACE_CUR("LOCK_MSG: GLOBAL UNLOCK\n");
		raw_spin_unlock(&global_lock);
		local_irq_restore(flags);
	} else {
		/* if the reservation is partitioned reservation */
		state = cpu_state_for(cpu);
		for (i = 0; i < NR_MODES; i++){
			local_irq_save(flags);
			raw_spin_lock(&state->lock);
			
	//		res = sup_find_by_id(state->sup_env, reservation_id);
			sup_env = &(state->sup_env_modes[i]);
			list_for_each_entry_safe(res, next, &sup_env->depleted_reservations, list) {
				if (res->id == reservation_id) {
/*
					if (lv == CRIT_LEVEL_A) {
							struct table_driven_reservation *tdres;
							tdres = container_of(res, struct table_driven_reservation, res);
							kfree(tdres->intervals);
					}
*/
					list_del(&res->list);
					kfree(res);
					found = 1;
					ret = 0;
					TRACE_CUR("FOUND id %d mode %d\n",res->id, res->mode);
				}
			}
			if (!found) {
				list_for_each_entry_safe(res, next, &sup_env->inactive_reservations, list) {
					if (res->id == reservation_id) {
/*						if (lv == CRIT_LEVEL_A) {
							struct table_driven_reservation *tdres;
							tdres = container_of(res, struct table_driven_reservation, res);
							kfree(tdres->intervals);
						}
*/
						list_del(&res->list);
						kfree(res);
						found = 1;
						ret = 0;
						TRACE_CUR("FOUND id %d mode %d\n",res->id, res->mode);
					}
				}
			}
			if (!found) {
				list_for_each_entry_safe(res, next, &sup_env->active_reservations, list) {
					if (res->id == reservation_id) {
/*						if (lv == CRIT_LEVEL_A) {
							struct table_driven_reservation *tdres;
							tdres = container_of(res, struct table_driven_reservation, res);
							kfree(tdres->intervals);
						}
*/		
						list_del(&res->list);
						kfree(res);
						found = 1;
						ret = 0;
						TRACE_CUR("FOUND id %d mode %d\n",res->id, res->mode);
					}
				}
			}

			raw_spin_unlock(&state->lock);
			local_irq_restore(flags);
		}
	}
	
	TRACE("Rerservation destroyed ret = %d\n", ret);
	return ret;
}

/* mc2_task_exit - Task became a normal task (not real-time task)
 */
static void mc2_task_exit(struct task_struct *tsk)
{
	unsigned long flags;
	struct mc2_task_state* tinfo = get_mc2_state(tsk);
	struct mc2_cpu_state *state;
	enum crit_level lv = tinfo->mc2_param.crit;
	//struct crit_entry* ce;
	int cpu;
	int i;

	local_irq_save(flags);
	if (tinfo->cpu != -1)
		state = cpu_state_for(tinfo->cpu);
	else 
		state = local_cpu_state();
	
	raw_spin_lock(&state->lock);
	
	if (state->scheduled == tsk)
		state->scheduled = NULL;

	//ce = &state->crit_entries[lv];
	//if (ce->running == tsk)
	//	ce->running = NULL;
	
	/* remove from queues */
	if (is_running(tsk)) {
		/* Assumption: litmus_clock() is synchronized across cores
		 * [see comment in pres_task_resume()] */
		
		/* update both global and partitioned */
		if (lv < CRIT_LEVEL_C) {
			sup_update_time(state->sup_env, litmus_clock());
/*			TRACE_CUR("LOCK_MSG: GLOBAL LOCK\n");raw_spin_lock(&global_lock);
			gmp_update_time(_global_env, litmus_clock());
			TRACE_CUR("LOCK_MSG: GLOBAL UNLOCK\n");raw_spin_unlock(&global_lock);
*/		}
		else if (lv == CRIT_LEVEL_C) {
			//TRACE_CUR("LOCK_MSG: GLOBAL LOCK\n");
			raw_spin_lock(&global_lock);
			gmp_update_time(_global_env, litmus_clock());
			//raw_spin_unlock(&_global_env.lock);
		}
		/* 9/20/2015 fix 
		mc2_update_ghost_state(state);
		*/
		task_departs(tsk, 0);
		if (lv == CRIT_LEVEL_C){
			//TRACE_CUR("LOCK_MSG: GLOBAL UNLOCK\n");
			raw_spin_unlock(&global_lock);
		}
		/* NOTE: drops state->lock */
		TRACE("mc2_exit()\n");

		atomic_dec(&num_sync_released);

		mc2_update_timer_and_unlock(state);	
	} else {
		raw_spin_unlock(&state->lock);
		
	}

	if (lv == CRIT_LEVEL_C) {
		//raw_spin_lock(&global_lock);
		raw_spin_lock(&mode_lock);
		for(i = 1; i < NR_MODES; i++){
			if ( !(tsk_mc2_data(tsk)->mode_mask & (1<<i)) )
				continue;
			mode_sizes[i]--;
		}
		mode_sizes[0]--;
		raw_spin_unlock(&mode_lock);
		//raw_spin_unlock(&global_lock);

		for_each_online_cpu(cpu) {
			state = cpu_state_for(cpu);
			if (state == local_cpu_state())
				continue;
			raw_spin_lock(&state->lock);
			
			if (state->scheduled == tsk)
				state->scheduled = NULL;
			
			//ce = &state->crit_entries[lv];
			//if (ce->running == tsk)
			//	ce->running = NULL;
			
			raw_spin_unlock(&state->lock);
		}
	}
	
	local_irq_restore(flags);

	if (is_mode_poll_task(tsk)){// && (tinfo->cpu == 0))  {
		//cpu_0_spin_flag = !cpu_0_spin_flag; // release other cpu before exit.
		//cpu_0_task_exist = false;
		mode_poll_exited = true;
	}
	
	kfree(tsk_rt(tsk)->plugin_state);
	tsk_rt(tsk)->plugin_state = NULL;
	kfree(tsk_rt(tsk)->mc2_data);
	tsk_rt(tsk)->mc2_data = NULL;
}

/* create_polling_reservation - create a new polling reservation
 */
static long create_polling_reservation(
	int res_type,
	struct reservation_config *config)
{
	struct mc2_cpu_state *state = NULL;
	//struct reservation* res = NULL;
	struct polling_reservation *pres;
	unsigned long flags;
	int use_edf  = config->priority == LITMUS_NO_PRIORITY;
	int periodic =  res_type == PERIODIC_POLLING;
	long err = -EINVAL;
	bool resExist = false;

	/* sanity checks */
	if (config->polling_params.budget >
	    config->polling_params.period) {
		printk(KERN_ERR "invalid polling reservation (%u): "   "budget > period\n", config->id);
		return -EINVAL;
	}
	if (config->polling_params.budget >
	    config->polling_params.relative_deadline
	    && config->polling_params.relative_deadline) {
		printk(KERN_ERR "invalid polling reservation (%u): "
		       "budget > deadline\n", config->id);
		return -EINVAL;
	}
	if (config->polling_params.offset >
	    config->polling_params.period) {
		printk(KERN_ERR "invalid polling reservation (%u): "
		       "offset > period\n", config->id);
		return -EINVAL;
	}
	//Added sanity check for mode
	if (config->mode < 0 || config->mode >= NR_MODES){
		printk(KERN_ERR "invalid polling reservation (%u): "
		       "Mode outside range\n", config->id);
		return -EINVAL;
	}

	/* Allocate before we grab a spin lock.
	 * Todo: would be nice to use a core-local allocation.
	 */
	pres = kzalloc(sizeof(*pres), GFP_KERNEL);
	if (!pres)
		return -ENOMEM;

	TRACE("CREATE_POLLING_RESERVATION id %d mode %d\n", config->id, config->mode);
	if (config->cpu != -1) {
		int i, is_exist = 0;
		//raw_spin_lock_irqsave(&_global_env.lock, flags);
		state = cpu_state_for(config->cpu);
		raw_spin_lock_irqsave(&state->lock, flags);
		
		/* check if it is the first creation of reservartion */
		for (i = 0; i < NR_MODES; i++) {
			if( sup_find_by_id(&(state->sup_env_modes[i]), config->id) )
				is_exist = 1;
		}
		if (!is_exist && config->mode != 0) {
			/* create mode 0 reservation first */
			struct polling_reservation *pres_0 = kzalloc(sizeof(*pres_0), GFP_ATOMIC);
			
			TRACE_CUR("The first mode_num = %d\n",config->mode);
			
			if (!pres_0) {
				raw_spin_unlock_irqrestore(&state->lock, flags);
				kfree(pres);
				return -ENOMEM;
			}
			polling_reservation_init(pres_0, use_edf, periodic,
				config->polling_params.budget,
				config->polling_params.period,
				config->polling_params.relative_deadline,
				config->polling_params.offset);
			pres_0->res.id = config->id;
			pres_0->res.blocked_by_ghost = 0;
			pres_0->res.is_ghost = NO_CPU;
			pres_0->res.mode = config->mode;

			if (!use_edf)
				pres_0->res.priority = config->priority;
			sup_add_new_reservation(&(state->sup_env_modes[0]), &pres_0->res);
			TRACE_CUR("SUP reservation created R%d for mode 0 priority : %llu\n", config->id, pres_0->res.priority);
			pres_0->res.reported = 0;
			pres_0->res.tsk = current;
		}

		//force reservation id unique inside of res_config->mode
		if( sup_find_by_id(&(state->sup_env_modes[config->mode]), config->id) ){
			resExist = true;
		}
		if (!resExist) {
			polling_reservation_init(pres, use_edf, periodic,
				config->polling_params.budget,
				config->polling_params.period,
				config->polling_params.relative_deadline,
				config->polling_params.offset);
			pres->res.id = config->id;
			pres->res.blocked_by_ghost = 0;
			pres->res.is_ghost = NO_CPU;
			pres->res.mode = config->mode;
			/*if (config->priority == LITMUS_MAX_PRIORITY) {
				level_a_priorities[config->cpu]++;
				pres->res.priority = level_a_priorities[config->cpu];
			}*/
			if (!use_edf)
				pres->res.priority = config->priority;
			sup_add_new_reservation(&(state->sup_env_modes[config->mode]), &pres->res);
			err = config->id;
			TRACE_CUR("SUP reservation created R%d for mode %d priority : %llu\n", config->id, config->mode, pres->res.priority);
		} else {
			err = -EEXIST;
		}

		raw_spin_unlock_irqrestore(&state->lock, flags);
		//raw_spin_unlock_irqrestore(&_global_env.lock, flags);

	} else {
		int i, is_exist = 0;
		raw_spin_lock_irqsave(&global_lock, flags);
		
		/* check if it is the first creation of reservartion */
		for (i = 0; i < NR_MODES; i++) {
			if(gmp_find_by_id(&(_global_env_modes[i]), config->id))
				is_exist = 1;
		}
		if (!is_exist && config->mode != 0) {
			/* create mode 0 reservation first */
			struct polling_reservation *pres_0 = kzalloc(sizeof(*pres_0), GFP_ATOMIC);
			
			TRACE_CUR("The first mode_num = %d\n",config->mode);
			
			if (!pres_0) {
				raw_spin_unlock_irqrestore(&global_lock, flags);
				kfree(pres);
				return -ENOMEM;
			}
			polling_reservation_init(pres_0, use_edf, periodic,
				config->polling_params.budget,
				config->polling_params.period,
				config->polling_params.relative_deadline,
				config->polling_params.offset);
			pres_0->res.id = config->id;
			pres_0->res.blocked_by_ghost = 0;
			pres_0->res.scheduled_on = NO_CPU;
			pres_0->res.is_ghost = NO_CPU;
			pres_0->res.mode = config->mode;

			if (!use_edf)
				pres_0->res.priority = config->priority;
			gmp_add_new_reservation(&(_global_env_modes[0]), &pres_0->res);
			TRACE_CUR("GMP reservation created R%d for mode 0 priority : %llu\n", config->id, pres_0->res.priority);
			pres_0->res.reported = 0;
			pres_0->res.tsk = current;
		}

		
		//force id's unique within desired mode
		if (gmp_find_by_id(&(_global_env_modes[config->mode]), config->id)){
			resExist = true;
		}
		if (!resExist) {
			polling_reservation_init(pres, use_edf, periodic,
				config->polling_params.budget,
				config->polling_params.period,
				config->polling_params.relative_deadline,
				config->polling_params.offset);
			pres->res.id = config->id;
			pres->res.blocked_by_ghost = 0;
			pres->res.scheduled_on = NO_CPU;
			pres->res.is_ghost = NO_CPU;
			pres->res.mode = config->mode;
			if (!use_edf)
				pres->res.priority = config->priority;
			gmp_add_new_reservation(&(_global_env_modes[config->mode]), &pres->res);
			TRACE_CUR("GMP reservation created R%d for mode %d priority : %llu\n", config->id, config->mode, pres->res.priority);
			err = config->id;
		} else {
			err = -EEXIST;
		}
		raw_spin_unlock_irqrestore(&global_lock, flags);		
	}
	

	pres->res.reported = 0;
	pres->res.tsk = current;

	if (err < 0)
		kfree(pres);

	return err;
}

#define MAX_INTERVALS 1024

/* create_table_driven_reservation - create a table_driven reservation
 */
static long create_table_driven_reservation(
	struct reservation_config *config)
{
	struct mc2_cpu_state *state;
	//struct reservation* res = NULL;
	struct table_driven_reservation *td_res = NULL;
	struct lt_interval *slots = NULL;
	size_t slots_size;
	unsigned int i, num_slots;
	unsigned long flags;
	long err = -EINVAL;
	bool resExist = false;

	if (!config->table_driven_params.num_intervals) {
		printk(KERN_ERR "invalid table-driven reservation (%u): "
		       "no intervals\n", config->id);
		return -EINVAL;
	}

	if (config->table_driven_params.num_intervals > MAX_INTERVALS) {
		printk(KERN_ERR "invalid table-driven reservation (%u): "
		       "too many intervals (max: %d)\n", config->id, MAX_INTERVALS);
		return -EINVAL;
	}

	if (config->mode >= NR_MODES || config->mode < 0){
		printk(KERN_ERR "invalid table-driven reservation (%u): "
		       "mode outside of range\n", config->id);
		return -EINVAL;
	}

	num_slots = config->table_driven_params.num_intervals;
	slots_size = sizeof(slots[0]) * num_slots;
	slots = kzalloc(slots_size, GFP_KERNEL);
	if (!slots)
		return -ENOMEM;

	td_res = kzalloc(sizeof(*td_res), GFP_KERNEL);
	if (!td_res)
		err = -ENOMEM;
	else
		err = copy_from_user(slots,
			config->table_driven_params.intervals, slots_size);

	if (!err) {
		/* sanity checks */
		for (i = 0; !err && i < num_slots; i++)
			if (slots[i].end <= slots[i].start) {
				printk(KERN_ERR
				       "invalid table-driven reservation (%u): "
				       "invalid interval %u => [%llu, %llu]\n",
				       config->id, i,
				       slots[i].start, slots[i].end);
				err = -EINVAL;
			}

		for (i = 0; !err && i + 1 < num_slots; i++)
			if (slots[i + 1].start <= slots[i].end) {
				printk(KERN_ERR
				       "invalid table-driven reservation (%u): "
				       "overlapping intervals %u, %u\n",
				       config->id, i, i + 1);
				err = -EINVAL;
			}

		if (slots[num_slots - 1].end >
			config->table_driven_params.major_cycle_length) {
			printk(KERN_ERR
				"invalid table-driven reservation (%u): last "
				"interval ends past major cycle %llu > %llu\n",
				config->id,
				slots[num_slots - 1].end,
				config->table_driven_params.major_cycle_length);
			err = -EINVAL;
		}
	}

	if (!err) {
		state = cpu_state_for(config->cpu);
		raw_spin_lock_irqsave(&state->lock, flags);
		
		//force unique id's across all modes
		for(i = 0; i < NR_MODES; i++){
			if (sup_find_by_id(&(state->sup_env_modes[i]), config->id)){
				resExist = true;
				break;
			}
		}
		if (!resExist) {
			table_driven_reservation_init(td_res,
				config->table_driven_params.major_cycle_length,
				slots, num_slots);
			td_res->res.id = config->id;
			td_res->res.priority = config->priority;
			td_res->res.blocked_by_ghost = 0;
			td_res->res.mode = config->mode;
			sup_add_new_reservation(&(state->sup_env_modes[config->mode]), &td_res->res);
			err = config->id;
		} else {
			err = -EEXIST;
		}

		raw_spin_unlock_irqrestore(&state->lock, flags);
	}

	td_res->res.reported = 0;
	td_res->res.tsk = current;
	
	if (err < 0) {
		kfree(slots);
		kfree(td_res);
	}

	return err;
}

/* mc2_reservation_create - reservation_create system call backend
 */
static long mc2_reservation_create(int res_type, void* __user _config)
{
	long ret = -EINVAL;
	struct reservation_config config;

	TRACE("Attempt to create reservation (%d)\n", res_type);
	
	if (copy_from_user(&config, _config, sizeof(config)))
		return -EFAULT;

	TRACE("Attempt to create reservation id %d mode %d\n", config.id, config.mode);

	if (config.cpu != -1) {
		if (config.cpu < 0 || !cpu_online(config.cpu)) {
			printk(KERN_ERR "invalid polling reservation (%u): "
				   "CPU %d offline\n", config.id, config.cpu);
			return -EINVAL;
		}
	}

	switch (res_type) {
		case PERIODIC_POLLING:
		case SPORADIC_POLLING:
			ret = create_polling_reservation(res_type, &config);
			break;

		case TABLE_DRIVEN:
			ret = create_table_driven_reservation(&config);
			break;

		default:
			return -EINVAL;
	};

	return ret;
}

static struct domain_proc_info mc2_domain_proc_info;

static long mc2_get_domain_proc_info(struct domain_proc_info **ret)
{
	*ret = &mc2_domain_proc_info;
	return 0;
}

static void mc2_setup_domain_proc(void)
{
	int i, cpu;
	int num_rt_cpus = num_online_cpus();

	struct cd_mapping *cpu_map, *domain_map;

	memset(&mc2_domain_proc_info, sizeof(mc2_domain_proc_info), 0);
	init_domain_proc_info(&mc2_domain_proc_info, num_rt_cpus, num_rt_cpus);
	mc2_domain_proc_info.num_cpus = num_rt_cpus;
	mc2_domain_proc_info.num_domains = num_rt_cpus;

	i = 0;
	for_each_online_cpu(cpu) {
		cpu_map = &mc2_domain_proc_info.cpu_to_domains[i];
		domain_map = &mc2_domain_proc_info.domain_to_cpus[i];

		cpu_map->id = cpu;
		domain_map->id = i;
		cpumask_set_cpu(i, cpu_map->mask);
		cpumask_set_cpu(cpu, domain_map->mask);
		++i;
	}
}

static long mc2_activate_plugin(void)
{
	int cpu;//, lv;
	struct mc2_cpu_state *state;
	struct cpu_entry *ce;
	int i;
	//unsigned long *cpu_counter;
	
	for(i = 0; i < NR_MODES; i++){
		gmp_init(&(_global_env_modes[i]));
	}
	_global_env = &_global_env_modes[0];
	
	//raw_spin_lock_init(&_lowest_prio_cpu.lock);
	raw_spin_lock_init(&mode_lock);
	raw_spin_lock_init(&global_lock);

	seen_once = false;

	for_each_online_cpu(cpu) {
		TRACE("Initializing CPU%d...\n", cpu);

		resched_cpu[cpu] = 0;
		//level_a_priorities[cpu] = 0;
		this_cpu_write(mode_counter, 0);
		
		state = cpu_state_for(cpu);
		ce = &_lowest_prio_cpu.cpu_entries[cpu];
		
		ce->cpu = cpu;
		ce->scheduled = NULL;
		ce->deadline = ULLONG_MAX;
		ce->lv = NUM_CRIT_LEVELS;
		ce->will_schedule = false;

		raw_spin_lock_init(&state->lock);
		printk(KERN_ALERT "CPU%d state->lock %p\n", cpu, &state->lock);
		state->cpu = cpu;
		state->scheduled = NULL;
		//for (lv = 0; lv < NUM_CRIT_LEVELS; lv++) {
		//	struct crit_entry *cr_entry = &state->crit_entries[lv];
		//	cr_entry->level = lv;
		//	cr_entry->running = NULL;
		//}
		for(i = 0; i < NR_MODES; i++){
			sup_init(&(state->sup_env_modes[i]));
		}
		state->sup_env = &(state->sup_env_modes[0]);

		hrtimer_init(&state->timer, CLOCK_MONOTONIC, HRTIMER_MODE_ABS_PINNED);
		state->timer.function = on_scheduling_timer;
		//state->spin_flag = false;
		this_cpu_write(mode_counter, 0);
	}

	mc2_setup_domain_proc();

	mode_poll_exited = false;

	mode = 0;
	requested_mode = 0;

	for(i = 0; i < NR_MODES; i++){
		mode_sizes[i] = 0;
	}
	res_reported = 0;
	//cpu_0_spin_flag = false;
	//cpu_0_task_exist = false;

	return 0;
}

static void mc2_finish_switch(struct task_struct *prev)
{
	int cpus;
	enum crit_level lv = get_task_crit_level(prev);
	struct mc2_cpu_state *state = local_cpu_state();
	
	state->scheduled = is_realtime(current) ? current : NULL;
	if (lv == CRIT_LEVEL_C) {
		for (cpus = 0; cpus<NR_CPUS; cpus++) {
			if (resched_cpu[cpus]) {
				resched_cpu[cpus] = 0;
				litmus_reschedule(cpus);
			}
		}
	}
}

static long mc2_deactivate_plugin(void)
{
	int cpu;
	struct mc2_cpu_state *state;
	struct reservation *res;
	struct next_timer_event *event;
	struct cpu_entry *ce;
	int i;

	for_each_online_cpu(cpu) {
		state = cpu_state_for(cpu);
		raw_spin_lock(&state->lock);

		hrtimer_cancel(&state->timer);

		ce = &_lowest_prio_cpu.cpu_entries[cpu];
		
		ce->cpu = cpu;
		ce->scheduled = NULL;
		ce->deadline = ULLONG_MAX;
		ce->lv = NUM_CRIT_LEVELS;
		ce->will_schedule = false;


		for(i = 0; i < NR_MODES; i++){
			/* Delete all reservations --- assumes struct reservation
			 * is prefix of containing struct. */
			state->sup_env = &(state->sup_env_modes[i]);
			while (!list_empty(&state->sup_env->active_reservations)) {
				res = list_first_entry(
					&state->sup_env->active_reservations,
				        struct reservation, list);
				list_del(&res->list);
				kfree(res);
			}

			while (!list_empty(&state->sup_env->inactive_reservations)) {
				res = list_first_entry(
					&state->sup_env->inactive_reservations,
				        struct reservation, list);
				list_del(&res->list);
				kfree(res);
			}

			while (!list_empty(&state->sup_env->depleted_reservations)) {
				res = list_first_entry(
					&state->sup_env->depleted_reservations,
				        struct reservation, list);
				list_del(&res->list);
				kfree(res);
			}
		}

		raw_spin_unlock(&state->lock);
	}

	//TRACE_CUR("LOCK_MSG: GLOBAL LOCK\n");
	raw_spin_lock(&global_lock);
	for(i = 0; i < NR_MODES; i++){
		_global_env = &_global_env_modes[i];
		while (!list_empty(&_global_env->active_reservations)) {
			res = list_first_entry(
				&_global_env->active_reservations,
					struct reservation, list);
			list_del(&res->list);
			kfree(res);
		}
	
		while (!list_empty(&_global_env->inactive_reservations)) {
			res = list_first_entry(
				&_global_env->inactive_reservations,
					struct reservation, list);
			list_del(&res->list);
			kfree(res);
		}

		while (!list_empty(&_global_env->depleted_reservations)) {
			res = list_first_entry(
				&_global_env->depleted_reservations,
					struct reservation, list);
			list_del(&res->list);
			kfree(res);
		}
	
		while (!list_empty(&_global_env->next_events)) {
			event = list_first_entry(
				&_global_env->next_events,
					struct next_timer_event, list);
			list_del(&event->list);
			kfree(event);
		}
	
	}
	raw_spin_unlock(&global_lock);
	
	atomic_set(&num_sync_released, 0);
	destroy_domain_proc_info(&mc2_domain_proc_info);
	return 0;
}

static struct sched_plugin mc2_plugin = {
	.plugin_name			= "MC2",
	.schedule				= mc2_schedule,
	.finish_switch			= mc2_finish_switch,
	.task_wake_up			= mc2_task_resume,
	.admit_task				= mc2_admit_task,
	.task_new				= mc2_task_new,
	.task_exit				= mc2_task_exit,
	.complete_job           = mc2_complete_job,
	.get_domain_proc_info   = mc2_get_domain_proc_info,
	.activate_plugin		= mc2_activate_plugin,
	.deactivate_plugin      = mc2_deactivate_plugin,
	.reservation_create     = mc2_reservation_create,
	.reservation_destroy	= mc2_reservation_destroy,
};

static int __init init_mc2(void)
{
	return register_sched_plugin(&mc2_plugin);
}

module_init(init_mc2);