blob: f5feea9a5765e465df0cd607f3651450074673a7 [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>
8#include <algorithm>
9#include <atomic>
10#include <chrono>
Austin Schuh39788ff2019-12-01 18:22:57 -080011#include <iterator>
Alex Perrycb7da4b2019-08-28 19:35:56 -070012#include <stdexcept>
13
14#include "aos/events/epoll.h"
Austin Schuh39788ff2019-12-01 18:22:57 -080015#include "aos/events/event_loop_generated.h"
16#include "aos/events/timing_statistics.h"
Alex Perrycb7da4b2019-08-28 19:35:56 -070017#include "aos/ipc_lib/lockless_queue.h"
Austin Schuh39788ff2019-12-01 18:22:57 -080018#include "aos/ipc_lib/signalfd.h"
Alex Perrycb7da4b2019-08-28 19:35:56 -070019#include "aos/realtime.h"
Austin Schuh32fd5a72019-12-01 22:20:26 -080020#include "aos/stl_mutex/stl_mutex.h"
Alex Perrycb7da4b2019-08-28 19:35:56 -070021#include "aos/util/phased_loop.h"
Austin Schuh39788ff2019-12-01 18:22:57 -080022#include "glog/logging.h"
Alex Perrycb7da4b2019-08-28 19:35:56 -070023
24DEFINE_string(shm_base, "/dev/shm/aos",
25 "Directory to place queue backing mmaped files in.");
26DEFINE_uint32(permissions, 0770,
27 "Permissions to make shared memory files and folders.");
28
29namespace aos {
30
31std::string ShmFolder(const Channel *channel) {
32 CHECK(channel->has_name());
33 CHECK_EQ(channel->name()->string_view()[0], '/');
34 return FLAGS_shm_base + channel->name()->str() + "/";
35}
36std::string ShmPath(const Channel *channel) {
37 CHECK(channel->has_type());
38 return ShmFolder(channel) + channel->type()->str() + ".v0";
39}
40
41class MMapedQueue {
42 public:
43 MMapedQueue(const Channel *channel) {
44 std::string path = ShmPath(channel);
45
Austin Schuh80c7fce2019-12-05 20:48:43 -080046 config_.num_watchers = channel->num_watchers();
47 config_.num_senders = channel->num_senders();
Alex Perrycb7da4b2019-08-28 19:35:56 -070048 config_.queue_size = 2 * channel->frequency();
49 config_.message_data_size = channel->max_size();
50
51 size_ = ipc_lib::LocklessQueueMemorySize(config_);
52
53 MkdirP(path);
54
55 // There are 2 cases. Either the file already exists, or it does not
56 // already exist and we need to create it. Start by trying to create it. If
57 // that fails, the file has already been created and we can open it
58 // normally.. Once the file has been created it wil never be deleted.
59 fd_ = open(path.c_str(), O_RDWR | O_CREAT | O_EXCL,
60 O_CLOEXEC | FLAGS_permissions);
61 if (fd_ == -1 && errno == EEXIST) {
62 VLOG(1) << path << " already created.";
63 // File already exists.
64 fd_ = open(path.c_str(), O_RDWR, O_CLOEXEC);
65 PCHECK(fd_ != -1) << ": Failed to open " << path;
66 while (true) {
67 struct stat st;
68 PCHECK(fstat(fd_, &st) == 0);
69 if (st.st_size != 0) {
70 CHECK_EQ(static_cast<size_t>(st.st_size), size_)
71 << ": Size of " << path
72 << " doesn't match expected size of backing queue file. Did the "
73 "queue definition change?";
74 break;
75 } else {
76 // The creating process didn't get around to it yet. Give it a bit.
77 std::this_thread::sleep_for(std::chrono::milliseconds(10));
78 VLOG(1) << path << " is zero size, waiting";
79 }
80 }
81 } else {
82 VLOG(1) << "Created " << path;
83 PCHECK(fd_ != -1) << ": Failed to open " << path;
84 PCHECK(ftruncate(fd_, size_) == 0);
85 }
86
87 data_ = mmap(NULL, size_, PROT_READ | PROT_WRITE, MAP_SHARED, fd_, 0);
88 PCHECK(data_ != MAP_FAILED);
89
90 ipc_lib::InitializeLocklessQueueMemory(memory(), config_);
91 }
92
93 ~MMapedQueue() {
94 PCHECK(munmap(data_, size_) == 0);
95 PCHECK(close(fd_) == 0);
96 }
97
98 ipc_lib::LocklessQueueMemory *memory() const {
99 return reinterpret_cast<ipc_lib::LocklessQueueMemory *>(data_);
100 }
101
Austin Schuh39788ff2019-12-01 18:22:57 -0800102 const ipc_lib::LocklessQueueConfiguration &config() const { return config_; }
Alex Perrycb7da4b2019-08-28 19:35:56 -0700103
104 private:
James Kuszmaul3ae42262019-11-08 12:33:41 -0800105 void MkdirP(std::string_view path) {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700106 struct stat st;
107 auto last_slash_pos = path.find_last_of("/");
108
James Kuszmaul3ae42262019-11-08 12:33:41 -0800109 std::string folder(last_slash_pos == std::string_view::npos
110 ? std::string_view("")
Alex Perrycb7da4b2019-08-28 19:35:56 -0700111 : path.substr(0, last_slash_pos));
112 if (stat(folder.c_str(), &st) == -1) {
113 PCHECK(errno == ENOENT);
114 CHECK_NE(folder, "") << ": Base path doesn't exist";
115 MkdirP(folder);
116 VLOG(1) << "Creating " << folder;
117 PCHECK(mkdir(folder.c_str(), FLAGS_permissions) == 0);
118 }
119 }
120
121 ipc_lib::LocklessQueueConfiguration config_;
122
123 int fd_;
124
125 size_t size_;
126 void *data_;
127};
128
129// Returns the portion of the path after the last /.
James Kuszmaul3ae42262019-11-08 12:33:41 -0800130std::string_view Filename(std::string_view path) {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700131 auto last_slash_pos = path.find_last_of("/");
132
James Kuszmaul3ae42262019-11-08 12:33:41 -0800133 return last_slash_pos == std::string_view::npos
Alex Perrycb7da4b2019-08-28 19:35:56 -0700134 ? path
135 : path.substr(last_slash_pos + 1, path.size());
136}
137
138ShmEventLoop::ShmEventLoop(const Configuration *configuration)
139 : EventLoop(configuration), name_(Filename(program_invocation_name)) {}
140
141namespace {
142
143namespace chrono = ::std::chrono;
144
Austin Schuh39788ff2019-12-01 18:22:57 -0800145} // namespace
146
147namespace internal {
148
149class SimpleShmFetcher {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700150 public:
Austin Schuh39788ff2019-12-01 18:22:57 -0800151 explicit SimpleShmFetcher(const Channel *channel)
152 : lockless_queue_memory_(channel),
Alex Perrycb7da4b2019-08-28 19:35:56 -0700153 lockless_queue_(lockless_queue_memory_.memory(),
154 lockless_queue_memory_.config()),
155 data_storage_(static_cast<AlignedChar *>(aligned_alloc(
156 alignof(AlignedChar), channel->max_size())),
157 &free) {
158 context_.data = nullptr;
159 // Point the queue index at the next index to read starting now. This
160 // makes it such that FetchNext will read the next message sent after
161 // the fetcher is created.
162 PointAtNextQueueIndex();
163 }
164
Austin Schuh39788ff2019-12-01 18:22:57 -0800165 ~SimpleShmFetcher() {}
Alex Perrycb7da4b2019-08-28 19:35:56 -0700166
167 // Points the next message to fetch at the queue index which will be
168 // populated next.
169 void PointAtNextQueueIndex() {
170 actual_queue_index_ = lockless_queue_.LatestQueueIndex();
171 if (!actual_queue_index_.valid()) {
172 // Nothing in the queue. The next element will show up at the 0th
173 // index in the queue.
174 actual_queue_index_ =
175 ipc_lib::QueueIndex::Zero(lockless_queue_.queue_size());
176 } else {
177 actual_queue_index_ = actual_queue_index_.Increment();
178 }
179 }
180
Austin Schuh39788ff2019-12-01 18:22:57 -0800181 bool FetchNext() {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700182 // TODO(austin): Write a test which starts with nothing in the queue,
183 // and then calls FetchNext() after something is sent.
184 // TODO(austin): Get behind and make sure it dies both here and with
185 // Fetch.
186 ipc_lib::LocklessQueue::ReadResult read_result = lockless_queue_.Read(
187 actual_queue_index_.index(), &context_.monotonic_sent_time,
188 &context_.realtime_sent_time, &context_.size,
189 reinterpret_cast<char *>(data_storage_.get()));
190 if (read_result == ipc_lib::LocklessQueue::ReadResult::GOOD) {
191 context_.queue_index = actual_queue_index_.index();
Austin Schuh39788ff2019-12-01 18:22:57 -0800192 context_.data = reinterpret_cast<char *>(data_storage_.get()) +
193 lockless_queue_.message_data_size() - context_.size;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700194 actual_queue_index_ = actual_queue_index_.Increment();
195 }
196
197 // Make sure the data wasn't modified while we were reading it. This
198 // can only happen if you are reading the last message *while* it is
199 // being written to, which means you are pretty far behind.
200 CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::OVERWROTE)
201 << ": Got behind while reading and the last message was modified "
202 "out "
203 "from under us while we were reading it. Don't get so far "
204 "behind.";
205
206 CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::TOO_OLD)
207 << ": The next message is no longer available.";
208 return read_result == ipc_lib::LocklessQueue::ReadResult::GOOD;
209 }
210
Austin Schuh39788ff2019-12-01 18:22:57 -0800211 bool Fetch() {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700212 const ipc_lib::QueueIndex queue_index = lockless_queue_.LatestQueueIndex();
213 // actual_queue_index_ is only meaningful if it was set by Fetch or
214 // FetchNext. This happens when valid_data_ has been set. So, only
215 // skip checking if valid_data_ is true.
216 //
217 // Also, if the latest queue index is invalid, we are empty. So there
218 // is nothing to fetch.
Austin Schuh39788ff2019-12-01 18:22:57 -0800219 if ((context_.data != nullptr &&
Alex Perrycb7da4b2019-08-28 19:35:56 -0700220 queue_index == actual_queue_index_.DecrementBy(1u)) ||
221 !queue_index.valid()) {
222 return false;
223 }
224
225 ipc_lib::LocklessQueue::ReadResult read_result =
226 lockless_queue_.Read(queue_index.index(), &context_.monotonic_sent_time,
227 &context_.realtime_sent_time, &context_.size,
228 reinterpret_cast<char *>(data_storage_.get()));
229 if (read_result == ipc_lib::LocklessQueue::ReadResult::GOOD) {
230 context_.queue_index = queue_index.index();
Austin Schuh39788ff2019-12-01 18:22:57 -0800231 context_.data = reinterpret_cast<char *>(data_storage_.get()) +
232 lockless_queue_.message_data_size() - context_.size;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700233 actual_queue_index_ = queue_index.Increment();
234 }
235
236 // Make sure the data wasn't modified while we were reading it. This
237 // can only happen if you are reading the last message *while* it is
238 // being written to, which means you are pretty far behind.
239 CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::OVERWROTE)
240 << ": Got behind while reading and the last message was modified "
241 "out "
242 "from under us while we were reading it. Don't get so far "
243 "behind.";
244
245 CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::NOTHING_NEW)
246 << ": Queue index went backwards. This should never happen.";
247
248 // We fell behind between when we read the index and read the value.
249 // This isn't worth recovering from since this means we went to sleep
250 // for a long time in the middle of this function.
251 CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::TOO_OLD)
252 << ": The next message is no longer available.";
253 return read_result == ipc_lib::LocklessQueue::ReadResult::GOOD;
254 }
255
Austin Schuh39788ff2019-12-01 18:22:57 -0800256 Context context() const { return context_; }
257
Alex Perrycb7da4b2019-08-28 19:35:56 -0700258 bool RegisterWakeup(int priority) {
259 return lockless_queue_.RegisterWakeup(priority);
260 }
261
262 void UnregisterWakeup() { lockless_queue_.UnregisterWakeup(); }
263
264 private:
265 MMapedQueue lockless_queue_memory_;
266 ipc_lib::LocklessQueue lockless_queue_;
267
268 ipc_lib::QueueIndex actual_queue_index_ =
269 ipc_lib::LocklessQueue::empty_queue_index();
270
271 struct AlignedChar {
272 alignas(32) char data;
273 };
274
275 std::unique_ptr<AlignedChar, decltype(&free)> data_storage_;
Austin Schuh39788ff2019-12-01 18:22:57 -0800276
277 Context context_;
278};
279
280class ShmFetcher : public RawFetcher {
281 public:
282 explicit ShmFetcher(EventLoop *event_loop, const Channel *channel)
283 : RawFetcher(event_loop, channel), simple_shm_fetcher_(channel) {}
284
285 ~ShmFetcher() { context_.data = nullptr; }
286
287 std::pair<bool, monotonic_clock::time_point> DoFetchNext() override {
288 if (simple_shm_fetcher_.FetchNext()) {
289 context_ = simple_shm_fetcher_.context();
290 return std::make_pair(true, monotonic_clock::now());
291 }
292 return std::make_pair(false, monotonic_clock::min_time);
293 }
294
295 std::pair<bool, monotonic_clock::time_point> DoFetch() override {
296 if (simple_shm_fetcher_.Fetch()) {
297 context_ = simple_shm_fetcher_.context();
298 return std::make_pair(true, monotonic_clock::now());
299 }
300 return std::make_pair(false, monotonic_clock::min_time);
301 }
302
303 private:
304 SimpleShmFetcher simple_shm_fetcher_;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700305};
306
307class ShmSender : public RawSender {
308 public:
Austin Schuh39788ff2019-12-01 18:22:57 -0800309 explicit ShmSender(EventLoop *event_loop, const Channel *channel)
310 : RawSender(event_loop, channel),
Alex Perrycb7da4b2019-08-28 19:35:56 -0700311 lockless_queue_memory_(channel),
312 lockless_queue_(lockless_queue_memory_.memory(),
313 lockless_queue_memory_.config()),
314 lockless_queue_sender_(lockless_queue_.MakeSender()) {}
315
Austin Schuh39788ff2019-12-01 18:22:57 -0800316 ~ShmSender() override {}
317
Alex Perrycb7da4b2019-08-28 19:35:56 -0700318 void *data() override { return lockless_queue_sender_.Data(); }
319 size_t size() override { return lockless_queue_sender_.size(); }
Austin Schuh39788ff2019-12-01 18:22:57 -0800320 bool DoSend(size_t length) override {
321 lockless_queue_sender_.Send(length);
322 lockless_queue_.Wakeup(event_loop()->priority());
Alex Perrycb7da4b2019-08-28 19:35:56 -0700323 return true;
324 }
325
Austin Schuh39788ff2019-12-01 18:22:57 -0800326 bool DoSend(const void *msg, size_t length) override {
Austin Schuh4726ce92019-11-29 13:23:18 -0800327 lockless_queue_sender_.Send(reinterpret_cast<const char *>(msg), length);
Austin Schuh39788ff2019-12-01 18:22:57 -0800328 lockless_queue_.Wakeup(event_loop()->priority());
Alex Perrycb7da4b2019-08-28 19:35:56 -0700329 // TODO(austin): Return an error if we send too fast.
330 return true;
331 }
332
Alex Perrycb7da4b2019-08-28 19:35:56 -0700333 private:
Alex Perrycb7da4b2019-08-28 19:35:56 -0700334 MMapedQueue lockless_queue_memory_;
335 ipc_lib::LocklessQueue lockless_queue_;
336 ipc_lib::LocklessQueue::Sender lockless_queue_sender_;
337};
338
Alex Perrycb7da4b2019-08-28 19:35:56 -0700339// Class to manage the state for a Watcher.
Austin Schuh39788ff2019-12-01 18:22:57 -0800340class WatcherState : public aos::WatcherState {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700341 public:
342 WatcherState(
Austin Schuh39788ff2019-12-01 18:22:57 -0800343 EventLoop *event_loop, const Channel *channel,
344 std::function<void(const Context &context, const void *message)> fn)
345 : aos::WatcherState(event_loop, channel, std::move(fn)),
346 simple_shm_fetcher_(channel) {}
Alex Perrycb7da4b2019-08-28 19:35:56 -0700347
Austin Schuh39788ff2019-12-01 18:22:57 -0800348 ~WatcherState() override {}
349
350 void Startup(EventLoop *event_loop) override {
351 PointAtNextQueueIndex();
352 CHECK(RegisterWakeup(event_loop->priority()));
353 }
Alex Perrycb7da4b2019-08-28 19:35:56 -0700354
355 // Points the next message to fetch at the queue index which will be populated
356 // next.
Austin Schuh39788ff2019-12-01 18:22:57 -0800357 void PointAtNextQueueIndex() { simple_shm_fetcher_.PointAtNextQueueIndex(); }
Alex Perrycb7da4b2019-08-28 19:35:56 -0700358
359 // Returns true if there is new data available.
360 bool HasNewData() {
361 if (!has_new_data_) {
Austin Schuh39788ff2019-12-01 18:22:57 -0800362 has_new_data_ = simple_shm_fetcher_.FetchNext();
Alex Perrycb7da4b2019-08-28 19:35:56 -0700363 }
364
365 return has_new_data_;
366 }
367
368 // Returns the time of the current data sample.
369 aos::monotonic_clock::time_point event_time() const {
Austin Schuh39788ff2019-12-01 18:22:57 -0800370 return simple_shm_fetcher_.context().monotonic_sent_time;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700371 }
372
373 // Consumes the data by calling the callback.
374 void CallCallback() {
375 CHECK(has_new_data_);
Austin Schuh39788ff2019-12-01 18:22:57 -0800376 DoCallCallback(monotonic_clock::now, simple_shm_fetcher_.context());
Alex Perrycb7da4b2019-08-28 19:35:56 -0700377 has_new_data_ = false;
378 }
379
Austin Schuh39788ff2019-12-01 18:22:57 -0800380 // Registers us to receive a signal on event reception.
Alex Perrycb7da4b2019-08-28 19:35:56 -0700381 bool RegisterWakeup(int priority) {
Austin Schuh39788ff2019-12-01 18:22:57 -0800382 return simple_shm_fetcher_.RegisterWakeup(priority);
Alex Perrycb7da4b2019-08-28 19:35:56 -0700383 }
384
Austin Schuh39788ff2019-12-01 18:22:57 -0800385 void UnregisterWakeup() { return simple_shm_fetcher_.UnregisterWakeup(); }
Alex Perrycb7da4b2019-08-28 19:35:56 -0700386
387 private:
388 bool has_new_data_ = false;
389
Austin Schuh39788ff2019-12-01 18:22:57 -0800390 SimpleShmFetcher simple_shm_fetcher_;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700391};
392
393// Adapter class to adapt a timerfd to a TimerHandler.
Alex Perrycb7da4b2019-08-28 19:35:56 -0700394class TimerHandlerState : public TimerHandler {
395 public:
396 TimerHandlerState(ShmEventLoop *shm_event_loop, ::std::function<void()> fn)
Austin Schuh39788ff2019-12-01 18:22:57 -0800397 : TimerHandler(shm_event_loop, std::move(fn)),
398 shm_event_loop_(shm_event_loop) {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700399 shm_event_loop_->epoll_.OnReadable(timerfd_.fd(), [this]() {
Austin Schuhde8a8ff2019-11-30 15:25:36 -0800400 const uint64_t elapsed_cycles = timerfd_.Read();
401
Austin Schuh39788ff2019-12-01 18:22:57 -0800402 Call(monotonic_clock::now, base_);
Austin Schuhde8a8ff2019-11-30 15:25:36 -0800403
404 base_ += repeat_offset_ * elapsed_cycles;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700405 });
406 }
407
408 ~TimerHandlerState() { shm_event_loop_->epoll_.DeleteFd(timerfd_.fd()); }
409
410 void Setup(monotonic_clock::time_point base,
411 monotonic_clock::duration repeat_offset) override {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700412 timerfd_.SetTime(base, repeat_offset);
Austin Schuhde8a8ff2019-11-30 15:25:36 -0800413 base_ = base;
414 repeat_offset_ = repeat_offset;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700415 }
416
Austin Schuh39788ff2019-12-01 18:22:57 -0800417 void Disable() override { timerfd_.Disable(); }
Alex Perrycb7da4b2019-08-28 19:35:56 -0700418
419 private:
420 ShmEventLoop *shm_event_loop_;
421
422 TimerFd timerfd_;
423
Austin Schuhde8a8ff2019-11-30 15:25:36 -0800424 monotonic_clock::time_point base_;
425 monotonic_clock::duration repeat_offset_;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700426};
427
428// Adapter class to the timerfd and PhasedLoop.
Alex Perrycb7da4b2019-08-28 19:35:56 -0700429class PhasedLoopHandler : public ::aos::PhasedLoopHandler {
430 public:
431 PhasedLoopHandler(ShmEventLoop *shm_event_loop, ::std::function<void(int)> fn,
432 const monotonic_clock::duration interval,
433 const monotonic_clock::duration offset)
Austin Schuh39788ff2019-12-01 18:22:57 -0800434 : aos::PhasedLoopHandler(shm_event_loop, std::move(fn), interval, offset),
435 shm_event_loop_(shm_event_loop) {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700436 shm_event_loop_->epoll_.OnReadable(timerfd_.fd(), [this]() {
437 timerfd_.Read();
Austin Schuh39788ff2019-12-01 18:22:57 -0800438 Call(monotonic_clock::now,
439 [this](monotonic_clock::time_point sleep_time) {
440 Schedule(sleep_time);
441 });
Alex Perrycb7da4b2019-08-28 19:35:56 -0700442 });
443 }
444
Austin Schuh39788ff2019-12-01 18:22:57 -0800445 ~PhasedLoopHandler() override {
446 shm_event_loop_->epoll_.DeleteFd(timerfd_.fd());
Alex Perrycb7da4b2019-08-28 19:35:56 -0700447 }
448
449 private:
Austin Schuhde8a8ff2019-11-30 15:25:36 -0800450 // Reschedules the timer.
Austin Schuh39788ff2019-12-01 18:22:57 -0800451 void Schedule(monotonic_clock::time_point sleep_time) override {
452 timerfd_.SetTime(sleep_time, ::aos::monotonic_clock::zero());
Alex Perrycb7da4b2019-08-28 19:35:56 -0700453 }
454
455 ShmEventLoop *shm_event_loop_;
456
457 TimerFd timerfd_;
Alex Perrycb7da4b2019-08-28 19:35:56 -0700458};
459} // namespace internal
460
461::std::unique_ptr<RawFetcher> ShmEventLoop::MakeRawFetcher(
462 const Channel *channel) {
Austin Schuh39788ff2019-12-01 18:22:57 -0800463 return ::std::unique_ptr<RawFetcher>(new internal::ShmFetcher(this, channel));
Alex Perrycb7da4b2019-08-28 19:35:56 -0700464}
465
466::std::unique_ptr<RawSender> ShmEventLoop::MakeRawSender(
467 const Channel *channel) {
468 Take(channel);
Austin Schuh39788ff2019-12-01 18:22:57 -0800469
470 return ::std::unique_ptr<RawSender>(new internal::ShmSender(this, channel));
Alex Perrycb7da4b2019-08-28 19:35:56 -0700471}
472
473void ShmEventLoop::MakeRawWatcher(
474 const Channel *channel,
475 std::function<void(const Context &context, const void *message)> watcher) {
Alex Perrycb7da4b2019-08-28 19:35:56 -0700476 Take(channel);
477
Austin Schuh39788ff2019-12-01 18:22:57 -0800478 NewWatcher(::std::unique_ptr<WatcherState>(
479 new internal::WatcherState(this, channel, std::move(watcher))));
Alex Perrycb7da4b2019-08-28 19:35:56 -0700480}
481
482TimerHandler *ShmEventLoop::AddTimer(::std::function<void()> callback) {
Austin Schuh39788ff2019-12-01 18:22:57 -0800483 return NewTimer(::std::unique_ptr<TimerHandler>(
484 new internal::TimerHandlerState(this, ::std::move(callback))));
Alex Perrycb7da4b2019-08-28 19:35:56 -0700485}
486
487PhasedLoopHandler *ShmEventLoop::AddPhasedLoop(
488 ::std::function<void(int)> callback,
489 const monotonic_clock::duration interval,
490 const monotonic_clock::duration offset) {
Austin Schuh39788ff2019-12-01 18:22:57 -0800491 return NewPhasedLoop(
492 ::std::unique_ptr<PhasedLoopHandler>(new internal::PhasedLoopHandler(
493 this, ::std::move(callback), interval, offset)));
Alex Perrycb7da4b2019-08-28 19:35:56 -0700494}
495
496void ShmEventLoop::OnRun(::std::function<void()> on_run) {
497 on_run_.push_back(::std::move(on_run));
498}
499
Austin Schuh39788ff2019-12-01 18:22:57 -0800500void ShmEventLoop::HandleWatcherSignal() {
501 while (true) {
502 // Call the handlers in time order of their messages.
503 aos::monotonic_clock::time_point min_event_time =
504 aos::monotonic_clock::max_time;
505 size_t min_watcher_index = -1;
506 size_t watcher_index = 0;
507 for (::std::unique_ptr<WatcherState> &base_watcher : watchers_) {
508 internal::WatcherState *watcher =
509 reinterpret_cast<internal::WatcherState *>(base_watcher.get());
510
511 if (watcher->HasNewData()) {
512 if (watcher->event_time() < min_event_time) {
513 min_watcher_index = watcher_index;
514 min_event_time = watcher->event_time();
515 }
516 }
517 ++watcher_index;
518 }
519
520 if (min_event_time == aos::monotonic_clock::max_time) {
521 break;
522 }
523
524 reinterpret_cast<internal::WatcherState *>(
525 watchers_[min_watcher_index].get())
526 ->CallCallback();
527 }
528}
529
Austin Schuh32fd5a72019-12-01 22:20:26 -0800530// RAII class to mask signals.
531class ScopedSignalMask {
532 public:
533 ScopedSignalMask(std::initializer_list<int> signals) {
534 sigset_t sigset;
535 PCHECK(sigemptyset(&sigset) == 0);
536 for (int signal : signals) {
537 PCHECK(sigaddset(&sigset, signal) == 0);
538 }
539
540 PCHECK(sigprocmask(SIG_BLOCK, &sigset, &old_) == 0);
541 }
542
543 ~ScopedSignalMask() { PCHECK(sigprocmask(SIG_SETMASK, &old_, nullptr) == 0); }
544
545 private:
546 sigset_t old_;
547};
548
549// Class to manage the static state associated with killing multiple event
550// loops.
551class SignalHandler {
552 public:
553 // Gets the singleton.
554 static SignalHandler *global() {
555 static SignalHandler loop;
556 return &loop;
557 }
558
559 // Handles the signal with the singleton.
560 static void HandleSignal(int) { global()->DoHandleSignal(); }
561
562 // Registers an event loop to receive Exit() calls.
563 void Register(ShmEventLoop *event_loop) {
564 // Block signals while we have the mutex so we never race with the signal
565 // handler.
566 ScopedSignalMask mask({SIGINT, SIGHUP, SIGTERM});
567 std::unique_lock<stl_mutex> locker(mutex_);
568 if (event_loops_.size() == 0) {
569 // The first caller registers the signal handler.
570 struct sigaction new_action;
571 sigemptyset(&new_action.sa_mask);
572 // This makes it so that 2 control c's to a stuck process will kill it by
573 // restoring the original signal handler.
574 new_action.sa_flags = SA_RESETHAND;
575 new_action.sa_handler = &HandleSignal;
576
577 PCHECK(sigaction(SIGINT, &new_action, &old_action_int_) == 0);
578 PCHECK(sigaction(SIGHUP, &new_action, &old_action_hup_) == 0);
579 PCHECK(sigaction(SIGTERM, &new_action, &old_action_term_) == 0);
580 }
581
582 event_loops_.push_back(event_loop);
583 }
584
585 // Unregisters an event loop to receive Exit() calls.
586 void Unregister(ShmEventLoop *event_loop) {
587 // Block signals while we have the mutex so we never race with the signal
588 // handler.
589 ScopedSignalMask mask({SIGINT, SIGHUP, SIGTERM});
590 std::unique_lock<stl_mutex> locker(mutex_);
591
592 event_loops_.erase(std::find(event_loops_.begin(), event_loops_.end(), event_loop));
593
594 if (event_loops_.size() == 0u) {
595 // The last caller restores the original signal handlers.
596 PCHECK(sigaction(SIGINT, &old_action_int_, nullptr) == 0);
597 PCHECK(sigaction(SIGHUP, &old_action_hup_, nullptr) == 0);
598 PCHECK(sigaction(SIGTERM, &old_action_term_, nullptr) == 0);
599 }
600 }
601
602 private:
603 void DoHandleSignal() {
604 // We block signals while grabbing the lock, so there should never be a
605 // race. Confirm that this is true using trylock.
606 CHECK(mutex_.try_lock()) << ": sigprocmask failed to block signals while "
607 "modifing the event loop list.";
608 for (ShmEventLoop *event_loop : event_loops_) {
609 event_loop->Exit();
610 }
611 mutex_.unlock();
612 }
613
614 // Mutex to protect all state.
615 stl_mutex mutex_;
616 std::vector<ShmEventLoop *> event_loops_;
617 struct sigaction old_action_int_;
618 struct sigaction old_action_hup_;
619 struct sigaction old_action_term_;
620};
621
Alex Perrycb7da4b2019-08-28 19:35:56 -0700622void ShmEventLoop::Run() {
Austin Schuh32fd5a72019-12-01 22:20:26 -0800623 SignalHandler::global()->Register(this);
Austin Schuh39788ff2019-12-01 18:22:57 -0800624
Alex Perrycb7da4b2019-08-28 19:35:56 -0700625 std::unique_ptr<ipc_lib::SignalFd> signalfd;
626
627 if (watchers_.size() > 0) {
628 signalfd.reset(new ipc_lib::SignalFd({ipc_lib::kWakeupSignal}));
629
630 epoll_.OnReadable(signalfd->fd(), [signalfd_ptr = signalfd.get(), this]() {
631 signalfd_siginfo result = signalfd_ptr->Read();
632 CHECK_EQ(result.ssi_signo, ipc_lib::kWakeupSignal);
633
634 // TODO(austin): We should really be checking *everything*, not just
635 // watchers, and calling the oldest thing first. That will improve
636 // determinism a lot.
637
Austin Schuh39788ff2019-12-01 18:22:57 -0800638 HandleWatcherSignal();
Alex Perrycb7da4b2019-08-28 19:35:56 -0700639 });
640 }
641
Austin Schuh39788ff2019-12-01 18:22:57 -0800642 MaybeScheduleTimingReports();
643
644 // Now, all the callbacks are setup. Lock everything into memory and go RT.
Alex Perrycb7da4b2019-08-28 19:35:56 -0700645 if (priority_ != 0) {
646 ::aos::InitRT();
647
648 LOG(INFO) << "Setting priority to " << priority_;
649 ::aos::SetCurrentThreadRealtimePriority(priority_);
650 }
651
652 set_is_running(true);
653
654 // Now that we are realtime (but before the OnRun handlers run), snap the
655 // queue index.
Austin Schuh39788ff2019-12-01 18:22:57 -0800656 for (::std::unique_ptr<WatcherState> &watcher : watchers_) {
657 watcher->Startup(this);
Alex Perrycb7da4b2019-08-28 19:35:56 -0700658 }
659
660 // Now that we are RT, run all the OnRun handlers.
661 for (const auto &run : on_run_) {
662 run();
663 }
664
Alex Perrycb7da4b2019-08-28 19:35:56 -0700665 // And start our main event loop which runs all the timers and handles Quit.
666 epoll_.Run();
667
668 // Once epoll exits, there is no useful nonrt work left to do.
669 set_is_running(false);
670
671 // Nothing time or synchronization critical needs to happen after this point.
672 // Drop RT priority.
673 ::aos::UnsetCurrentThreadRealtimePriority();
674
Austin Schuh39788ff2019-12-01 18:22:57 -0800675 for (::std::unique_ptr<WatcherState> &base_watcher : watchers_) {
676 internal::WatcherState *watcher =
677 reinterpret_cast<internal::WatcherState *>(base_watcher.get());
Alex Perrycb7da4b2019-08-28 19:35:56 -0700678 watcher->UnregisterWakeup();
679 }
680
681 if (watchers_.size() > 0) {
682 epoll_.DeleteFd(signalfd->fd());
683 signalfd.reset();
684 }
Austin Schuh32fd5a72019-12-01 22:20:26 -0800685
686 SignalHandler::global()->Unregister(this);
Alex Perrycb7da4b2019-08-28 19:35:56 -0700687}
688
689void ShmEventLoop::Exit() { epoll_.Quit(); }
690
691ShmEventLoop::~ShmEventLoop() {
Austin Schuh39788ff2019-12-01 18:22:57 -0800692 // Trigger any remaining senders or fetchers to be cleared before destroying
693 // the event loop so the book keeping matches.
694 timing_report_sender_.reset();
695
696 // Force everything with a registered fd with epoll to be destroyed now.
697 timers_.clear();
698 phased_loops_.clear();
699 watchers_.clear();
700
Alex Perrycb7da4b2019-08-28 19:35:56 -0700701 CHECK(!is_running()) << ": ShmEventLoop destroyed while running";
702}
703
704void ShmEventLoop::Take(const Channel *channel) {
705 CHECK(!is_running()) << ": Cannot add new objects while running.";
706
707 // Cheat aggresively. Use the shared memory path as a proxy for a unique
708 // identifier for the channel.
709 const std::string path = ShmPath(channel);
710
711 const auto prior = ::std::find(taken_.begin(), taken_.end(), path);
712 CHECK(prior == taken_.end()) << ": " << path << " is already being used.";
713
714 taken_.emplace_back(path);
715}
716
717void ShmEventLoop::SetRuntimeRealtimePriority(int priority) {
718 if (is_running()) {
719 LOG(FATAL) << "Cannot set realtime priority while running.";
720 }
721 priority_ = priority;
722}
723
Austin Schuh39788ff2019-12-01 18:22:57 -0800724pid_t ShmEventLoop::GetTid() { return syscall(SYS_gettid); }
725
Alex Perrycb7da4b2019-08-28 19:35:56 -0700726} // namespace aos