STM32 clock driver

libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp

@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/driver/system_clock.hpp>
+
+namespace uavcan_stm32
+{
+
+namespace clock
+{
+/**
+ * Starts the clock.
+ * Can be called multiple times, only the first call will be effective.
+ */
+void init();
+
+/**
+ * For CAN timestamping.
+ */
+uavcan::uint64_t getUtcUSecFromInterrupt();
+
+/**
+ * For general usage.
+ */
+uavcan::MonotonicTime getMonotonic();
+uavcan::UtcTime getUtc();
+
+/**
+ * Performs UTC time adjustment.
+ * The UTC time will be zero until first adjustment has been performed.
+ */
+void adjustUtc(uavcan::UtcDuration adjustment);
+
+/**
+ * Clock speed error.
+ * Positive if the hardware timer is slower than reference time
+ */
+uavcan::int32_t getUtcSpeedCorrectionPPM();
+
+/**
+ * Number of non-gradual adjustments performed so far.
+ * Ideally should be zero.
+ */
+uavcan::uint32_t getUtcAjdustmentJumpCount();
+
+}
+
+/**
+ * Clock interface for CAN driver.
+ */
+class ICanTimestampingClock : public uavcan::ISystemClock
+{
+public:
+    virtual uavcan::uint64_t getUtcUSecFromInterrupt() const = 0;
+};
+
+/**
+ * Trivial system clock implementation; can be redefined by the application.
+ * Uses a simple 16-bit hardware timer for both UTC and monotonic clocks.
+ */
+class SystemClock : public ICanTimestampingClock, uavcan::Noncopyable
+{
+    SystemClock() { }
+
+public:
+    static SystemClock& instance();
+
+    virtual uavcan::uint64_t getUtcUSecFromInterrupt() const { return clock::getUtcUSecFromInterrupt(); }
+
+    virtual uavcan::MonotonicTime getMonotonic()     const { return clock::getMonotonic(); }
+    virtual uavcan::UtcTime getUtc()                 const { return clock::getUtc(); }
+    virtual void adjustUtc(uavcan::UtcDuration adjustment) { clock::adjustUtc(adjustment); }
+};
+
+}

libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp

@@ -15,11 +15,11 @@
 namespace uavcan_stm32
 {
 
-#if UAVCAN_STM32_CHIBIOS
-
 class Event
 {
+#if UAVCAN_STM32_CHIBIOS
     chibios_rt::CounterSemaphore sem_;
+#endif
 
 public:
     Event() : sem_(0) { }
@@ -31,6 +31,4 @@ public:
     void signalFromInterrupt();
 };
 
-#endif
-
 }

libuavcan_drivers/stm32/driver/include/uavcan_stm32/uavcan_stm32.hpp

@@ -4,5 +4,6 @@
 
 #pragma once
 
-#include <uavcan_stm32/can.hpp>
 #include <uavcan_stm32/thread.hpp>
+#include <uavcan_stm32/clock.hpp>
+#include <uavcan_stm32/can.hpp>

libuavcan_drivers/stm32/driver/src/internal.hpp

@@ -0,0 +1,47 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#if UAVCAN_STM32_CHIBIOS
+# include <hal.h>
+#else
+# error "Unknown OS"
+#endif
+
+/**
+ * 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()
+#else
+# define UAVCAN_STM32_IRQ_HANDLER(id)  void id(void)
+# define UAVCAN_STM32_IRQ_PROLOGUE()
+# define UAVCAN_STM32_IRQ_EPILOGUE()
+#endif
+
+/**
+ * Priority mask for timer and CAN interrupts.
+ * Medium priority by default.
+ */
+#ifndef UAVCAN_STM32_IRQ_PRIORITY_MASK
+# define UAVCAN_STM32_IRQ_PRIORITY_MASK  CORTEX_PRIORITY_MASK((CORTEX_MAXIMUM_PRIORITY + CORTEX_MINIMUM_PRIORITY) / 2)
+#endif
+
+/**
+ * Any General-Purpose timer (TIM2, TIM3, TIM4, TIM5)
+ * e.g. -DUAVCAN_STM32_TIMER_NUMBER=2
+ */
+#ifndef UAVCAN_STM32_TIMER_NUMBER
+# error UAVCAN_STM32_TIMER_NUMBER
+#endif
+
+
+#define UAVCAN_STM32_GLUE2_(A, B)       A##B
+#define UAVCAN_STM32_GLUE2(A, B)        UAVCAN_STM32_GLUE2_(A, B)
+
+#define UAVCAN_STM32_GLUE3_(A, B, C)    A##B##C
+#define UAVCAN_STM32_GLUE3(A, B, C)     UAVCAN_STM32_GLUE3_(A, B, C)

libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp

@@ -0,0 +1,240 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <cassert>
+#include <uavcan_stm32/clock.hpp>
+#include "internal.hpp"
+
+/*
+ * Timer instance
+ */
+#define TIMX                    UAVCAN_STM32_GLUE2(TIM, UAVCAN_STM32_TIMER_NUMBER)
+#define TIMX_IRQn               UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQn)
+#define TIMX_IRQHandler         UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQHandler)
+
+#if UAVCAN_STM32_TIMER_NUMBER >= 2 && UAVCAN_STM32_TIMER_NUMBER <= 5
+# define TIMX_RCC_ENR           RCC->APB1ENR
+# define TIMX_RCC_RSTR          RCC->APB1RSTR
+# define TIMX_RCC_ENR_MASK      UAVCAN_STM32_GLUE3(RCC_APB1ENR_TIM,  UAVCAN_STM32_TIMER_NUMBER, EN)
+# define TIMX_RCC_RSTR_MASK     UAVCAN_STM32_GLUE3(RCC_APB1RSTR_TIM, UAVCAN_STM32_TIMER_NUMBER, RST)
+# define TIMX_INPUT_CLOCK       STM32_TIMCLK1
+#else
+# error "This UAVCAN_STM32_TIMER_NUMBER is not supported yet"
+#endif
+
+#define TIMX_IRQ_ENABLE()       TIMX->DIER = TIM_DIER_UIE
+#define TIMX_IRQ_DISABLE()      TIMX->DIER = 0
+
+namespace uavcan_stm32
+{
+namespace clock
+{
+namespace
+{
+
+bool initialized = false;
+
+bool utc_set = false;
+
+uavcan::uint32_t utc_jump_cnt = 0;
+uavcan::int32_t utc_correction_usec_per_overflow = 0;
+
+uavcan::uint64_t time_mono = 0;
+uavcan::uint64_t time_utc = 0;
+
+const uavcan::uint32_t USecPerOverflow = 65536;
+const uavcan::int32_t MaxUtcSpeedCorrection = 500; // x / 65536
+
+}
+
+void init()
+{
+    if (initialized)
+    {
+        return;
+    }
+    initialized = true;
+
+    chSysDisable();
+
+    // Power-on and reset
+    TIMX_RCC_ENR |= TIMX_RCC_ENR_MASK;
+    TIMX_RCC_RSTR |=  TIMX_RCC_RSTR_MASK;
+    TIMX_RCC_RSTR &= ~TIMX_RCC_RSTR_MASK;
+
+    chSysEnable();
+
+    // 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;
+    TIMX->PSC  = (TIMX_INPUT_CLOCK / 1000000) - 1;  // 1 tick == 1 microsecond
+    TIMX->CR1  = TIM_CR1_URS;
+    TIMX->SR   = 0;
+    TIMX->EGR  = TIM_EGR_UG;     // Reload immediately
+    TIMX->DIER = TIM_DIER_UIE;
+    TIMX->CR1  = TIM_CR1_CEN;    // Start
+}
+
+/**
+ * Callable from any context
+ */
+static uavcan::uint64_t sampleSpecifiedTime(const volatile uavcan::uint64_t* const value)
+{
+    assert(initialized);
+    assert(TIMX->DIER & TIM_DIER_UIE);
+
+    TIMX_IRQ_DISABLE();
+
+    volatile uavcan::uint64_t time = *value;
+    volatile uavcan::uint32_t cnt = TIMX->CNT;
+
+    if (TIMX->SR & TIM_SR_UIF)
+    {
+        /*
+         * The timer has overflowed either before or after CNT sample was obtained.
+         * We need to sample it once more to be sure that the obtained
+         * counter value has wrapped over zero.
+         */
+        cnt = TIMX->CNT;
+        /*
+         * The timer interrupt was set, but not handled yet.
+         * Thus we need to adjust the tick counter manually.
+         */
+        time += USecPerOverflow;
+    }
+
+    TIMX_IRQ_ENABLE();
+
+    return time + cnt;
+}
+
+uavcan::uint64_t getUtcUSecFromInterrupt()
+{
+    return utc_set ? sampleSpecifiedTime(&time_utc) : 0;
+}
+
+uavcan::MonotonicTime getMonotonic()
+{
+    const uavcan::uint64_t usec = sampleSpecifiedTime(&time_mono);
+#if !NDEBUG
+    static uavcan::uint64_t prev_usec = 0;  // Self-test
+    assert(prev_usec <= usec);
+    prev_usec = usec;
+#endif
+    return uavcan::MonotonicTime::fromUSec(usec);
+}
+
+uavcan::UtcTime getUtc()
+{
+    return utc_set ? uavcan::UtcTime::fromUSec(sampleSpecifiedTime(&time_utc)) : uavcan::UtcTime();
+}
+
+void adjustUtc(uavcan::UtcDuration adjustment)
+{
+    assert(initialized);
+    if (adjustment.isZero() && utc_set)
+    {
+        return; // Perfect sync
+    }
+
+    /*
+     * Naive speed adjustment
+     * TODO needs better solution
+     */
+    if (adjustment.isPositive())
+    {
+        if (utc_correction_usec_per_overflow < MaxUtcSpeedCorrection)
+        {
+            utc_correction_usec_per_overflow++;
+        }
+    }
+    else
+    {
+        if (utc_correction_usec_per_overflow > -MaxUtcSpeedCorrection)
+        {
+            utc_correction_usec_per_overflow--;
+        }
+    }
+
+    /*
+     * Clock value adjustment
+     * For small adjustments we will rely only on speed change
+     */
+    if (adjustment.getAbs().toMSec() > 1 || !utc_set)
+    {
+        if (adjustment.isNegative() &&
+            uavcan::uint64_t(adjustment.getAbs().toUSec()) > time_utc)
+        {
+            TIMX_IRQ_DISABLE();
+            time_utc = 1;
+            TIMX_IRQ_ENABLE();
+        }
+        else
+        {
+            TIMX_IRQ_DISABLE();
+            time_utc += adjustment.toUSec();
+            TIMX_IRQ_ENABLE();
+        }
+
+        if (utc_set)
+        {
+            utc_jump_cnt++;
+        }
+        else
+        {
+            utc_set = true;
+            utc_correction_usec_per_overflow = 0;
+        }
+    }
+}
+
+uavcan::int32_t getUtcSpeedCorrectionPPM()
+{
+    return uavcan::int64_t(utc_correction_usec_per_overflow * 1000000) / USecPerOverflow;
+}
+
+uavcan::uint32_t getUtcAjdustmentJumpCount() { return utc_jump_cnt; }
+
+} // namespace clock
+
+SystemClock& SystemClock::instance()
+{
+    if (!clock::initialized)
+    {
+        clock::init();
+    }
+    static SystemClock inst;
+    return inst;
+}
+
+} // namespace uavcan_stm32
+
+
+/**
+ * Timer interrupt handler
+ */
+extern "C"
+UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler)
+{
+    UAVCAN_STM32_IRQ_PROLOGUE();
+
+    TIMX->SR = ~TIM_SR_UIF;
+
+    using namespace uavcan_stm32::clock;
+    assert(initialized);
+
+    time_mono += USecPerOverflow;
+    if (utc_set)
+    {
+        time_utc += USecPerOverflow + utc_correction_usec_per_overflow;
+    }
+
+    UAVCAN_STM32_IRQ_EPILOGUE();
+}

libuavcan_drivers/stm32/test_stm32f107/Makefile

@@ -16,6 +16,8 @@ UDEFS = -DUAVCAN_STM32_CHIBIOS=1
 # UAVCAN library
 #
 
+UDEFS += -DUAVCAN_STM32_TIMER_NUMBER=2
+
 include ../../../libuavcan/include.mk
 CPPSRC += $(LIBUAVCAN_SRC)
 UINCDIR += $(LIBUAVCAN_INC)

libuavcan_drivers/stm32/test_stm32f107/src/main.cpp

@@ -75,5 +75,17 @@ int main()
         app::ledSet(true);
         sleep(1);
         app::led_turn_off_event.signal();
+
+        printf("Mono clock: %lu  %lu\n",
+               static_cast<unsigned long>(uavcan_stm32::SystemClock::instance().getMonotonic().toUSec()),
+               static_cast<unsigned long>(uavcan_stm32::SystemClock::instance().getMonotonic().toMSec()));
+
+        printf("UTC  clock: %lu\n",
+               static_cast<unsigned long>(uavcan_stm32::SystemClock::instance().getUtc().toUSec()));
+
+        if (uavcan_stm32::SystemClock::instance().getMonotonic().toMSec() / 1000 == 10)
+        {
+            uavcan_stm32::SystemClock::instance().adjustUtc(uavcan::UtcDuration::fromMSec(10000));
+        }
     }
 }