1 files changed, 301 insertions, 2 deletions

diff --git a/litmus/reservation.c b/litmus/reservation.c
index 0e43479ff2e1..35b3b5de5d0c 100644
--- a/litmus/reservation.c
+++ b/litmus/reservation.c
@@ -1,4 +1,5 @@
 #include <linux/sched.h>
+#include <linux/slab.h>
 
 #include <litmus/litmus.h>
 #include <litmus/reservation.h>
@@ -203,8 +204,7 @@ static void sup_charge_budget(
                 {
                         /* make sure scheduler is invoked when this reservation expires
                          * its remaining budget */
-                         TRACE("requesting scheduler update for reservation %u in %llu nanoseconds\n",
-                                res->id, res->cur_budget);
+                         //TRACE("requesting scheduler update for reservation %u in %llu nanoseconds\n", res->id, res->cur_budget);
                          sup_scheduler_update_after(sup_env, res->cur_budget);
                 }
                 if (encountered_active)
@@ -317,3 +317,302 @@ void sup_init(struct sup_reservation_environment* sup_env)
 
         sup_env->next_scheduler_update = SUP_NO_SCHEDULER_UPDATE;
 }
+
+static void gmp_scheduler_update_at(
+        struct gmp_reservation_environment* gmp_env,
+        lt_t when)
+{
+        struct next_timer_event *nevent;
+        struct list_head *pos;
+        struct next_timer_event *queued;
+        int found = 0;
+        
+        nevent = kzalloc(sizeof(*nevent), GFP_KERNEL);
+        nevent->next_event = when;
+        nevent->timer_armed_on = NO_CPU;
+        
+        list_for_each(pos, &gmp_env->next_events) {
+                queued = list_entry(pos, struct next_timer_event, list);
+                if (queued->next_event > nevent->next_event) {
+                        list_add(&nevent->list, pos->prev);
+                        found = 1;
+                        TRACE("NEXT_EVENT ADDED after %llu\n", queued->next_event);
+                        break;
+                }
+        }
+        
+        if (!found) {
+                list_add_tail(&nevent->list, &gmp_env->next_events);
+                TRACE("NEXT_EVENT ADDED at [0]\n");
+        }
+        
+        TRACE("GMP_SCHEDULER_UPDATE_AT update_time: %llu\n", nevent->next_event);
+}
+
+static void gmp_scheduler_update_after(
+        struct gmp_reservation_environment* gmp_env,
+        lt_t timeout)
+{
+        gmp_scheduler_update_at(gmp_env, gmp_env->env.current_time + timeout);
+}
+
+static int _gmp_queue_depleted(
+        struct gmp_reservation_environment* gmp_env,
+        struct reservation *res)
+{
+        struct list_head *pos;
+        struct reservation *queued;
+        int passed_earlier = 0;
+
+        list_for_each(pos, &gmp_env->depleted_reservations) {
+                queued = list_entry(pos, struct reservation, list);
+                if (queued->next_replenishment > res->next_replenishment) {
+                        list_add(&res->list, pos->prev);
+                        return passed_earlier;
+                } else
+                        passed_earlier = 1;
+        }
+
+        list_add_tail(&res->list, &gmp_env->depleted_reservations);
+
+        return passed_earlier;
+}
+
+static void gmp_queue_depleted(
+        struct gmp_reservation_environment* gmp_env,
+        struct reservation *res)
+{
+        int passed_earlier = _gmp_queue_depleted(gmp_env, res);
+
+        /* check for updated replenishment time */
+        if (!passed_earlier)
+                gmp_scheduler_update_at(gmp_env, res->next_replenishment);
+}
+
+static int _gmp_queue_active(
+        struct gmp_reservation_environment* gmp_env,
+        struct reservation *res)
+{
+        struct list_head *pos;
+        struct reservation *queued;
+        int passed_active = 0;
+
+        list_for_each(pos, &gmp_env->active_reservations) {
+                queued = list_entry(pos, struct reservation, list);
+                if (queued->priority > res->priority) {
+                        list_add(&res->list, pos->prev);
+                        return passed_active;
+                } else if (queued->state == RESERVATION_ACTIVE)
+                        passed_active = 1;
+        }
+
+        list_add_tail(&res->list, &gmp_env->active_reservations);
+        return passed_active;
+}
+
+static void gmp_queue_active(
+        struct gmp_reservation_environment* gmp_env,
+        struct reservation *res)
+{
+        int passed_active = _gmp_queue_active(gmp_env, res);
+
+        /* check for possible preemption */
+        if (res->state == RESERVATION_ACTIVE && !passed_active) {
+                gmp_env->schedule_now = true;
+        } else {
+                /* Active means this reservation is draining budget => make sure
+                 * the scheduler is called to notice when the reservation budget has been
+                 * drained completely. */
+                gmp_scheduler_update_after(gmp_env, res->cur_budget);
+        }
+}
+
+static void gmp_queue_reservation(
+        struct gmp_reservation_environment* gmp_env,
+        struct reservation *res)
+{
+        TRACE("GMP_QUEUE_RES state (%d)\n", res->state);
+        switch (res->state) {
+                case RESERVATION_INACTIVE:
+                        list_add(&res->list, &gmp_env->inactive_reservations);
+                        break;
+
+                case RESERVATION_DEPLETED:
+                        gmp_queue_depleted(gmp_env, res);
+                        break;
+
+                case RESERVATION_ACTIVE_IDLE:
+                case RESERVATION_ACTIVE:
+                        gmp_queue_active(gmp_env, res);
+                        break;
+        }
+}
+
+void gmp_add_new_reservation(
+        struct gmp_reservation_environment* gmp_env,
+        struct reservation* new_res)
+{
+        new_res->env = &gmp_env->env;
+        TRACE("GMP_ADD_NEW_RES\n");
+        gmp_queue_reservation(gmp_env, new_res);
+}
+
+struct reservation* gmp_find_by_id(struct gmp_reservation_environment* gmp_env,
+        unsigned int id)
+{
+        struct reservation *res;
+
+        list_for_each_entry(res, &gmp_env->active_reservations, list) {
+                if (res->id == id)
+                        return res;
+        }
+        list_for_each_entry(res, &gmp_env->inactive_reservations, list) {
+                if (res->id == id)
+                        return res;
+        }
+        list_for_each_entry(res, &gmp_env->depleted_reservations, list) {
+                if (res->id == id)
+                        return res;
+        }
+
+        return NULL;
+}
+
+static void gmp_charge_budget(
+        struct gmp_reservation_environment* gmp_env,
+        lt_t delta)
+{
+        struct list_head *pos, *next;
+        struct reservation *res;
+
+        //int encountered_active = 0;
+
+        list_for_each_safe(pos, next, &gmp_env->active_reservations) {
+                /* charge all ACTIVE_IDLE up to the current scheduled ACTIVE reservation */
+                res = list_entry(pos, struct reservation, list);
+                if (res->state == RESERVATION_ACTIVE) {
+                        res->ops->drain_budget(res, delta);
+                        //encountered_active = 1;
+                } else {
+                        BUG_ON(res->state != RESERVATION_ACTIVE_IDLE);
+                        res->ops->drain_budget(res, delta);
+                }
+                if (res->state == RESERVATION_ACTIVE ||
+                        res->state == RESERVATION_ACTIVE_IDLE)
+                {
+                        /* make sure scheduler is invoked when this reservation expires
+                         * its remaining budget */
+                        TRACE("[GMP] requesting scheduler update for reservation %u in %llu nanoseconds\n", res->id, res->cur_budget);
+                        gmp_scheduler_update_after(gmp_env, res->cur_budget);
+                }
+//              if (encountered_active)
+                        /* stop at the first ACTIVE reservation */
+//                      break;
+        }
+        //TRACE("finished charging budgets\n");
+}
+
+static void gmp_replenish_budgets(struct gmp_reservation_environment* gmp_env)
+{
+        struct list_head *pos, *next;
+        struct reservation *res;
+
+        list_for_each_safe(pos, next, &gmp_env->depleted_reservations) {
+                res = list_entry(pos, struct reservation, list);
+                if (res->next_replenishment <= gmp_env->env.current_time) {
+                        res->ops->replenish(res);
+                } else {
+                        /* list is ordered by increasing depletion times */
+                        break;
+                }
+        }
+        //TRACE("finished replenishing budgets\n");
+
+        /* request a scheduler update at the next replenishment instant */
+        res = list_first_entry_or_null(&gmp_env->depleted_reservations,
+                struct reservation, list);
+        if (res)
+                gmp_scheduler_update_at(gmp_env, res->next_replenishment);
+}
+
+void gmp_update_time(
+        struct gmp_reservation_environment* gmp_env,
+        lt_t now)
+{
+        lt_t delta;
+
+        /* If the time didn't advance, there is nothing to do.
+         * This check makes it safe to call gmp_advance_time() potentially
+         * multiple times (e.g., via different code paths. */
+        //TRACE("(gmp_update_time) now: %llu, current_time: %llu\n", now, gmp_env->env.current_time);
+        if (unlikely(now <= gmp_env->env.current_time))
+                return;
+
+        delta = now - gmp_env->env.current_time;
+        gmp_env->env.current_time = now;
+
+        /* deplete budgets by passage of time */
+        gmp_charge_budget(gmp_env, delta);
+
+        /* check if any budgets where replenished */
+        gmp_replenish_budgets(gmp_env);
+}
+
+struct task_struct* gmp_dispatch(struct gmp_reservation_environment* gmp_env)
+{
+        struct reservation *res, *next;
+        struct task_struct *tsk = NULL;
+        lt_t time_slice;
+
+        list_for_each_entry_safe(res, next, &gmp_env->active_reservations, list) {
+                if (res->state == RESERVATION_ACTIVE && res->scheduled_on == NO_CPU) {
+                        tsk = res->ops->dispatch_client(res, &time_slice);
+                        if (likely(tsk)) {
+                                if (time_slice)
+                                    gmp_scheduler_update_after(gmp_env, time_slice);
+                                gmp_scheduler_update_after(gmp_env, res->cur_budget);
+                                return tsk;
+                        }
+                }
+        }
+
+        return NULL;
+}
+
+static void gmp_res_change_state(
+        struct reservation_environment* env,
+        struct reservation *res,
+        reservation_state_t new_state)
+{
+        struct gmp_reservation_environment* gmp_env;
+
+        gmp_env = container_of(env, struct gmp_reservation_environment, env);
+
+        TRACE("[GMP] reservation R%d state %d->%d at %llu\n",
+                res->id, res->state, new_state, env->current_time);
+
+        list_del(&res->list);
+        /* check if we need to reschedule because we lost an active reservation */
+        if (res->state == RESERVATION_ACTIVE && !gmp_env->will_schedule)
+                gmp_env->schedule_now = true;
+        res->state = new_state;
+        gmp_queue_reservation(gmp_env, res);
+}
+
+void gmp_init(struct gmp_reservation_environment* gmp_env)
+{
+        memset(gmp_env, sizeof(*gmp_env), 0);
+
+        INIT_LIST_HEAD(&gmp_env->active_reservations);
+        INIT_LIST_HEAD(&gmp_env->depleted_reservations);
+        INIT_LIST_HEAD(&gmp_env->inactive_reservations);
+
+        gmp_env->env.change_state = gmp_res_change_state;
+
+        gmp_env->schedule_now = false;
+        
+        //raw_spin_lock_init(&gmp_env->lock);
+        INIT_LIST_HEAD(&gmp_env->next_events);
+        
+        raw_spin_lock_init(&gmp_env->event_lock);
+}