aos/events/shm_event_loop.cc

#include "aos/events/shm_event_loop.h"

#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/syscall.h>
#include <sys/types.h>
#include <unistd.h>
#include <algorithm>
#include <atomic>
#include <chrono>
#include <iterator>
#include <stdexcept>

#include "aos/events/epoll.h"
#include "aos/events/event_loop_generated.h"
#include "aos/events/timing_statistics.h"
#include "aos/ipc_lib/lockless_queue.h"
#include "aos/ipc_lib/signalfd.h"
#include "aos/realtime.h"
#include "aos/util/phased_loop.h"
#include "glog/logging.h"

DEFINE_string(shm_base, "/dev/shm/aos",
              "Directory to place queue backing mmaped files in.");
DEFINE_uint32(permissions, 0770,
              "Permissions to make shared memory files and folders.");

namespace aos {

std::string ShmFolder(const Channel *channel) {
  CHECK(channel->has_name());
  CHECK_EQ(channel->name()->string_view()[0], '/');
  return FLAGS_shm_base + channel->name()->str() + "/";
}
std::string ShmPath(const Channel *channel) {
  CHECK(channel->has_type());
  return ShmFolder(channel) + channel->type()->str() + ".v0";
}

class MMapedQueue {
 public:
  MMapedQueue(const Channel *channel) {
    std::string path = ShmPath(channel);

    // TODO(austin): Pull these out into the config if there is a need.
    config_.num_watchers = 10;
    config_.num_senders = 10;
    config_.queue_size = 2 * channel->frequency();
    config_.message_data_size = channel->max_size();

    size_ = ipc_lib::LocklessQueueMemorySize(config_);

    MkdirP(path);

    // There are 2 cases.  Either the file already exists, or it does not
    // already exist and we need to create it.  Start by trying to create it. If
    // that fails, the file has already been created and we can open it
    // normally..  Once the file has been created it wil never be deleted.
    fd_ = open(path.c_str(), O_RDWR | O_CREAT | O_EXCL,
               O_CLOEXEC | FLAGS_permissions);
    if (fd_ == -1 && errno == EEXIST) {
      VLOG(1) << path << " already created.";
      // File already exists.
      fd_ = open(path.c_str(), O_RDWR, O_CLOEXEC);
      PCHECK(fd_ != -1) << ": Failed to open " << path;
      while (true) {
        struct stat st;
        PCHECK(fstat(fd_, &st) == 0);
        if (st.st_size != 0) {
          CHECK_EQ(static_cast<size_t>(st.st_size), size_)
              << ": Size of " << path
              << " doesn't match expected size of backing queue file.  Did the "
                 "queue definition change?";
          break;
        } else {
          // The creating process didn't get around to it yet.  Give it a bit.
          std::this_thread::sleep_for(std::chrono::milliseconds(10));
          VLOG(1) << path << " is zero size, waiting";
        }
      }
    } else {
      VLOG(1) << "Created " << path;
      PCHECK(fd_ != -1) << ": Failed to open " << path;
      PCHECK(ftruncate(fd_, size_) == 0);
    }

    data_ = mmap(NULL, size_, PROT_READ | PROT_WRITE, MAP_SHARED, fd_, 0);
    PCHECK(data_ != MAP_FAILED);

    ipc_lib::InitializeLocklessQueueMemory(memory(), config_);
  }

  ~MMapedQueue() {
    PCHECK(munmap(data_, size_) == 0);
    PCHECK(close(fd_) == 0);
  }

  ipc_lib::LocklessQueueMemory *memory() const {
    return reinterpret_cast<ipc_lib::LocklessQueueMemory *>(data_);
  }

  const ipc_lib::LocklessQueueConfiguration &config() const { return config_; }

 private:
  void MkdirP(std::string_view path) {
    struct stat st;
    auto last_slash_pos = path.find_last_of("/");

    std::string folder(last_slash_pos == std::string_view::npos
                           ? std::string_view("")
                           : path.substr(0, last_slash_pos));
    if (stat(folder.c_str(), &st) == -1) {
      PCHECK(errno == ENOENT);
      CHECK_NE(folder, "") << ": Base path doesn't exist";
      MkdirP(folder);
      VLOG(1) << "Creating " << folder;
      PCHECK(mkdir(folder.c_str(), FLAGS_permissions) == 0);
    }
  }

  ipc_lib::LocklessQueueConfiguration config_;

  int fd_;

  size_t size_;
  void *data_;
};

// Returns the portion of the path after the last /.
std::string_view Filename(std::string_view path) {
  auto last_slash_pos = path.find_last_of("/");

  return last_slash_pos == std::string_view::npos
             ? path
             : path.substr(last_slash_pos + 1, path.size());
}

ShmEventLoop::ShmEventLoop(const Configuration *configuration)
    : EventLoop(configuration), name_(Filename(program_invocation_name)) {}

namespace {

namespace chrono = ::std::chrono;

}  // namespace

namespace internal {

class SimpleShmFetcher {
 public:
  explicit SimpleShmFetcher(const Channel *channel)
      : lockless_queue_memory_(channel),
        lockless_queue_(lockless_queue_memory_.memory(),
                        lockless_queue_memory_.config()),
        data_storage_(static_cast<AlignedChar *>(aligned_alloc(
                          alignof(AlignedChar), channel->max_size())),
                      &free) {
    context_.data = nullptr;
    // Point the queue index at the next index to read starting now.  This
    // makes it such that FetchNext will read the next message sent after
    // the fetcher is created.
    PointAtNextQueueIndex();
  }

  ~SimpleShmFetcher() {}

  // Points the next message to fetch at the queue index which will be
  // populated next.
  void PointAtNextQueueIndex() {
    actual_queue_index_ = lockless_queue_.LatestQueueIndex();
    if (!actual_queue_index_.valid()) {
      // Nothing in the queue.  The next element will show up at the 0th
      // index in the queue.
      actual_queue_index_ =
          ipc_lib::QueueIndex::Zero(lockless_queue_.queue_size());
    } else {
      actual_queue_index_ = actual_queue_index_.Increment();
    }
  }

  bool FetchNext() {
    // TODO(austin): Write a test which starts with nothing in the queue,
    // and then calls FetchNext() after something is sent.
    // TODO(austin): Get behind and make sure it dies both here and with
    // Fetch.
    ipc_lib::LocklessQueue::ReadResult read_result = lockless_queue_.Read(
        actual_queue_index_.index(), &context_.monotonic_sent_time,
        &context_.realtime_sent_time, &context_.size,
        reinterpret_cast<char *>(data_storage_.get()));
    if (read_result == ipc_lib::LocklessQueue::ReadResult::GOOD) {
      context_.queue_index = actual_queue_index_.index();
      context_.data = reinterpret_cast<char *>(data_storage_.get()) +
                      lockless_queue_.message_data_size() - context_.size;
      actual_queue_index_ = actual_queue_index_.Increment();
    }

    // Make sure the data wasn't modified while we were reading it.  This
    // can only happen if you are reading the last message *while* it is
    // being written to, which means you are pretty far behind.
    CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::OVERWROTE)
        << ": Got behind while reading and the last message was modified "
           "out "
           "from under us while we were reading it.  Don't get so far "
           "behind.";

    CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::TOO_OLD)
        << ": The next message is no longer available.";
    return read_result == ipc_lib::LocklessQueue::ReadResult::GOOD;
  }

  bool Fetch() {
    const ipc_lib::QueueIndex queue_index = lockless_queue_.LatestQueueIndex();
    // actual_queue_index_ is only meaningful if it was set by Fetch or
    // FetchNext.  This happens when valid_data_ has been set.  So, only
    // skip checking if valid_data_ is true.
    //
    // Also, if the latest queue index is invalid, we are empty.  So there
    // is nothing to fetch.
    if ((context_.data != nullptr &&
         queue_index == actual_queue_index_.DecrementBy(1u)) ||
        !queue_index.valid()) {
      return false;
    }

    ipc_lib::LocklessQueue::ReadResult read_result =
        lockless_queue_.Read(queue_index.index(), &context_.monotonic_sent_time,
                             &context_.realtime_sent_time, &context_.size,
                             reinterpret_cast<char *>(data_storage_.get()));
    if (read_result == ipc_lib::LocklessQueue::ReadResult::GOOD) {
      context_.queue_index = queue_index.index();
      context_.data = reinterpret_cast<char *>(data_storage_.get()) +
                      lockless_queue_.message_data_size() - context_.size;
      actual_queue_index_ = queue_index.Increment();
    }

    // Make sure the data wasn't modified while we were reading it.  This
    // can only happen if you are reading the last message *while* it is
    // being written to, which means you are pretty far behind.
    CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::OVERWROTE)
        << ": Got behind while reading and the last message was modified "
           "out "
           "from under us while we were reading it.  Don't get so far "
           "behind.";

    CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::NOTHING_NEW)
        << ": Queue index went backwards.  This should never happen.";

    // We fell behind between when we read the index and read the value.
    // This isn't worth recovering from since this means we went to sleep
    // for a long time in the middle of this function.
    CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::TOO_OLD)
        << ": The next message is no longer available.";
    return read_result == ipc_lib::LocklessQueue::ReadResult::GOOD;
  }

  Context context() const { return context_; }

  bool RegisterWakeup(int priority) {
    return lockless_queue_.RegisterWakeup(priority);
  }

  void UnregisterWakeup() { lockless_queue_.UnregisterWakeup(); }

 private:
  MMapedQueue lockless_queue_memory_;
  ipc_lib::LocklessQueue lockless_queue_;

  ipc_lib::QueueIndex actual_queue_index_ =
      ipc_lib::LocklessQueue::empty_queue_index();

  struct AlignedChar {
    alignas(32) char data;
  };

  std::unique_ptr<AlignedChar, decltype(&free)> data_storage_;

  Context context_;
};

class ShmFetcher : public RawFetcher {
 public:
  explicit ShmFetcher(EventLoop *event_loop, const Channel *channel)
      : RawFetcher(event_loop, channel), simple_shm_fetcher_(channel) {}

  ~ShmFetcher() { context_.data = nullptr; }

  std::pair<bool, monotonic_clock::time_point> DoFetchNext() override {
    if (simple_shm_fetcher_.FetchNext()) {
      context_ = simple_shm_fetcher_.context();
      return std::make_pair(true, monotonic_clock::now());
    }
    return std::make_pair(false, monotonic_clock::min_time);
  }

  std::pair<bool, monotonic_clock::time_point> DoFetch() override {
    if (simple_shm_fetcher_.Fetch()) {
      context_ = simple_shm_fetcher_.context();
      return std::make_pair(true, monotonic_clock::now());
    }
    return std::make_pair(false, monotonic_clock::min_time);
  }

 private:
  SimpleShmFetcher simple_shm_fetcher_;
};

class ShmSender : public RawSender {
 public:
  explicit ShmSender(EventLoop *event_loop, const Channel *channel)
      : RawSender(event_loop, channel),
        lockless_queue_memory_(channel),
        lockless_queue_(lockless_queue_memory_.memory(),
                        lockless_queue_memory_.config()),
        lockless_queue_sender_(lockless_queue_.MakeSender()) {}

  ~ShmSender() override {}

  void *data() override { return lockless_queue_sender_.Data(); }
  size_t size() override { return lockless_queue_sender_.size(); }
  bool DoSend(size_t length) override {
    lockless_queue_sender_.Send(length);
    lockless_queue_.Wakeup(event_loop()->priority());
    return true;
  }

  bool DoSend(const void *msg, size_t length) override {
    lockless_queue_sender_.Send(reinterpret_cast<const char *>(msg), length);
    lockless_queue_.Wakeup(event_loop()->priority());
    // TODO(austin): Return an error if we send too fast.
    return true;
  }

 private:
  MMapedQueue lockless_queue_memory_;
  ipc_lib::LocklessQueue lockless_queue_;
  ipc_lib::LocklessQueue::Sender lockless_queue_sender_;
};

// Class to manage the state for a Watcher.
class WatcherState : public aos::WatcherState {
 public:
  WatcherState(
      EventLoop *event_loop, const Channel *channel,
      std::function<void(const Context &context, const void *message)> fn)
      : aos::WatcherState(event_loop, channel, std::move(fn)),
        simple_shm_fetcher_(channel) {}

  ~WatcherState() override {}

  void Startup(EventLoop *event_loop) override {
    PointAtNextQueueIndex();
    CHECK(RegisterWakeup(event_loop->priority()));
  }

  // Points the next message to fetch at the queue index which will be populated
  // next.
  void PointAtNextQueueIndex() { simple_shm_fetcher_.PointAtNextQueueIndex(); }

  // Returns true if there is new data available.
  bool HasNewData() {
    if (!has_new_data_) {
      has_new_data_ = simple_shm_fetcher_.FetchNext();
    }

    return has_new_data_;
  }

  // Returns the time of the current data sample.
  aos::monotonic_clock::time_point event_time() const {
    return simple_shm_fetcher_.context().monotonic_sent_time;
  }

  // Consumes the data by calling the callback.
  void CallCallback() {
    CHECK(has_new_data_);
    DoCallCallback(monotonic_clock::now, simple_shm_fetcher_.context());
    has_new_data_ = false;
  }

  // Registers us to receive a signal on event reception.
  bool RegisterWakeup(int priority) {
    return simple_shm_fetcher_.RegisterWakeup(priority);
  }

  void UnregisterWakeup() { return simple_shm_fetcher_.UnregisterWakeup(); }

 private:
  bool has_new_data_ = false;

  SimpleShmFetcher simple_shm_fetcher_;
};

// Adapter class to adapt a timerfd to a TimerHandler.
class TimerHandlerState : public TimerHandler {
 public:
  TimerHandlerState(ShmEventLoop *shm_event_loop, ::std::function<void()> fn)
      : TimerHandler(shm_event_loop, std::move(fn)),
        shm_event_loop_(shm_event_loop) {
    shm_event_loop_->epoll_.OnReadable(timerfd_.fd(), [this]() {
      const uint64_t elapsed_cycles = timerfd_.Read();

      Call(monotonic_clock::now, base_);

      base_ += repeat_offset_ * elapsed_cycles;
    });
  }

  ~TimerHandlerState() { shm_event_loop_->epoll_.DeleteFd(timerfd_.fd()); }

  void Setup(monotonic_clock::time_point base,
             monotonic_clock::duration repeat_offset) override {
    timerfd_.SetTime(base, repeat_offset);
    base_ = base;
    repeat_offset_ = repeat_offset;
  }

  void Disable() override { timerfd_.Disable(); }

 private:
  ShmEventLoop *shm_event_loop_;

  TimerFd timerfd_;

  monotonic_clock::time_point base_;
  monotonic_clock::duration repeat_offset_;
};

// Adapter class to the timerfd and PhasedLoop.
class PhasedLoopHandler : public ::aos::PhasedLoopHandler {
 public:
  PhasedLoopHandler(ShmEventLoop *shm_event_loop, ::std::function<void(int)> fn,
                    const monotonic_clock::duration interval,
                    const monotonic_clock::duration offset)
      : aos::PhasedLoopHandler(shm_event_loop, std::move(fn), interval, offset),
        shm_event_loop_(shm_event_loop) {
    shm_event_loop_->epoll_.OnReadable(timerfd_.fd(), [this]() {
      timerfd_.Read();
      Call(monotonic_clock::now,
           [this](monotonic_clock::time_point sleep_time) {
             Schedule(sleep_time);
           });
    });
  }

  ~PhasedLoopHandler() override {
    shm_event_loop_->epoll_.DeleteFd(timerfd_.fd());
  }

 private:
  // Reschedules the timer.
  void Schedule(monotonic_clock::time_point sleep_time) override {
    timerfd_.SetTime(sleep_time, ::aos::monotonic_clock::zero());
  }

  ShmEventLoop *shm_event_loop_;

  TimerFd timerfd_;
};
}  // namespace internal

::std::unique_ptr<RawFetcher> ShmEventLoop::MakeRawFetcher(
    const Channel *channel) {
  return ::std::unique_ptr<RawFetcher>(new internal::ShmFetcher(this, channel));
}

::std::unique_ptr<RawSender> ShmEventLoop::MakeRawSender(
    const Channel *channel) {
  Take(channel);

  return ::std::unique_ptr<RawSender>(new internal::ShmSender(this, channel));
}

void ShmEventLoop::MakeRawWatcher(
    const Channel *channel,
    std::function<void(const Context &context, const void *message)> watcher) {
  Take(channel);

  NewWatcher(::std::unique_ptr<WatcherState>(
      new internal::WatcherState(this, channel, std::move(watcher))));
}

TimerHandler *ShmEventLoop::AddTimer(::std::function<void()> callback) {
  return NewTimer(::std::unique_ptr<TimerHandler>(
      new internal::TimerHandlerState(this, ::std::move(callback))));
}

PhasedLoopHandler *ShmEventLoop::AddPhasedLoop(
    ::std::function<void(int)> callback,
    const monotonic_clock::duration interval,
    const monotonic_clock::duration offset) {
  return NewPhasedLoop(
      ::std::unique_ptr<PhasedLoopHandler>(new internal::PhasedLoopHandler(
          this, ::std::move(callback), interval, offset)));
}

void ShmEventLoop::OnRun(::std::function<void()> on_run) {
  on_run_.push_back(::std::move(on_run));
}

void ShmEventLoop::HandleWatcherSignal() {
  while (true) {
    // Call the handlers in time order of their messages.
    aos::monotonic_clock::time_point min_event_time =
        aos::monotonic_clock::max_time;
    size_t min_watcher_index = -1;
    size_t watcher_index = 0;
    for (::std::unique_ptr<WatcherState> &base_watcher : watchers_) {
      internal::WatcherState *watcher =
          reinterpret_cast<internal::WatcherState *>(base_watcher.get());

      if (watcher->HasNewData()) {
        if (watcher->event_time() < min_event_time) {
          min_watcher_index = watcher_index;
          min_event_time = watcher->event_time();
        }
      }
      ++watcher_index;
    }

    if (min_event_time == aos::monotonic_clock::max_time) {
      break;
    }

    reinterpret_cast<internal::WatcherState *>(
        watchers_[min_watcher_index].get())
        ->CallCallback();
  }
}

void ShmEventLoop::Run() {
  // TODO(austin): Automatically register ^C with a sigaction so 2 in a row send
  // an actual control C.

  std::unique_ptr<ipc_lib::SignalFd> signalfd;

  if (watchers_.size() > 0) {
    signalfd.reset(new ipc_lib::SignalFd({ipc_lib::kWakeupSignal}));

    epoll_.OnReadable(signalfd->fd(), [signalfd_ptr = signalfd.get(), this]() {
      signalfd_siginfo result = signalfd_ptr->Read();
      CHECK_EQ(result.ssi_signo, ipc_lib::kWakeupSignal);

      // TODO(austin): We should really be checking *everything*, not just
      // watchers, and calling the oldest thing first.  That will improve
      // determinism a lot.

      HandleWatcherSignal();
    });
  }

  MaybeScheduleTimingReports();

  // Now, all the callbacks are setup.  Lock everything into memory and go RT.
  if (priority_ != 0) {
    ::aos::InitRT();

    LOG(INFO) << "Setting priority to " << priority_;
    ::aos::SetCurrentThreadRealtimePriority(priority_);
  }

  set_is_running(true);

  // Now that we are realtime (but before the OnRun handlers run), snap the
  // queue index.
  for (::std::unique_ptr<WatcherState> &watcher : watchers_) {
    watcher->Startup(this);
  }

  // Now that we are RT, run all the OnRun handlers.
  for (const auto &run : on_run_) {
    run();
  }

  // And start our main event loop which runs all the timers and handles Quit.
  epoll_.Run();

  // Once epoll exits, there is no useful nonrt work left to do.
  set_is_running(false);

  // Nothing time or synchronization critical needs to happen after this point.
  // Drop RT priority.
  ::aos::UnsetCurrentThreadRealtimePriority();

  for (::std::unique_ptr<WatcherState> &base_watcher : watchers_) {
    internal::WatcherState *watcher =
        reinterpret_cast<internal::WatcherState *>(base_watcher.get());
    watcher->UnregisterWakeup();
  }

  if (watchers_.size() > 0) {
    epoll_.DeleteFd(signalfd->fd());
    signalfd.reset();
  }
}

void ShmEventLoop::Exit() { epoll_.Quit(); }

ShmEventLoop::~ShmEventLoop() {
  // Trigger any remaining senders or fetchers to be cleared before destroying
  // the event loop so the book keeping matches.
  timing_report_sender_.reset();

  // Force everything with a registered fd with epoll to be destroyed now.
  timers_.clear();
  phased_loops_.clear();
  watchers_.clear();

  CHECK(!is_running()) << ": ShmEventLoop destroyed while running";
}

void ShmEventLoop::Take(const Channel *channel) {
  CHECK(!is_running()) << ": Cannot add new objects while running.";

  // Cheat aggresively.  Use the shared memory path as a proxy for a unique
  // identifier for the channel.
  const std::string path = ShmPath(channel);

  const auto prior = ::std::find(taken_.begin(), taken_.end(), path);
  CHECK(prior == taken_.end()) << ": " << path << " is already being used.";

  taken_.emplace_back(path);
}

void ShmEventLoop::SetRuntimeRealtimePriority(int priority) {
  if (is_running()) {
    LOG(FATAL) << "Cannot set realtime priority while running.";
  }
  priority_ = priority;
}

pid_t ShmEventLoop::GetTid() { return syscall(SYS_gettid); }

}  // namespace aos