allwpilib/wpilibc/Athena/src/Notifier.cpp

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.
 */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
/*----------------------------------------------------------------------------*/

#include "Notifier.h"
#include "Timer.h"
#include "Utility.h"
#include "WPIErrors.h"
#include "HAL/HAL.hpp"

Notifier *Notifier::timerQueueHead = nullptr;
priority_recursive_mutex Notifier::queueMutex;
priority_mutex Notifier::halMutex;
void *Notifier::m_notifier = nullptr;
std::atomic<int> Notifier::refcount = ATOMIC_VAR_INIT(0);

/**
 * Create a Notifier for timer event notification.
 * @param handler The handler is called at the notification time which is set
 * using StartSingle or StartPeriodic.
 */
Notifier::Notifier(TimerEventHandler handler, void *param) {
  if (handler == nullptr)
    wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr");
  m_handler = handler;
  m_param = param;
  // do the first time intialization of static variables
  if (refcount.fetch_add(1) == 0) {
    int32_t status = 0;
    {
      std::lock_guard<priority_mutex> sync(halMutex);
      if (!m_notifier)
        m_notifier = initializeNotifier(ProcessQueue, nullptr, &status);
    }
    wpi_setErrorWithContext(status, getHALErrorMessage(status));
  }
}

/**
 * Free the resources for a timer event.
 * All resources will be freed and the timer event will be removed from the
 * queue if necessary.
 */
Notifier::~Notifier() {
  {
    std::lock_guard<priority_recursive_mutex> sync(queueMutex);
    DeleteFromQueue();
  }

  // Delete the static variables when the last one is going away
  if (refcount.fetch_sub(1) == 1) {
    int32_t status = 0;
    {
      std::lock_guard<priority_mutex> sync(halMutex);
      if (m_notifier) {
        cleanNotifier(m_notifier, &status);
        m_notifier = nullptr;
      }
    }
    wpi_setErrorWithContext(status, getHALErrorMessage(status));
  }

  // Acquire the mutex; this makes certain that the handler is
  // not being executed by the interrupt manager.
  std::lock_guard<priority_mutex> lock(m_handlerMutex);
}

/**
 * Update the alarm hardware to reflect the current first element in the queue.
 * Compute the time the next alarm should occur based on the current time and
 * the
 * period for the first element in the timer queue.
 * WARNING: this method does not do synchronization! It must be called from
 * somewhere
 * that is taking care of synchronizing access to the queue.
 */
void Notifier::UpdateAlarm() {
  if (timerQueueHead != nullptr) {
    int32_t status = 0;
    // This locking is necessary in order to avoid two things:
    //  1) Race condition issues with calling cleanNotifer() and
    //     updateNotifierAlarm() at the same time.
    //  2) Avoid deadlock by making it so that this won't block waiting
    //     for the mutex to unlock.
    // Checking refcount as well is unnecessary, but will not hurt.
    if (halMutex.try_lock() && refcount != 0) {
      if (m_notifier)
        updateNotifierAlarm(m_notifier,
                            (uint32_t)(timerQueueHead->m_expirationTime * 1e6),
                            &status);
      halMutex.unlock();
    }
    wpi_setStaticErrorWithContext(timerQueueHead, status,
                                  getHALErrorMessage(status));
  }
}

/**
 * ProcessQueue is called whenever there is a timer interrupt.
 * We need to wake up and process the current top item in the timer queue as
 * long
 * as its scheduled time is after the current time. Then the item is removed or
 * rescheduled (repetitive events) in the queue.
 */
void Notifier::ProcessQueue(uint32_t currentTimeInt, void *params) {
  Notifier *current;
  while (true)  // keep processing past events until no more
  {
    {
      std::lock_guard<priority_recursive_mutex> sync(queueMutex);
      double currentTime = currentTimeInt * 1.0e-6;
      current = timerQueueHead;
      if (current == nullptr || current->m_expirationTime > currentTime) {
        break;  // no more timer events to process
      }
      // need to process this entry
      timerQueueHead = current->m_nextEvent;
      if (current->m_periodic) {
        // if periodic, requeue the event
        // compute when to put into queue
        current->InsertInQueue(true);
      } else {
        // not periodic; removed from queue
        current->m_queued = false;
      }
      // Take handler mutex while holding queue mutex to make sure
      //  the handler will execute to completion in case we are being deleted.
      current->m_handlerMutex.lock();
    }

    current->m_handler(current->m_param);  // call the event handler
    current->m_handlerMutex.unlock();
  }
  // reschedule the first item in the queue
  std::lock_guard<priority_recursive_mutex> sync(queueMutex);
  UpdateAlarm();
}

/**
 * Insert this Notifier into the timer queue in right place.
 * WARNING: this method does not do synchronization! It must be called from
 * somewhere
 * that is taking care of synchronizing access to the queue.
 * @param reschedule If false, the scheduled alarm is based on the current time
 * and UpdateAlarm
 * method is called which will enable the alarm if necessary.
 * If true, update the time by adding the period (no drift) when rescheduled
 * periodic from ProcessQueue.
 * This ensures that the public methods only update the queue after finishing
 * inserting.
 */
void Notifier::InsertInQueue(bool reschedule) {
  if (reschedule) {
    m_expirationTime += m_period;
  } else {
    m_expirationTime = GetClock() + m_period;
  }
  if (m_expirationTime > Timer::kRolloverTime) {
    m_expirationTime -= Timer::kRolloverTime;
  }
  if (timerQueueHead == nullptr ||
      timerQueueHead->m_expirationTime >= this->m_expirationTime) {
    // the queue is empty or greater than the new entry
    // the new entry becomes the first element
    this->m_nextEvent = timerQueueHead;
    timerQueueHead = this;
    if (!reschedule) {
      // since the first element changed, update alarm, unless we already plan
      // to
      UpdateAlarm();
    }
  } else {
    for (Notifier **npp = &(timerQueueHead->m_nextEvent);;
         npp = &(*npp)->m_nextEvent) {
      Notifier *n = *npp;
      if (n == nullptr || n->m_expirationTime > this->m_expirationTime) {
        *npp = this;
        this->m_nextEvent = n;
        break;
      }
    }
  }
  m_queued = true;
}

/**
 * Delete this Notifier from the timer queue.
 * WARNING: this method does not do synchronization! It must be called from
 * somewhere
 * that is taking care of synchronizing access to the queue.
 * Remove this Notifier from the timer queue and adjust the next interrupt time
 * to reflect
 * the current top of the queue.
 */
void Notifier::DeleteFromQueue() {
  if (m_queued) {
    m_queued = false;
    wpi_assert(timerQueueHead != nullptr);
    if (timerQueueHead == this) {
      // remove the first item in the list - update the alarm
      timerQueueHead = this->m_nextEvent;
      UpdateAlarm();
    } else {
      for (Notifier *n = timerQueueHead; n != nullptr; n = n->m_nextEvent) {
        if (n->m_nextEvent == this) {
          // this element is the next element from *n from the queue
          n->m_nextEvent = this->m_nextEvent;  // point around this one
        }
      }
    }
  }
}

/**
 * Register for single event notification.
 * A timer event is queued for a single event after the specified delay.
 * @param delay Seconds to wait before the handler is called.
 */
void Notifier::StartSingle(double delay) {
  std::lock_guard<priority_recursive_mutex> sync(queueMutex);
  m_periodic = false;
  m_period = delay;
  DeleteFromQueue();
  InsertInQueue(false);
}

/**
 * Register for periodic event notification.
 * A timer event is queued for periodic event notification. Each time the
 * interrupt
 * occurs, the event will be immediately requeued for the same time interval.
 * @param period Period in seconds to call the handler starting one period after
 * the call to this method.
 */
void Notifier::StartPeriodic(double period) {
  std::lock_guard<priority_recursive_mutex> sync(queueMutex);
  m_periodic = true;
  m_period = period;
  DeleteFromQueue();
  InsertInQueue(false);
}

/**
 * Stop timer events from occuring.
 * Stop any repeating timer events from occuring. This will also remove any
 * single
 * notification events from the queue.
 * If a timer-based call to the registered handler is in progress, this function
 * will
 * block until the handler call is complete.
 */
void Notifier::Stop() {
  {
    std::lock_guard<priority_recursive_mutex> sync(queueMutex);
    DeleteFromQueue();
  }
  // Wait for a currently executing handler to complete before returning from
  // Stop()
  std::lock_guard<priority_mutex> sync(m_handlerMutex);
}