STM32: Added mutex; clock made thread safe

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();
+    }
+};
+
 }

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();

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
 
 }