From 9e197f129ba50f91d81e4ce86f279369d659e965 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Sat, 5 Apr 2014 11:19:20 +0400
Subject: [PATCH] STM32: Added mutex; clock made thread safe

---
 .../driver/include/uavcan_stm32/thread.hpp    | 29 +++++++++++++++++++
 .../stm32/driver/src/uc_stm32_clock.cpp       | 13 ++++++++-
 .../stm32/driver/src/uc_stm32_thread.cpp      | 17 ++++++++++-
 3 files changed, 57 insertions(+), 2 deletions(-)

diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp
index cf0c435814..716c46f674 100644
--- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp
+++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp
@@ -31,4 +31,33 @@ public:
     void signalFromInterrupt();
 };
 
+
+class Mutex
+{
+#if UAVCAN_STM32_CHIBIOS
+    chibios_rt::Mutex mtx_;
+#endif
+
+public:
+    void lock();
+    void unlock();
+};
+
+
+class MutexLocker
+{
+    Mutex& mutex_;
+
+public:
+    MutexLocker(Mutex& mutex)
+        : mutex_(mutex)
+    {
+        mutex_.lock();
+    }
+    ~MutexLocker()
+    {
+        mutex_.unlock();
+    }
+};
+
 }
diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
index da159ff4fc..0222a9af46 100644
--- a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
+++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
@@ -4,6 +4,7 @@
 
 #include <cassert>
 #include <uavcan_stm32/clock.hpp>
+#include <uavcan_stm32/thread.hpp>
 #include "internal.hpp"
 
 /*
@@ -30,6 +31,8 @@ namespace clock
 namespace
 {
 
+Mutex mutex;
+
 bool initialized = false;
 
 bool utc_set = false;
@@ -141,6 +144,8 @@ uavcan::UtcTime getUtc()
 
 void adjustUtc(uavcan::UtcDuration adjustment)
 {
+    MutexLocker mlocker(mutex);
+
     assert(initialized);
     if (adjustment.isZero() && utc_set)
     {
@@ -200,15 +205,21 @@ void adjustUtc(uavcan::UtcDuration adjustment)
 
 uavcan::int32_t getUtcSpeedCorrectionPPM()
 {
+    MutexLocker mlocker(mutex);
     return uavcan::int64_t(utc_correction_usec_per_overflow * 1000000) / USecPerOverflow;
 }
 
-uavcan::uint32_t getUtcAjdustmentJumpCount() { return utc_jump_cnt; }
+uavcan::uint32_t getUtcAjdustmentJumpCount()
+{
+    MutexLocker mlocker(mutex);
+    return utc_jump_cnt;
+}
 
 } // namespace clock
 
 SystemClock& SystemClock::instance()
 {
+    MutexLocker mlocker(clock::mutex);
     if (!clock::initialized)
     {
         clock::init();
diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp
index 69be6247c5..380f2e3513 100644
--- a/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp
+++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp
@@ -8,7 +8,9 @@ namespace uavcan_stm32
 {
 
 #if UAVCAN_STM32_CHIBIOS
-
+/*
+ * Event
+ */
 bool Event::wait(uavcan::MonotonicDuration duration)
 {
     msg_t ret = msg_t();
@@ -35,6 +37,19 @@ void Event::signalFromInterrupt()
     chSysUnlockFromIsr();
 }
 
+/*
+ * Mutex
+ */
+void Mutex::lock()
+{
+    mtx_.lock();
+}
+
+void Mutex::unlock()
+{
+    chibios_rt::BaseThread::unlockMutex();
+}
+
 #endif
 
 }