diff --git a/libuavcan/include/uavcan/scheduler.hpp b/libuavcan/include/uavcan/scheduler.hpp new file mode 100644 index 0000000000..834307cd10 --- /dev/null +++ b/libuavcan/include/uavcan/scheduler.hpp @@ -0,0 +1,58 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include + +namespace uavcan +{ + +class Scheduler : Noncopyable +{ + enum { DefaultTimerResolutionMs = 5 }; + enum { DefaultCleanupPeriodMs = 1000 }; + + LinkedListRoot 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; } +}; + +} diff --git a/libuavcan/include/uavcan/timer.hpp b/libuavcan/include/uavcan/timer.hpp new file mode 100644 index 0000000000..325c5ce845 --- /dev/null +++ b/libuavcan/include/uavcan/timer.hpp @@ -0,0 +1,87 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include + +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, 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 +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); + } +}; + +} diff --git a/libuavcan/src/scheduler.cpp b/libuavcan/src/scheduler.cpp new file mode 100644 index 0000000000..647722e061 --- /dev/null +++ b/libuavcan/src/scheduler.cpp @@ -0,0 +1,126 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include + +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::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; +} + +} diff --git a/libuavcan/src/timer.cpp b/libuavcan/src/timer.cpp new file mode 100644 index 0000000000..188e640557 --- /dev/null +++ b/libuavcan/src/timer.cpp @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +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); +} + +} diff --git a/libuavcan/test/scheduler.cpp b/libuavcan/test/scheduler.cpp new file mode 100644 index 0000000000..ba75963bc7 --- /dev/null +++ b/libuavcan/test/scheduler.cpp @@ -0,0 +1,105 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "common.hpp" +#include "transport/can/iface_mock.hpp" + +static const unsigned int TimestampPrecisionMs = 10; + +struct TimerCallCounter +{ + std::vector events_a; + std::vector 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 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 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 a(sch, TimerCallCounter::Binder(&tcc, &TimerCallCounter::callA)); + uavcan::Timer 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 +}