1 files changed, 44 insertions, 25 deletions

diff --git a/litmus/reservations/gedf_reservation.c b/litmus/reservations/gedf_reservation.c
index 41a4feeb6680..1c604212acf2 100644
--- a/litmus/reservations/gedf_reservation.c
+++ b/litmus/reservations/gedf_reservation.c
@@ -201,7 +201,7 @@ static void check_for_preemptions(struct gedf_reservation_environment* gedf_env)
                         }
                 }
 
-                if (last->linked && last->linked->res.cur_budget)
+                if (last->linked)
                         requeue(gedf_env, last->linked);
 
                 link_task_to_cpu(gedf_env, gedf_res, last);
@@ -685,6 +685,8 @@ static void gedf_env_resume(
         /* adds cpu back to scheduling consideration */
         bheap_insert(cpu_lower_prio, &gedf_env->cpu_heap, entry->hn);
         gedf_env->num_cpus++;
+        gedf_env->num_active_cpus = (gedf_env->num_cpus > gedf_env->num_active_cpus) ?
+                                                                        gedf_env->num_cpus : gedf_env->num_active_cpus;
 
         raw_spin_unlock_irqrestore(&gedf_env->domain.ready_lock, flags);
 
@@ -715,7 +717,15 @@ static struct task_struct* gedf_env_dispatch(
         if (entry->scheduled)
                 np = entry->scheduled->res.ops->is_np(&entry->scheduled->res, cpu);
 
+        /* if flagged for removal from environment, invoke shutdown callback */
+        if (entry->scheduled && entry->scheduled->will_remove) {
+                /* assumed to already been unlinked by whatever set will_remove */
+                entry->scheduled->res.ops->shutdown(&entry->scheduled->res);
+                entry->scheduled = NULL;
+        }
+
         raw_spin_lock_irqsave(&gedf_env->domain.ready_lock, flags);
+
         /* Budget replenishment has to happen under a lock, otherwise
          * check_for_preemptions() may be called concurrently, and try to link and
          * schedule our now-out-of-budget task on another CPU. This would result in
@@ -727,7 +737,8 @@ static struct task_struct* gedf_env_dispatch(
                 entry->scheduled->res.ops->replenish_budget(&entry->scheduled->res, cpu);
                 /* unlink and requeue if not blocked and not np*/
                 if (!entry->scheduled->blocked &&
-                                !entry->scheduled->res.ops->is_np(&entry->scheduled->res, cpu)) {
+                                !entry->scheduled->res.ops->is_np(&entry->scheduled->res, cpu) &&
+                                !entry->scheduled->will_remove) {
                         unlink(gedf_env, entry->scheduled);
                         requeue(gedf_env, entry->scheduled);
                         check_for_preemptions(gedf_env);
@@ -798,13 +809,6 @@ static void gedf_env_update_time(
          * Therefore, no lock is needed for this operation
          */
         entry->scheduled->res.ops->drain_budget(&entry->scheduled->res, how_much, cpu);
-
-        /* if flagged for removal from environment, invoke shutdown callback */
-        if (entry->scheduled->will_remove) {
-                /* assumed to already been unlinked by whatever set will_remove */
-                entry->scheduled->res.ops->shutdown(&entry->scheduled->res);
-                entry->scheduled = NULL;
-        }
 }
 
 /* callback for how the domain will release jobs */
@@ -990,8 +994,12 @@ static struct task_struct* omlp_dequeue(struct omlp_semaphore *sem)
 {
         struct task_struct* first = __waitqueue_remove_first(&sem->fifo_wait);
 
-        if (first && !omlp_move(sem))
+        /* don't replace tmp with omlp_move! shortcircuiting will break omlp for m=1 */
+        int tmp = omlp_move(sem);
+        if (first && !tmp)
                 sem->num_free++;
+        if (!first && tmp)
+                first = __waitqueue_remove_first(&sem->fifo_wait);
 
         return first;
 }
@@ -1119,6 +1127,7 @@ static enum hrtimer_restart wake_fz_waiter(struct hrtimer* timer)
 {
         struct omlp_semaphore* sem = container_of(timer, struct omlp_semaphore, wake_timer);
         sem->policed = 0;
+        BUG_ON(!sem->owner);
         wake_up_process(sem->owner);
 
         return HRTIMER_NORESTART;
@@ -1129,6 +1138,7 @@ static lt_t next_component_start(struct mtd_reservation* mtd_res, int cpu)
         int i_index;
         lt_t resume_time;
 
+        BUG_ON(mtd_res->major_cycle_start[cpu] + mtd_res->intervals[cpu][mtd_res->interval_index[cpu]].start > litmus_clock());
         i_index = (mtd_res->interval_index[cpu] + 1) % mtd_res->num_intervals[cpu];
         resume_time = mtd_res->major_cycle_start[cpu];
         if (!i_index)
@@ -1169,10 +1179,10 @@ int gedf_env_omlp_lock_cs(struct litmus_lock* l, lt_t cs_len)
                 init_prio_waitqueue_entry(&wait, t, ULLONG_MAX - t_res->priority);
                 wait.wq.cs_len = cs_len;
 
-                set_current_state(TASK_UNINTERRUPTIBLE);
-
                 omlp_enqueue(sem, &wait);
 
+                set_current_state(TASK_UNINTERRUPTIBLE);
+
                 /* check if we need to activate priority inheritance */
                 if (higher_res_prio(t_res, sem->hp_waiter_res)) {
                         sem->hp_waiter = t;
@@ -1251,6 +1261,7 @@ static int gedf_env_omlp_access_fz_check(struct litmus_lock* l, lt_t fz_len, lt_
                 schedule();
                 TS_LOCK_RESUME;
         }
+
         sem->policed = 0;
 
         t_res = (struct ext_reservation *) tsk_rt(t)->plugin_state;
@@ -1275,22 +1286,23 @@ static int gedf_env_omlp_access_fz_check(struct litmus_lock* l, lt_t fz_len, lt_
                 cutahead_t = omlp_cutahead(sem, ULLONG_MAX);
                 if (cutahead_t) {
                         wake_up_process(cutahead_t);
-                        local_irq_restore(flags);
                         set_current_state(TASK_UNINTERRUPTIBLE);
                         TS_LOCK_SUSPEND;
+                        local_irq_restore(flags);
                         schedule();
                         TS_LOCK_RESUME;
                 } else {
                         /* go on a wait queue to be woken up when the parent reservation
                         * is next scheduled */
                         BUG_ON(t != sem->owner);
-                        resume_time = next_component_start(mtd_res, cpu);
+                        resume_time = max(next_component_start(mtd_res, cpu), litmus_clock());
                         hrtimer_start(&sem->wake_timer,
                                 ns_to_ktime(resume_time),
                                 HRTIMER_MODE_ABS_PINNED_HARD);
-                        local_irq_restore(flags);
-                        set_current_state(TASK_UNINTERRUPTIBLE);
+                        BUG_ON(!hrtimer_active(&sem->wake_timer));
+                        set_current_state(TASK_INTERRUPTIBLE);
                         TS_LOCK_SUSPEND;
+                        local_irq_restore(flags);
                         schedule();
                         TS_LOCK_RESUME;
                 }
@@ -1330,6 +1342,7 @@ static int gedf_env_omlp_cancel_watchdog(struct litmus_lock* l)
                 schedule();
                 TS_LOCK_RESUME;
         }
+
         sem->policed = 0;
 
         BUG_ON(!tsk_rt(t)->ctrl_page);
@@ -1347,6 +1360,8 @@ static void __gedf_env_omlp_unlock(struct omlp_semaphore* sem, struct task_struc
         
         tsk_rt(t)->num_locks_held--;
 
+        BUG_ON(sem->owner != t);
+
         /* check if there are jobs waiting vor this resource */
         next = omlp_dequeue(sem);
         if (next) {
@@ -1381,9 +1396,10 @@ static void __gedf_env_omlp_unlock(struct omlp_semaphore* sem, struct task_struc
 
                 /* wake up next */
                 wake_up_process(next);
-        } else
+        } else {
                 /* becomes available */
                 sem->owner = NULL;
+        }
 
         /* we lose the benefit of priority inheritance (if any) */
         if (tsk_rt(t)->plugin_state && ((struct ext_reservation *)tsk_rt(t)->plugin_state)->inh_res)
@@ -1397,6 +1413,7 @@ static int gedf_env_omlp_unlock(struct litmus_lock* l)
         unsigned long flags;
 
         if (sem->owner != t) {
+                BUG_ON(true);
                 return -EINVAL;
         }
 
@@ -1411,6 +1428,7 @@ static int gedf_env_omlp_unlock(struct litmus_lock* l)
                 TS_LOCK_RESUME;
         }
         sem->policed = 0;
+
         spin_lock_irqsave(&sem->fifo_wait.lock, flags);
 
         __gedf_env_omlp_unlock(sem, t);
@@ -1451,23 +1469,21 @@ static enum hrtimer_restart omlp_fz_police(struct hrtimer *timer)
         mtd_res = (struct mtd_reservation*)t_res->par_env->res;
 
         cpu = smp_processor_id();
-        
-        sem->policed = 1;
-
-        BUG_ON(t != sem->owner);
 
         // set wake timer
-        resume_time = next_component_start(mtd_res, cpu);
+        resume_time = max(next_component_start(mtd_res, cpu), litmus_clock());
         hrtimer_start(&sem->wake_timer,
                 ns_to_ktime(resume_time),
                 HRTIMER_MODE_ABS_PINNED_HARD);
+        BUG_ON(!hrtimer_active(&sem->wake_timer));
+        set_current_state(TASK_INTERRUPTIBLE);
+        sem->policed = 1;
 
         //memset(&info, 0, sizeof(struct kernel_siginfo));
         //info.si_signo = 31;
         //info.si_code = SI_KERNEL;
         //send_sig_info(31, &info, t);
 
-        set_current_state(TASK_UNINTERRUPTIBLE);
         litmus_reschedule_local();
 
         return HRTIMER_NORESTART;
@@ -1515,8 +1531,10 @@ static struct litmus_lock* gedf_env_new_omlp(void)
         struct gedf_reservation_environment *gedf_env;
 
         sem = kmalloc(sizeof(*sem), GFP_ATOMIC);
-        if (!sem)
+        if (!sem) {
+                BUG_ON(true);
                 return NULL;
+        }
 
         t_res = (struct ext_reservation *) tsk_rt(current)->plugin_state;
         gedf_env = container_of(t_res->par_env, struct gedf_reservation_environment, env);
@@ -1528,7 +1546,7 @@ static struct litmus_lock* gedf_env_new_omlp(void)
         init_waitqueue_head(&sem->prio_wait);
         sem->litmus_lock.ops = &gedf_env_omlp_lock_ops;
         /* free = cpus-1 since ->owner is the head and also counted */
-        sem->num_free = gedf_env->num_cpus - 1;
+        sem->num_free = 0; //gedf_env->num_active_cpus - 1;
         hrtimer_init(&sem->police_timer, CLOCK_MONOTONIC, HRTIMER_MODE_ABS_PINNED_HARD);
         hrtimer_init(&sem->wake_timer, CLOCK_MONOTONIC, HRTIMER_MODE_ABS_PINNED_HARD);
         sem->police_timer.function = omlp_fz_police;
@@ -1614,6 +1632,7 @@ long alloc_gedf_reservation_environment(
         INIT_LIST_HEAD(&gedf_env->env.all_reservations);
 
         gedf_env->num_cpus = 0;
+        gedf_env->num_active_cpus = 0;
         bheap_init(&gedf_env->cpu_heap);
         for (i = 0; i < max_cpus; i++) {
                 gedf_env->cpu_entries[i].id = i;