blob: b3026fa2ce354d739725af59c996a591bc36f09e [file] [log] [blame]
Alex Perrycb7da4b2019-08-28 19:35:56 -07001#include "aos/events/shm_event_loop.h"
2
3#include <sys/mman.h>
4#include <sys/stat.h>
Austin Schuh39788ff2019-12-01 18:22:57 -08005#include <sys/syscall.h>
Alex Perrycb7da4b2019-08-28 19:35:56 -07006#include <sys/types.h>
7#include <unistd.h>
Tyler Chatow67ddb032020-01-12 14:30:04 -08008
Alex Perrycb7da4b2019-08-28 19:35:56 -07009#include <algorithm>
10#include <atomic>
11#include <chrono>
Austin Schuh39788ff2019-12-01 18:22:57 -080012#include <iterator>
Alex Perrycb7da4b2019-08-28 19:35:56 -070013#include <stdexcept>
14
Tyler Chatow67ddb032020-01-12 14:30:04 -080015#include "aos/events/aos_logging.h"
Alex Perrycb7da4b2019-08-28 19:35:56 -070016#include "aos/events/epoll.h"
Austin Schuh39788ff2019-12-01 18:22:57 -080017#include "aos/events/event_loop_generated.h"
18#include "aos/events/timing_statistics.h"
Alex Perrycb7da4b2019-08-28 19:35:56 -070019#include "aos/ipc_lib/lockless_queue.h"
Austin Schuh39788ff2019-12-01 18:22:57 -080020#include "aos/ipc_lib/signalfd.h"
Alex Perrycb7da4b2019-08-28 19:35:56 -070021#include "aos/realtime.h"
Austin Schuh32fd5a72019-12-01 22:20:26 -080022#include "aos/stl_mutex/stl_mutex.h"
Austin Schuhfccb2d02020-01-26 16:11:19 -080023#include "aos/util/file.h"
Alex Perrycb7da4b2019-08-28 19:35:56 -070024#include "aos/util/phased_loop.h"
Austin Schuh39788ff2019-12-01 18:22:57 -080025#include "glog/logging.h"
Alex Perrycb7da4b2019-08-28 19:35:56 -070026
Austin Schuhe84c3ed2019-12-14 15:29:48 -080027namespace {
28
29// Returns the portion of the path after the last /. This very much assumes
30// that the application name is null terminated.
31const char *Filename(const char *path) {
32 const std::string_view path_string_view = path;
33 auto last_slash_pos = path_string_view.find_last_of("/");
34
35 return last_slash_pos == std::string_view::npos ? path
36 : path + last_slash_pos + 1;
37}
38
39} // namespace
40
Alex Perrycb7da4b2019-08-28 19:35:56 -070041DEFINE_string(shm_base, "/dev/shm/aos",
42 "Directory to place queue backing mmaped files in.");
43DEFINE_uint32(permissions, 0770,
44 "Permissions to make shared memory files and folders.");
Austin Schuhe84c3ed2019-12-14 15:29:48 -080045DEFINE_string(application_name, Filename(program_invocation_name),
46 "The application name");
Alex Perrycb7da4b2019-08-28 19:35:56 -070047
48namespace aos {
49
Austin Schuhcdab6192019-12-29 17:47:46 -080050void SetShmBase(const std::string_view base) {
51 FLAGS_shm_base = std::string(base) + "/dev/shm/aos";
52}
53
Alex Perrycb7da4b2019-08-28 19:35:56 -070054std::string ShmFolder(const Channel *channel) {
55 CHECK(channel->has_name());
56 CHECK_EQ(channel->name()->string_view()[0], '/');
57 return FLAGS_shm_base + channel->name()->str() + "/";
58}
59std::string ShmPath(const Channel *channel) {
60 CHECK(channel->has_type());
Austin Schuhad154822019-12-27 15:45:13 -080061 return ShmFolder(channel) + channel->type()->str() + ".v1";
Alex Perrycb7da4b2019-08-28 19:35:56 -070062}
63
64class MMapedQueue {
65 public:
Austin Schuhaa79e4e2019-12-29 20:43:32 -080066 MMapedQueue(const Channel *channel,
67 const std::chrono::seconds channel_storage_duration) {
Alex Perrycb7da4b2019-08-28 19:35:56 -070068 std::string path = ShmPath(channel);
69
Austin Schuh80c7fce2019-12-05 20:48:43 -080070 config_.num_watchers = channel->num_watchers();
71 config_.num_senders = channel->num_senders();
Austin Schuhaa79e4e2019-12-29 20:43:32 -080072 config_.queue_size =
73 channel_storage_duration.count() * channel->frequency();
Alex Perrycb7da4b2019-08-28 19:35:56 -070074 config_.message_data_size = channel->max_size();
75
76 size_ = ipc_lib::LocklessQueueMemorySize(config_);
77
Austin Schuhfccb2d02020-01-26 16:11:19 -080078 util::MkdirP(path, FLAGS_permissions);
Alex Perrycb7da4b2019-08-28 19:35:56 -070079
80 // There are 2 cases. Either the file already exists, or it does not
81 // already exist and we need to create it. Start by trying to create it. If
82 // that fails, the file has already been created and we can open it
83 // normally.. Once the file has been created it wil never be deleted.
84 fd_ = open(path.c_str(), O_RDWR | O_CREAT | O_EXCL,
85 O_CLOEXEC | FLAGS_permissions);
86 if (fd_ == -1 && errno == EEXIST) {
87 VLOG(1) << path << " already created.";
88 // File already exists.
89 fd_ = open(path.c_str(), O_RDWR, O_CLOEXEC);
90 PCHECK(fd_ != -1) << ": Failed to open " << path;
91 while (true) {
92 struct stat st;
93 PCHECK(fstat(fd_, &st) == 0);
94 if (st.st_size != 0) {
95 CHECK_EQ(static_cast<size_t>(st.st_size), size_)
96 << ": Size of " << path
97 << " doesn't match expected size of backing queue file. Did the "
98 "queue definition change?";
99 break;
100 } else {
101 // The creating process didn't get around to it yet. Give it a bit.
102 std::this_thread::sleep_for(std::chrono::milliseconds(10));
103 VLOG(1) << path << " is zero size, waiting";
104 }
105 }
106 } else {
107 VLOG(1) << "Created " << path;
108 PCHECK(fd_ != -1) << ": Failed to open " << path;
109 PCHECK(ftruncate(fd_, size_) == 0);
110 }
111
112 data_ = mmap(NULL, size_, PROT_READ | PROT_WRITE, MAP_SHARED, fd_, 0);
113 PCHECK(data_ != MAP_FAILED);
114
115 ipc_lib::InitializeLocklessQueueMemory(memory(), config_);
116 }
117
118 ~MMapedQueue() {
119 PCHECK(munmap(data_, size_) == 0);
120 PCHECK(close(fd_) == 0);
121 }
122
123 ipc_lib::LocklessQueueMemory *memory() const {
124 return reinterpret_cast<ipc_lib::LocklessQueueMemory *>(data_);
125 }
126
Austin Schuh39788ff2019-12-01 18:22:57 -0800127 const ipc_lib::LocklessQueueConfiguration &config() const { return config_; }
Alex Perrycb7da4b2019-08-28 19:35:56 -0700128
Brian Silverman5120afb2020-01-31 17:44:35 -0800129 absl::Span<char> GetSharedMemory() const {
130 return absl::Span<char>(static_cast<char *>(data_), size_);
131 }
132
Alex Perrycb7da4b2019-08-28 19:35:56 -0700133 private:
Alex Perrycb7da4b2019-08-28 19:35:56 -0700134 ipc_lib::LocklessQueueConfiguration config_;
135
136 int fd_;
137
138 size_t size_;
139 void *data_;
140};
141
Austin Schuh217a9782019-12-21 23:02:50 -0800142namespace {
143
Austin Schuh217a9782019-12-21 23:02:50 -0800144const Node *MaybeMyNode(const Configuration *configuration) {
145 if (!configuration->has_nodes()) {
146 return nullptr;
147 }
Alex Perrycb7da4b2019-08-28 19:35:56 -0700148
Austin Schuh217a9782019-12-21 23:02:50 -0800149 return configuration::GetMyNode(configuration);
150}
Alex Perrycb7da4b2019-08-28 19:35:56 -0700151
152namespace chrono = ::std::chrono;
153
Austin Schuh39788ff2019-12-01 18:22:57 -0800154} // namespace
155
Austin Schuh217a9782019-12-21 23:02:50 -0800156ShmEventLoop::ShmEventLoop(const Configuration *configuration)
157 : EventLoop(configuration),
Austin Schuhe84c3ed2019-12-14 15:29:48 -0800158 name_(FLAGS_application_name),
Austin Schuh15649d62019-12-28 16:36:38 -0800159 node_(MaybeMyNode(configuration)) {
160 if (configuration->has_nodes()) {
161 CHECK(node_ != nullptr) << ": Couldn't find node in config.";
162 }
163}
Austin Schuh217a9782019-12-21 23:02:50 -0800164
Austin Schuh39788ff2019-12-01 18:22:57 -0800165namespace internal {
166
167class SimpleShmFetcher {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700168 public:
Austin Schuhaa79e4e2019-12-29 20:43:32 -0800169 explicit SimpleShmFetcher(EventLoop *event_loop, const Channel *channel)
Austin Schuhf5652592019-12-29 16:26:15 -0800170 : channel_(channel),
Austin Schuhaa79e4e2019-12-29 20:43:32 -0800171 lockless_queue_memory_(
172 channel,
Brian Silverman587da252020-01-01 17:00:47 -0800173 chrono::ceil<chrono::seconds>(chrono::nanoseconds(
Austin Schuhaa79e4e2019-12-29 20:43:32 -0800174 event_loop->configuration()->channel_storage_duration()))),
Alex Perrycb7da4b2019-08-28 19:35:56 -0700175 lockless_queue_(lockless_queue_memory_.memory(),
176 lockless_queue_memory_.config()),
Brian Silvermana1652f32020-01-29 20:41:44 -0800177 data_storage_(static_cast<char *>(malloc(channel->max_size() +
178 kChannelDataAlignment - 1)),
Alex Perrycb7da4b2019-08-28 19:35:56 -0700179 &free) {
180 context_.data = nullptr;
181 // Point the queue index at the next index to read starting now. This
182 // makes it such that FetchNext will read the next message sent after
183 // the fetcher is created.
184 PointAtNextQueueIndex();
185 }
186
Austin Schuh39788ff2019-12-01 18:22:57 -0800187 ~SimpleShmFetcher() {}
Alex Perrycb7da4b2019-08-28 19:35:56 -0700188
189 // Points the next message to fetch at the queue index which will be
190 // populated next.
191 void PointAtNextQueueIndex() {
192 actual_queue_index_ = lockless_queue_.LatestQueueIndex();
193 if (!actual_queue_index_.valid()) {
194 // Nothing in the queue. The next element will show up at the 0th
195 // index in the queue.
196 actual_queue_index_ =
197 ipc_lib::QueueIndex::Zero(lockless_queue_.queue_size());
198 } else {
199 actual_queue_index_ = actual_queue_index_.Increment();
200 }
201 }
202
Austin Schuh39788ff2019-12-01 18:22:57 -0800203 bool FetchNext() {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700204 // TODO(austin): Get behind and make sure it dies both here and with
205 // Fetch.
206 ipc_lib::LocklessQueue::ReadResult read_result = lockless_queue_.Read(
Austin Schuhad154822019-12-27 15:45:13 -0800207 actual_queue_index_.index(), &context_.monotonic_event_time,
208 &context_.realtime_event_time, &context_.monotonic_remote_time,
209 &context_.realtime_remote_time, &context_.remote_queue_index,
Brian Silvermana1652f32020-01-29 20:41:44 -0800210 &context_.size, data_storage_start());
Alex Perrycb7da4b2019-08-28 19:35:56 -0700211 if (read_result == ipc_lib::LocklessQueue::ReadResult::GOOD) {
212 context_.queue_index = actual_queue_index_.index();
Austin Schuhad154822019-12-27 15:45:13 -0800213 if (context_.remote_queue_index == 0xffffffffu) {
214 context_.remote_queue_index = context_.queue_index;
215 }
216 if (context_.monotonic_remote_time == aos::monotonic_clock::min_time) {
217 context_.monotonic_remote_time = context_.monotonic_event_time;
218 }
219 if (context_.realtime_remote_time == aos::realtime_clock::min_time) {
220 context_.realtime_remote_time = context_.realtime_event_time;
221 }
Brian Silvermana1652f32020-01-29 20:41:44 -0800222 context_.data = data_storage_start() +
Austin Schuh39788ff2019-12-01 18:22:57 -0800223 lockless_queue_.message_data_size() - context_.size;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700224 actual_queue_index_ = actual_queue_index_.Increment();
225 }
226
227 // Make sure the data wasn't modified while we were reading it. This
228 // can only happen if you are reading the last message *while* it is
229 // being written to, which means you are pretty far behind.
230 CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::OVERWROTE)
231 << ": Got behind while reading and the last message was modified "
Austin Schuhf5652592019-12-29 16:26:15 -0800232 "out from under us while we were reading it. Don't get so far "
233 "behind. "
234 << configuration::CleanedChannelToString(channel_);
Alex Perrycb7da4b2019-08-28 19:35:56 -0700235
236 CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::TOO_OLD)
Austin Schuhf5652592019-12-29 16:26:15 -0800237 << ": The next message is no longer available. "
238 << configuration::CleanedChannelToString(channel_);
Alex Perrycb7da4b2019-08-28 19:35:56 -0700239 return read_result == ipc_lib::LocklessQueue::ReadResult::GOOD;
240 }
241
Austin Schuh39788ff2019-12-01 18:22:57 -0800242 bool Fetch() {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700243 const ipc_lib::QueueIndex queue_index = lockless_queue_.LatestQueueIndex();
244 // actual_queue_index_ is only meaningful if it was set by Fetch or
245 // FetchNext. This happens when valid_data_ has been set. So, only
246 // skip checking if valid_data_ is true.
247 //
248 // Also, if the latest queue index is invalid, we are empty. So there
249 // is nothing to fetch.
Austin Schuh39788ff2019-12-01 18:22:57 -0800250 if ((context_.data != nullptr &&
Alex Perrycb7da4b2019-08-28 19:35:56 -0700251 queue_index == actual_queue_index_.DecrementBy(1u)) ||
252 !queue_index.valid()) {
253 return false;
254 }
255
Austin Schuhad154822019-12-27 15:45:13 -0800256 ipc_lib::LocklessQueue::ReadResult read_result = lockless_queue_.Read(
257 queue_index.index(), &context_.monotonic_event_time,
258 &context_.realtime_event_time, &context_.monotonic_remote_time,
259 &context_.realtime_remote_time, &context_.remote_queue_index,
Brian Silvermana1652f32020-01-29 20:41:44 -0800260 &context_.size, data_storage_start());
Alex Perrycb7da4b2019-08-28 19:35:56 -0700261 if (read_result == ipc_lib::LocklessQueue::ReadResult::GOOD) {
262 context_.queue_index = queue_index.index();
Austin Schuhad154822019-12-27 15:45:13 -0800263 if (context_.remote_queue_index == 0xffffffffu) {
264 context_.remote_queue_index = context_.queue_index;
265 }
266 if (context_.monotonic_remote_time == aos::monotonic_clock::min_time) {
267 context_.monotonic_remote_time = context_.monotonic_event_time;
268 }
269 if (context_.realtime_remote_time == aos::realtime_clock::min_time) {
270 context_.realtime_remote_time = context_.realtime_event_time;
271 }
Brian Silvermana1652f32020-01-29 20:41:44 -0800272 context_.data = data_storage_start() +
Austin Schuh39788ff2019-12-01 18:22:57 -0800273 lockless_queue_.message_data_size() - context_.size;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700274 actual_queue_index_ = queue_index.Increment();
275 }
276
277 // Make sure the data wasn't modified while we were reading it. This
278 // can only happen if you are reading the last message *while* it is
279 // being written to, which means you are pretty far behind.
280 CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::OVERWROTE)
281 << ": Got behind while reading and the last message was modified "
Austin Schuhf5652592019-12-29 16:26:15 -0800282 "out from under us while we were reading it. Don't get so far "
283 "behind."
284 << configuration::CleanedChannelToString(channel_);
Alex Perrycb7da4b2019-08-28 19:35:56 -0700285
286 CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::NOTHING_NEW)
Austin Schuhf5652592019-12-29 16:26:15 -0800287 << ": Queue index went backwards. This should never happen. "
288 << configuration::CleanedChannelToString(channel_);
Alex Perrycb7da4b2019-08-28 19:35:56 -0700289
290 // We fell behind between when we read the index and read the value.
291 // This isn't worth recovering from since this means we went to sleep
292 // for a long time in the middle of this function.
293 CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::TOO_OLD)
Austin Schuhf5652592019-12-29 16:26:15 -0800294 << ": The next message is no longer available. "
295 << configuration::CleanedChannelToString(channel_);
Alex Perrycb7da4b2019-08-28 19:35:56 -0700296 return read_result == ipc_lib::LocklessQueue::ReadResult::GOOD;
297 }
298
Austin Schuh39788ff2019-12-01 18:22:57 -0800299 Context context() const { return context_; }
300
Alex Perrycb7da4b2019-08-28 19:35:56 -0700301 bool RegisterWakeup(int priority) {
302 return lockless_queue_.RegisterWakeup(priority);
303 }
304
305 void UnregisterWakeup() { lockless_queue_.UnregisterWakeup(); }
306
Brian Silverman5120afb2020-01-31 17:44:35 -0800307 absl::Span<char> GetSharedMemory() const {
308 return lockless_queue_memory_.GetSharedMemory();
309 }
310
Alex Perrycb7da4b2019-08-28 19:35:56 -0700311 private:
Brian Silvermana1652f32020-01-29 20:41:44 -0800312 char *data_storage_start() {
313 return RoundChannelData(data_storage_.get(), channel_->max_size());
314 }
315
Austin Schuhf5652592019-12-29 16:26:15 -0800316 const Channel *const channel_;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700317 MMapedQueue lockless_queue_memory_;
318 ipc_lib::LocklessQueue lockless_queue_;
319
320 ipc_lib::QueueIndex actual_queue_index_ =
321 ipc_lib::LocklessQueue::empty_queue_index();
322
Brian Silvermana1652f32020-01-29 20:41:44 -0800323 std::unique_ptr<char, decltype(&free)> data_storage_;
Austin Schuh39788ff2019-12-01 18:22:57 -0800324
325 Context context_;
326};
327
328class ShmFetcher : public RawFetcher {
329 public:
330 explicit ShmFetcher(EventLoop *event_loop, const Channel *channel)
Austin Schuhaa79e4e2019-12-29 20:43:32 -0800331 : RawFetcher(event_loop, channel),
332 simple_shm_fetcher_(event_loop, channel) {}
Austin Schuh39788ff2019-12-01 18:22:57 -0800333
334 ~ShmFetcher() { context_.data = nullptr; }
335
336 std::pair<bool, monotonic_clock::time_point> DoFetchNext() override {
337 if (simple_shm_fetcher_.FetchNext()) {
338 context_ = simple_shm_fetcher_.context();
339 return std::make_pair(true, monotonic_clock::now());
340 }
341 return std::make_pair(false, monotonic_clock::min_time);
342 }
343
344 std::pair<bool, monotonic_clock::time_point> DoFetch() override {
345 if (simple_shm_fetcher_.Fetch()) {
346 context_ = simple_shm_fetcher_.context();
347 return std::make_pair(true, monotonic_clock::now());
348 }
349 return std::make_pair(false, monotonic_clock::min_time);
350 }
351
352 private:
353 SimpleShmFetcher simple_shm_fetcher_;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700354};
355
356class ShmSender : public RawSender {
357 public:
Austin Schuh39788ff2019-12-01 18:22:57 -0800358 explicit ShmSender(EventLoop *event_loop, const Channel *channel)
359 : RawSender(event_loop, channel),
Austin Schuhaa79e4e2019-12-29 20:43:32 -0800360 lockless_queue_memory_(
361 channel,
Brian Silverman587da252020-01-01 17:00:47 -0800362 chrono::ceil<chrono::seconds>(chrono::nanoseconds(
Austin Schuhaa79e4e2019-12-29 20:43:32 -0800363 event_loop->configuration()->channel_storage_duration()))),
Alex Perrycb7da4b2019-08-28 19:35:56 -0700364 lockless_queue_(lockless_queue_memory_.memory(),
365 lockless_queue_memory_.config()),
366 lockless_queue_sender_(lockless_queue_.MakeSender()) {}
367
Austin Schuh39788ff2019-12-01 18:22:57 -0800368 ~ShmSender() override {}
369
Alex Perrycb7da4b2019-08-28 19:35:56 -0700370 void *data() override { return lockless_queue_sender_.Data(); }
371 size_t size() override { return lockless_queue_sender_.size(); }
Austin Schuhad154822019-12-27 15:45:13 -0800372 bool DoSend(size_t length,
373 aos::monotonic_clock::time_point monotonic_remote_time,
374 aos::realtime_clock::time_point realtime_remote_time,
375 uint32_t remote_queue_index) override {
376 lockless_queue_sender_.Send(
377 length, monotonic_remote_time, realtime_remote_time, remote_queue_index,
378 &monotonic_sent_time_, &realtime_sent_time_, &sent_queue_index_);
Austin Schuh39788ff2019-12-01 18:22:57 -0800379 lockless_queue_.Wakeup(event_loop()->priority());
Alex Perrycb7da4b2019-08-28 19:35:56 -0700380 return true;
381 }
382
Austin Schuhad154822019-12-27 15:45:13 -0800383 bool DoSend(const void *msg, size_t length,
384 aos::monotonic_clock::time_point monotonic_remote_time,
385 aos::realtime_clock::time_point realtime_remote_time,
386 uint32_t remote_queue_index) override {
387 lockless_queue_sender_.Send(reinterpret_cast<const char *>(msg), length,
388 monotonic_remote_time, realtime_remote_time,
389 remote_queue_index, &monotonic_sent_time_,
390 &realtime_sent_time_, &sent_queue_index_);
Austin Schuh39788ff2019-12-01 18:22:57 -0800391 lockless_queue_.Wakeup(event_loop()->priority());
Alex Perrycb7da4b2019-08-28 19:35:56 -0700392 // TODO(austin): Return an error if we send too fast.
393 return true;
394 }
395
Brian Silverman5120afb2020-01-31 17:44:35 -0800396 absl::Span<char> GetSharedMemory() const {
397 return lockless_queue_memory_.GetSharedMemory();
398 }
399
Alex Perrycb7da4b2019-08-28 19:35:56 -0700400 private:
Alex Perrycb7da4b2019-08-28 19:35:56 -0700401 MMapedQueue lockless_queue_memory_;
402 ipc_lib::LocklessQueue lockless_queue_;
403 ipc_lib::LocklessQueue::Sender lockless_queue_sender_;
404};
405
Alex Perrycb7da4b2019-08-28 19:35:56 -0700406// Class to manage the state for a Watcher.
Austin Schuh39788ff2019-12-01 18:22:57 -0800407class WatcherState : public aos::WatcherState {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700408 public:
409 WatcherState(
Austin Schuh7d87b672019-12-01 20:23:49 -0800410 ShmEventLoop *event_loop, const Channel *channel,
Austin Schuh39788ff2019-12-01 18:22:57 -0800411 std::function<void(const Context &context, const void *message)> fn)
412 : aos::WatcherState(event_loop, channel, std::move(fn)),
Austin Schuh7d87b672019-12-01 20:23:49 -0800413 event_loop_(event_loop),
414 event_(this),
Austin Schuhaa79e4e2019-12-29 20:43:32 -0800415 simple_shm_fetcher_(event_loop, channel) {}
Alex Perrycb7da4b2019-08-28 19:35:56 -0700416
Austin Schuh7d87b672019-12-01 20:23:49 -0800417 ~WatcherState() override { event_loop_->RemoveEvent(&event_); }
Austin Schuh39788ff2019-12-01 18:22:57 -0800418
419 void Startup(EventLoop *event_loop) override {
Austin Schuh7d87b672019-12-01 20:23:49 -0800420 simple_shm_fetcher_.PointAtNextQueueIndex();
Austin Schuh39788ff2019-12-01 18:22:57 -0800421 CHECK(RegisterWakeup(event_loop->priority()));
422 }
Alex Perrycb7da4b2019-08-28 19:35:56 -0700423
Alex Perrycb7da4b2019-08-28 19:35:56 -0700424 // Returns true if there is new data available.
Austin Schuh7d87b672019-12-01 20:23:49 -0800425 bool CheckForNewData() {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700426 if (!has_new_data_) {
Austin Schuh39788ff2019-12-01 18:22:57 -0800427 has_new_data_ = simple_shm_fetcher_.FetchNext();
Austin Schuh7d87b672019-12-01 20:23:49 -0800428
429 if (has_new_data_) {
430 event_.set_event_time(
Austin Schuhad154822019-12-27 15:45:13 -0800431 simple_shm_fetcher_.context().monotonic_event_time);
Austin Schuh7d87b672019-12-01 20:23:49 -0800432 event_loop_->AddEvent(&event_);
433 }
Alex Perrycb7da4b2019-08-28 19:35:56 -0700434 }
435
436 return has_new_data_;
437 }
438
Alex Perrycb7da4b2019-08-28 19:35:56 -0700439 // Consumes the data by calling the callback.
Austin Schuh7d87b672019-12-01 20:23:49 -0800440 void HandleEvent() {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700441 CHECK(has_new_data_);
Austin Schuh39788ff2019-12-01 18:22:57 -0800442 DoCallCallback(monotonic_clock::now, simple_shm_fetcher_.context());
Alex Perrycb7da4b2019-08-28 19:35:56 -0700443 has_new_data_ = false;
Austin Schuh7d87b672019-12-01 20:23:49 -0800444 CheckForNewData();
Alex Perrycb7da4b2019-08-28 19:35:56 -0700445 }
446
Austin Schuh39788ff2019-12-01 18:22:57 -0800447 // Registers us to receive a signal on event reception.
Alex Perrycb7da4b2019-08-28 19:35:56 -0700448 bool RegisterWakeup(int priority) {
Austin Schuh39788ff2019-12-01 18:22:57 -0800449 return simple_shm_fetcher_.RegisterWakeup(priority);
Alex Perrycb7da4b2019-08-28 19:35:56 -0700450 }
451
Austin Schuh39788ff2019-12-01 18:22:57 -0800452 void UnregisterWakeup() { return simple_shm_fetcher_.UnregisterWakeup(); }
Alex Perrycb7da4b2019-08-28 19:35:56 -0700453
Brian Silverman5120afb2020-01-31 17:44:35 -0800454 absl::Span<char> GetSharedMemory() const {
455 return simple_shm_fetcher_.GetSharedMemory();
456 }
457
Alex Perrycb7da4b2019-08-28 19:35:56 -0700458 private:
459 bool has_new_data_ = false;
460
Austin Schuh7d87b672019-12-01 20:23:49 -0800461 ShmEventLoop *event_loop_;
462 EventHandler<WatcherState> event_;
Austin Schuh39788ff2019-12-01 18:22:57 -0800463 SimpleShmFetcher simple_shm_fetcher_;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700464};
465
466// Adapter class to adapt a timerfd to a TimerHandler.
Austin Schuh7d87b672019-12-01 20:23:49 -0800467class TimerHandlerState final : public TimerHandler {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700468 public:
469 TimerHandlerState(ShmEventLoop *shm_event_loop, ::std::function<void()> fn)
Austin Schuh39788ff2019-12-01 18:22:57 -0800470 : TimerHandler(shm_event_loop, std::move(fn)),
Austin Schuh7d87b672019-12-01 20:23:49 -0800471 shm_event_loop_(shm_event_loop),
472 event_(this) {
473 shm_event_loop_->epoll_.OnReadable(
474 timerfd_.fd(), [this]() { shm_event_loop_->HandleEvent(); });
Alex Perrycb7da4b2019-08-28 19:35:56 -0700475 }
476
Austin Schuh7d87b672019-12-01 20:23:49 -0800477 ~TimerHandlerState() {
478 Disable();
479 shm_event_loop_->epoll_.DeleteFd(timerfd_.fd());
480 }
481
482 void HandleEvent() {
483 uint64_t elapsed_cycles = timerfd_.Read();
484 if (elapsed_cycles == 0u) {
485 // We got called before the timer interrupt could happen, but because we
486 // are checking the time, we got called on time. Push the timer out by 1
487 // cycle.
488 elapsed_cycles = 1u;
489 timerfd_.SetTime(base_ + repeat_offset_, repeat_offset_);
490 }
491
492 Call(monotonic_clock::now, base_);
493
494 base_ += repeat_offset_ * elapsed_cycles;
495
496 if (repeat_offset_ != chrono::seconds(0)) {
497 event_.set_event_time(base_);
498 shm_event_loop_->AddEvent(&event_);
499 }
500 }
Alex Perrycb7da4b2019-08-28 19:35:56 -0700501
502 void Setup(monotonic_clock::time_point base,
503 monotonic_clock::duration repeat_offset) override {
Austin Schuh7d87b672019-12-01 20:23:49 -0800504 if (event_.valid()) {
505 shm_event_loop_->RemoveEvent(&event_);
506 }
507
Alex Perrycb7da4b2019-08-28 19:35:56 -0700508 timerfd_.SetTime(base, repeat_offset);
Austin Schuhde8a8ff2019-11-30 15:25:36 -0800509 base_ = base;
510 repeat_offset_ = repeat_offset;
Austin Schuh7d87b672019-12-01 20:23:49 -0800511 event_.set_event_time(base_);
512 shm_event_loop_->AddEvent(&event_);
Alex Perrycb7da4b2019-08-28 19:35:56 -0700513 }
514
Austin Schuh7d87b672019-12-01 20:23:49 -0800515 void Disable() override {
516 shm_event_loop_->RemoveEvent(&event_);
517 timerfd_.Disable();
518 }
Alex Perrycb7da4b2019-08-28 19:35:56 -0700519
520 private:
521 ShmEventLoop *shm_event_loop_;
Austin Schuh7d87b672019-12-01 20:23:49 -0800522 EventHandler<TimerHandlerState> event_;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700523
524 TimerFd timerfd_;
525
Austin Schuhde8a8ff2019-11-30 15:25:36 -0800526 monotonic_clock::time_point base_;
527 monotonic_clock::duration repeat_offset_;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700528};
529
530// Adapter class to the timerfd and PhasedLoop.
Austin Schuh7d87b672019-12-01 20:23:49 -0800531class PhasedLoopHandler final : public ::aos::PhasedLoopHandler {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700532 public:
533 PhasedLoopHandler(ShmEventLoop *shm_event_loop, ::std::function<void(int)> fn,
534 const monotonic_clock::duration interval,
535 const monotonic_clock::duration offset)
Austin Schuh39788ff2019-12-01 18:22:57 -0800536 : aos::PhasedLoopHandler(shm_event_loop, std::move(fn), interval, offset),
Austin Schuh7d87b672019-12-01 20:23:49 -0800537 shm_event_loop_(shm_event_loop),
538 event_(this) {
539 shm_event_loop_->epoll_.OnReadable(
540 timerfd_.fd(), [this]() { shm_event_loop_->HandleEvent(); });
541 }
542
543 void HandleEvent() {
544 // The return value for read is the number of cycles that have elapsed.
545 // Because we check to see when this event *should* have happened, there are
546 // cases where Read() will return 0, when 1 cycle has actually happened.
547 // This occurs when the timer interrupt hasn't triggered yet. Therefore,
548 // ignore it. Call handles rescheduling and calculating elapsed cycles
549 // without any extra help.
550 timerfd_.Read();
551 event_.Invalidate();
552
553 Call(monotonic_clock::now, [this](monotonic_clock::time_point sleep_time) {
554 Schedule(sleep_time);
Alex Perrycb7da4b2019-08-28 19:35:56 -0700555 });
556 }
557
Austin Schuh39788ff2019-12-01 18:22:57 -0800558 ~PhasedLoopHandler() override {
559 shm_event_loop_->epoll_.DeleteFd(timerfd_.fd());
Austin Schuh7d87b672019-12-01 20:23:49 -0800560 shm_event_loop_->RemoveEvent(&event_);
Alex Perrycb7da4b2019-08-28 19:35:56 -0700561 }
562
563 private:
Austin Schuhde8a8ff2019-11-30 15:25:36 -0800564 // Reschedules the timer.
Austin Schuh39788ff2019-12-01 18:22:57 -0800565 void Schedule(monotonic_clock::time_point sleep_time) override {
Austin Schuh7d87b672019-12-01 20:23:49 -0800566 if (event_.valid()) {
567 shm_event_loop_->RemoveEvent(&event_);
568 }
569
Austin Schuh39788ff2019-12-01 18:22:57 -0800570 timerfd_.SetTime(sleep_time, ::aos::monotonic_clock::zero());
Austin Schuh7d87b672019-12-01 20:23:49 -0800571 event_.set_event_time(sleep_time);
572 shm_event_loop_->AddEvent(&event_);
Alex Perrycb7da4b2019-08-28 19:35:56 -0700573 }
574
575 ShmEventLoop *shm_event_loop_;
Austin Schuh7d87b672019-12-01 20:23:49 -0800576 EventHandler<PhasedLoopHandler> event_;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700577
578 TimerFd timerfd_;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700579};
580} // namespace internal
581
582::std::unique_ptr<RawFetcher> ShmEventLoop::MakeRawFetcher(
583 const Channel *channel) {
Austin Schuhca4828c2019-12-28 14:21:35 -0800584 if (!configuration::ChannelIsReadableOnNode(channel, node())) {
585 LOG(FATAL) << "Channel { \"name\": \"" << channel->name()->string_view()
586 << "\", \"type\": \"" << channel->type()->string_view()
587 << "\" } is not able to be fetched on this node. Check your "
588 "configuration.";
Austin Schuh217a9782019-12-21 23:02:50 -0800589 }
590
Austin Schuh39788ff2019-12-01 18:22:57 -0800591 return ::std::unique_ptr<RawFetcher>(new internal::ShmFetcher(this, channel));
Alex Perrycb7da4b2019-08-28 19:35:56 -0700592}
593
594::std::unique_ptr<RawSender> ShmEventLoop::MakeRawSender(
595 const Channel *channel) {
Brian Silverman0fc69932020-01-24 21:54:02 -0800596 TakeSender(channel);
Austin Schuh39788ff2019-12-01 18:22:57 -0800597
598 return ::std::unique_ptr<RawSender>(new internal::ShmSender(this, channel));
Alex Perrycb7da4b2019-08-28 19:35:56 -0700599}
600
601void ShmEventLoop::MakeRawWatcher(
602 const Channel *channel,
603 std::function<void(const Context &context, const void *message)> watcher) {
Brian Silverman0fc69932020-01-24 21:54:02 -0800604 TakeWatcher(channel);
Austin Schuh217a9782019-12-21 23:02:50 -0800605
Austin Schuh39788ff2019-12-01 18:22:57 -0800606 NewWatcher(::std::unique_ptr<WatcherState>(
607 new internal::WatcherState(this, channel, std::move(watcher))));
Alex Perrycb7da4b2019-08-28 19:35:56 -0700608}
609
610TimerHandler *ShmEventLoop::AddTimer(::std::function<void()> callback) {
Austin Schuh39788ff2019-12-01 18:22:57 -0800611 return NewTimer(::std::unique_ptr<TimerHandler>(
612 new internal::TimerHandlerState(this, ::std::move(callback))));
Alex Perrycb7da4b2019-08-28 19:35:56 -0700613}
614
615PhasedLoopHandler *ShmEventLoop::AddPhasedLoop(
616 ::std::function<void(int)> callback,
617 const monotonic_clock::duration interval,
618 const monotonic_clock::duration offset) {
Austin Schuh39788ff2019-12-01 18:22:57 -0800619 return NewPhasedLoop(
620 ::std::unique_ptr<PhasedLoopHandler>(new internal::PhasedLoopHandler(
621 this, ::std::move(callback), interval, offset)));
Alex Perrycb7da4b2019-08-28 19:35:56 -0700622}
623
624void ShmEventLoop::OnRun(::std::function<void()> on_run) {
625 on_run_.push_back(::std::move(on_run));
626}
627
Austin Schuh7d87b672019-12-01 20:23:49 -0800628void ShmEventLoop::HandleEvent() {
629 // Update all the times for handlers.
630 for (::std::unique_ptr<WatcherState> &base_watcher : watchers_) {
631 internal::WatcherState *watcher =
632 reinterpret_cast<internal::WatcherState *>(base_watcher.get());
633
634 watcher->CheckForNewData();
635 }
636
Austin Schuh39788ff2019-12-01 18:22:57 -0800637 while (true) {
Austin Schuh7d87b672019-12-01 20:23:49 -0800638 if (EventCount() == 0 ||
639 PeekEvent()->event_time() > monotonic_clock::now()) {
Austin Schuh39788ff2019-12-01 18:22:57 -0800640 break;
641 }
642
Austin Schuh7d87b672019-12-01 20:23:49 -0800643 EventLoopEvent *event = PopEvent();
644 event->HandleEvent();
Austin Schuh39788ff2019-12-01 18:22:57 -0800645 }
646}
647
Austin Schuh32fd5a72019-12-01 22:20:26 -0800648// RAII class to mask signals.
649class ScopedSignalMask {
650 public:
651 ScopedSignalMask(std::initializer_list<int> signals) {
652 sigset_t sigset;
653 PCHECK(sigemptyset(&sigset) == 0);
654 for (int signal : signals) {
655 PCHECK(sigaddset(&sigset, signal) == 0);
656 }
657
658 PCHECK(sigprocmask(SIG_BLOCK, &sigset, &old_) == 0);
659 }
660
661 ~ScopedSignalMask() { PCHECK(sigprocmask(SIG_SETMASK, &old_, nullptr) == 0); }
662
663 private:
664 sigset_t old_;
665};
666
667// Class to manage the static state associated with killing multiple event
668// loops.
669class SignalHandler {
670 public:
671 // Gets the singleton.
672 static SignalHandler *global() {
673 static SignalHandler loop;
674 return &loop;
675 }
676
677 // Handles the signal with the singleton.
678 static void HandleSignal(int) { global()->DoHandleSignal(); }
679
680 // Registers an event loop to receive Exit() calls.
681 void Register(ShmEventLoop *event_loop) {
682 // Block signals while we have the mutex so we never race with the signal
683 // handler.
684 ScopedSignalMask mask({SIGINT, SIGHUP, SIGTERM});
685 std::unique_lock<stl_mutex> locker(mutex_);
686 if (event_loops_.size() == 0) {
687 // The first caller registers the signal handler.
688 struct sigaction new_action;
689 sigemptyset(&new_action.sa_mask);
690 // This makes it so that 2 control c's to a stuck process will kill it by
691 // restoring the original signal handler.
692 new_action.sa_flags = SA_RESETHAND;
693 new_action.sa_handler = &HandleSignal;
694
695 PCHECK(sigaction(SIGINT, &new_action, &old_action_int_) == 0);
696 PCHECK(sigaction(SIGHUP, &new_action, &old_action_hup_) == 0);
697 PCHECK(sigaction(SIGTERM, &new_action, &old_action_term_) == 0);
698 }
699
700 event_loops_.push_back(event_loop);
701 }
702
703 // Unregisters an event loop to receive Exit() calls.
704 void Unregister(ShmEventLoop *event_loop) {
705 // Block signals while we have the mutex so we never race with the signal
706 // handler.
707 ScopedSignalMask mask({SIGINT, SIGHUP, SIGTERM});
708 std::unique_lock<stl_mutex> locker(mutex_);
709
Brian Silverman5120afb2020-01-31 17:44:35 -0800710 event_loops_.erase(
711 std::find(event_loops_.begin(), event_loops_.end(), event_loop));
Austin Schuh32fd5a72019-12-01 22:20:26 -0800712
713 if (event_loops_.size() == 0u) {
714 // The last caller restores the original signal handlers.
715 PCHECK(sigaction(SIGINT, &old_action_int_, nullptr) == 0);
716 PCHECK(sigaction(SIGHUP, &old_action_hup_, nullptr) == 0);
717 PCHECK(sigaction(SIGTERM, &old_action_term_, nullptr) == 0);
718 }
719 }
720
721 private:
722 void DoHandleSignal() {
723 // We block signals while grabbing the lock, so there should never be a
724 // race. Confirm that this is true using trylock.
725 CHECK(mutex_.try_lock()) << ": sigprocmask failed to block signals while "
726 "modifing the event loop list.";
727 for (ShmEventLoop *event_loop : event_loops_) {
728 event_loop->Exit();
729 }
730 mutex_.unlock();
731 }
732
733 // Mutex to protect all state.
734 stl_mutex mutex_;
735 std::vector<ShmEventLoop *> event_loops_;
736 struct sigaction old_action_int_;
737 struct sigaction old_action_hup_;
738 struct sigaction old_action_term_;
739};
740
Alex Perrycb7da4b2019-08-28 19:35:56 -0700741void ShmEventLoop::Run() {
Austin Schuh32fd5a72019-12-01 22:20:26 -0800742 SignalHandler::global()->Register(this);
Austin Schuh39788ff2019-12-01 18:22:57 -0800743
Alex Perrycb7da4b2019-08-28 19:35:56 -0700744 std::unique_ptr<ipc_lib::SignalFd> signalfd;
745
746 if (watchers_.size() > 0) {
747 signalfd.reset(new ipc_lib::SignalFd({ipc_lib::kWakeupSignal}));
748
749 epoll_.OnReadable(signalfd->fd(), [signalfd_ptr = signalfd.get(), this]() {
750 signalfd_siginfo result = signalfd_ptr->Read();
751 CHECK_EQ(result.ssi_signo, ipc_lib::kWakeupSignal);
752
753 // TODO(austin): We should really be checking *everything*, not just
754 // watchers, and calling the oldest thing first. That will improve
755 // determinism a lot.
756
Austin Schuh7d87b672019-12-01 20:23:49 -0800757 HandleEvent();
Alex Perrycb7da4b2019-08-28 19:35:56 -0700758 });
759 }
760
Austin Schuh39788ff2019-12-01 18:22:57 -0800761 MaybeScheduleTimingReports();
762
Austin Schuh7d87b672019-12-01 20:23:49 -0800763 ReserveEvents();
764
Tyler Chatow67ddb032020-01-12 14:30:04 -0800765 {
766 AosLogToFbs aos_logger;
767 if (!skip_logger_) {
768 aos_logger.Initialize(MakeSender<logging::LogMessageFbs>("/aos"));
769 }
Alex Perrycb7da4b2019-08-28 19:35:56 -0700770
Tyler Chatow67ddb032020-01-12 14:30:04 -0800771 aos::SetCurrentThreadName(name_.substr(0, 16));
772 // Now, all the callbacks are setup. Lock everything into memory and go RT.
773 if (priority_ != 0) {
774 ::aos::InitRT();
775
776 LOG(INFO) << "Setting priority to " << priority_;
777 ::aos::SetCurrentThreadRealtimePriority(priority_);
778 }
779
780 set_is_running(true);
781
782 // Now that we are realtime (but before the OnRun handlers run), snap the
783 // queue index.
784 for (::std::unique_ptr<WatcherState> &watcher : watchers_) {
785 watcher->Startup(this);
786 }
787
788 // Now that we are RT, run all the OnRun handlers.
789 for (const auto &run : on_run_) {
790 run();
791 }
792
793 // And start our main event loop which runs all the timers and handles Quit.
794 epoll_.Run();
795
796 // Once epoll exits, there is no useful nonrt work left to do.
797 set_is_running(false);
798
799 // Nothing time or synchronization critical needs to happen after this
800 // point. Drop RT priority.
801 ::aos::UnsetCurrentThreadRealtimePriority();
Alex Perrycb7da4b2019-08-28 19:35:56 -0700802 }
803
Austin Schuh39788ff2019-12-01 18:22:57 -0800804 for (::std::unique_ptr<WatcherState> &base_watcher : watchers_) {
805 internal::WatcherState *watcher =
806 reinterpret_cast<internal::WatcherState *>(base_watcher.get());
Alex Perrycb7da4b2019-08-28 19:35:56 -0700807 watcher->UnregisterWakeup();
808 }
809
810 if (watchers_.size() > 0) {
811 epoll_.DeleteFd(signalfd->fd());
812 signalfd.reset();
813 }
Austin Schuh32fd5a72019-12-01 22:20:26 -0800814
815 SignalHandler::global()->Unregister(this);
Austin Schuhe84c3ed2019-12-14 15:29:48 -0800816
817 // Trigger any remaining senders or fetchers to be cleared before destroying
818 // the event loop so the book keeping matches. Do this in the thread that
819 // created the timing reporter.
820 timing_report_sender_.reset();
Alex Perrycb7da4b2019-08-28 19:35:56 -0700821}
822
823void ShmEventLoop::Exit() { epoll_.Quit(); }
824
825ShmEventLoop::~ShmEventLoop() {
Austin Schuh39788ff2019-12-01 18:22:57 -0800826 // Force everything with a registered fd with epoll to be destroyed now.
827 timers_.clear();
828 phased_loops_.clear();
829 watchers_.clear();
830
Alex Perrycb7da4b2019-08-28 19:35:56 -0700831 CHECK(!is_running()) << ": ShmEventLoop destroyed while running";
832}
833
Alex Perrycb7da4b2019-08-28 19:35:56 -0700834void ShmEventLoop::SetRuntimeRealtimePriority(int priority) {
835 if (is_running()) {
836 LOG(FATAL) << "Cannot set realtime priority while running.";
837 }
838 priority_ = priority;
839}
840
James Kuszmaul57c2baa2020-01-19 14:52:52 -0800841void ShmEventLoop::set_name(const std::string_view name) {
842 name_ = std::string(name);
843 UpdateTimingReport();
844}
845
Brian Silverman5120afb2020-01-31 17:44:35 -0800846absl::Span<char> ShmEventLoop::GetWatcherSharedMemory(const Channel *channel) {
847 internal::WatcherState *const watcher_state =
848 static_cast<internal::WatcherState *>(GetWatcherState(channel));
849 return watcher_state->GetSharedMemory();
850}
851
852absl::Span<char> ShmEventLoop::GetShmSenderSharedMemory(
853 const aos::RawSender *sender) const {
854 return static_cast<const internal::ShmSender *>(sender)->GetSharedMemory();
855}
856
Austin Schuh39788ff2019-12-01 18:22:57 -0800857pid_t ShmEventLoop::GetTid() { return syscall(SYS_gettid); }
858
Alex Perrycb7da4b2019-08-28 19:35:56 -0700859} // namespace aos