Scheduler

libuavcan/include/uavcan/scheduler.hpp

@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/timer.hpp>
+#include <uavcan/internal/linked_list.hpp>
+#include <uavcan/internal/transport/dispatcher.hpp>
+
+namespace uavcan
+{
+
+class Scheduler : Noncopyable
+{
+    enum { DefaultTimerResolutionMs = 5 };
+    enum { DefaultCleanupPeriodMs = 1000 };
+
+    LinkedListRoot<TimerBase> ordered_timers_;  // Ordered by deadline, lowest first
+    Dispatcher dispatcher_;
+    uint64_t prev_cleanup_ts_;
+    uint64_t timer_resolution_;
+    uint64_t cleanup_period_;
+
+    uint64_t computeDispatcherSpinDeadline(uint64_t spin_deadline) const;
+    uint64_t pollTimersAndGetMonotonicTimestamp();
+    void pollCleanup(uint64_t mono_ts, uint32_t num_frames_processed_with_last_spin);
+
+public:
+    Scheduler(ICanDriver& can_driver, IAllocator& allocator, ISystemClock& sysclock, IOutgoingTransferRegistry& otr,
+             NodeID self_node_id)
+    : dispatcher_(can_driver, allocator, sysclock, otr, self_node_id)
+    , prev_cleanup_ts_(sysclock.getMonotonicMicroseconds())
+    , timer_resolution_(DefaultTimerResolutionMs * 1000)
+    , cleanup_period_(DefaultCleanupPeriodMs * 1000)
+    { }
+
+    int spin(uint64_t monotonic_deadline);
+
+    void registerOneShotTimer(TimerBase* timer);
+    void unregisterOneShotTimer(TimerBase* timer);
+    bool isOneShotTimerRegistered(const TimerBase* timer) const;
+    unsigned int getNumOneShotTimers() const { return ordered_timers_.getLength(); }
+
+    Dispatcher& getDispatcher() { return dispatcher_; }
+
+    ISystemClock& getSystemClock()         { return dispatcher_.getSystemClock(); }
+    uint64_t getMonotonicTimestamp() const { return dispatcher_.getSystemClock().getMonotonicMicroseconds(); }
+    uint64_t getUtcTimestamp()       const { return dispatcher_.getSystemClock().getUtcMicroseconds(); }
+
+    uint64_t getTimerResolution() const { return timer_resolution_; }
+    void setTimerResolution(uint64_t res_usec) { timer_resolution_ = res_usec; }
+
+    uint64_t getCleanupPeriod() const { return cleanup_period_; }
+    void setCleanupPeriod(uint64_t period_usec) { cleanup_period_ = period_usec; }
+};
+
+}

libuavcan/include/uavcan/timer.hpp

@@ -0,0 +1,87 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <uavcan/internal/util.hpp>
+#include <uavcan/internal/linked_list.hpp>
+
+namespace uavcan
+{
+
+class Scheduler;
+class TimerBase;
+
+struct TimerEvent
+{
+    uint64_t scheduled_monotonic_deadline;
+    uint64_t monotonic_timestamp;
+    TimerBase* timer;
+
+    TimerEvent(uint64_t scheduled_monotonic_deadline, uint64_t monotonic_timestamp, TimerBase& timer)
+    : scheduled_monotonic_deadline(scheduled_monotonic_deadline)
+    , monotonic_timestamp(monotonic_timestamp)
+    , timer(&timer)
+    { }
+};
+
+
+class TimerBase : public LinkedListNode<TimerBase>, Noncopyable
+{
+    friend class Scheduler;
+
+    uint64_t monotonic_deadline_;
+    uint64_t period_;
+    Scheduler& scheduler_;
+
+    void handleOneShotTimeout(uint64_t ts_monotonic);
+    void genericStart();
+
+public:
+    static const uint64_t InfinitePeriod = 0xFFFFFFFFFFFFFFFFUL;
+
+    TimerBase(Scheduler& scheduler)
+    : monotonic_deadline_(0)
+    , period_(InfinitePeriod)
+    , scheduler_(scheduler)
+    { }
+
+    virtual ~TimerBase() { stop(); }
+
+    uint64_t getMonotonicDeadline() const { return monotonic_deadline_; }
+
+    void startOneShotDeadline(uint64_t monotonic_deadline_usec);
+    void startOneShotDelay(uint64_t delay_usec);
+    void startPeriodic(uint64_t period_usec);
+
+    void stop();
+    bool isRunning() const;
+
+    uint64_t getPeriod() const { return period_; }
+
+    virtual void onTimerEvent(TimerEvent& event) = 0;
+};
+
+
+template <typename Functor>
+class Timer : public TimerBase
+{
+    Functor functor_;
+
+public:
+    Timer(Scheduler& node, Functor functor)
+    : TimerBase(node)
+    , functor_(functor)
+    { }
+
+    const Functor& getFunctor() const { return functor_; }
+
+    void onTimerEvent(TimerEvent& event)
+    {
+        functor_(event);
+    }
+};
+
+}

libuavcan/src/scheduler.cpp

@@ -0,0 +1,126 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <cassert>
+#include <limits>
+#include <uavcan/scheduler.hpp>
+#include <uavcan/internal/debug.hpp>
+
+namespace uavcan
+{
+
+struct TimerInsertionComparator
+{
+    const uint64_t ts;
+    TimerInsertionComparator(uint64_t ts) : ts(ts) { }
+    bool operator()(const TimerBase* t) const
+    {
+        return t->getMonotonicDeadline() > ts;
+    }
+};
+
+uint64_t Scheduler::computeDispatcherSpinDeadline(uint64_t spin_deadline) const
+{
+    uint64_t timer_deadline = std::numeric_limits<uint64_t>::max();
+    TimerBase* const timer = ordered_timers_.get();
+    if (timer)
+        timer_deadline = timer->getMonotonicDeadline();
+
+    const uint64_t earliest = std::min(timer_deadline, spin_deadline);
+    const uint64_t ts = getMonotonicTimestamp();
+    if (earliest > ts)
+    {
+        if (ts - earliest > timer_resolution_)
+            return ts + timer_resolution_;
+    }
+    return earliest;
+}
+
+uint64_t Scheduler::pollTimersAndGetMonotonicTimestamp()
+{
+    while (true)
+    {
+        TimerBase* const timer = ordered_timers_.get();
+        if (!timer)
+            return getMonotonicTimestamp();
+#if UAVCAN_DEBUG
+        if (timer->getNextListNode())      // Order check
+            assert(timer->getMonotonicDeadline() <= timer->getNextListNode()->getMonotonicDeadline());
+#endif
+
+        const uint64_t ts = getMonotonicTimestamp();
+        if (ts < timer->getMonotonicDeadline())
+            return ts;
+
+        ordered_timers_.remove(timer);
+        timer->handleOneShotTimeout(ts);   // This timer can be re-registered immediately
+    }
+    assert(0);
+    return 0;
+}
+
+void Scheduler::pollCleanup(uint64_t mono_ts, uint32_t num_frames_processed_with_last_spin)
+{
+    // cleanup will be performed less frequently if the stack handles more frames per second
+    const uint64_t deadline = prev_cleanup_ts_ + cleanup_period_ * (num_frames_processed_with_last_spin + 1);
+    if (mono_ts > deadline)
+    {
+        UAVCAN_TRACE("Scheduler", "Cleanup with %u processed frames", num_frames_processed_with_last_spin);
+        prev_cleanup_ts_ = mono_ts;
+        dispatcher_.cleanup(mono_ts);
+    }
+}
+
+int Scheduler::spin(uint64_t monotonic_deadline)
+{
+    int retval = 0;
+    while (true)
+    {
+        const uint64_t dl = computeDispatcherSpinDeadline(monotonic_deadline);
+        retval = dispatcher_.spin(dl);
+        if (retval < 0)
+            break;
+
+        const uint64_t ts = pollTimersAndGetMonotonicTimestamp();
+        pollCleanup(ts, retval);
+        if (ts >= monotonic_deadline)
+            break;
+    }
+    return retval;
+}
+
+void Scheduler::registerOneShotTimer(TimerBase* timer)
+{
+    assert(timer);
+    ordered_timers_.insertBefore(timer, TimerInsertionComparator(timer->getMonotonicDeadline()));
+}
+
+void Scheduler::unregisterOneShotTimer(TimerBase* timer)
+{
+    assert(timer);
+    ordered_timers_.remove(timer);
+}
+
+bool Scheduler::isOneShotTimerRegistered(const TimerBase* timer) const
+{
+    assert(timer);
+    const TimerBase* p = ordered_timers_.get();
+#if UAVCAN_DEBUG
+    uint64_t prev_deadline = 0;
+#endif
+    while (p)
+    {
+#if UAVCAN_DEBUG
+        if (prev_deadline > p->getMonotonicDeadline())  // Self check
+            std::abort();
+        prev_deadline = p->getMonotonicDeadline();
+#endif
+        if (p == timer)
+            return true;
+        p = p->getNextListNode();
+    }
+    return false;
+}
+
+}

libuavcan/src/timer.cpp

@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <cassert>
+#include <uavcan/timer.hpp>
+#include <uavcan/scheduler.hpp>
+
+namespace uavcan
+{
+
+const uint64_t TimerBase::InfinitePeriod;
+
+void TimerBase::handleOneShotTimeout(uint64_t ts_monotonic)
+{
+    assert(!scheduler_.isOneShotTimerRegistered(this));
+
+    if (period_ != InfinitePeriod)
+    {
+        monotonic_deadline_ += period_;
+        genericStart();
+    }
+
+    // Application can re-register the timer with different params, it's OK
+    TimerEvent event(monotonic_deadline_, ts_monotonic, *this);
+    onTimerEvent(event);
+}
+
+void TimerBase::genericStart()
+{
+    scheduler_.registerOneShotTimer(this);
+}
+
+void TimerBase::startOneShotDeadline(uint64_t monotonic_deadline_usec)
+{
+    assert(monotonic_deadline_usec > 0);
+    stop();
+    period_ = InfinitePeriod;
+    monotonic_deadline_ = monotonic_deadline_usec;
+    genericStart();
+}
+
+void TimerBase::startOneShotDelay(uint64_t delay_usec)
+{
+    stop();
+    startOneShotDeadline(scheduler_.getMonotonicTimestamp() + delay_usec);
+}
+
+void TimerBase::startPeriodic(uint64_t period_usec)
+{
+    assert(period_usec != InfinitePeriod);
+    stop();
+    period_ = period_usec;
+    monotonic_deadline_ = scheduler_.getMonotonicTimestamp() + period_usec;
+    genericStart();
+}
+
+void TimerBase::stop()
+{
+    scheduler_.unregisterOneShotTimer(this);
+}
+
+bool TimerBase::isRunning() const
+{
+    return scheduler_.isOneShotTimerRegistered(this);
+}
+
+}

libuavcan/test/scheduler.cpp

@@ -0,0 +1,105 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/scheduler.hpp>
+#include "common.hpp"
+#include "transport/can/iface_mock.hpp"
+
+static const unsigned int TimestampPrecisionMs = 10;
+
+struct TimerCallCounter
+{
+    std::vector<uavcan::TimerEvent> events_a;
+    std::vector<uavcan::TimerEvent> events_b;
+
+    void callA(const uavcan::TimerEvent& ev) { events_a.push_back(ev); }
+    void callB(const uavcan::TimerEvent& ev) { events_b.push_back(ev); }
+
+    typedef uavcan::MethodBinder<TimerCallCounter*, void(TimerCallCounter::*)(const uavcan::TimerEvent&)> Binder;
+};
+
+static bool timestampsEqual(int64_t a, int64_t b)
+{
+    return std::abs(a - b) < (TimestampPrecisionMs * 1000);
+}
+
+/*
+ * This test can fail on a non real time system. That's kinda sad but okay.
+ */
+TEST(Scheduler, Timers)
+{
+    uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 8, uavcan::MemPoolBlockSize> pool;
+    uavcan::PoolManager<1> poolmgr;
+    poolmgr.addPool(&pool);
+
+    SystemClockDriver clock_driver;
+    CanDriverMock can_driver(2, clock_driver);
+
+    uavcan::OutgoingTransferRegistry<8> out_trans_reg(poolmgr);
+
+    uavcan::Scheduler sch(can_driver, poolmgr, clock_driver, out_trans_reg, uavcan::NodeID(1));
+
+    /*
+     * Registration
+     */
+    {
+        TimerCallCounter tcc;
+        uavcan::Timer<TimerCallCounter::Binder> a(sch, TimerCallCounter::Binder(&tcc, &TimerCallCounter::callA));
+        uavcan::Timer<TimerCallCounter::Binder> b(sch, TimerCallCounter::Binder(&tcc, &TimerCallCounter::callB));
+
+        ASSERT_EQ(0, sch.getNumOneShotTimers());
+        ASSERT_FALSE(sch.isOneShotTimerRegistered(&a));
+        ASSERT_FALSE(sch.isOneShotTimerRegistered(&b));
+
+        const uint64_t start_ts = clock_driver.getMonotonicMicroseconds();
+
+        a.startOneShotDeadline(start_ts + 100000);
+        b.startPeriodic(1000);
+
+        ASSERT_EQ(2, sch.getNumOneShotTimers());
+        ASSERT_TRUE(sch.isOneShotTimerRegistered(&a));
+        ASSERT_TRUE(sch.isOneShotTimerRegistered(&b));
+
+        /*
+         * Spinning
+         */
+        ASSERT_EQ(0, sch.spin(start_ts + 1000000));
+
+        ASSERT_EQ(1, tcc.events_a.size());
+        ASSERT_TRUE(timestampsEqual(tcc.events_a[0].scheduled_monotonic_deadline, start_ts + 100000));
+        ASSERT_TRUE(timestampsEqual(tcc.events_a[0].monotonic_timestamp, tcc.events_a[0].scheduled_monotonic_deadline));
+        ASSERT_EQ(&a, tcc.events_a[0].timer);
+
+        ASSERT_LT(900, tcc.events_b.size());
+        ASSERT_GT(1100, tcc.events_b.size());
+        {
+            uint64_t next_expected_deadline = start_ts + 1000;
+            for (unsigned int i = 0; i < tcc.events_b.size(); i++)
+            {
+                ASSERT_TRUE(timestampsEqual(tcc.events_b[i].scheduled_monotonic_deadline, next_expected_deadline));
+                ASSERT_TRUE(timestampsEqual(tcc.events_b[i].monotonic_timestamp,
+                                            tcc.events_b[i].scheduled_monotonic_deadline));
+                ASSERT_EQ(&b, tcc.events_b[i].timer);
+                next_expected_deadline += 1000;
+            }
+        }
+
+        /*
+         * Deinitialization
+         */
+        ASSERT_EQ(1, sch.getNumOneShotTimers());
+
+        ASSERT_FALSE(sch.isOneShotTimerRegistered(&a));
+        ASSERT_FALSE(a.isRunning());
+        ASSERT_EQ(uavcan::TimerBase::InfinitePeriod, a.getPeriod());
+
+        ASSERT_TRUE(sch.isOneShotTimerRegistered(&b));
+        ASSERT_TRUE(b.isRunning());
+        ASSERT_EQ(1000, b.getPeriod());
+    }
+
+    ASSERT_EQ(0, sch.getNumOneShotTimers());                                 // Both timers were destroyed now
+    ASSERT_EQ(0, sch.spin(clock_driver.getMonotonicMicroseconds() + 1000));  // Spin some more without timers
+}