timer_context

timer_context.cpp

/*
 * Copyright 2019-present Facebook, Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <Arduino.h>
#include "stdlibpatch.h"
#include "timer_context.hpp"

namespace unifex {

namespace _timer_context {

timer_context* unique_timer_context = nullptr;

} // namespace _timer_context 

timer_context::timer_context()
{
  assert(_timer_context::unique_timer_context == nullptr);
  _timer_context::unique_timer_context = this;
}

timer_context::~timer_context() {
  stop_ = true;
  assert(head_ == nullptr);
  _timer_context::unique_timer_context = nullptr;
}

void timer_context::enqueue(task_base* task) noexcept {
  noInterrupts();                          // disable all interrupts while interrupt parameters are changing
  if (head_ == nullptr || task->dueTime_ < head_->dueTime_) {
    // Insert at the head of the queue.
    task->next_ = head_;
    task->prevNextPtr_ = &head_;
    if (head_ != nullptr) {
      head_->prevNextPtr_ = &task->next_;
    }
    head_ = task;

    auto now = clock_t::now();
    const auto remaining = head_->dueTime_ - now;
    const std::uint16_t click_count = std::min(remaining, arduino::max_clicks).count();

    // New minimum due-time has changed, set the hardware timer.
    TCCR1A = 0;                              // reset all bits
    TCCR1B = 0;                              // reset all bits
    OCR1A = click_count;                     // input is already in clicks, in theory should subtract 1
    TCCR1B |= (1 << WGM12);                  // CTC mode
    TCCR1B |= arduino::timer_prescaler_mask; // On a 16MHz Arduino, a prescaler of 64 gives a 4usecond click
    TIFR1 |= (1 << OCF1A);                   // clear

    TIMSK1 |= (1 << OCIE1A);                 // enable timer compare interrupt
    TCNT1 = 0;                               // set counter to 0
  } else {
    auto* queuedTask = head_;
    while (queuedTask->next_ != nullptr &&
           queuedTask->next_->dueTime_ <= task->dueTime_) {
      queuedTask = queuedTask->next_;
    }

    // Insert after queuedTask
    task->prevNextPtr_ = &queuedTask->next_;
    task->next_ = queuedTask->next_;
    if (task->next_ != nullptr) {
      task->next_->prevNextPtr_ = &task->next_;
    }
    queuedTask->next_ = task;
  }
  interrupts();                            // enable all interrupts
}

void timer_context::run() {
  noInterrupts();                       // disable all interrupts
  TIMSK1 &= ~(1 << OCIE1A);             // disable future interrupts ie. CTC interrupt to give a 'one-shot' effect

  while (!stop_ && head_ != nullptr) {
    auto now = clock_t::now();
    auto nextDueTime = head_->dueTime_;
    if (nextDueTime <= now) {
      // Ready to run

      // Dequeue item
      auto* task = head_;
      head_ = task->next_;
      if (head_ != nullptr) {
        head_->prevNextPtr_ = &head_;
      }

      // Flag the task as dequeued.
      task->prevNextPtr_ = nullptr;

      interrupts();                            // enable all interrupts
      task->execute();
      noInterrupts();                          // disable all interrupts while interrupt parameters are changing
    } else {
      // Not yet ready to run.
      std::uint16_t click_count = std::min(nextDueTime - now, arduino::max_clicks).count();

      TCCR1A = 0;                              // reset all bits
      TCCR1B = 0;                              // reset all bits
      OCR1A = click_count;                     // input is already in clicks, in theory should subtract 1
      TCCR1B |= (1 << WGM12);                  // CTC mode
      TCCR1B |= arduino::timer_prescaler_mask; // On a 16MHz Arduino, a prescaler of 64 gives a 4usecond click
      TIFR1 |= (1 << OCF1A);                   // clear

      TIMSK1 |= (1 << OCIE1A);                 // enable timer compare interrupt
      TCNT1 = 0;                               // set counter to 0

      break;
    }
  }
  interrupts();                            // enable all interrupts
}

void _timer_context::cancel_callback::operator()() noexcept {
  auto now = clock_t::now();
  if (now < task_->dueTime_) {
    task_->dueTime_ = now;

    if (task_->prevNextPtr_ != nullptr) {
      // Task is still in the queue, dequeue and requeue it.
      
      noInterrupts();                          // disable all interrupts while interrupt parameters are changing

      // Remove from the queue.
      *task_->prevNextPtr_ = task_->next_;
      if (task_->next_ != nullptr) {
        task_->next_->prevNextPtr_ = task_->prevNextPtr_;
      }
      task_->prevNextPtr_ = nullptr;

      // And requeue with an updated time.
      task_->dueTime_ = now;
      interrupts();                            // enable all interrupts

      task_->context_->enqueue(task_);
    }
  }
}

} // namespace unifex

// **********   Hardware timing interrupt event  ***************

ISR(TIMER1_COMPA_vect) // timer compare interrupt service routine
{
  if (unifex::_timer_context::unique_timer_context != nullptr) {
   unifex::_timer_context::unique_timer_context->run();
  }
}

timer_context.hpp

/*
 * Copyright 2019-present Facebook, Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#pragma once

#include <cassert>
#include <type_traits>
#include <chrono>

#include <unifex/config.hpp>
#include <unifex/get_stop_token.hpp>
#include <unifex/manual_lifetime.hpp>
#include <unifex/receiver_concepts.hpp>
#include <unifex/scheduler_concepts.hpp>
#include <unifex/stop_token_concepts.hpp>

#include "sequence.hpp"

#include <unifex/detail/prologue.hpp>

namespace arduino {
  constexpr long clockHz = 16'000'000;
  constexpr int timer_prescaler = 8;
  constexpr int timer_prescaler_mask = (0 << CS12) | (1 << CS11) | (0 << CS10);
  constexpr long clicks_per_second = clockHz / timer_prescaler;

  using clicks = std::chrono::duration<float, std::ratio<1, clicks_per_second>>;

  constexpr clicks max_clicks{65'535};                                   // 16bit clock

  struct steady_clock
  {
    typedef clicks 	                                         duration;
    typedef duration::rep                                    rep;
    typedef duration::period                                 period;
    typedef std::chrono::time_point<steady_clock, duration>  time_point;

    static constexpr bool is_steady = true;

    static time_point
    now() noexcept {
      // make sure that the time_points for now are stable 
      // across rollover of the micros() counter
      // based on https://arduino.stackexchange.com/a/12588
      static clicks lowClicks, highClicks;
      constexpr clicks rolloverIncrementClicks = std::chrono::duration_cast<clicks>(std::chrono::microseconds(std::numeric_limits<std::uint32_t>::max())) + std::chrono::duration_cast<clicks>(std::chrono::microseconds(1));
      const clicks nowClicks = std::chrono::duration_cast<clicks>(std::chrono::microseconds(micros()));
      if (nowClicks < lowClicks) {
        highClicks += rolloverIncrementClicks;
      }
      lowClicks = nowClicks;
      return time_point{lowClicks + highClicks};
    }
  };
}

namespace unifex {
class timer_context;

namespace _timer_context {
  using clock_t = arduino::steady_clock;
  using time_point = typename clock_t::time_point;

  struct task_base {
    using execute_fn = void(task_base*) noexcept;

    explicit task_base(timer_context& context, execute_fn* execute) noexcept
      : context_(&context), execute_(execute) {}

    timer_context* const context_;
    task_base* next_ = nullptr;
    task_base** prevNextPtr_ = nullptr;
    execute_fn* execute_;
    time_point dueTime_;

    void execute() noexcept {
      execute_(this);
    }
  };

  class cancel_callback {
    task_base* const task_;
   public:
    explicit cancel_callback(task_base* task) noexcept
      : task_(task) {}

    void operator()() noexcept;
  };

  class scheduler;

  template <typename Duration>
  struct _schedule_after_sender {
    class type;
  };
  template <typename Duration>
  using schedule_after_sender = typename _schedule_after_sender<Duration>::type;

  template <typename Duration, typename Receiver>
  struct _after_op {
    class type;
    class unwinder;
  };
  template <typename Duration, typename Receiver>
  using after_operation =
      typename _after_op<Duration, remove_cvref_t<Receiver>>::type;

  template <typename Duration, typename Receiver>
  struct _after_op<Duration, Receiver>::unwinder {
    using op_t = typename _after_op<Duration, Receiver>::type;
    op_t* op_;
    Receiver& get_receiver() {
      return op_->receiver_;
    } 
    template <typename Cpo, typename... Vn>
    friend void tag_invoke(unifex::tag_t<unifex::unwind>, unwinder& self, Cpo cpo, Vn&&... vn) noexcept {
      unifex::unwound(self.get_receiver(), cpo, (Vn&&) vn...);
    }
  };

  template <typename Duration, typename Receiver>
  class _after_op<Duration, Receiver>::type final : task_base {
    using unwinder_t = typename _after_op<Duration, Receiver>::unwinder;
    friend class _after_op<Duration, Receiver>::unwinder;
    friend schedule_after_sender<Duration>;

    template <typename Receiver2>
    explicit type(
        timer_context& context,
        Duration duration,
        Receiver2&& receiver)
        : task_base(context, &type::execute_impl),
          duration_(duration),
          receiver_((Receiver2 &&) receiver) {
      assert(context_ != nullptr);
    }

    static void execute_impl(task_base* t) noexcept {
      auto& self = *static_cast<type*>(t);
      self.cancelCallback_.destruct();
      if constexpr (is_stop_never_possible_v<
                        stop_token_type_t<Receiver&>>) {
        unifex::set_value(static_cast<Receiver&&>(self.receiver_));
      } else {
        if (get_stop_token(self.receiver_).stop_requested()) {
          unifex::set_done(static_cast<Receiver&&>(self.receiver_));
        } else {
          unifex::set_value(static_cast<Receiver&&>(self.receiver_));
        }
      }
    }

    Duration duration_;
    UNIFEX_NO_UNIQUE_ADDRESS Receiver receiver_;
    UNIFEX_NO_UNIQUE_ADDRESS manual_lifetime<typename stop_token_type_t<
        Receiver&>::template callback_type<cancel_callback>>
        cancelCallback_;

   public:

    friend unwinder_t tag_invoke(unifex::tag_t<unifex::get_unwinder>, const type& self) noexcept {
      return unwinder_t{const_cast<type*>(&self)};
    }

    friend void tag_invoke(unifex::tag_t<unifex::step>, type&) noexcept {
    }

    void start() noexcept;
  };

  template <typename Duration>
  class _schedule_after_sender<Duration>::type {
    friend scheduler;

    explicit type(
        timer_context& context,
        Duration duration) noexcept
      : context_(&context), duration_(duration) {}

    timer_context* context_;
    Duration duration_;

   public:
    template <
        template <typename...> class Variant,
        template <typename...> class Tuple>
    using value_types = Variant<Tuple<>>;

    template <template <typename...> class Variant>
    using error_types = Variant<>;

    static constexpr bool sends_done = true;

    template <typename Receiver>
    after_operation<Duration, Receiver> connect(Receiver&& receiver) const {
      return after_operation<Duration, Receiver>{
          *context_, duration_, (Receiver &&) receiver};
    }
  };

  template <typename Receiver>
  struct _at_op {
    class type;
    class unwinder;
  };
  template <typename Receiver>
  using at_operation = typename _at_op<remove_cvref_t<Receiver>>::type;

  template<typename Receiver>
  struct _at_op<Receiver>::unwinder {
    using op_t = typename _at_op<Receiver>::type;
    op_t* op_;
    Receiver& get_receiver() {
      return op_->receiver_;
    } 
    template <typename Cpo, typename... Vn>
    friend void tag_invoke(unifex::tag_t<unifex::unwind>, unwinder& self, Cpo cpo, Vn&&... vn) noexcept {
      unifex::unwound(self.get_receiver(), cpo, (Vn&&) vn...);
    }
  };

  template <typename Receiver>
  class _at_op<Receiver>::type final : task_base {
    using unwinder_t = typename _at_op<Receiver>::unwinder;
    friend class _at_op<Receiver>::unwinder;

    static void execute_impl(task_base* p) noexcept {
      auto& self = *static_cast<type*>(p);
      self.cancelCallback_.destruct();
      if constexpr (is_stop_never_possible_v<
                        stop_token_type_t<Receiver&>>) {
        unifex::set_value(static_cast<Receiver&&>(self.receiver_));
      } else {
        if (get_stop_token(self.receiver_).stop_requested()) {
          unifex::set_done(static_cast<Receiver&&>(self.receiver_));
        } else {
          unifex::set_value(static_cast<Receiver&&>(self.receiver_));
        }
      }
    }

    UNIFEX_NO_UNIQUE_ADDRESS Receiver receiver_;
    UNIFEX_NO_UNIQUE_ADDRESS manual_lifetime<typename stop_token_type_t<
        Receiver&>::template callback_type<cancel_callback>>
        cancelCallback_;

   public:
    template <typename Receiver2>
    explicit type(
        timer_context& scheduler,
        clock_t::time_point dueTime,
        Receiver2&& receiver)
        : task_base(scheduler, &type::execute_impl)
        , receiver_((Receiver2 &&) receiver) {
      this->dueTime_ = dueTime;
    }

    friend unwinder_t tag_invoke(unifex::tag_t<unifex::get_unwinder>, const type& self) noexcept {
      return unwinder_t{const_cast<type*>(&self)};
    }

    friend void tag_invoke(unifex::tag_t<unifex::step>, type&) noexcept {
    }

    void start() noexcept;
  };

  class schedule_at_sender {
    friend scheduler;

    explicit schedule_at_sender(
        timer_context& context,
        time_point dueTime)
      : context_(&context), dueTime_(dueTime) {}

    timer_context* context_;
    time_point dueTime_;
   public:
    template <
        template <typename...> class Variant,
        template <typename...> class Tuple>
    using value_types = Variant<Tuple<>>;

    template <template <typename...> class Variant>
    using error_types = Variant<>;

    static constexpr bool sends_done = true;

    template <typename Receiver>
    at_operation<remove_cvref_t<Receiver>> connect(Receiver&& receiver) {
      return at_operation<remove_cvref_t<Receiver>>{
          *context_, dueTime_, (Receiver &&) receiver};
    }
  };

  class scheduler {
    friend timer_context;

    explicit scheduler(timer_context& context) noexcept
      : context_(&context) {}

    friend bool operator==(scheduler a, scheduler b) noexcept {
      return a.context_ == b.context_;
    }
    friend bool operator!=(scheduler a, scheduler b) noexcept {
      return a.context_ != b.context_;
    }

    timer_context* context_;
   public:
    template <typename Rep, typename Ratio>
    auto schedule_after(std::chrono::duration<Rep, Ratio> delay) const noexcept
        -> schedule_after_sender<std::chrono::duration<Rep, Ratio>> {
      return schedule_after_sender<std::chrono::duration<Rep, Ratio>>{
          *context_, delay};
    }

    auto schedule_at(clock_t::time_point dueTime) const noexcept {
      return schedule_at_sender{*context_, dueTime};
    }

    auto schedule() const noexcept {
      return schedule_after(std::chrono::milliseconds{0});
    }

    auto now() const noexcept {
      return clock_t::now();
    }
  };
} // namespace _timer_context

class timer_context {
  using scheduler = _timer_context::scheduler;
  using task_base = _timer_context::task_base;
  using cancel_callback = _timer_context::cancel_callback;
  friend cancel_callback;
  friend scheduler;
  template <typename Duration, typename Receiver>
  friend struct _timer_context::_after_op;
  template <typename Receiver>
  friend struct _timer_context::_at_op;

  void enqueue(task_base* task) noexcept;

  // Head of a linked-list in ascending order of due-time.
  task_base* head_ = nullptr;
  bool stop_ = false;

 public:
  using clock_t = _timer_context::clock_t;
  using time_point = _timer_context::time_point;

  timer_context();
  ~timer_context();

  void run();

  scheduler get_scheduler() noexcept {
    return scheduler{*this};
  }

  friend scheduler tag_invoke(tag_t<unifex::get_scheduler>, const timer_context& self) noexcept {
    return const_cast<timer_context&>(self).get_scheduler();
  }
};

namespace _timer_context {
  template <typename Duration, typename Receiver>
  inline void _after_op<Duration, Receiver>::type::start() noexcept {
    this->dueTime_ = clock_t::now() + std::chrono::duration_cast<typename clock_t::duration>(duration_);
    cancelCallback_.construct(
        get_stop_token(receiver_), cancel_callback{this});
    context_->enqueue(this);
  }

  template <typename Receiver>
  inline void _at_op<Receiver>::type::start() noexcept {
    cancelCallback_.construct(
        get_stop_token(receiver_), cancel_callback{this});
    this->context_->enqueue(this);
  }
} // namespace _timer_context
} // namespace unifex

#include <unifex/detail/epilogue.hpp>