Disable interrupts in MillisTimer critical sections

teensy3/EventResponder.cpp

 
 void MillisTimer::addToList()
 {
+	bool irq = disableTimerInterrupt();
 	if (list == nullptr) {
 		// list is empty, easy case
 		_next = nullptr;
 				timer->_prev = this;
 				_prev->_next = this;
 				isQueued = true;
+				enableTimerInterrupt(irq);
 				return;
 			}
 		}
 		timer->_next = this;
 	}
 	isQueued = true;
+	enableTimerInterrupt(irq);
 }
 
 void MillisTimer::end()
 {
+	bool irq = disableTimerInterrupt();
 	if (isQueued) {
 		if (_next) {
 			_next->_prev = _prev;
 		}
 		isQueued = false;
 	}
+	enableTimerInterrupt(irq);
 }
 
 void MillisTimer::runFromTimer()
 {
+	bool irq = disableTimerInterrupt();
 	MillisTimer *timer = list;
 	while (timer) {
 		if (timer->_ms > 0) {
 			MillisTimer *next = timer->_next;
 			if (next) next->_prev = nullptr;
 			list = next;
+			enableTimerInterrupt(irq);
 			timer->isQueued = false;
 			EventResponderRef event = *(timer->_event);
 			event.triggerEvent(0, timer);
 				timer->_ms = timer->_reload;
 				timer->addToList();
 			}
+			irq = disableTimerInterrupt();
 			timer = list;
 		}
 	}
+	enableTimerInterrupt(irq);
 }
 
 extern "C" volatile uint32_t systick_millis_count;

teensy3/EventResponder.h

 		uint32_t primask;
 		__asm__ volatile("mrs %0, primask\n" : "=r" (primask)::);
 		__disable_irq();
-		return (primask == 0) ? false : true;
+		return (primask == 0) ? true : false;
 	}
 	static void enableInterrupts(bool doit) {
 		if (doit) __enable_irq();
 	EventResponder *_event = nullptr;
 	bool isQueued = false;
 	static MillisTimer *list;
+	static bool disableTimerInterrupt() {
+		uint32_t primask;
+		__asm__ volatile("mrs %0, primask\n" : "=r" (primask)::);
+		__disable_irq();
+		return (primask == 0) ? true : false;
+	}
+	static void enableTimerInterrupt(bool doit) {
+		if (doit) __enable_irq();
+	}
 };
 
 #endif