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

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#include "hal/Notifier.h"

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

#include <fmt/core.h>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>

#include "HALInitializer.h"
#include "HALInternal.h"
#include "hal/ChipObject.h"
#include "hal/Errors.h"
#include "hal/HAL.h"
#include "hal/Threads.h"
#include "hal/handles/UnlimitedHandleResource.h"
#include "hal/roborio/InterruptManager.h"

using namespace hal;

static constexpr int32_t kTimerInterruptNumber = 28;

static wpi::mutex notifierMutex;
static std::unique_ptr<tAlarm> notifierAlarm;
static std::thread notifierThread;
static HAL_Bool notifierThreadRealTime = false;
static int32_t notifierThreadPriority = 0;
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};
static std::atomic_bool notifierRunning{false};

using namespace hal;

class NotifierHandleContainer
    : public UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
                                     HAL_HandleEnum::Notifier> {
 public:
  ~NotifierHandleContainer() {
    ForEach([](HAL_NotifierHandle handle, Notifier* notifier) {
      {
        std::scoped_lock 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() {
  std::scoped_lock 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 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 (notifierAlarm && 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 notifierThreadMain() {
  InterruptManager& manager = InterruptManager::GetInstance();
  NiFpga_IrqContext context = manager.GetContext();
  uint32_t mask = 1 << kTimerInterruptNumber;
  int32_t status = 0;

  while (notifierRunning) {
    status = 0;
    auto triggeredMask =
        manager.WaitForInterrupt(context, mask, false, 10000, &status);
    if (!notifierRunning) {
      break;
    }
    if (triggeredMask == 0) {
      continue;
    }
    alarmCallback();
  }
}

static void cleanupNotifierAtExit() {
  int32_t status = 0;
  if (notifierAlarm) {
    notifierAlarm->writeEnable(false, &status);
  }
  notifierAlarm = nullptr;
  notifierRunning = false;
  hal::ReleaseFPGAInterrupt(kTimerInterruptNumber);
  if (notifierThread.joinable()) {
    notifierThread.join();
  }
}

namespace hal::init {
void InitializeNotifier() {
  static NotifierHandleContainer nH;
  notifierHandles = &nH;
}
}  // namespace hal::init

extern "C" {

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

  if (notifierRefCount.fetch_add(1) == 0) {
    std::scoped_lock lock(notifierMutex);
    notifierRunning = true;
    notifierThread = std::thread(notifierThreadMain);

    auto native = notifierThread.native_handle();
    HAL_SetThreadPriority(&native, notifierThreadRealTime,
                          notifierThreadPriority, status);
    if (*status == HAL_THREAD_PRIORITY_ERROR) {
      *status = 0;
      fmt::print("{}: HAL Notifier thread\n",
                 HAL_THREAD_PRIORITY_ERROR_MESSAGE);
    }
    if (*status == HAL_THREAD_PRIORITY_RANGE_ERROR) {
      *status = 0;
      fmt::print("{}: HAL Notifier thread\n",
                 HAL_THREAD_PRIORITY_RANGE_ERROR_MESSAGE);
    }

    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;
}

HAL_Bool HAL_SetNotifierThreadPriority(HAL_Bool realTime, int32_t priority,
                                       int32_t* status) {
  std::scoped_lock lock(notifierMutex);
  notifierThreadRealTime = realTime;
  notifierThreadPriority = priority;
  if (notifierThread.joinable()) {
    auto native = notifierThread.native_handle();
    return HAL_SetThreadPriority(&native, realTime, priority, status);
  } else {
    return true;
  }
}

void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
                         int32_t* status) {}

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

  {
    std::scoped_lock 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::scoped_lock lock(notifier->mutex);
    notifier->triggerTime = UINT64_MAX;
    notifier->triggeredTime = 0;
    notifier->active = false;
  }
  notifier->cond.notify_all();

  if (notifierRefCount.fetch_sub(1) == 1) {
    // if this was the last notifier, clean up alarm and thread
    // the notifier can call back into our callback, so don't hold the lock
    // here (the atomic fetch_sub will prevent multiple parallel entries
    // into this function)

    if (notifierAlarm) {
      notifierAlarm->writeEnable(false, status);
    }
    notifierRunning = false;
    hal::ReleaseFPGAInterrupt(kTimerInterruptNumber);
    if (notifierThread.joinable()) {
      notifierThread.join();
    }

    std::scoped_lock lock(notifierMutex);
    notifierAlarm = 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::scoped_lock lock(notifier->mutex);
    notifier->triggerTime = triggerTime;
    notifier->triggeredTime = UINT64_MAX;
  }

  std::scoped_lock 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::scoped_lock 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 lock(notifier->mutex);
  notifier->cond.wait(lock, [&] {
    return !notifier->active || notifier->triggeredTime != UINT64_MAX;
  });
  return notifier->active ? notifier->triggeredTime : 0;
}

}  // extern "C"