allwpilib/hal/src/main/native/athena/Notifier.cpp

/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2017 FIRST. 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 the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

#include "HAL/Notifier.h"

#include <atomic>
#include <cstdlib>  // For std::atexit()
#include <memory>

#include <support/condition_variable.h>
#include <support/mutex.h>

#include "HAL/ChipObject.h"
#include "HAL/Errors.h"
#include "HAL/HAL.h"
#include "HAL/cpp/make_unique.h"
#include "HAL/handles/UnlimitedHandleResource.h"

using namespace hal;

static const int32_t kTimerInterruptNumber = 28;

static wpi::mutex notifierMutex;
static std::unique_ptr<tAlarm> notifierAlarm;
static std::unique_ptr<tInterruptManager> notifierManager;
static uint64_t closestTrigger = UINT64_MAX;

namespace {

struct Notifier {
  uint64_t triggerTime = UINT64_MAX;
  uint64_t triggeredTime = UINT64_MAX;
  bool active = true;
  wpi::mutex mutex;
  wpi::condition_variable cond;
};

}  // namespace

static std::atomic_flag notifierAtexitRegistered = ATOMIC_FLAG_INIT;
static std::atomic_int notifierRefCount{0};

using namespace hal;

class NotifierHandleContainer
    : public UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
                                     HAL_HandleEnum::Notifier> {
 public:
  ~NotifierHandleContainer() {
    ForEach([](HAL_NotifierHandle handle, Notifier* notifier) {
      {
        std::lock_guard<wpi::mutex> lock(notifier->mutex);
        notifier->triggerTime = UINT64_MAX;
        notifier->triggeredTime = 0;
        notifier->active = false;
      }
      notifier->cond.notify_all();  // wake up any waiting threads
    });
  }
};

static NotifierHandleContainer notifierHandles;

static void alarmCallback(uint32_t, void*) {
  std::lock_guard<wpi::mutex> lock(notifierMutex);
  int32_t status = 0;
  uint64_t currentTime = 0;

  // the hardware disables itself after each alarm
  closestTrigger = UINT64_MAX;

  // process all notifiers
  notifierHandles.ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
    if (notifier->triggerTime == UINT64_MAX) return;
    if (currentTime == 0) currentTime = HAL_GetFPGATime(&status);
    std::unique_lock<wpi::mutex> lock(notifier->mutex);
    if (notifier->triggerTime < currentTime) {
      notifier->triggerTime = UINT64_MAX;
      notifier->triggeredTime = currentTime;
      lock.unlock();
      notifier->cond.notify_all();
    } else if (notifier->triggerTime < closestTrigger) {
      closestTrigger = notifier->triggerTime;
    }
  });

  if (closestTrigger != UINT64_MAX) {
    // Simply truncate the hardware trigger time to 32-bit.
    notifierAlarm->writeTriggerTime(static_cast<uint32_t>(closestTrigger),
                                    &status);
    // Enable the alarm.  The hardware disables itself after each alarm.
    notifierAlarm->writeEnable(true, &status);
  }
}

static void cleanupNotifierAtExit() {
  notifierAlarm = nullptr;
  notifierManager = nullptr;
}

extern "C" {

HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status) {
  if (!notifierAtexitRegistered.test_and_set())
    std::atexit(cleanupNotifierAtExit);

  if (notifierRefCount.fetch_add(1) == 0) {
    std::lock_guard<wpi::mutex> lock(notifierMutex);
    // create manager and alarm if not already created
    if (!notifierManager) {
      notifierManager = std::make_unique<tInterruptManager>(
          1 << kTimerInterruptNumber, false, status);
      notifierManager->registerHandler(alarmCallback, nullptr, status);
      notifierManager->enable(status);
    }
    if (!notifierAlarm) notifierAlarm.reset(tAlarm::create(status));
  }

  std::shared_ptr<Notifier> notifier = std::make_shared<Notifier>();
  HAL_NotifierHandle handle = notifierHandles.Allocate(notifier);
  if (handle == HAL_kInvalidHandle) {
    *status = HAL_HANDLE_ERROR;
    return HAL_kInvalidHandle;
  }
  return handle;
}

void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
  auto notifier = notifierHandles.Get(notifierHandle);
  if (!notifier) return;

  {
    std::lock_guard<wpi::mutex> lock(notifier->mutex);
    notifier->triggerTime = UINT64_MAX;
    notifier->triggeredTime = 0;
    notifier->active = false;
  }
  notifier->cond.notify_all();  // wake up any waiting threads
}

void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
  auto notifier = notifierHandles.Free(notifierHandle);
  if (!notifier) return;

  // Just in case HAL_StopNotifier() wasn't called...
  {
    std::lock_guard<wpi::mutex> lock(notifier->mutex);
    notifier->triggerTime = UINT64_MAX;
    notifier->triggeredTime = 0;
    notifier->active = false;
  }
  notifier->cond.notify_all();

  if (notifierRefCount.fetch_sub(1) == 1) {
    std::lock_guard<wpi::mutex> lock(notifierMutex);
    // if this was the last notifier, clean up alarm and manager
    if (notifierAlarm) {
      notifierAlarm->writeEnable(false, status);
      notifierAlarm = nullptr;
    }
    if (notifierManager) {
      notifierManager->disable(status);
      notifierManager = nullptr;
    }
    closestTrigger = UINT64_MAX;
  }
}

void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
                             uint64_t triggerTime, int32_t* status) {
  auto notifier = notifierHandles.Get(notifierHandle);
  if (!notifier) return;

  {
    std::lock_guard<wpi::mutex> lock(notifier->mutex);
    notifier->triggerTime = triggerTime;
    notifier->triggeredTime = UINT64_MAX;
  }

  std::lock_guard<wpi::mutex> lock(notifierMutex);
  // Update alarm time if closer than current.
  if (triggerTime < closestTrigger) {
    bool wasActive = (closestTrigger != UINT64_MAX);
    closestTrigger = triggerTime;
    // Simply truncate the hardware trigger time to 32-bit.
    notifierAlarm->writeTriggerTime(static_cast<uint32_t>(closestTrigger),
                                    status);
    // Enable the alarm.
    if (!wasActive) notifierAlarm->writeEnable(true, status);
  }
}

void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
                             int32_t* status) {
  auto notifier = notifierHandles.Get(notifierHandle);
  if (!notifier) return;

  {
    std::lock_guard<wpi::mutex> lock(notifier->mutex);
    notifier->triggerTime = UINT64_MAX;
  }
}

uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
                                  int32_t* status) {
  auto notifier = notifierHandles.Get(notifierHandle);
  if (!notifier) return 0;
  std::unique_lock<wpi::mutex> lock(notifier->mutex);
  notifier->cond.wait(lock, [&] {
    return !notifier->active || notifier->triggeredTime != UINT64_MAX;
  });
  return notifier->active ? notifier->triggeredTime : 0;
}

}  // extern "C"