clock.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <uavcan_lpc11c24/clock.hpp>
00006 #include <uavcan/util/templates.hpp>
00007 #include <chip.h>
00008 #include "internal.hpp"
00009 
00010 namespace uavcan_lpc11c24
00011 {
00012 namespace clock
00013 {
00014 namespace
00015 {
00016 
00017 bool initialized = false;
00018 bool utc_set = false;
00019 
00020 std::int32_t utc_correction_usec_per_overflow_x16 = 0;
00021 std::int64_t prev_adjustment = 0;
00022 
00023 std::uint64_t time_mono = 0;
00024 std::uint64_t time_utc = 0;
00025 
00026 /**
00027  * If this value is too large for the given core clock, reload value will be out of the 24-bit integer range.
00028  * This will be detected at run time during timer initialization - refer to SysTick_Config().
00029  */
00030 constexpr std::uint32_t USecPerOverflow = 65536 * 2;
00031 constexpr std::int32_t MaxUtcSpeedCorrectionX16 = 100 * 16;
00032 
00033 }
00034 
00035 #if __GNUC__
00036 __attribute__((noreturn))
00037 #endif
00038 static void fail()
00039 {
00040     while (true) { }
00041 }
00042 
00043 void init()
00044 {
00045     CriticalSectionLocker lock;
00046     if (!initialized)
00047     {
00048         initialized = true;
00049 
00050         if ((SystemCoreClock % 1000000) != 0)  // Core clock frequency validation
00051         {
00052             fail();
00053         }
00054 
00055         if (SysTick_Config((SystemCoreClock / 1000000) * USecPerOverflow) != 0)
00056         {
00057             fail();
00058         }
00059     }
00060 }
00061 
00062 static std::uint64_t sampleFromCriticalSection(const volatile std::uint64_t* const value)
00063 {
00064     const std::uint32_t reload = SysTick->LOAD + 1;  // SysTick counts downwards, hence the value subtracted from reload
00065 
00066     volatile std::uint64_t time = *value;
00067     volatile std::uint32_t cycles = reload - SysTick->VAL;
00068 
00069     if ((SCB->ICSR & SCB_ICSR_PENDSTSET_Msk) == SCB_ICSR_PENDSTSET_Msk)
00070     {
00071         cycles = reload - SysTick->VAL;
00072         time += USecPerOverflow;
00073     }
00074     const std::uint32_t cycles_per_usec = SystemCoreClock / 1000000;
00075     return time + (cycles / cycles_per_usec);
00076 }
00077 
00078 std::uint64_t getUtcUSecFromCanInterrupt()
00079 {
00080     return utc_set ? sampleFromCriticalSection(&time_utc) : 0;
00081 }
00082 
00083 uavcan::MonotonicTime getMonotonic()
00084 {
00085     if (!initialized)
00086     {
00087         fail();
00088     }
00089     std::uint64_t usec = 0;
00090     {
00091         CriticalSectionLocker locker;
00092         usec = sampleFromCriticalSection(&time_mono);
00093     }
00094     return uavcan::MonotonicTime::fromUSec(usec);
00095 }
00096 
00097 uavcan::UtcTime getUtc()
00098 {
00099     if (!initialized)
00100     {
00101         fail();
00102     }
00103     std::uint64_t usec = 0;
00104     if (utc_set)
00105     {
00106         CriticalSectionLocker locker;
00107         usec = sampleFromCriticalSection(&time_utc);
00108     }
00109     return uavcan::UtcTime::fromUSec(usec);
00110 }
00111 
00112 uavcan::UtcDuration getPrevUtcAdjustment()
00113 {
00114     return uavcan::UtcDuration::fromUSec(prev_adjustment);
00115 }
00116 
00117 void adjustUtc(uavcan::UtcDuration adjustment)
00118 {
00119     const std::int64_t adj_delta = adjustment.toUSec() - prev_adjustment;  // This is the P term
00120     prev_adjustment = adjustment.toUSec();
00121 
00122     utc_correction_usec_per_overflow_x16 += adjustment.isPositive() ? 1 : -1; // I
00123     utc_correction_usec_per_overflow_x16 += (adj_delta > 0) ? 1 : -1;         // P
00124 
00125     utc_correction_usec_per_overflow_x16 =
00126         uavcan::max(utc_correction_usec_per_overflow_x16, -MaxUtcSpeedCorrectionX16);
00127     utc_correction_usec_per_overflow_x16 =
00128         uavcan::min(utc_correction_usec_per_overflow_x16,  MaxUtcSpeedCorrectionX16);
00129 
00130     if (adjustment.getAbs().toMSec() > 9 || !utc_set)
00131     {
00132         const std::int64_t adj_usec = adjustment.toUSec();
00133         {
00134             CriticalSectionLocker locker;
00135             if ((adj_usec < 0) && std::uint64_t(-adj_usec) > time_utc)
00136             {
00137                 time_utc = 1;
00138             }
00139             else
00140             {
00141                 time_utc = std::uint64_t(std::int64_t(time_utc) + adj_usec);
00142             }
00143         }
00144         if (!utc_set)
00145         {
00146             utc_set = true;
00147             utc_correction_usec_per_overflow_x16 = 0;
00148         }
00149     }
00150 }
00151 
00152 } // namespace clock
00153 
00154 SystemClock SystemClock::self;
00155 
00156 SystemClock& SystemClock::instance()
00157 {
00158     clock::init();
00159     return self;
00160 }
00161 
00162 }
00163 
00164 /*
00165  * Timer interrupt handler
00166  */
00167 extern "C"
00168 {
00169 
00170 void SysTick_Handler();
00171 
00172 void SysTick_Handler()
00173 {
00174     using namespace uavcan_lpc11c24::clock;
00175     if (initialized)
00176     {
00177         time_mono += USecPerOverflow;
00178         if (utc_set)
00179         {
00180             // Values below 16 are ignored
00181             time_utc += std::uint64_t(std::int32_t(USecPerOverflow) + (utc_correction_usec_per_overflow_x16 / 16));
00182         }
00183     }
00184     else
00185     {
00186         fail();
00187     }
00188 }
00189 
00190 }