From 905443c746be165c98c1c3b8000b7f995f69f5b2 Mon Sep 17 00:00:00 2001
From: Jakob Pietron <jakob.pietron@uni-ulm.de>
Date: Sat, 8 Aug 2020 16:39:31 +0200
Subject: [PATCH] apply millis adjustment by mcci

---
 cores/arduino/delay.c | 23 +++++++++++++++++++++++
 cores/arduino/delay.h | 11 +++++++++++
 2 files changed, 34 insertions(+)

diff --git a/cores/arduino/delay.c b/cores/arduino/delay.c
index 7833512a6..d712f0cde 100644
--- a/cores/arduino/delay.c
+++ b/cores/arduino/delay.c
@@ -31,7 +31,30 @@ unsigned long millis( void )
 // todo: ensure no interrupts
   return _ulTickCount ;
 }
+    
+// atomically add delta to _ulTickCount. Momentarily disables interrupts, assuming that
+// the systick interrupt is maskable.
+uint32_t adjust_millis_forward(uint32_t delta)
+    {
+      // copy old interrupt-enable state to flags.
+    uint32_t const flags = __get_PRIMASK();
+
+    // disable interrupts
+    __set_PRIMASK(1);
+
+    // observe _ulTickCount, and advance it.
+    uint32_t const tickCount = _ulTickCount + delta;
 
+    // save _ulTickCount
+    _ulTickCount = tickCount;
+
+    // restore interrupts (does nothing if ints were disabled on entry)
+    __set_PRIMASK(flags);
+
+    // return the new value of _ulTickCount.
+    return tickCount;
+}
+    
 // Interrupt-compatible version of micros
 // Theory: repeatedly take readings of SysTick counter, millis counter and SysTick interrupt pending flag.
 // When it appears that millis counter and pending is stable and SysTick hasn't rolled over, use these
diff --git a/cores/arduino/delay.h b/cores/arduino/delay.h
index 3d3a6d195..991f8f146 100644
--- a/cores/arduino/delay.h
+++ b/cores/arduino/delay.h
@@ -36,6 +36,17 @@ extern "C" {
  */
 extern unsigned long millis( void ) ;
 
+/**
+ * \brief Atomically adjust the millis() clock forward.
+ *
+ * \param delta is the value to add to the clock (uint32_t)
+ *
+ * \return New value of the millis() clock.
+ *
+ * \note See also millis(). Overflows are ignored.
+ */
+extern uint32_t adjust_millis_forward( uint32_t );
+
 /**
  * \brief Returns the number of microseconds since the Arduino board began running the current program.
  *