hal/lib/athena/Notifier.cpp - RealtimeRoboticsGroup/test - Gitiles

 /*----------------------------------------------------------------------------*/
 /* Copyright (c) FIRST 2016-2017. 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"

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

 #include <atomic>
 #include <memory>
 #include <mutex>

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

 using namespace hal;

 static const int32_t kTimerInterruptNumber = 28;

 static priority_mutex notifierInterruptMutex;
 static priority_recursive_mutex notifierMutex;
 static std::unique_ptr<tAlarm> notifierAlarm;
 static std::unique_ptr<tInterruptManager> notifierManager;
 static uint64_t closestTrigger = UINT64_MAX;

 namespace {
 struct Notifier {
   std::shared_ptr<Notifier> prev, next;
   void* param;
   HAL_NotifierProcessFunction process;
   uint64_t triggerTime = UINT64_MAX;
   HAL_NotifierHandle handle;
   bool threaded;
 };

 // Safe thread to allow callbacks to run on their own thread
 class NotifierThread : public wpi::SafeThread {
  public:
   void Main() {
     std::unique_lock<std::mutex> lock(m_mutex);
     while (m_active) {
       m_cond.wait(lock, [&] { return !m_active || m_notify; });
       if (!m_active) break;
       m_notify = false;
       uint64_t currentTime = m_currentTime;
       HAL_NotifierHandle handle = m_handle;
       HAL_NotifierProcessFunction process = m_process;
       lock.unlock();  // don't hold mutex during callback execution
       process(currentTime, handle);
       lock.lock();
     }
   }

   bool m_notify = false;
   HAL_NotifierHandle m_handle = HAL_kInvalidHandle;
   HAL_NotifierProcessFunction m_process;
   uint64_t m_currentTime;
 };

 class NotifierThreadOwner : public wpi::SafeThreadOwner<NotifierThread> {
  public:
   void SetFunc(HAL_NotifierProcessFunction process, void* param) {
     auto thr = GetThread();
     if (!thr) return;
     thr->m_process = process;
     m_param = param;
   }

   void Notify(uint64_t currentTime, HAL_NotifierHandle handle) {
     auto thr = GetThread();
     if (!thr) return;
     thr->m_currentTime = currentTime;
     thr->m_handle = handle;
     thr->m_notify = true;
     thr->m_cond.notify_one();
   }

   void* m_param;
 };
 }  // namespace

 static std::shared_ptr<Notifier> notifiers;
 static std::atomic_flag notifierAtexitRegistered = ATOMIC_FLAG_INIT;
 static std::atomic_int notifierRefCount{0};

 using namespace hal;

 static UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
                                HAL_HandleEnum::Notifier>
     notifierHandles;

 // internal version of updateAlarm used during the alarmCallback when we know
 // that the pointer is a valid pointer.
 void updateNotifierAlarmInternal(std::shared_ptr<Notifier> notifierPointer,
                                  uint64_t triggerTime, int32_t* status) {
   std::lock_guard<priority_recursive_mutex> sync(notifierMutex);

   auto notifier = notifierPointer;
   // no need for a null check, as this must always be a valid pointer.
   notifier->triggerTime = triggerTime;
   bool wasActive = (closestTrigger != UINT64_MAX);

   if (!notifierInterruptMutex.try_lock() || notifierRefCount == 0 ||
       !notifierAlarm)
     return;

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

   notifierInterruptMutex.unlock();
 }

 static void alarmCallback(uint32_t, void*) {
   std::unique_lock<priority_recursive_mutex> sync(notifierMutex);

   int32_t status = 0;
   uint64_t currentTime = 0;

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

   // process all notifiers
   std::shared_ptr<Notifier> notifier = notifiers;
   while (notifier) {
     if (notifier->triggerTime != UINT64_MAX) {
       if (currentTime == 0) currentTime = HAL_GetFPGATime(&status);
       if (notifier->triggerTime < currentTime) {
         notifier->triggerTime = UINT64_MAX;
         auto process = notifier->process;
         auto handle = notifier->handle;
         sync.unlock();
         process(currentTime, handle);
         sync.lock();
       } else if (notifier->triggerTime < closestTrigger) {
         updateNotifierAlarmInternal(notifier, notifier->triggerTime, &status);
       }
     }
     notifier = notifier->next;
   }
 }

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

 static void threadedNotifierHandler(uint64_t currentTimeInt,
                                     HAL_NotifierHandle handle) {
   // Grab notifier and get handler param
   auto notifier = notifierHandles.Get(handle);
   if (!notifier) return;
   auto notifierPointer = notifier->param;
   if (notifierPointer == nullptr) return;
   NotifierThreadOwner* owner =
       static_cast<NotifierThreadOwner*>(notifierPointer);
   owner->Notify(currentTimeInt, handle);
 }

 extern "C" {

 HAL_NotifierHandle HAL_InitializeNotifier(HAL_NotifierProcessFunction process,
                                           void* param, int32_t* status) {
   if (!process) {
     *status = NULL_PARAMETER;
     return 0;
   }
   if (!notifierAtexitRegistered.test_and_set())
     std::atexit(cleanupNotifierAtExit);
   if (notifierRefCount.fetch_add(1) == 0) {
     std::lock_guard<priority_mutex> sync(notifierInterruptMutex);
     // 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::lock_guard<priority_recursive_mutex> sync(notifierMutex);
   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;
   }
   // create notifier structure and add to list
   notifier->next = notifiers;
   if (notifier->next) notifier->next->prev = notifier;
   notifier->param = param;
   notifier->process = process;
   notifier->handle = handle;
   notifier->threaded = false;
   notifiers = notifier;
   return handle;
 }

 HAL_NotifierHandle HAL_InitializeNotifierThreaded(
     HAL_NotifierProcessFunction process, void* param, int32_t* status) {
   NotifierThreadOwner* notify = new NotifierThreadOwner;
   notify->Start();
   notify->SetFunc(process, param);

   auto notifierHandle =
       HAL_InitializeNotifier(threadedNotifierHandler, notify, status);

   if (notifierHandle == HAL_kInvalidHandle || *status != 0) {
     delete notify;
     return HAL_kInvalidHandle;
   }

   auto notifier = notifierHandles.Get(notifierHandle);
   if (!notifier) {
     return HAL_kInvalidHandle;
   }
   notifier->threaded = true;

   return notifierHandle;
 }

 void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
   {
     std::lock_guard<priority_recursive_mutex> sync(notifierMutex);
     auto notifier = notifierHandles.Get(notifierHandle);
     if (!notifier) return;

     // remove from list
     if (notifier->prev) notifier->prev->next = notifier->next;
     if (notifier->next) notifier->next->prev = notifier->prev;
     if (notifiers == notifier) notifiers = notifier->next;
     notifierHandles.Free(notifierHandle);

     if (notifier->threaded) {
       NotifierThreadOwner* owner =
           static_cast<NotifierThreadOwner*>(notifier->param);
       delete owner;
     }
   }

   if (notifierRefCount.fetch_sub(1) == 1) {
     std::lock_guard<priority_mutex> sync(notifierInterruptMutex);
     // 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_GetNotifierParam(HAL_NotifierHandle notifierHandle, int32_t* status) {
   auto notifier = notifierHandles.Get(notifierHandle);
   if (!notifier) return nullptr;
   if (notifier->threaded) {
     // If threaded, return thread param rather then notifier param
     NotifierThreadOwner* owner =
         static_cast<NotifierThreadOwner*>(notifier->param);
     return owner->m_param;
   }
   return notifier->param;
 }

 void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
                              uint64_t triggerTime, int32_t* status) {
   std::lock_guard<priority_recursive_mutex> sync(notifierMutex);

   auto notifier = notifierHandles.Get(notifierHandle);
   if (!notifier) return;
   updateNotifierAlarmInternal(notifier, triggerTime, status);
 }

 void HAL_StopNotifierAlarm(HAL_NotifierHandle notifierHandle, int32_t* status) {
   std::lock_guard<priority_recursive_mutex> sync(notifierMutex);
   auto notifier = notifierHandles.Get(notifierHandle);
   if (!notifier) return;
   notifier->triggerTime = UINT64_MAX;
 }

 }  // extern "C"
	/----------------------------------------------------------------------------/
	/* Copyright (c) FIRST 2016-2017. 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"

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

	#include <atomic>
	#include <memory>
	#include <mutex>

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

	using namespace hal;

	static const int32_t kTimerInterruptNumber = 28;

	static priority_mutex notifierInterruptMutex;
	static priority_recursive_mutex notifierMutex;
	static std::unique_ptr<tAlarm> notifierAlarm;
	static std::unique_ptr<tInterruptManager> notifierManager;
	static uint64_t closestTrigger = UINT64_MAX;

	namespace {
	struct Notifier {
	std::shared_ptr<Notifier> prev, next;
	void* param;
	HAL_NotifierProcessFunction process;
	uint64_t triggerTime = UINT64_MAX;
	HAL_NotifierHandle handle;
	bool threaded;
	};

	// Safe thread to allow callbacks to run on their own thread
	class NotifierThread : public wpi::SafeThread {
	public:
	void Main() {
	std::unique_lock<std::mutex> lock(m_mutex);
	while (m_active) {
	m_cond.wait(lock, [&] { return !m_active \|\| m_notify; });
	if (!m_active) break;
	m_notify = false;
	uint64_t currentTime = m_currentTime;
	HAL_NotifierHandle handle = m_handle;
	HAL_NotifierProcessFunction process = m_process;
	lock.unlock(); // don't hold mutex during callback execution
	process(currentTime, handle);
	lock.lock();
	}
	}

	bool m_notify = false;
	HAL_NotifierHandle m_handle = HAL_kInvalidHandle;
	HAL_NotifierProcessFunction m_process;
	uint64_t m_currentTime;
	};

	class NotifierThreadOwner : public wpi::SafeThreadOwner<NotifierThread> {
	public:
	void SetFunc(HAL_NotifierProcessFunction process, void* param) {
	auto thr = GetThread();
	if (!thr) return;
	thr->m_process = process;
	m_param = param;
	}

	void Notify(uint64_t currentTime, HAL_NotifierHandle handle) {
	auto thr = GetThread();
	if (!thr) return;
	thr->m_currentTime = currentTime;
	thr->m_handle = handle;
	thr->m_notify = true;
	thr->m_cond.notify_one();
	}

	void* m_param;
	};
	} // namespace

	static std::shared_ptr<Notifier> notifiers;
	static std::atomic_flag notifierAtexitRegistered = ATOMIC_FLAG_INIT;
	static std::atomic_int notifierRefCount{0};

	using namespace hal;

	static UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
	HAL_HandleEnum::Notifier>
	notifierHandles;

	// internal version of updateAlarm used during the alarmCallback when we know
	// that the pointer is a valid pointer.
	void updateNotifierAlarmInternal(std::shared_ptr<Notifier> notifierPointer,
	uint64_t triggerTime, int32_t* status) {
	std::lock_guard<priority_recursive_mutex> sync(notifierMutex);

	auto notifier = notifierPointer;
	// no need for a null check, as this must always be a valid pointer.
	notifier->triggerTime = triggerTime;
	bool wasActive = (closestTrigger != UINT64_MAX);

	if (!notifierInterruptMutex.try_lock() \|\| notifierRefCount == 0 \|\|
	!notifierAlarm)
	return;

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

	notifierInterruptMutex.unlock();
	}

	static void alarmCallback(uint32_t, void*) {
	std::unique_lock<priority_recursive_mutex> sync(notifierMutex);

	int32_t status = 0;
	uint64_t currentTime = 0;

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

	// process all notifiers
	std::shared_ptr<Notifier> notifier = notifiers;
	while (notifier) {
	if (notifier->triggerTime != UINT64_MAX) {
	if (currentTime == 0) currentTime = HAL_GetFPGATime(&status);
	if (notifier->triggerTime < currentTime) {
	notifier->triggerTime = UINT64_MAX;
	auto process = notifier->process;
	auto handle = notifier->handle;
	sync.unlock();
	process(currentTime, handle);
	sync.lock();
	} else if (notifier->triggerTime < closestTrigger) {
	updateNotifierAlarmInternal(notifier, notifier->triggerTime, &status);
	}
	}
	notifier = notifier->next;
	}
	}

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

	static void threadedNotifierHandler(uint64_t currentTimeInt,
	HAL_NotifierHandle handle) {
	// Grab notifier and get handler param
	auto notifier = notifierHandles.Get(handle);
	if (!notifier) return;
	auto notifierPointer = notifier->param;
	if (notifierPointer == nullptr) return;
	NotifierThreadOwner* owner =
	static_cast<NotifierThreadOwner*>(notifierPointer);
	owner->Notify(currentTimeInt, handle);
	}

	extern "C" {

	HAL_NotifierHandle HAL_InitializeNotifier(HAL_NotifierProcessFunction process,
	void* param, int32_t* status) {
	if (!process) {
	*status = NULL_PARAMETER;
	return 0;
	}
	if (!notifierAtexitRegistered.test_and_set())
	std::atexit(cleanupNotifierAtExit);
	if (notifierRefCount.fetch_add(1) == 0) {
	std::lock_guard<priority_mutex> sync(notifierInterruptMutex);
	// 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::lock_guard<priority_recursive_mutex> sync(notifierMutex);
	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;
	}
	// create notifier structure and add to list
	notifier->next = notifiers;
	if (notifier->next) notifier->next->prev = notifier;
	notifier->param = param;
	notifier->process = process;
	notifier->handle = handle;
	notifier->threaded = false;
	notifiers = notifier;
	return handle;
	}

	HAL_NotifierHandle HAL_InitializeNotifierThreaded(
	HAL_NotifierProcessFunction process, void* param, int32_t* status) {
	NotifierThreadOwner* notify = new NotifierThreadOwner;
	notify->Start();
	notify->SetFunc(process, param);

	auto notifierHandle =
	HAL_InitializeNotifier(threadedNotifierHandler, notify, status);

	if (notifierHandle == HAL_kInvalidHandle \|\| *status != 0) {
	delete notify;
	return HAL_kInvalidHandle;
	}

	auto notifier = notifierHandles.Get(notifierHandle);
	if (!notifier) {
	return HAL_kInvalidHandle;
	}
	notifier->threaded = true;

	return notifierHandle;
	}

	void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
	{
	std::lock_guard<priority_recursive_mutex> sync(notifierMutex);
	auto notifier = notifierHandles.Get(notifierHandle);
	if (!notifier) return;

	// remove from list
	if (notifier->prev) notifier->prev->next = notifier->next;
	if (notifier->next) notifier->next->prev = notifier->prev;
	if (notifiers == notifier) notifiers = notifier->next;
	notifierHandles.Free(notifierHandle);

	if (notifier->threaded) {
	NotifierThreadOwner* owner =
	static_cast<NotifierThreadOwner*>(notifier->param);
	delete owner;
	}
	}

	if (notifierRefCount.fetch_sub(1) == 1) {
	std::lock_guard<priority_mutex> sync(notifierInterruptMutex);
	// 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_GetNotifierParam(HAL_NotifierHandle notifierHandle, int32_t* status) {
	auto notifier = notifierHandles.Get(notifierHandle);
	if (!notifier) return nullptr;
	if (notifier->threaded) {
	// If threaded, return thread param rather then notifier param
	NotifierThreadOwner* owner =
	static_cast<NotifierThreadOwner*>(notifier->param);
	return owner->m_param;
	}
	return notifier->param;
	}

	void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
	uint64_t triggerTime, int32_t* status) {
	std::lock_guard<priority_recursive_mutex> sync(notifierMutex);

	auto notifier = notifierHandles.Get(notifierHandle);
	if (!notifier) return;
	updateNotifierAlarmInternal(notifier, triggerTime, status);
	}

	void HAL_StopNotifierAlarm(HAL_NotifierHandle notifierHandle, int32_t* status) {
	std::lock_guard<priority_recursive_mutex> sync(notifierMutex);
	auto notifier = notifierHandles.Get(notifierHandle);
	if (!notifier) return;
	notifier->triggerTime = UINT64_MAX;
	}

	} // extern "C"