diff --git a/libuavcan/include/uavcan/scheduler.hpp b/libuavcan/include/uavcan/scheduler.hpp index 834307cd10..beeea9ad35 100644 --- a/libuavcan/include/uavcan/scheduler.hpp +++ b/libuavcan/include/uavcan/scheduler.hpp @@ -4,26 +4,69 @@ #pragma once -#include #include #include namespace uavcan { +class Scheduler; + +class MonotonicDeadlineHandler : public LinkedListNode, Noncopyable +{ + uint64_t monotonic_deadline_; + +protected: + Scheduler& scheduler_; + + MonotonicDeadlineHandler(Scheduler& scheduler) + : monotonic_deadline_(0) + , scheduler_(scheduler) + { } + + virtual ~MonotonicDeadlineHandler() { stop(); } + +public: + virtual void onMonotonicDeadline(uint64_t monotonic_timestamp) = 0; + + void startWithDeadline(uint64_t monotonic_deadline); + void startWithDelay(uint64_t delay_usec); + + void stop(); + + bool isRunning() const; + + uint64_t getMonotonicDeadline() const { return monotonic_deadline_; } +}; + + +class MonotonicDeadlineScheduler : Noncopyable +{ + LinkedListRoot handlers_; // Ordered by deadline, lowest first + +public: + void add(MonotonicDeadlineHandler* mdh); + void remove(MonotonicDeadlineHandler* mdh); + bool doesExist(const MonotonicDeadlineHandler* mdh) const; + unsigned int getNumHandlers() const { return handlers_.getLength(); } + + uint64_t pollAndGetMonotonicTimestamp(ISystemClock& sysclock); + uint64_t getEarliestDeadline() const; +}; + + class Scheduler : Noncopyable { - enum { DefaultTimerResolutionMs = 5 }; + enum { DefaultMonotonicDeadlineResolutionMs = 5 }; enum { DefaultCleanupPeriodMs = 1000 }; - LinkedListRoot ordered_timers_; // Ordered by deadline, lowest first + MonotonicDeadlineScheduler deadline_scheduler_; Dispatcher dispatcher_; uint64_t prev_cleanup_ts_; - uint64_t timer_resolution_; + uint64_t monotonic_deadline_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: @@ -31,25 +74,21 @@ public: NodeID self_node_id) : dispatcher_(can_driver, allocator, sysclock, otr, self_node_id) , prev_cleanup_ts_(sysclock.getMonotonicMicroseconds()) - , timer_resolution_(DefaultTimerResolutionMs * 1000) + , monotonic_deadline_resolution_(DefaultMonotonicDeadlineResolutionMs * 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(); } - + MonotonicDeadlineScheduler& getMonotonicDeadlineScheduler() { return deadline_scheduler_; } 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 getMonotonicDeadlineResolution() const { return monotonic_deadline_resolution_; } + void setMonotonicDeadlineResolution(uint64_t res_usec) { monotonic_deadline_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 index 325c5ce845..3b5701f77a 100644 --- a/libuavcan/include/uavcan/timer.hpp +++ b/libuavcan/include/uavcan/timer.hpp @@ -5,80 +5,71 @@ #pragma once #include +#include #include #include namespace uavcan { -class Scheduler; -class TimerBase; +class Timer; struct TimerEvent { uint64_t scheduled_monotonic_deadline; uint64_t monotonic_timestamp; - TimerBase* timer; + Timer* timer; - TimerEvent(uint64_t scheduled_monotonic_deadline, uint64_t monotonic_timestamp, TimerBase& timer) + TimerEvent(Timer* timer, uint64_t scheduled_monotonic_deadline, uint64_t monotonic_timestamp) : scheduled_monotonic_deadline(scheduled_monotonic_deadline) , monotonic_timestamp(monotonic_timestamp) - , timer(&timer) + , timer(timer) { } }; -class TimerBase : public LinkedListNode, Noncopyable +class Timer : private MonotonicDeadlineHandler { - friend class Scheduler; - - uint64_t monotonic_deadline_; uint64_t period_; - Scheduler& scheduler_; - void handleOneShotTimeout(uint64_t ts_monotonic); - void genericStart(); + void onMonotonicDeadline(uint64_t monotonic_timestamp); public: static const uint64_t InfinitePeriod = 0xFFFFFFFFFFFFFFFFUL; - TimerBase(Scheduler& scheduler) - : monotonic_deadline_(0) + using MonotonicDeadlineHandler::stop; + using MonotonicDeadlineHandler::isRunning; + using MonotonicDeadlineHandler::getMonotonicDeadline; + + Timer(Scheduler& scheduler) + : MonotonicDeadlineHandler(scheduler) , 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 startOneShotWithDeadline(uint64_t monotonic_deadline); + void startOneShotWithDelay(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; + virtual void onTimerEvent(const TimerEvent& event) = 0; }; template -class Timer : public TimerBase +class TimerEventForwarder : public Timer { Functor functor_; public: - Timer(Scheduler& node, Functor functor) - : TimerBase(node) + TimerEventForwarder(Scheduler& node, Functor functor) + : Timer(node) , functor_(functor) { } const Functor& getFunctor() const { return functor_; } - void onTimerEvent(TimerEvent& event) + void onTimerEvent(const TimerEvent& event) { functor_(event); } diff --git a/libuavcan/src/scheduler.cpp b/libuavcan/src/scheduler.cpp index 647722e061..d59f7ae77d 100644 --- a/libuavcan/src/scheduler.cpp +++ b/libuavcan/src/scheduler.cpp @@ -9,57 +9,124 @@ namespace uavcan { +/* + * MonotonicDeadlineHandler + */ +void MonotonicDeadlineHandler::startWithDeadline(uint64_t monotonic_deadline) +{ + assert(monotonic_deadline > 0); + stop(); + monotonic_deadline_ = monotonic_deadline; + scheduler_.getMonotonicDeadlineScheduler().add(this); +} -struct TimerInsertionComparator +void MonotonicDeadlineHandler::startWithDelay(uint64_t delay_usec) +{ + startWithDeadline(scheduler_.getMonotonicTimestamp() + delay_usec); +} + +void MonotonicDeadlineHandler::stop() +{ + scheduler_.getMonotonicDeadlineScheduler().remove(this); +} + +bool MonotonicDeadlineHandler::isRunning() const +{ + return scheduler_.getMonotonicDeadlineScheduler().doesExist(this); +} + +/* + * MonotonicDeadlineScheduler + */ +struct MonotonicDeadlineHandlerInsertionComparator { const uint64_t ts; - TimerInsertionComparator(uint64_t ts) : ts(ts) { } - bool operator()(const TimerBase* t) const + MonotonicDeadlineHandlerInsertionComparator(uint64_t ts) : ts(ts) { } + bool operator()(const MonotonicDeadlineHandler* t) const { return t->getMonotonicDeadline() > ts; } }; -uint64_t Scheduler::computeDispatcherSpinDeadline(uint64_t spin_deadline) const +void MonotonicDeadlineScheduler::add(MonotonicDeadlineHandler* mdh) { - 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; + assert(mdh); + handlers_.insertBefore(mdh, MonotonicDeadlineHandlerInsertionComparator(mdh->getMonotonicDeadline())); } -uint64_t Scheduler::pollTimersAndGetMonotonicTimestamp() +void MonotonicDeadlineScheduler::remove(MonotonicDeadlineHandler* mdh) +{ + assert(mdh); + handlers_.remove(mdh); +} + +bool MonotonicDeadlineScheduler::doesExist(const MonotonicDeadlineHandler* mdh) const +{ + assert(mdh); + const MonotonicDeadlineHandler* p = handlers_.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 == mdh) + return true; + p = p->getNextListNode(); + } + return false; +} + +uint64_t MonotonicDeadlineScheduler::pollAndGetMonotonicTimestamp(ISystemClock& sysclock) { while (true) { - TimerBase* const timer = ordered_timers_.get(); - if (!timer) - return getMonotonicTimestamp(); + MonotonicDeadlineHandler* const mdh = handlers_.get(); + if (!mdh) + return sysclock.getMonotonicMicroseconds(); #if UAVCAN_DEBUG - if (timer->getNextListNode()) // Order check - assert(timer->getMonotonicDeadline() <= timer->getNextListNode()->getMonotonicDeadline()); + if (mdh->getNextListNode()) // Order check + assert(mdh->getMonotonicDeadline() <= mdh->getNextListNode()->getMonotonicDeadline()); #endif - const uint64_t ts = getMonotonicTimestamp(); - if (ts < timer->getMonotonicDeadline()) + const uint64_t ts = sysclock.getMonotonicMicroseconds(); + if (ts < mdh->getMonotonicDeadline()) return ts; - ordered_timers_.remove(timer); - timer->handleOneShotTimeout(ts); // This timer can be re-registered immediately + handlers_.remove(mdh); + mdh->onMonotonicDeadline(ts); // This handler can be re-registered immediately } assert(0); return 0; } +uint64_t MonotonicDeadlineScheduler::getEarliestDeadline() const +{ + const MonotonicDeadlineHandler* const mdh = handlers_.get(); + if (mdh) + return mdh->getMonotonicDeadline(); + return std::numeric_limits::max(); +} + +/* + * Scheduler + */ +uint64_t Scheduler::computeDispatcherSpinDeadline(uint64_t spin_deadline) const +{ + const uint64_t earliest = std::min(deadline_scheduler_.getEarliestDeadline(), spin_deadline); + const uint64_t ts = getMonotonicTimestamp(); + if (earliest > ts) + { + if (ts - earliest > monotonic_deadline_resolution_) + return ts + monotonic_deadline_resolution_; + } + return earliest; +} + 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 @@ -82,7 +149,7 @@ int Scheduler::spin(uint64_t monotonic_deadline) if (retval < 0) break; - const uint64_t ts = pollTimersAndGetMonotonicTimestamp(); + const uint64_t ts = deadline_scheduler_.pollAndGetMonotonicTimestamp(getSystemClock()); pollCleanup(ts, retval); if (ts >= monotonic_deadline) break; @@ -90,37 +157,4 @@ int Scheduler::spin(uint64_t monotonic_deadline) 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 index 188e640557..f58925acca 100644 --- a/libuavcan/src/timer.cpp +++ b/libuavcan/src/timer.cpp @@ -4,65 +4,41 @@ #include #include -#include namespace uavcan { -const uint64_t TimerBase::InfinitePeriod; +const uint64_t Timer::InfinitePeriod; -void TimerBase::handleOneShotTimeout(uint64_t ts_monotonic) +void Timer::onMonotonicDeadline(uint64_t monotonic_timestamp) { - assert(!scheduler_.isOneShotTimerRegistered(this)); + assert(!isRunning()); if (period_ != InfinitePeriod) - { - monotonic_deadline_ += period_; - genericStart(); - } + startWithDeadline(getMonotonicDeadline() + period_); // Application can re-register the timer with different params, it's OK - TimerEvent event(monotonic_deadline_, ts_monotonic, *this); - onTimerEvent(event); + onTimerEvent(TimerEvent(this, getMonotonicDeadline(), monotonic_timestamp)); } -void TimerBase::genericStart() +void Timer::startOneShotWithDeadline(uint64_t monotonic_deadline_usec) { - 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(); + MonotonicDeadlineHandler::startWithDeadline(monotonic_deadline_usec); } -void TimerBase::startOneShotDelay(uint64_t delay_usec) +void Timer::startOneShotWithDelay(uint64_t delay_usec) { - stop(); - startOneShotDeadline(scheduler_.getMonotonicTimestamp() + delay_usec); + period_ = InfinitePeriod; + MonotonicDeadlineHandler::startWithDelay(delay_usec); } -void TimerBase::startPeriodic(uint64_t period_usec) +void Timer::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); + MonotonicDeadlineHandler::startWithDelay(period_usec); } } diff --git a/libuavcan/test/scheduler.cpp b/libuavcan/test/scheduler.cpp index ba75963bc7..88d7bd4f67 100644 --- a/libuavcan/test/scheduler.cpp +++ b/libuavcan/test/scheduler.cpp @@ -3,7 +3,7 @@ */ #include -#include +#include #include "common.hpp" #include "transport/can/iface_mock.hpp" @@ -46,21 +46,19 @@ TEST(Scheduler, Timers) */ { TimerCallCounter tcc; - uavcan::Timer a(sch, TimerCallCounter::Binder(&tcc, &TimerCallCounter::callA)); - uavcan::Timer b(sch, TimerCallCounter::Binder(&tcc, &TimerCallCounter::callB)); + uavcan::TimerEventForwarder + a(sch, TimerCallCounter::Binder(&tcc, &TimerCallCounter::callA)); + uavcan::TimerEventForwarder + b(sch, TimerCallCounter::Binder(&tcc, &TimerCallCounter::callB)); - ASSERT_EQ(0, sch.getNumOneShotTimers()); - ASSERT_FALSE(sch.isOneShotTimerRegistered(&a)); - ASSERT_FALSE(sch.isOneShotTimerRegistered(&b)); + ASSERT_EQ(0, sch.getMonotonicDeadlineScheduler().getNumHandlers()); const uint64_t start_ts = clock_driver.getMonotonicMicroseconds(); - a.startOneShotDeadline(start_ts + 100000); + a.startOneShotWithDeadline(start_ts + 100000); b.startPeriodic(1000); - ASSERT_EQ(2, sch.getNumOneShotTimers()); - ASSERT_TRUE(sch.isOneShotTimerRegistered(&a)); - ASSERT_TRUE(sch.isOneShotTimerRegistered(&b)); + ASSERT_EQ(2, sch.getMonotonicDeadlineScheduler().getNumHandlers()); /* * Spinning @@ -89,17 +87,15 @@ TEST(Scheduler, Timers) /* * Deinitialization */ - ASSERT_EQ(1, sch.getNumOneShotTimers()); + ASSERT_EQ(1, sch.getMonotonicDeadlineScheduler().getNumHandlers()); - ASSERT_FALSE(sch.isOneShotTimerRegistered(&a)); ASSERT_FALSE(a.isRunning()); - ASSERT_EQ(uavcan::TimerBase::InfinitePeriod, a.getPeriod()); + ASSERT_EQ(uavcan::Timer::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.getMonotonicDeadlineScheduler().getNumHandlers()); // Both timers were destroyed now ASSERT_EQ(0, sch.spin(clock_driver.getMonotonicMicroseconds() + 1000)); // Spin some more without timers }