From cb725828056b9564ab5d723bd0738375a83c077c Mon Sep 17 00:00:00 2001
From: Giovanni Di Sirio <gdisirio@gmail.com>
Date: Fri, 13 Oct 2017 12:35:40 +0000
Subject: [PATCH] Fixed and optimizations, NOW it seems to work for intervals
 larger than system time.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/branches/rt5_dev_point1@10818 35acf78f-673a-0410-8e92-d51de3d6d3f4
---
 demos/STM32/RT-STM32F746G-DISCOVERY/main.c |  8 ++++----
 os/rt/include/chtime.h                     |  4 ++--
 os/rt/include/chvt.h                       | 20 +++++++++++---------
 3 files changed, 17 insertions(+), 15 deletions(-)

diff --git a/demos/STM32/RT-STM32F746G-DISCOVERY/main.c b/demos/STM32/RT-STM32F746G-DISCOVERY/main.c
index 0e62e3604..5199ddf37 100644
--- a/demos/STM32/RT-STM32F746G-DISCOVERY/main.c
+++ b/demos/STM32/RT-STM32F746G-DISCOVERY/main.c
@@ -29,9 +29,9 @@ static THD_FUNCTION(Thread1, arg) {
   chRegSetThreadName("blinker");
   while (true) {
     palSetLine(LINE_ARD_D13);
-    chThdSleepMilliseconds(500);
+    chThdSleepMilliseconds(1000);
     palClearLine(LINE_ARD_D13);
-    chThdSleepMilliseconds(500);
+    chThdSleepMilliseconds(1000);
   }
 }
 
@@ -64,7 +64,7 @@ int main(void) {
   /*
    * Creates the example thread.
    */
-  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
+  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO+1, Thread1, NULL);
 
   /*
    * Normal main() thread activity, in this demo it does nothing except
@@ -73,6 +73,6 @@ int main(void) {
   while (true) {
 //    if (palReadLine(LINE_BUTTON_USER))
 //      test_execute((BaseSequentialStream *)&SD1);
-    chThdSleepMilliseconds(500);
+    chThdSleepMilliseconds(2000);
   }
 }
diff --git a/os/rt/include/chtime.h b/os/rt/include/chtime.h
index e2e3402ef..701dfb4d5 100644
--- a/os/rt/include/chtime.h
+++ b/os/rt/include/chtime.h
@@ -488,7 +488,7 @@ static inline systime_t chTimeAddX(systime_t systime,
  */
 static inline sysinterval_t chTimeDiffX(systime_t start, systime_t end) {
 
-  return (sysinterval_t)(end - start);
+  return (sysinterval_t)((systime_t)(end - start));
 }
 
 
@@ -509,7 +509,7 @@ static inline bool chTimeIsInRangeX(systime_t time,
                                     systime_t start,
                                     systime_t end) {
 
-  return (bool)((systime_t)(time - start) < (systime_t)(end - start));
+  return (bool)((time - start) < (end - start));
 }
 
 /** @} */
diff --git a/os/rt/include/chvt.h b/os/rt/include/chvt.h
index b3a803c1a..e115bec63 100644
--- a/os/rt/include/chvt.h
+++ b/os/rt/include/chvt.h
@@ -393,12 +393,11 @@ static inline void chVTDoTickI(void) {
 #else /* CH_CFG_ST_TIMEDELTA > 0 */
   virtual_timer_t *vtp;
   systime_t now;
-  sysinterval_t delta;
+  sysinterval_t delta, nowdelta;
 
   /* Looping through timers.*/
   vtp = ch.vtlist.next;
   while (true) {
-    sysinterval_t nowdelta;
 
     /* Getting the system time as reference.*/
     now = chVTGetSystemTimeX();
@@ -407,7 +406,7 @@ static inline void chVTDoTickI(void) {
     /* The list scan is limited by the timers header having
        "ch.vtlist.vt_delta == (sysinterval_t)-1" which is
        greater than all deltas.*/
-    if (vtp->delta > nowdelta) {
+    if (nowdelta < vtp->delta) {
       break;
     }
 
@@ -415,8 +414,9 @@ static inline void chVTDoTickI(void) {
     do {
       vtfunc_t fn;
 
-      /* Recalculated delta.*/
-      nowdelta = nowdelta - vtp->delta;
+      /* The "last time" becomes this timer's expiration time.*/
+      ch.vtlist.lasttime += vtp->delta;
+      nowdelta -= vtp->delta;
 
       vtp->next->prev = (virtual_timer_t *)&ch.vtlist;
       ch.vtlist.next = vtp->next;
@@ -437,9 +437,6 @@ static inline void chVTDoTickI(void) {
       vtp = ch.vtlist.next;
     }
     while (vtp->delta <= nowdelta);
-
-    /* The "last time" is set to the current time.*/
-    ch.vtlist.lasttime = now;
   }
 
   /* if the list is empty, nothing else to do.*/
@@ -447,6 +444,11 @@ static inline void chVTDoTickI(void) {
     return;
   }
 
+  /* The "unprocessed nowdelta" time slice is added to "last time"
+     and subtracted to next timer's delta.*/
+  ch.vtlist.lasttime += nowdelta;
+  ch.vtlist.next->delta -= nowdelta;
+
   /* Recalculating the next alarm time.*/
   delta = chTimeDiffX(now, chTimeAddX(ch.vtlist.lasttime, vtp->delta));
   if (delta < (sysinterval_t)CH_CFG_ST_TIMEDELTA) {
@@ -454,7 +456,7 @@ static inline void chVTDoTickI(void) {
   }
 #if CH_CFG_INTERVALS_SIZE > CH_CFG_ST_RESOLUTION
   /* The delta could be too large for the physical timer to handle.*/
-  if (delta > (sysinterval_t)TIME_MAX_SYSTIME) {
+  else if (delta > (sysinterval_t)TIME_MAX_SYSTIME) {
     delta = (sysinterval_t)TIME_MAX_SYSTIME;
   }
 #endif