From 534aee17e9112eb52240e6ecab2f448955206c22 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 30 Apr 2012 16:11:05 +1000
Subject: [PATCH] TimerProcess: added suspend_timer()/resume_timer()

this will be used to avoid races in driver initialisation
---
 .../AP_PeriodicProcessStub.cpp                |  2 ++
 .../AP_PeriodicProcessStub.h                  |  3 ++
 .../AP_PeriodicProcess/AP_TimerProcess.cpp    | 28 ++++++++++++++-----
 .../AP_PeriodicProcess/AP_TimerProcess.h      |  3 ++
 .../AP_PeriodicProcess/PeriodicProcess.h      |  9 ++++--
 5 files changed, 35 insertions(+), 10 deletions(-)

diff --git a/libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.cpp b/libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.cpp
index aa6e0483bb..0e57d49a7a 100644
--- a/libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.cpp
+++ b/libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.cpp
@@ -4,4 +4,6 @@ AP_PeriodicProcessStub::AP_PeriodicProcessStub(uint8_t period) {}
 void AP_PeriodicProcessStub::init( Arduino_Mega_ISR_Registry * isr_reg ){}
 void AP_PeriodicProcessStub::register_process(ap_procedure proc) {}
 void AP_PeriodicProcessStub::set_failsafe(ap_procedure proc) {}
+void AP_PeriodicProcessStub::suspend_timer(void) {}
+void AP_PeriodicProcessStub::resume_timer(void) {}
 void AP_PeriodicProcessStub::run(void) {}
diff --git a/libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.h b/libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.h
index c9e9c35670..9544a9a77b 100644
--- a/libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.h
+++ b/libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.h
@@ -13,11 +13,14 @@ class AP_PeriodicProcessStub : public AP_PeriodicProcess
         void init( Arduino_Mega_ISR_Registry * isr_reg );
         void register_process(ap_procedure proc);
         void set_failsafe(ap_procedure proc);
+        void suspend_timer(void);
+        void resume_timer(void);
         static void run(void);
     protected:
         static uint8_t _period;
         static void (*_proc)(void);
         static void (*_failsafe)(void);
+        static bool _suspended;
 };
 
 #endif
diff --git a/libraries/AP_PeriodicProcess/AP_TimerProcess.cpp b/libraries/AP_PeriodicProcess/AP_TimerProcess.cpp
index 767efacfb1..0395c51340 100644
--- a/libraries/AP_PeriodicProcess/AP_TimerProcess.cpp
+++ b/libraries/AP_PeriodicProcess/AP_TimerProcess.cpp
@@ -18,6 +18,7 @@ ap_procedure AP_TimerProcess::_proc[AP_TIMERPROCESS_MAX_PROCS];
 ap_procedure AP_TimerProcess::_failsafe;
 bool AP_TimerProcess::_in_timer_call;
 uint8_t AP_TimerProcess::_pidx = 0;
+bool AP_TimerProcess::_suspended;
 
 AP_TimerProcess::AP_TimerProcess(uint8_t period)
 {
@@ -34,7 +35,8 @@ void AP_TimerProcess::init( Arduino_Mega_ISR_Registry * isr_reg )
 	TIFR2  = _BV(TOV2);	        // clear pending interrupts;
 	TIMSK2 = _BV(TOIE2);        // enable the overflow interrupt
 
-	_failsafe = NULL;
+    _failsafe = NULL;
+    _suspended = false;
 	_in_timer_call = false;
 
 	for (uint8_t i = 0; i < AP_TIMERPROCESS_MAX_PROCS; i++)
@@ -64,6 +66,16 @@ void AP_TimerProcess::set_failsafe(ap_procedure proc)
 	_failsafe = proc;
 }
 
+void AP_TimerProcess::suspend_timer(void)
+{
+	_suspended = true;
+}
+
+void AP_TimerProcess::resume_timer(void)
+{
+	_suspended = false;
+}
+
 void AP_TimerProcess::run(void)
 {
 	// we enable the interrupt again immediately and also enable
@@ -95,12 +107,14 @@ void AP_TimerProcess::run(void)
 	}
 	_in_timer_call = true;
 
-	// now call the timer based drivers
-	for (int i = 0; i < _pidx; i++) {
-		if (_proc[i] != NULL) {
-			_proc[i](tnow);
-		}
-	}
+    if (!_suspended) {
+        // now call the timer based drivers
+        for (int i = 0; i < _pidx; i++) {
+            if (_proc[i] != NULL) {
+                _proc[i](tnow);
+            }
+        }
+    }
 
 	// and the failsafe, if one is setup
 	if (_failsafe != NULL) {
diff --git a/libraries/AP_PeriodicProcess/AP_TimerProcess.h b/libraries/AP_PeriodicProcess/AP_TimerProcess.h
index 140c5996f8..f90fb3cbef 100644
--- a/libraries/AP_PeriodicProcess/AP_TimerProcess.h
+++ b/libraries/AP_PeriodicProcess/AP_TimerProcess.h
@@ -17,6 +17,8 @@ class AP_TimerProcess : public AP_PeriodicProcess
         void init( Arduino_Mega_ISR_Registry * isr_reg );
         void register_process(ap_procedure proc);
         void set_failsafe(ap_procedure proc);
+        void suspend_timer(void);
+        void resume_timer(void);
         static void run(void);
     protected:
         static uint8_t _period;
@@ -24,6 +26,7 @@ class AP_TimerProcess : public AP_PeriodicProcess
         static ap_procedure _failsafe;
         static uint8_t _pidx;
 	static bool _in_timer_call;
+	static bool _suspended;
 };
 
 #endif // __AP_TIMERPROCESS_H__
diff --git a/libraries/AP_PeriodicProcess/PeriodicProcess.h b/libraries/AP_PeriodicProcess/PeriodicProcess.h
index 021e84f060..2c57499382 100644
--- a/libraries/AP_PeriodicProcess/PeriodicProcess.h
+++ b/libraries/AP_PeriodicProcess/PeriodicProcess.h
@@ -1,3 +1,4 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
 #ifndef __PERIODICPROCESS_H__
 #define __PERIODICPROCESS_H__
@@ -10,9 +11,11 @@ typedef void (*ap_procedure)(uint32_t );
 
 class AP_PeriodicProcess
 {
-    public:
-        virtual void register_process(ap_procedure proc) = 0;
-        virtual void set_failsafe(ap_procedure proc) = 0;
+public:
+    virtual void register_process(ap_procedure proc) = 0;
+    virtual void set_failsafe(ap_procedure proc) = 0;
+    virtual void suspend_timer(void) = 0;
+    virtual void resume_timer(void) = 0;
 };
 
 #endif // __PERIODICPROCESS_H__