From 4c80149b23e3acf383cd879b310a263ea82d59ad Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 7 Aug 2015 10:44:13 -1000 Subject: [PATCH 1/6] Adds Nuttx Support to uc_stm32_clock --- .../driver/include/uavcan_stm32/thread.hpp | 51 ++++++- .../stm32/driver/src/internal.hpp | 15 +- .../stm32/driver/src/uc_stm32_clock.cpp | 141 ++++++++++++++---- 3 files changed, 167 insertions(+), 40 deletions(-) diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp index 1ad2d30716..e3d357fe5d 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp @@ -24,7 +24,6 @@ namespace uavcan_stm32 { - class CanDriver; #if UAVCAN_STM32_CHIBIOS @@ -92,6 +91,54 @@ 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_); + } +}; + +class MutexLocker +{ + Mutex& mutex_; + +public: + MutexLocker(Mutex& mutex) + : mutex_(mutex) + { + mutex_.lock(); + } + + ~MutexLocker() + { + mutex_.unlock(); + } +}; + #endif @@ -107,6 +154,7 @@ public: { mutex_.lock(); } + ~MutexLocker() { mutex_.unlock(); @@ -114,5 +162,4 @@ public: }; #endif - } diff --git a/libuavcan_drivers/stm32/driver/src/internal.hpp b/libuavcan_drivers/stm32/driver/src/internal.hpp index 3a63ef5de0..4b96ccd317 100644 --- a/libuavcan_drivers/stm32/driver/src/internal.hpp +++ b/libuavcan_drivers/stm32/driver/src/internal.hpp @@ -10,6 +10,8 @@ # include #elif UAVCAN_STM32_NUTTX # include +# include +# include # include #else # error "Unknown OS" @@ -20,7 +22,7 @@ */ #ifndef UAVCAN_STM32_LOG // lowsyslog() crashes the system in this context -//# if UAVCAN_STM32_NUTTX && CONFIG_ARCH_LOWPUTC +// # if UAVCAN_STM32_NUTTX && CONFIG_ARCH_LOWPUTC # if 0 # define UAVCAN_STM32_LOG(fmt, ...) lowsyslog("uavcan_stm32: " fmt "\n", ##__VA_ARGS__) # else @@ -36,13 +38,14 @@ # 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() - +#elif UAVCAN_STM32_NUTTX +# define UAVCAN_STM32_IRQ_HANDLER(id) int id(int irq, FAR void* context) +# define UAVCAN_STM32_IRQ_PROLOGUE() +# define UAVCAN_STM32_IRQ_EPILOGUE() return 0; #else - # define UAVCAN_STM32_IRQ_HANDLER(id) void id(void) # define UAVCAN_STM32_IRQ_PROLOGUE() # define UAVCAN_STM32_IRQ_EPILOGUE() - #endif #if UAVCAN_STM32_CHIBIOS @@ -65,7 +68,6 @@ namespace uavcan_stm32 { - #if UAVCAN_STM32_CHIBIOS struct CriticalSectionLocker @@ -94,9 +96,6 @@ struct CriticalSectionLocker namespace clock { - uavcan::uint64_t getUtcUSecFromCanInterrupt(); - } - } diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp index 046aa30a78..8af1f4a40d 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp @@ -14,19 +14,35 @@ /* * 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_CHIBIOS +# define TIMX UAVCAN_STM32_GLUE2(TIM, UAVCAN_STM32_TIMER_NUMBER) +# define TIMX_IRQn UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQn) +# define TIMX_INPUT_CLOCK STM32_TIMCLK1 +# endif -#if UAVCAN_STM32_TIMER_NUMBER >= 2 && UAVCAN_STM32_TIMER_NUMBER <= 7 -# 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 +# if UAVCAN_STM32_NUTTX +# define TIMX UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _BASE) +# define TMR_REG(o) (TIMX + (o)) +# define TIMX_INPUT_CLOCK STM32_TIM18_FREQUENCY + +# define TIMX_IRQn UAVCAN_STM32_GLUE2(STM32_IRQ_TIM, UAVCAN_STM32_TIMER_NUMBER) +# endif +# define TIMX_IRQHandler UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQHandler) + +# if UAVCAN_STM32_TIMER_NUMBER >= 2 && UAVCAN_STM32_TIMER_NUMBER <= 7 +# 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) +# else +# error "This UAVCAN_STM32_TIMER_NUMBER is not supported yet" +# endif + +# if (TIMX_INPUT_CLOCK % 1000000) != 0 +# error "No way, timer clock must be divisible to 1e6. FIXME!" +# endif + +extern "C" UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler); namespace uavcan_stm32 { @@ -34,7 +50,6 @@ namespace clock { namespace { - const uavcan::uint32_t USecPerOverflow = 65536; Mutex mutex; @@ -54,9 +69,9 @@ uavcan::MonotonicTime prev_utc_adj_at; uavcan::uint64_t time_mono = 0; uavcan::uint64_t time_utc = 0; - } + void init() { CriticalSectionLocker lock; @@ -66,6 +81,8 @@ void init() } initialized = true; + +# if UAVCAN_STM32_CHIBIOS // Power-on and reset TIMX_RCC_ENR |= TIMX_RCC_ENR_MASK; TIMX_RCC_RSTR |= TIMX_RCC_RSTR_MASK; @@ -74,9 +91,6 @@ 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; @@ -86,10 +100,39 @@ void init() TIMX->EGR = TIM_EGR_UG; // Reload immediately TIMX->DIER = TIM_DIER_UIE; TIMX->CR1 = TIM_CR1_CEN; // Start + +# endif + +# if UAVCAN_STM32_NUTTX + + // Attach IRQ + irq_attach(TIMX_IRQn, &TIMX_IRQHandler); + + // Power-on and reset + modifyreg32(STM32_RCC_APB1ENR, 0, TIMX_RCC_ENR_MASK); + modifyreg32(STM32_RCC_APB1RSTR, 0, TIMX_RCC_RSTR_MASK); + modifyreg32(STM32_RCC_APB1RSTR, TIMX_RCC_RSTR_MASK, 0); + + + // Start the timer + putreg32(0xFFFF, TMR_REG(STM32_BTIM_ARR_OFFSET)); + putreg16((TIMX_INPUT_CLOCK / 1000000), TMR_REG(STM32_BTIM_PSC_OFFSET)); + putreg16(BTIM_CR1_URS, TMR_REG(STM32_BTIM_CR1_OFFSET)); + putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET)); + putreg16(BTIM_EGR_UG, TMR_REG(STM32_BTIM_EGR_OFFSET)); // Reload immediately + putreg16(BTIM_DIER_UIE, TMR_REG(STM32_BTIM_DIER_OFFSET)); + putreg16(BTIM_CR1_CEN, TMR_REG(STM32_BTIM_CR1_OFFSET)); // Start + + // Prioritize and Enable IRQ + up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY); + up_enable_irq(TIMX_IRQn); + +# endif } static uavcan::uint64_t sampleUtcFromCriticalSection() { +# if UAVCAN_STM32_CHIBIOS UAVCAN_ASSERT(initialized); UAVCAN_ASSERT(TIMX->DIER & TIM_DIER_UIE); @@ -104,6 +147,25 @@ static uavcan::uint64_t sampleUtcFromCriticalSection() time = uavcan::uint64_t(uavcan::int64_t(time) + add); } return time + cnt; +# endif + +# if UAVCAN_STM32_NUTTX + + UAVCAN_ASSERT(initialized); + UAVCAN_ASSERT(getreg16(TMR_REG(STM32_BTIM_DIER_OFFSET)) & BTIM_DIER_UIE); + + volatile uavcan::uint64_t time = time_utc; + volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET)); + + if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) + { + cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET)); + const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) + + (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000; + time = uavcan::uint64_t(uavcan::int64_t(time) + add); + } + return time + cnt; +# endif } uavcan::uint64_t getUtcUSecFromCanInterrupt() @@ -118,21 +180,34 @@ uavcan::MonotonicTime getMonotonic() CriticalSectionLocker locker; volatile uavcan::uint64_t time = time_mono; +# if UAVCAN_STM32_CHIBIOS + volatile uavcan::uint32_t cnt = TIMX->CNT; if (TIMX->SR & TIM_SR_UIF) { cnt = TIMX->CNT; - time += USecPerOverflow; - } - usec = time + cnt; +# endif +# if UAVCAN_STM32_NUTTX -#ifndef NDEBUG - static uavcan::uint64_t prev_usec = 0; // Self-test - UAVCAN_ASSERT(prev_usec <= usec); - prev_usec = usec; -#endif + volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET)); + + if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) + { + cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET)); +# endif + time += USecPerOverflow; } - return uavcan::MonotonicTime::fromUSec(usec); + usec = time + cnt; + +# ifndef NDEBUG + static uavcan::uint64_t prev_usec = 0; // Self-test + UAVCAN_ASSERT(prev_usec <= usec); + (void)prev_usec; + prev_usec = usec; +# endif +} + +return uavcan::MonotonicTime::fromUSec(usec); } uavcan::UtcTime getUtc() @@ -201,7 +276,8 @@ static void updateRatePID(uavcan::UtcDuration adjustment) utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F)); // lowsyslog("$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n", -// adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm, total_rate_correction_ppm); +// adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm, +// total_rate_correction_ppm); } void adjustUtc(uavcan::UtcDuration adjustment) @@ -275,7 +351,6 @@ void setUtcSyncParams(const UtcSyncParams& params) // Add some sanity check utc_sync_params = params; } - } // namespace clock SystemClock& SystemClock::instance() @@ -288,28 +363,34 @@ SystemClock& SystemClock::instance() long long _aligner_1; long double _aligner_2; } storage; + SystemClock* const ptr = reinterpret_cast(storage.buffer); if (!clock::initialized) { clock::init(); - new (ptr) SystemClock(); + new (ptr)SystemClock(); } return *ptr; } - } // namespace uavcan_stm32 /** * Timer interrupt handler */ + extern "C" UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler) { UAVCAN_STM32_IRQ_PROLOGUE(); +# if UAVCAN_STM32_CHIBIOS TIMX->SR = 0; +# endif +# if UAVCAN_STM32_NUTTX + putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET)); +# endif using namespace uavcan_stm32::clock; UAVCAN_ASSERT(initialized); From 9e51b2b12577bad3ab7b6bb1a9db223deefd0fbe Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Sat, 8 Aug 2015 07:18:24 -1000 Subject: [PATCH 2/6] Adding mechanism to update the drivers notion of UTC from the system RTC Fixed Clock source and prescale calculation --- .../driver/include/uavcan_stm32/clock.hpp | 8 +++++ .../stm32/driver/src/uc_stm32_clock.cpp | 34 +++++++++++++++---- 2 files changed, 35 insertions(+), 7 deletions(-) diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp index 7ab754251a..b11438d787 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp @@ -24,6 +24,14 @@ void init(); */ uavcan::MonotonicTime getMonotonic(); +/** + * Sets the driver's notion of the system UTC. It should be called + * at startup and any time the system clock is updated from an + * external source that is not the UAVCAN Timesync master. + * This function is thread safe. + */ +void setUtc(uavcan::UtcTime time); + /** * Returns UTC time if it has been set, otherwise returns zero time. * This function is thread safe. diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp index 8af1f4a40d..0acdf6cec9 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp @@ -23,7 +23,7 @@ # if UAVCAN_STM32_NUTTX # define TIMX UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _BASE) # define TMR_REG(o) (TIMX + (o)) -# define TIMX_INPUT_CLOCK STM32_TIM18_FREQUENCY +# define TIMX_INPUT_CLOCK STM32_TIM27_FREQUENCY # define TIMX_IRQn UAVCAN_STM32_GLUE2(STM32_IRQ_TIM, UAVCAN_STM32_TIMER_NUMBER) # endif @@ -116,7 +116,7 @@ void init() // Start the timer putreg32(0xFFFF, TMR_REG(STM32_BTIM_ARR_OFFSET)); - putreg16((TIMX_INPUT_CLOCK / 1000000), TMR_REG(STM32_BTIM_PSC_OFFSET)); + putreg16(((TIMX_INPUT_CLOCK / 1000000)-1), TMR_REG(STM32_BTIM_PSC_OFFSET)); putreg16(BTIM_CR1_URS, TMR_REG(STM32_BTIM_CR1_OFFSET)); putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET)); putreg16(BTIM_EGR_UG, TMR_REG(STM32_BTIM_EGR_OFFSET)); // Reload immediately @@ -130,6 +130,23 @@ void init() # endif } +void setUtc(uavcan::UtcTime time) +{ + MutexLocker mlocker(mutex); + UAVCAN_ASSERT(initialized); + + { + CriticalSectionLocker locker; + time_utc = time.toUSec(); + } + + utc_set = true; + utc_locked = false; + utc_jump_cnt++; + utc_prev_adj = 0; + utc_rel_rate_ppm = 0; +} + static uavcan::uint64_t sampleUtcFromCriticalSection() { # if UAVCAN_STM32_CHIBIOS @@ -176,10 +193,12 @@ uavcan::uint64_t getUtcUSecFromCanInterrupt() uavcan::MonotonicTime getMonotonic() { uavcan::uint64_t usec = 0; + // Scope Critical section { CriticalSectionLocker locker; volatile uavcan::uint64_t time = time_mono; + # if UAVCAN_STM32_CHIBIOS volatile uavcan::uint32_t cnt = TIMX->CNT; @@ -187,6 +206,7 @@ uavcan::MonotonicTime getMonotonic() { cnt = TIMX->CNT; # endif + # if UAVCAN_STM32_NUTTX volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET)); @@ -195,9 +215,9 @@ uavcan::MonotonicTime getMonotonic() { cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET)); # endif - time += USecPerOverflow; - } - usec = time + cnt; + time += USecPerOverflow; + } + usec = time + cnt; # ifndef NDEBUG static uavcan::uint64_t prev_usec = 0; // Self-test @@ -205,9 +225,9 @@ uavcan::MonotonicTime getMonotonic() (void)prev_usec; prev_usec = usec; # endif -} + } // End Scope Critical section -return uavcan::MonotonicTime::fromUSec(usec); + return uavcan::MonotonicTime::fromUSec(usec); } uavcan::UtcTime getUtc() From b0fef2ed1f930df3fc510745fb1b70d50e98f10f Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Sat, 8 Aug 2015 09:42:59 -1000 Subject: [PATCH 3/6] Backing out setting priority of driver's clock tick under nuttx as it is hard faulting the system in IRQ --- libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp index 0acdf6cec9..e612c21df1 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp @@ -124,7 +124,9 @@ void init() putreg16(BTIM_CR1_CEN, TMR_REG(STM32_BTIM_CR1_OFFSET)); // Start // Prioritize and Enable IRQ - up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY); +// todo: Currently changing the NVIC_SYSH_HIGH_PRIORITY is HARD faulting +// need to investigate +// up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY); up_enable_irq(TIMX_IRQn); # endif From 9750e3f58dd6972dc2e0f90b93958e1bf438feaa Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 12 Aug 2015 15:21:02 -1000 Subject: [PATCH 4/6] Initail STM32 Baremetal build --- .../driver/include/uavcan_stm32/thread.hpp | 41 ++++++++++++------- .../stm32/driver/src/internal.hpp | 27 ++++++++++++ .../stm32/driver/src/uc_stm32_can.cpp | 35 ++++++++++++++-- .../stm32/driver/src/uc_stm32_clock.cpp | 23 +++++++++-- 4 files changed, 105 insertions(+), 21 deletions(-) diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp index e3d357fe5d..a358edc67d 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp @@ -16,6 +16,7 @@ # include # include # include +#elif UAVCAN_STM32_BAREMETAL #else # error "Unknown OS" #endif @@ -24,6 +25,7 @@ namespace uavcan_stm32 { + class CanDriver; #if UAVCAN_STM32_CHIBIOS @@ -91,35 +93,47 @@ public: void signalFromInterrupt(); }; -class Mutex +#elif UAVCAN_STM32_BAREMETAL + +class BusEvent { - pthread_mutex_t mutex_; + volatile bool ready; public: - Mutex() + BusEvent(CanDriver& can_driver) + : ready(false) { - init(); + (void)can_driver; } - int init() + bool wait(uavcan::MonotonicDuration duration) { - return pthread_mutex_init(&mutex_, NULL); + bool lready = ready; + return __atomic_exchange_n (&lready, false, __ATOMIC_SEQ_CST); } - int deinit() + void signal() { - return pthread_mutex_destroy(&mutex_); + __atomic_store_n (&ready, true, __ATOMIC_SEQ_CST); } + void signalFromInterrupt() + { + __atomic_store_n (&ready, true, __ATOMIC_SEQ_CST); + } +}; + +class Mutex +{ +public: void lock() { - (void)pthread_mutex_lock(&mutex_); - } + }; void unlock() { - (void)pthread_mutex_unlock(&mutex_); - } + + }; }; class MutexLocker @@ -132,7 +146,6 @@ public: { mutex_.lock(); } - ~MutexLocker() { mutex_.unlock(); @@ -154,7 +167,6 @@ public: { mutex_.lock(); } - ~MutexLocker() { mutex_.unlock(); @@ -162,4 +174,5 @@ public: }; #endif + } diff --git a/libuavcan_drivers/stm32/driver/src/internal.hpp b/libuavcan_drivers/stm32/driver/src/internal.hpp index 4b96ccd317..d258c11c50 100644 --- a/libuavcan_drivers/stm32/driver/src/internal.hpp +++ b/libuavcan_drivers/stm32/driver/src/internal.hpp @@ -13,6 +13,8 @@ # include # include # include +#elif UAVCAN_STM32_BAREMETAL +# include #else # error "Unknown OS" #endif @@ -57,6 +59,15 @@ # endif #endif +#if UAVCAN_STM32_BAREMETAL +/** + * Priority mask for timer and CAN interrupts. + */ +# ifndef UAVCAN_STM32_IRQ_PRIORITY_MASK +# define UAVCAN_STM32_IRQ_PRIORITY_MASK 0 +# endif +#endif + /** * Glue macros */ @@ -92,6 +103,22 @@ struct CriticalSectionLocker } }; +#elif UAVCAN_STM32_BAREMETAL + +struct CriticalSectionLocker +{ + + CriticalSectionLocker() + { + __disable_irq(); + } + + ~CriticalSectionLocker() + { + __enable_irq(); + } +}; + #endif namespace clock diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp index 7e794dc28d..b7d4defbf8 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp @@ -14,11 +14,13 @@ # include # include # include +#elif UAVCAN_STM32_BAREMETAL +#include #else # error "Unknown OS" #endif -#if !UAVCAN_STM32_NUTTX +#if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL # if !(defined(STM32F10X_CL) || defined(STM32F2XX) || defined(STM32F4XX)) // IRQ numbers # define CAN1_RX0_IRQn USB_LP_CAN1_RX0_IRQn @@ -188,7 +190,9 @@ int CanIface::computeTimings(const uavcan::uint32_t target_bitrate, Timings& out /* * Hardware configuration */ -#if UAVCAN_STM32_CHIBIOS +#if UAVCAN_STM32_BAREMETAL + const uavcan::uint32_t pclk = STM32_PCLK1_FREQUENCY; +#elif UAVCAN_STM32_CHIBIOS const uavcan::uint32_t pclk = STM32_PCLK1; #elif UAVCAN_STM32_NUTTX const uavcan::uint32_t pclk = STM32_PCLK1_FREQUENCY; @@ -838,6 +842,22 @@ uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks, return 1; // Return value doesn't matter as long as it is non-negative } + +#if UAVCAN_STM32_BAREMETAL + +static void nvicEnableVector(int irq, uint8_t prio) +{ + NVIC_InitTypeDef NVIC_InitStructure; + NVIC_InitStructure.NVIC_IRQChannel = irq; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = prio; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + +} + +#endif + int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode) { int res = 0; @@ -921,7 +941,7 @@ int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMod IRQ_ATTACH(STM32_IRQ_CAN2SCE, can2_irq); # endif # undef IRQ_ATTACH -#else +#elif UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL { CriticalSectionLocker lock; nvicEnableVector(CAN1_TX_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); @@ -1044,7 +1064,7 @@ static int can2_irq(const int irq, void*) # endif #else // UAVCAN_STM32_NUTTX - +UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler); UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler) { UAVCAN_STM32_IRQ_PROLOGUE(); @@ -1052,6 +1072,7 @@ UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler) UAVCAN_STM32_IRQ_EPILOGUE(); } +UAVCAN_STM32_IRQ_HANDLER(CAN1_RX0_IRQHandler); UAVCAN_STM32_IRQ_HANDLER(CAN1_RX0_IRQHandler) { UAVCAN_STM32_IRQ_PROLOGUE(); @@ -1059,6 +1080,7 @@ UAVCAN_STM32_IRQ_HANDLER(CAN1_RX0_IRQHandler) UAVCAN_STM32_IRQ_EPILOGUE(); } +UAVCAN_STM32_IRQ_HANDLER(CAN1_RX1_IRQHandler); UAVCAN_STM32_IRQ_HANDLER(CAN1_RX1_IRQHandler) { UAVCAN_STM32_IRQ_PROLOGUE(); @@ -1066,6 +1088,7 @@ UAVCAN_STM32_IRQ_HANDLER(CAN1_RX1_IRQHandler) UAVCAN_STM32_IRQ_EPILOGUE(); } +UAVCAN_STM32_IRQ_HANDLER(CAN1_SCE_IRQHandler); UAVCAN_STM32_IRQ_HANDLER(CAN1_SCE_IRQHandler) { UAVCAN_STM32_IRQ_PROLOGUE(); @@ -1075,6 +1098,7 @@ UAVCAN_STM32_IRQ_HANDLER(CAN1_SCE_IRQHandler) # if UAVCAN_STM32_NUM_IFACES > 1 +UAVCAN_STM32_IRQ_HANDLER(CAN2_TX_IRQHandler); UAVCAN_STM32_IRQ_HANDLER(CAN2_TX_IRQHandler) { UAVCAN_STM32_IRQ_PROLOGUE(); @@ -1082,6 +1106,7 @@ UAVCAN_STM32_IRQ_HANDLER(CAN2_TX_IRQHandler) UAVCAN_STM32_IRQ_EPILOGUE(); } +UAVCAN_STM32_IRQ_HANDLER(CAN2_RX0_IRQHandler); UAVCAN_STM32_IRQ_HANDLER(CAN2_RX0_IRQHandler) { UAVCAN_STM32_IRQ_PROLOGUE(); @@ -1089,6 +1114,7 @@ UAVCAN_STM32_IRQ_HANDLER(CAN2_RX0_IRQHandler) UAVCAN_STM32_IRQ_EPILOGUE(); } +UAVCAN_STM32_IRQ_HANDLER(CAN2_RX1_IRQHandler); UAVCAN_STM32_IRQ_HANDLER(CAN2_RX1_IRQHandler) { UAVCAN_STM32_IRQ_PROLOGUE(); @@ -1096,6 +1122,7 @@ UAVCAN_STM32_IRQ_HANDLER(CAN2_RX1_IRQHandler) UAVCAN_STM32_IRQ_EPILOGUE(); } +UAVCAN_STM32_IRQ_HANDLER(CAN2_SCE_IRQHandler); UAVCAN_STM32_IRQ_HANDLER(CAN2_SCE_IRQHandler) { UAVCAN_STM32_IRQ_PROLOGUE(); diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp index e612c21df1..1359fd970e 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp @@ -14,7 +14,7 @@ /* * Timer instance */ -# if UAVCAN_STM32_CHIBIOS +# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL # define TIMX UAVCAN_STM32_GLUE2(TIM, UAVCAN_STM32_TIMER_NUMBER) # define TIMX_IRQn UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQn) # define TIMX_INPUT_CLOCK STM32_TIMCLK1 @@ -70,6 +70,23 @@ 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 + +static void nvicEnableVector(int irq, uint8_t prio) +{ + NVIC_InitTypeDef NVIC_InitStructure; + NVIC_InitStructure.NVIC_IRQChannel = irq; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = prio; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + +} + +#endif + void init() @@ -82,7 +99,7 @@ void init() initialized = true; -# if UAVCAN_STM32_CHIBIOS +# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL // Power-on and reset TIMX_RCC_ENR |= TIMX_RCC_ENR_MASK; TIMX_RCC_RSTR |= TIMX_RCC_RSTR_MASK; @@ -201,7 +218,7 @@ uavcan::MonotonicTime getMonotonic() volatile uavcan::uint64_t time = time_mono; -# if UAVCAN_STM32_CHIBIOS +# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL volatile uavcan::uint32_t cnt = TIMX->CNT; if (TIMX->SR & TIM_SR_UIF) From 06ac74bd40e6e82448b1f1b897a3eebd9c4ac76a Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 14 Aug 2015 07:24:41 -1000 Subject: [PATCH 5/6] 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 From 7fc43d0cbf638c06cfd385a0dab075649159748c Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 18 Aug 2015 15:35:30 -1000 Subject: [PATCH 6/6] Fixed Typo --- libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp index b54bf1be86..a94a543beb 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp @@ -170,7 +170,7 @@ void setUtc(uavcan::UtcTime time) static uavcan::uint64_t sampleUtcFromCriticalSection() { -# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BARMETAL +# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL UAVCAN_ASSERT(initialized); UAVCAN_ASSERT(TIMX->DIER & TIM_DIER_UIE);