From a484164f768cdea9b346b2af2c71376e8e260d21 Mon Sep 17 00:00:00 2001
From: Giovanni Di Sirio <gdisirio@gmail.com>
Date: Tue, 31 Mar 2015 06:22:30 +0000
Subject: git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@7833
 35acf78f-673a-0410-8e92-d51de3d6d3f4

---
 os/rt/include/chvt.h | 30 ++++++++++++++++++------------
 1 file changed, 18 insertions(+), 12 deletions(-)

(limited to 'os/rt/include')

diff --git a/os/rt/include/chvt.h b/os/rt/include/chvt.h
index 736d572b4..0bb127250 100644
--- a/os/rt/include/chvt.h
+++ b/os/rt/include/chvt.h
@@ -466,7 +466,6 @@ static inline void chVTDoTickI(void) {
   }
 #else /* CH_CFG_ST_TIMEDELTA > 0 */
   virtual_timer_t *vtp;
-  systime_t now, delta;
 
   /* The list is assumed to be non-empty because an tick interrupt just
      occurred.*/
@@ -482,6 +481,10 @@ static inline void chVTDoTickI(void) {
                                 chVTGetSystemTimeX() + 1),
               "out of time window");
 
+/*  if (now > 21) {
+     __BKPT(0);
+  }*/
+
   /* Timers processing loop.*/
   while (true) {
     vtfunc_t fn;
@@ -495,11 +498,6 @@ static inline void chVTDoTickI(void) {
     fn = vtp->vt_func;
     vtp->vt_func = NULL;
 
-    /* If the list is empty then the timer is stopped.*/
-    if (&ch.vtlist == (virtual_timers_list_t *)ch.vtlist.vt_next) {
-      port_timer_stop_alarm();
-    }
-
     /* Leaving the system critical zone in order to execute the callback
        and in order to give a preemption chance to higher priority
        interrupts.*/
@@ -513,25 +511,33 @@ static inline void chVTDoTickI(void) {
     chSysLockFromISR();
 
     /* If the list is empty then ending the loop, the list has to be
-       re-checked because new timers could have been added from within
-       the callback.*/
+       re-checked because new timers could have been added/removed from
+       within the callback or other ISRs.*/
     vtp = ch.vtlist.vt_next;
     if (&ch.vtlist == (virtual_timers_list_t *)vtp) {
+      /* Timers list empty, stopping alarms.*/
+      port_timer_stop_alarm();
       return;
     }
 
     /* Getting the current system time and calculating the time window since
        the last time has expired.*/
     now = chVTGetSystemTimeX();
-    delta = now - ch.vtlist.vt_lasttime;
 
     /* The next element is outside the current time window, the loop
        is stopped here.*/
-    if (vtp->vt_delta > delta) {
+    if (vtp->vt_delta > now - ch.vtlist.vt_lasttime) {
       /* Updating the alarm to the next deadline, deadline that must not be
          closer in time than the minimum time delta.*/
-      delta = (vtp->vt_delta >= (systime_t)CH_CFG_ST_TIMEDELTA) ?
-               vtp->vt_delta : (systime_t)CH_CFG_ST_TIMEDELTA;
+      systime_t delta = ch.vtlist.vt_lasttime + vtp->vt_delta - now;
+      if (delta < (systime_t)CH_CFG_ST_TIMEDELTA) {
+        delta = (systime_t)CH_CFG_ST_TIMEDELTA;
+      }
+
+//      if (now + delta >= 23) {
+//         __BKPT(0);
+//      }
+
       port_timer_set_alarm(now + delta);
 
       chDbgAssert((chVTGetSystemTimeX() - ch.vtlist.vt_lasttime) < delta,
-- 
cgit v1.2.3