Add event time for timers and phased loops
This is the beginning of the framework needed to measure timing reports.
This fills in the last event times needed. Watchers and Fetchers
already have the times well covered.
Change-Id: I31ce8814cee3607a4e615b0359f1408ced1258bc
diff --git a/aos/events/epoll.cc b/aos/events/epoll.cc
index 3da04cf..e19cf59 100644
--- a/aos/events/epoll.cc
+++ b/aos/events/epoll.cc
@@ -29,11 +29,18 @@
PCHECK(timerfd_settime(fd_, TFD_TIMER_ABSTIME, &new_value, nullptr) == 0);
}
-void TimerFd::Read() {
+uint64_t TimerFd::Read() {
uint64_t buf;
ssize_t result = read(fd_, &buf, sizeof(buf));
+ if (result == -1) {
+ if (errno == EAGAIN) {
+ return 0;
+ }
+ }
PCHECK(result != -1);
CHECK_EQ(result, static_cast<int>(sizeof(buf)));
+
+ return buf;
}
EPoll::EPoll() : epoll_fd_(epoll_create1(EPOLL_CLOEXEC)) {
diff --git a/aos/events/epoll.h b/aos/events/epoll.h
index 0f335e5..7bc135c 100644
--- a/aos/events/epoll.h
+++ b/aos/events/epoll.h
@@ -36,8 +36,8 @@
SetTime(::aos::monotonic_clock::epoch(), ::aos::monotonic_clock::zero());
}
- // Reads the event. Ignores it.
- void Read();
+ // Reads the event. Returns the number of elapsed cycles.
+ uint64_t Read();
// Returns the file descriptor associated with the timerfd.
int fd() { return fd_; }
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index a83cfb1..3c47d1a 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -338,12 +338,12 @@
// Validates that channel exists inside configuration_.
void ValidateChannel(const Channel *channel);
- private:
- ::std::atomic<bool> is_running_{false};
-
// Context available for watchers.
Context context_;
+ private:
+ ::std::atomic<bool> is_running_{false};
+
const Configuration *configuration_;
};
diff --git a/aos/events/event_loop_param_test.cc b/aos/events/event_loop_param_test.cc
index 3cace56..8defcb5 100644
--- a/aos/events/event_loop_param_test.cc
+++ b/aos/events/event_loop_param_test.cc
@@ -474,22 +474,72 @@
// Verify that timer intervals and duration function properly.
TEST_P(AbstractEventLoopTest, TimerIntervalAndDuration) {
- auto loop = MakePrimary();
- ::std::vector<::aos::monotonic_clock::time_point> iteration_list;
+ const int kCount = 5;
- auto test_timer = loop->AddTimer([&iteration_list, &loop]() {
- iteration_list.push_back(loop->monotonic_now());
+ auto loop = MakePrimary();
+ ::std::vector<::aos::monotonic_clock::time_point> times;
+ ::std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+ auto test_timer = loop->AddTimer([this, ×, &expected_times, &loop]() {
+ times.push_back(loop->monotonic_now());
+ expected_times.push_back(loop->context().monotonic_sent_time);
+ if (times.size() == kCount) {
+ this->Exit();
+ }
});
+ monotonic_clock::time_point start_time = loop->monotonic_now();
// TODO(austin): This should be an error... Should be done in OnRun only.
- test_timer->Setup(loop->monotonic_now(), ::std::chrono::milliseconds(20));
- EndEventLoop(loop.get(), ::std::chrono::milliseconds(150));
- // Testing that the timer thread waits for the event loop to start before
- // running
- ::std::this_thread::sleep_for(std::chrono::milliseconds(2));
+ test_timer->Setup(start_time + chrono::seconds(1), chrono::seconds(1));
+
Run();
- EXPECT_EQ(iteration_list.size(), 8);
+ // Confirm that we got both the right number of samples, and it's odd.
+ EXPECT_EQ(times.size(), static_cast<size_t>(kCount));
+ EXPECT_EQ(times.size(), expected_times.size());
+ EXPECT_EQ((times.size() % 2), 1);
+
+ // Grab the middle sample.
+ ::aos::monotonic_clock::time_point average_time = times[times.size() / 2];
+
+ // Add up all the delays of all the times.
+ ::aos::monotonic_clock::duration sum = chrono::seconds(0);
+ for (const ::aos::monotonic_clock::time_point time : times) {
+ sum += time - average_time;
+ }
+
+ // Average and add to the middle to find the average time.
+ sum /= times.size();
+ average_time += sum;
+
+ // Compute the offset from the average and the expected average. It
+ // should be pretty close to 0.
+ const ::aos::monotonic_clock::duration remainder =
+ average_time - start_time - chrono::seconds(times.size() / 2 + 1);
+
+ const chrono::milliseconds kEpsilon(100);
+ EXPECT_LT(remainder, +kEpsilon);
+ EXPECT_GT(remainder, -kEpsilon);
+
+ // Make sure that the average duration is close to 1 second.
+ EXPECT_NEAR(chrono::duration_cast<chrono::duration<double>>(times.back() -
+ times.front())
+ .count() /
+ static_cast<double>(times.size() - 1),
+ 1.0, 0.1);
+
+ // Confirm that the ideal wakeup times increment correctly.
+ for (size_t i = 1; i < expected_times.size(); ++i) {
+ EXPECT_EQ(expected_times[i], expected_times[i - 1] + chrono::seconds(1));
+ }
+
+ for (size_t i = 0; i < expected_times.size(); ++i) {
+ EXPECT_EQ((expected_times[i] - start_time) % chrono::seconds(1),
+ chrono::seconds(0));
+ }
+
+ EXPECT_LT(expected_times[expected_times.size() / 2], average_time + kEpsilon);
+ EXPECT_GT(expected_times[expected_times.size() / 2], average_time - kEpsilon);
}
// Verify that we can change a timer's parameters during execution.
@@ -632,13 +682,14 @@
// Collect up a couple of samples.
::std::vector<::aos::monotonic_clock::time_point> times;
+ ::std::vector<::aos::monotonic_clock::time_point> expected_times;
// Run kCount iterations.
loop1->AddPhasedLoop(
- [×, &loop1, this](int count) {
+ [×, &expected_times, &loop1, this](int count) {
EXPECT_EQ(count, 1);
times.push_back(loop1->monotonic_now());
- LOG(INFO) << times.size();
+ expected_times.push_back(loop1->context().monotonic_sent_time);
if (times.size() == kCount) {
this->Exit();
}
@@ -653,26 +704,27 @@
// Confirm that we got both the right number of samples, and it's odd.
EXPECT_EQ(times.size(), static_cast<size_t>(kCount));
+ EXPECT_EQ(times.size(), expected_times.size());
EXPECT_EQ((times.size() % 2), 1);
// Grab the middle sample.
- ::aos::monotonic_clock::time_point middle_time = times[times.size() / 2 + 1];
+ ::aos::monotonic_clock::time_point average_time = times[times.size() / 2];
// Add up all the delays of all the times.
::aos::monotonic_clock::duration sum = chrono::seconds(0);
for (const ::aos::monotonic_clock::time_point time : times) {
- sum += time - middle_time;
+ sum += time - average_time;
}
// Average and add to the middle to find the average time.
sum /= times.size();
- middle_time += sum;
+ average_time += sum;
// Compute the offset from the start of the second of the average time. This
// should be pretty close to the offset.
const ::aos::monotonic_clock::duration remainder =
- middle_time.time_since_epoch() -
- chrono::duration_cast<chrono::seconds>(middle_time.time_since_epoch());
+ average_time.time_since_epoch() -
+ chrono::duration_cast<chrono::seconds>(average_time.time_since_epoch());
const chrono::milliseconds kEpsilon(100);
EXPECT_LT(remainder, kOffset + kEpsilon);
@@ -684,6 +736,19 @@
.count() /
static_cast<double>(times.size() - 1),
1.0, 0.1);
+
+ // Confirm that the ideal wakeup times increment correctly.
+ for (size_t i = 1; i < expected_times.size(); ++i) {
+ EXPECT_EQ(expected_times[i], expected_times[i - 1] + chrono::seconds(1));
+ }
+
+ for (size_t i = 0; i < expected_times.size(); ++i) {
+ EXPECT_EQ(expected_times[i].time_since_epoch() % chrono::seconds(1),
+ kOffset);
+ }
+
+ EXPECT_LT(expected_times[expected_times.size() / 2], average_time + kEpsilon);
+ EXPECT_GT(expected_times[expected_times.size() / 2], average_time - kEpsilon);
}
} // namespace testing
diff --git a/aos/events/shm_event_loop.cc b/aos/events/shm_event_loop.cc
index c05187b..9e18544 100644
--- a/aos/events/shm_event_loop.cc
+++ b/aos/events/shm_event_loop.cc
@@ -362,15 +362,22 @@
};
// Adapter class to adapt a timerfd to a TimerHandler.
-// The part of the API which is accessed by the TimerHandler interface needs to
-// be threadsafe. This means Setup and Disable.
class TimerHandlerState : public TimerHandler {
public:
TimerHandlerState(ShmEventLoop *shm_event_loop, ::std::function<void()> fn)
: shm_event_loop_(shm_event_loop), fn_(::std::move(fn)) {
shm_event_loop_->epoll_.OnReadable(timerfd_.fd(), [this]() {
- timerfd_.Read();
+ const uint64_t elapsed_cycles = timerfd_.Read();
+
+ shm_event_loop_->context_.monotonic_sent_time = base_;
+ shm_event_loop_->context_.realtime_sent_time = realtime_clock::min_time;
+ shm_event_loop_->context_.queue_index = 0;
+ shm_event_loop_->context_.size = 0;
+ shm_event_loop_->context_.data = nullptr;
+
fn_();
+
+ base_ += repeat_offset_ * elapsed_cycles;
});
}
@@ -378,8 +385,9 @@
void Setup(monotonic_clock::time_point base,
monotonic_clock::duration repeat_offset) override {
- // SetTime is threadsafe already.
timerfd_.SetTime(base, repeat_offset);
+ base_ = base;
+ repeat_offset_ = repeat_offset;
}
void Disable() override {
@@ -392,13 +400,13 @@
TimerFd timerfd_;
- // Function to be run on the thread
::std::function<void()> fn_;
+
+ monotonic_clock::time_point base_;
+ monotonic_clock::duration repeat_offset_;
};
// Adapter class to the timerfd and PhasedLoop.
-// The part of the API which is accessed by the PhasedLoopHandler interface
-// needs to be threadsafe. This means set_interval_and_offset
class PhasedLoopHandler : public ::aos::PhasedLoopHandler {
public:
PhasedLoopHandler(ShmEventLoop *shm_event_loop, ::std::function<void(int)> fn,
@@ -409,10 +417,28 @@
fn_(::std::move(fn)) {
shm_event_loop_->epoll_.OnReadable(timerfd_.fd(), [this]() {
timerfd_.Read();
- // Call the function. To avoid needing a recursive mutex, drop the lock
- // before running the function.
- fn_(cycles_elapsed_);
+ // Update the context to hold the desired wakeup time.
+ shm_event_loop_->context_.monotonic_sent_time = phased_loop_.sleep_time();
+ shm_event_loop_->context_.realtime_sent_time = realtime_clock::min_time;
+ shm_event_loop_->context_.queue_index = 0;
+ shm_event_loop_->context_.size = 0;
+ shm_event_loop_->context_.data = nullptr;
+
+ // Compute how many cycles elapsed and schedule the next wakeup.
Reschedule();
+
+ // Call the function with the elapsed cycles.
+ fn_(cycles_elapsed_);
+ cycles_elapsed_ = 0;
+
+ const monotonic_clock::time_point monotonic_end_time =
+ monotonic_clock::now();
+
+ // If the handler too too long so we blew by the previous deadline, we
+ // want to just try for the next deadline. Reschedule.
+ if (monotonic_end_time > phased_loop_.sleep_time()) {
+ Reschedule();
+ }
});
}
@@ -427,12 +453,16 @@
void Startup() {
phased_loop_.Reset(shm_event_loop_->monotonic_now());
Reschedule();
+ // The first time, we'll double count. Reschedule here will count cycles
+ // elapsed before now, and then the reschedule before runing the handler
+ // will count the time that elapsed then. So clear the count here.
+ cycles_elapsed_ = 0;
}
private:
- // Reschedules the timer. Must be called with the mutex held.
+ // Reschedules the timer.
void Reschedule() {
- cycles_elapsed_ = phased_loop_.Iterate(shm_event_loop_->monotonic_now());
+ cycles_elapsed_ += phased_loop_.Iterate(shm_event_loop_->monotonic_now());
timerfd_.SetTime(phased_loop_.sleep_time(), ::aos::monotonic_clock::zero());
}
@@ -441,7 +471,7 @@
TimerFd timerfd_;
time::PhasedLoop phased_loop_;
- int cycles_elapsed_ = 1;
+ int cycles_elapsed_ = 0;
// Function to be run
const ::std::function<void(int)> fn_;
diff --git a/aos/events/shm_event_loop_test.cc b/aos/events/shm_event_loop_test.cc
index 4406e01..16ac08c 100644
--- a/aos/events/shm_event_loop_test.cc
+++ b/aos/events/shm_event_loop_test.cc
@@ -24,9 +24,10 @@
}
// Clean up anything left there before.
- unlink((FLAGS_shm_base + "test/aos.TestMessage.v0").c_str());
- unlink((FLAGS_shm_base + "test1/aos.TestMessage.v0").c_str());
- unlink((FLAGS_shm_base + "test2/aos.TestMessage.v0").c_str());
+ unlink((FLAGS_shm_base + "/test/aos.TestMessage.v0").c_str());
+ unlink((FLAGS_shm_base + "/test1/aos.TestMessage.v0").c_str());
+ unlink((FLAGS_shm_base + "/test2/aos.TestMessage.v0").c_str());
+ unlink((FLAGS_shm_base + "/test2/aos.TestMessage.v0").c_str());
}
::std::unique_ptr<EventLoop> Make() override {
@@ -145,9 +146,9 @@
// Confirm that we see the missed count when we sleep.
if (times.size() == 0) {
- EXPECT_EQ(count, 1);
+ CHECK_EQ(count, 1);
} else {
- EXPECT_EQ(count, 3);
+ CHECK_EQ(count, 3);
}
times.push_back(loop1->monotonic_now());
diff --git a/aos/events/simulated_event_loop.cc b/aos/events/simulated_event_loop.cc
index f65e75d..3f6abd3 100644
--- a/aos/events/simulated_event_loop.cc
+++ b/aos/events/simulated_event_loop.cc
@@ -156,6 +156,8 @@
};
} // namespace
+class SimulatedEventLoop;
+
class SimulatedFetcher : public RawFetcher {
public:
explicit SimulatedFetcher(SimulatedChannel *queue)
@@ -212,36 +214,18 @@
class SimulatedTimerHandler : public TimerHandler {
public:
explicit SimulatedTimerHandler(EventScheduler *scheduler,
+ SimulatedEventLoop *simulated_event_loop,
::std::function<void()> fn)
- : scheduler_(scheduler), token_(scheduler_->InvalidToken()), fn_(fn) {}
+ : scheduler_(scheduler),
+ token_(scheduler_->InvalidToken()),
+ simulated_event_loop_(simulated_event_loop),
+ fn_(fn) {}
~SimulatedTimerHandler() {}
void Setup(monotonic_clock::time_point base,
- monotonic_clock::duration repeat_offset) override {
- Disable();
- const ::aos::monotonic_clock::time_point monotonic_now =
- scheduler_->monotonic_now();
- base_ = base;
- repeat_offset_ = repeat_offset;
- if (base < monotonic_now) {
- token_ = scheduler_->Schedule(monotonic_now, [this]() { HandleEvent(); });
- } else {
- token_ = scheduler_->Schedule(base, [this]() { HandleEvent(); });
- }
- }
+ monotonic_clock::duration repeat_offset) override;
- void HandleEvent() {
- const ::aos::monotonic_clock::time_point monotonic_now =
- scheduler_->monotonic_now();
- if (repeat_offset_ != ::aos::monotonic_clock::zero()) {
- // Reschedule.
- while (base_ <= monotonic_now) base_ += repeat_offset_;
- token_ = scheduler_->Schedule(base_, [this]() { HandleEvent(); });
- } else {
- token_ = scheduler_->InvalidToken();
- }
- fn_();
- }
+ void HandleEvent();
void Disable() override {
if (token_ != scheduler_->InvalidToken()) {
@@ -257,6 +241,8 @@
private:
EventScheduler *scheduler_;
EventScheduler::Token token_;
+
+ SimulatedEventLoop *simulated_event_loop_;
// Function to be run on the thread
::std::function<void()> fn_;
monotonic_clock::time_point base_;
@@ -266,10 +252,12 @@
class SimulatedPhasedLoopHandler : public PhasedLoopHandler {
public:
SimulatedPhasedLoopHandler(EventScheduler *scheduler,
+ SimulatedEventLoop *simulated_event_loop,
::std::function<void(int)> fn,
const monotonic_clock::duration interval,
const monotonic_clock::duration offset)
- : simulated_timer_handler_(scheduler, [this]() { HandleTimerWakeup(); }),
+ : simulated_timer_handler_(scheduler, simulated_event_loop,
+ [this]() { HandleTimerWakeup(); }),
phased_loop_(interval, simulated_timer_handler_.monotonic_now(),
offset),
fn_(fn) {
@@ -351,7 +339,7 @@
watcher) override;
TimerHandler *AddTimer(::std::function<void()> callback) override {
- timers_.emplace_back(new SimulatedTimerHandler(scheduler_, callback));
+ timers_.emplace_back(new SimulatedTimerHandler(scheduler_, this, callback));
return timers_.back().get();
}
@@ -359,8 +347,8 @@
const monotonic_clock::duration interval,
const monotonic_clock::duration offset =
::std::chrono::seconds(0)) override {
- phased_loops_.emplace_back(
- new SimulatedPhasedLoopHandler(scheduler_, callback, interval, offset));
+ phased_loops_.emplace_back(new SimulatedPhasedLoopHandler(
+ scheduler_, this, callback, interval, offset));
return phased_loops_.back().get();
}
@@ -382,6 +370,8 @@
}
private:
+ friend class SimulatedTimerHandler;
+
EventScheduler *scheduler_;
absl::btree_map<SimpleChannel, std::unique_ptr<SimulatedChannel>> *channels_;
std::vector<std::pair<EventLoop *, std::function<void(bool)>>>
@@ -471,6 +461,42 @@
: name(CHECK_NOTNULL(CHECK_NOTNULL(channel)->name())->str()),
type(CHECK_NOTNULL(CHECK_NOTNULL(channel)->type())->str()) {}
+void SimulatedTimerHandler::Setup(monotonic_clock::time_point base,
+ monotonic_clock::duration repeat_offset) {
+ Disable();
+ const ::aos::monotonic_clock::time_point monotonic_now =
+ scheduler_->monotonic_now();
+ base_ = base;
+ repeat_offset_ = repeat_offset;
+ if (base < monotonic_now) {
+ token_ = scheduler_->Schedule(monotonic_now, [this]() { HandleEvent(); });
+ } else {
+ token_ = scheduler_->Schedule(base, [this]() { HandleEvent(); });
+ }
+}
+
+void SimulatedTimerHandler::HandleEvent() {
+ const ::aos::monotonic_clock::time_point monotonic_now =
+ scheduler_->monotonic_now();
+ if (repeat_offset_ != ::aos::monotonic_clock::zero()) {
+ // Reschedule.
+ while (base_ <= monotonic_now) base_ += repeat_offset_;
+ token_ = scheduler_->Schedule(base_, [this]() { HandleEvent(); });
+ } else {
+ token_ = scheduler_->InvalidToken();
+ }
+ // The scheduler is perfect, so we will always wake up on time.
+ simulated_event_loop_->context_.monotonic_sent_time =
+ scheduler_->monotonic_now();
+ simulated_event_loop_->context_.realtime_sent_time = realtime_clock::min_time;
+ simulated_event_loop_->context_.queue_index = 0;
+ simulated_event_loop_->context_.size = 0;
+ simulated_event_loop_->context_.data = nullptr;
+
+ fn_();
+}
+
+
void SimulatedEventLoop::Take(const Channel *channel) {
CHECK(!is_running()) << ": Cannot add new objects while running.";
diff --git a/aos/time/time.cc b/aos/time/time.cc
index 549dae7..95044a4 100644
--- a/aos/time/time.cc
+++ b/aos/time/time.cc
@@ -53,6 +53,16 @@
#endif // __linux__
namespace aos {
+
+void PrintTo(const monotonic_clock::time_point t, std::ostream *os) {
+ auto s = std::chrono::duration_cast<std::chrono::seconds>(t.time_since_epoch());
+ *os << s.count() << "."
+ << std::chrono::duration_cast<std::chrono::nanoseconds>(
+ t.time_since_epoch() - s)
+ .count()
+ << "sec";
+}
+
namespace time {
struct timespec to_timespec(
diff --git a/aos/time/time.h b/aos/time/time.h
index 37455f4..3cc0c2a 100644
--- a/aos/time/time.h
+++ b/aos/time/time.h
@@ -65,6 +65,8 @@
time_point(duration(::std::numeric_limits<duration::rep>::max()))};
};
+void PrintTo(const monotonic_clock::time_point t, std::ostream *os);
+
namespace time {
#ifdef __linux__
diff --git a/aos/util/phased_loop.h b/aos/util/phased_loop.h
index 3e1752b..c9e4933 100644
--- a/aos/util/phased_loop.h
+++ b/aos/util/phased_loop.h
@@ -54,6 +54,9 @@
monotonic_clock::time_point sleep_time() const { return last_time_; }
+ monotonic_clock::duration interval() const { return interval_; }
+ monotonic_clock::duration offset() const { return offset_; }
+
private:
monotonic_clock::duration interval_, offset_;