From daa7b9ec1991b6c343c8173b4cd8ffad01139ed7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 3 Apr 2014 14:53:11 +0400 Subject: [PATCH] STM32 clock driver --- .../driver/include/uavcan_stm32/clock.hpp | 78 ++++++ .../driver/include/uavcan_stm32/thread.hpp | 6 +- .../include/uavcan_stm32/uavcan_stm32.hpp | 3 +- .../stm32/driver/src/internal.hpp | 47 ++++ .../stm32/driver/src/uc_stm32_clock.cpp | 240 ++++++++++++++++++ .../stm32/test_stm32f107/Makefile | 2 + .../stm32/test_stm32f107/src/main.cpp | 12 + 7 files changed, 383 insertions(+), 5 deletions(-) create mode 100644 libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp create mode 100644 libuavcan_drivers/stm32/driver/src/internal.hpp create mode 100644 libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp new file mode 100644 index 0000000000..600b5f4dec --- /dev/null +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include + +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); } +}; + +} diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp index 4e80705ce9..cf0c435814 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp +++ b/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 - } diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/uavcan_stm32.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/uavcan_stm32.hpp index 32947f5587..bb60e01470 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/uavcan_stm32.hpp +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/uavcan_stm32.hpp @@ -4,5 +4,6 @@ #pragma once -#include #include +#include +#include diff --git a/libuavcan_drivers/stm32/driver/src/internal.hpp b/libuavcan_drivers/stm32/driver/src/internal.hpp new file mode 100644 index 0000000000..6fc959aa5b --- /dev/null +++ b/libuavcan_drivers/stm32/driver/src/internal.hpp @@ -0,0 +1,47 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#if UAVCAN_STM32_CHIBIOS +# include +#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) diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp new file mode 100644 index 0000000000..33321efdce --- /dev/null +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp @@ -0,0 +1,240 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#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(); +} diff --git a/libuavcan_drivers/stm32/test_stm32f107/Makefile b/libuavcan_drivers/stm32/test_stm32f107/Makefile index ef309be651..47f5c8b07b 100644 --- a/libuavcan_drivers/stm32/test_stm32f107/Makefile +++ b/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) diff --git a/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp b/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp index 372b0c83b2..37cb285c8e 100644 --- a/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp +++ b/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(uavcan_stm32::SystemClock::instance().getMonotonic().toUSec()), + static_cast(uavcan_stm32::SystemClock::instance().getMonotonic().toMSec())); + + printf("UTC clock: %lu\n", + static_cast(uavcan_stm32::SystemClock::instance().getUtc().toUSec())); + + if (uavcan_stm32::SystemClock::instance().getMonotonic().toMSec() / 1000 == 10) + { + uavcan_stm32::SystemClock::instance().adjustUtc(uavcan::UtcDuration::fromMSec(10000)); + } } }