LPC11C24 clock driver. Not well tested yet, but generally seems to be OK

libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/clock.hpp

@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/driver/system_clock.hpp>
+
+namespace uavcan_lpc11c24
+{
+namespace clock
+{
+/**
+ * Starts the clock.
+ * Can be called multiple times, only the first call will be effective.
+ */
+void init();
+
+/**
+ * Returns current monotonic time passed since the moment when clock::init() was called.
+ * Note that both monotonic and UTC clocks are implemented using SysTick timer.
+ */
+uavcan::MonotonicTime getMonotonic();
+
+/**
+ * Returns UTC time if it has been set, otherwise returns zero time.
+ * Note that both monotonic and UTC clocks are implemented using SysTick timer.
+ */
+uavcan::UtcTime getUtc();
+
+/**
+ * Performs UTC time adjustment.
+ * The UTC time will be zero until first adjustment has been performed.
+ */
+void adjustUtc(uavcan::UtcDuration adjustment);
+
+/**
+ * Returns clock error sampled at previous UTC adjustment.
+ * Positive if the hardware timer is slower than reference time.
+ */
+uavcan::UtcDuration getPrevUtcAdjustment();
+
+}
+
+/**
+ * Adapter for uavcan::ISystemClock.
+ */
+class SystemClock : public uavcan::ISystemClock, uavcan::Noncopyable
+{
+    static SystemClock self;
+
+    SystemClock() { }
+
+    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); }
+
+public:
+    /**
+     * Calls clock::init() as needed.
+     * This function is thread safe.
+     */
+    static SystemClock& instance();
+};
+
+}

libuavcan_drivers/lpc11c24/driver/src/can.cpp

@@ -3,6 +3,7 @@
  */
 
 #include <uavcan_lpc11c24/can.hpp>
+#include <uavcan_lpc11c24/clock.hpp>
 #include <chip.h>
 #include "internal.hpp"
 
@@ -264,8 +265,8 @@ uavcan::int16_t CanDriver::send(const uavcan::CanFrame& frame, uavcan::Monotonic
 uavcan::int16_t CanDriver::receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
                                    uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags)
 {
-    out_ts_monotonic = uavcan::MonotonicTime(); // TODO: read monotonic
-    out_flags = 0;  // We don't support any flags
+    out_ts_monotonic = uavcan_lpc11c24::clock::getMonotonic();
+    out_flags = 0;                                            // We don't support any IO flags
 
     CriticalSectionLocker locker;
     if (rx_queue.getLength() == 0)
@@ -368,9 +369,9 @@ void canErrorCallback(uint32_t error_info)
     }
 }
 
-void CAN_IRQHandler(void)
+void CAN_IRQHandler()
 {
-    uavcan_lpc11c24::last_irq_utc_timestamp = 0; // TODO: Read UTC timestamp
+    uavcan_lpc11c24::last_irq_utc_timestamp = uavcan_lpc11c24::clock::getUtcUSecFromCanInterrupt();
     LPC_CCAN_API->isr();
 }
 

libuavcan_drivers/lpc11c24/driver/src/clock.cpp

@@ -0,0 +1,191 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan_lpc11c24/clock.hpp>
+#include <chip.h>
+#include "internal.hpp"
+
+namespace uavcan_lpc11c24
+{
+namespace clock
+{
+namespace
+{
+
+bool initialized = false;
+bool utc_set = false;
+
+int32_t utc_correction_usec_per_overflow_x16 = 0;
+int64_t prev_adjustment = 0;
+
+uint64_t time_mono = 0;
+uint64_t time_utc = 0;
+
+/**
+ * If this value is too large for the given core clock, reload value will be out of the 24-bit integer range.
+ * This will be detected at run time during timer initialization - refer to SysTick_Config().
+ */
+const uint32_t USecPerOverflow = 65536 * 2;
+const int32_t MaxUtcSpeedCorrectionX16 = 100 * 16;
+
+}
+
+__attribute((noreturn))
+void fail()
+{
+    while (true) { }
+}
+
+void init()
+{
+    CriticalSectionLocker lock;
+    if (!initialized)
+    {
+        initialized = true;
+
+        if ((SystemCoreClock % 1000000) != 0)  // Core clock frequency validation
+        {
+            fail();
+        }
+
+        if (SysTick_Config((SystemCoreClock / 1000000) * USecPerOverflow) != 0)
+        {
+            fail();
+        }
+    }
+}
+
+// Optimizer breaks this function.
+__attribute__((optimize("0")))
+static uint64_t sampleFromCriticalSection(const volatile uint64_t* const value)
+{
+    const uint32_t reload = SysTick->LOAD;  // SysTick counts downwards, hence the value subtracted from reload
+    volatile uint64_t time = *value;
+    volatile uint32_t cycles = reload - SysTick->VAL;
+
+    if (NVIC_GetPendingIRQ(SysTick_IRQn))
+    {
+        /*
+         * 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.
+         */
+        cycles = reload - SysTick->VAL;
+        /*
+         * The timer interrupt was set, but not handled yet.
+         * Thus we need to adjust the tick counter manually.
+         */
+        time += USecPerOverflow;
+    }
+    /*
+     * Raw counter value must be converted from core cycles to microseconds
+     */
+    return time + (cycles / (SystemCoreClock / 1000000));
+}
+
+uint64_t getUtcUSecFromCanInterrupt()
+{
+    return utc_set ? sampleFromCriticalSection(&time_utc) : 0;
+}
+
+uavcan::MonotonicTime getMonotonic()
+{
+    uint64_t usec = 0;
+    {
+        CriticalSectionLocker locker;
+        usec = sampleFromCriticalSection(&time_mono);
+    }
+    return uavcan::MonotonicTime::fromUSec(usec);
+}
+
+uavcan::UtcTime getUtc()
+{
+    if (utc_set)
+    {
+        uint64_t usec = 0;
+        {
+            CriticalSectionLocker locker;
+            usec = sampleFromCriticalSection(&time_utc);
+        }
+        return uavcan::UtcTime::fromUSec(usec);
+    }
+    return uavcan::UtcTime();
+}
+
+uavcan::UtcDuration getPrevUtcAdjustment()
+{
+    return uavcan::UtcDuration::fromUSec(prev_adjustment);
+}
+
+void adjustUtc(uavcan::UtcDuration adjustment)
+{
+    const int64_t adj_delta = adjustment.toUSec() - prev_adjustment;  // This is the P term
+    prev_adjustment = adjustment.toUSec();
+
+    utc_correction_usec_per_overflow_x16 += adjustment.isPositive() ? 1 : -1; // I
+    utc_correction_usec_per_overflow_x16 += (adj_delta > 0) ? 1 : -1;         // P
+
+    utc_correction_usec_per_overflow_x16 = std::max(utc_correction_usec_per_overflow_x16, -MaxUtcSpeedCorrectionX16);
+    utc_correction_usec_per_overflow_x16 = std::min(utc_correction_usec_per_overflow_x16,  MaxUtcSpeedCorrectionX16);
+
+    if (adjustment.getAbs().toMSec() > 2 || !utc_set)
+    {
+        const int64_t adj_usec = adjustment.toUSec();
+        {
+            CriticalSectionLocker locker;
+            if ((adj_usec < 0) && uint64_t(-adj_usec) > time_utc)
+            {
+                time_utc = 1;
+            }
+            else
+            {
+                time_utc += adj_usec;
+            }
+        }
+        if (!utc_set)
+        {
+            utc_set = true;
+            utc_correction_usec_per_overflow_x16 = 0;
+        }
+    }
+}
+
+} // namespace clock
+
+SystemClock SystemClock::self;
+
+SystemClock& SystemClock::instance()
+{
+    clock::init();
+    return self;
+}
+
+}
+
+/*
+ * Timer interrupt handler
+ */
+extern "C"
+{
+
+__attribute__((optimize("0")))
+void SysTick_Handler()
+{
+    using namespace uavcan_lpc11c24::clock;
+    if (initialized)
+    {
+        time_mono += USecPerOverflow;
+        if (utc_set)
+        {
+            // Values below 16 are ignored
+            time_utc += USecPerOverflow + (utc_correction_usec_per_overflow_x16 / 16);
+        }
+    }
+    else
+    {
+        fail();
+    }
+}
+
+}

libuavcan_drivers/lpc11c24/driver/src/internal.hpp

@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <chip.h>
+
 namespace uavcan_lpc11c24
 {
 
@@ -23,4 +25,14 @@ struct CriticalSectionLocker
     }
 };
 
+/**
+ * Internal for the driver
+ */
+namespace clock
+{
+
+uint64_t getUtcUSecFromCanInterrupt();
+
+}
+
 }

libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp

@@ -5,6 +5,7 @@
 #include <board.hpp>
 #include <chip.h>
 #include <uavcan_lpc11c24/can.hpp>
+#include <uavcan_lpc11c24/clock.hpp>
 
 #define ENFORCE(x)        \
     if ((x) == 0) {       \
@@ -18,9 +19,18 @@ int main()
         while (true) { }
     }
 
+    uavcan_lpc11c24::clock::init();
+
+    uavcan::MonotonicTime prev_mono;
+
     while (true)
     {
-        //for (volatile int i = 0; i < 2000000; i++) { __asm volatile ("nop"); }
+        const uavcan::MonotonicTime ts_mono = uavcan_lpc11c24::clock::getMonotonic();
+
+        while (prev_mono >= ts_mono)
+        {
+        }
+        prev_mono = ts_mono;
 
         board::setErrorLed(uavcan_lpc11c24::CanDriver::instance().getErrorCount() > 0);