From 06ac74bd40e6e82448b1f1b897a3eebd9c4ac76a Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Fri, 14 Aug 2015 07:24:41 -1000
Subject: [PATCH] Rebased on stm32_nuttx_clock

---
 .../driver/include/uavcan_stm32/thread.hpp    | 50 +++++++++++--------
 .../stm32/driver/src/internal.hpp             |  1 -
 .../stm32/driver/src/uc_stm32_clock.cpp       | 14 ++++--
 3 files changed, 39 insertions(+), 26 deletions(-)

diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp
index a358edc67d..3ad7dfe53f 100644
--- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp
+++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp
@@ -93,6 +93,36 @@ public:
     void signalFromInterrupt();
 };
 
+class Mutex
+{
+    pthread_mutex_t mutex_;
+
+public:
+    Mutex()
+    {
+        init();
+    }
+
+    int init()
+    {
+        return pthread_mutex_init(&mutex_, NULL);
+    }
+
+    int deinit()
+    {
+        return pthread_mutex_destroy(&mutex_);
+    }
+
+    void lock()
+    {
+        (void)pthread_mutex_lock(&mutex_);
+    }
+
+    void unlock()
+    {
+        (void)pthread_mutex_unlock(&mutex_);
+    }
+};
 #elif UAVCAN_STM32_BAREMETAL
 
 class BusEvent
@@ -136,27 +166,9 @@ public:
     };
 };
 
-class MutexLocker
-{
-    Mutex& mutex_;
-
-public:
-    MutexLocker(Mutex& mutex)
-        : mutex_(mutex)
-    {
-        mutex_.lock();
-    }
-    ~MutexLocker()
-    {
-        mutex_.unlock();
-    }
-};
-
 #endif
 
 
-#if UAVCAN_STM32_CHIBIOS
-
 class MutexLocker
 {
     Mutex& mutex_;
@@ -173,6 +185,4 @@ public:
     }
 };
 
-#endif
-
 }
diff --git a/libuavcan_drivers/stm32/driver/src/internal.hpp b/libuavcan_drivers/stm32/driver/src/internal.hpp
index d258c11c50..36789c2991 100644
--- a/libuavcan_drivers/stm32/driver/src/internal.hpp
+++ b/libuavcan_drivers/stm32/driver/src/internal.hpp
@@ -36,7 +36,6 @@
  * IRQ handler macros
  */
 #if UAVCAN_STM32_CHIBIOS
-
 # define UAVCAN_STM32_IRQ_HANDLER(id)  CH_IRQ_HANDLER(id)
 # define UAVCAN_STM32_IRQ_PROLOGUE()    CH_IRQ_PROLOGUE()
 # define UAVCAN_STM32_IRQ_EPILOGUE()    CH_IRQ_EPILOGUE()
diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
index 1359fd970e..b54bf1be86 100644
--- a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
+++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
@@ -50,6 +50,7 @@ namespace clock
 {
 namespace
 {
+
 const uavcan::uint32_t USecPerOverflow = 65536;
 
 Mutex mutex;
@@ -69,8 +70,8 @@ uavcan::MonotonicTime prev_utc_adj_at;
 
 uavcan::uint64_t time_mono = 0;
 uavcan::uint64_t time_utc = 0;
+
 }
-# if UAVCAN_STM32_NUTTX || UAVCAN_STM32_BAREMETAL
 
 #if UAVCAN_STM32_BAREMETAL
 
@@ -87,8 +88,6 @@ static void nvicEnableVector(int irq,  uint8_t prio)
 
 #endif
 
-
-
 void init()
 {
     CriticalSectionLocker lock;
@@ -108,6 +107,9 @@ void init()
     // Enable IRQ
     nvicEnableVector(TIMX_IRQn,  UAVCAN_STM32_IRQ_PRIORITY_MASK);
 
+# if (TIMX_INPUT_CLOCK % 1000000) != 0
+#  error "No way, timer clock must be divisible to 1e6. FIXME!"
+# endif
 
     // Start the timer
     TIMX->ARR  = 0xFFFF;
@@ -168,7 +170,7 @@ void setUtc(uavcan::UtcTime time)
 
 static uavcan::uint64_t sampleUtcFromCriticalSection()
 {
-# if UAVCAN_STM32_CHIBIOS
+# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BARMETAL
     UAVCAN_ASSERT(initialized);
     UAVCAN_ASSERT(TIMX->DIER & TIM_DIER_UIE);
 
@@ -390,6 +392,7 @@ void setUtcSyncParams(const UtcSyncParams& params)
     // Add some sanity check
     utc_sync_params = params;
 }
+
 } // namespace clock
 
 SystemClock& SystemClock::instance()
@@ -412,6 +415,7 @@ SystemClock& SystemClock::instance()
     }
     return *ptr;
 }
+
 } // namespace uavcan_stm32
 
 
@@ -424,7 +428,7 @@ UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler)
 {
     UAVCAN_STM32_IRQ_PROLOGUE();
 
-# if UAVCAN_STM32_CHIBIOS
+# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL
     TIMX->SR = 0;
 # endif
 # if UAVCAN_STM32_NUTTX