From 8f4a2e4c0e96da29dbc221a741cb217b80a424dd Mon Sep 17 00:00:00 2001
From: James Bielman <jamesjb@galois.com>
Date: Thu, 3 Jan 2013 15:52:07 -0800
Subject: [PATCH] AP_HAL_SMACCM: Scheduler timer process implemented correctly.

- Use "g_atomic" for suspend/resume timer procs.
---
 libraries/AP_HAL_SMACCM/Scheduler.cpp | 35 +++++++++++----------------
 libraries/AP_HAL_SMACCM/Scheduler.h   |  1 -
 2 files changed, 14 insertions(+), 22 deletions(-)

diff --git a/libraries/AP_HAL_SMACCM/Scheduler.cpp b/libraries/AP_HAL_SMACCM/Scheduler.cpp
index cc1efc3abf..c87d2d0316 100644
--- a/libraries/AP_HAL_SMACCM/Scheduler.cpp
+++ b/libraries/AP_HAL_SMACCM/Scheduler.cpp
@@ -64,17 +64,12 @@ static void scheduler_task(void *arg)
      * time. */
     now = xTaskGetTickCount();
     if (last_wake_time + SCHEDULER_TICKS <= now) {
-      hal.console->printf("now %lu  last %lu\r\n", last_wake_time, now);
       sched->run_failsafe_cb();
       last_wake_time = now;
     } else {
       vTaskDelayUntil(&last_wake_time, SCHEDULER_TICKS);
 
-      if (!xSemaphoreTakeRecursive(g_atomic, 5)) {
-        asm volatile ("bkpt");
-        hal.scheduler->panic("g_atomic took too long");
-      }
-
+      xSemaphoreTakeRecursive(g_atomic, portMAX_DELAY);
       sched->run_callbacks();
       xSemaphoreGiveRecursive(g_atomic);
     }
@@ -135,8 +130,7 @@ static void delay_cb_task(void *arg)
 }
 
 SMACCMScheduler::SMACCMScheduler()
-  : m_delay_cb(NULL), m_suspended(false),
-    m_task(NULL), m_delay_cb_task(NULL),
+  : m_delay_cb(NULL), m_task(NULL), m_delay_cb_task(NULL),
     m_failsafe_cb(NULL), m_num_procs(0)
 {
 }
@@ -218,28 +212,29 @@ void SMACCMScheduler::register_timer_failsafe(AP_HAL::TimedProc k, uint32_t)
 
 void SMACCMScheduler::suspend_timer_procs()
 {
-  m_suspended = true;
+  xSemaphoreTakeRecursive(g_atomic, portMAX_DELAY);
 }
 
 void SMACCMScheduler::resume_timer_procs()
 {
-  m_suspended = false;
+  xSemaphoreGiveRecursive(g_atomic);
 }
 
 void SMACCMScheduler::begin_atomic()
 {
-  xSemaphoreTakeRecursive(g_atomic, portMAX_DELAY);
 }
 
 void SMACCMScheduler::end_atomic()
 {
-  xSemaphoreGiveRecursive(g_atomic);
 }
 
 void SMACCMScheduler::panic(const prog_char_t *errormsg)
 {
   hal.console->println_P(errormsg);
-  m_suspended = true;
+
+  // Try to grab "g_atomic" to suspend timer processes, but with a
+  // timeout in case a timer proc is locked up.
+  xSemaphoreTakeRecursive(g_atomic, 10);
 
   for(;;)
     ;
@@ -256,15 +251,13 @@ void SMACCMScheduler::run_callbacks()
   uint32_t now = micros();
 
   // Run timer processes if not suspended.
-  if (!m_suspended) {
-    portENTER_CRITICAL();
-    uint8_t num_procs = m_num_procs;
-    portEXIT_CRITICAL();
+  portENTER_CRITICAL();
+  uint8_t num_procs = m_num_procs;
+  portEXIT_CRITICAL();
 
-    for (int i = 0; i < num_procs; ++i) {
-      if (m_procs[i] != NULL)
-        m_procs[i](now);
-    }
+  for (int i = 0; i < num_procs; ++i) {
+    if (m_procs[i] != NULL)
+      m_procs[i](now);
   }
 }
 
diff --git a/libraries/AP_HAL_SMACCM/Scheduler.h b/libraries/AP_HAL_SMACCM/Scheduler.h
index 7a4f471a28..c5ee8bb583 100644
--- a/libraries/AP_HAL_SMACCM/Scheduler.h
+++ b/libraries/AP_HAL_SMACCM/Scheduler.h
@@ -110,7 +110,6 @@ public:
 
 private:
   AP_HAL::Proc m_delay_cb;      /* delay callback */
-  bool m_suspended;             /* true if timers suspended */
   void *m_task;                 /* opaque scheduler task handle */
   void *m_delay_cb_task;        /* opaque delay cb task handle */
   AP_HAL::TimedProc m_procs[SMACCM_SCHEDULER_MAX_TIMER_PROCS];