Keep track of FPGA time rollovers with 64-bit time.

This allows both greater than 72 minute (2^32 * 1 us) timeouts and also
gracefully handles notifiers across the FPGA time counter rollover.

Change-Id: Ibde0b903155f60b618b0ca4d5f8f6dd49f90b020

hal/include/HAL/HAL.hpp

@@ -222,7 +222,7 @@ extern "C"
 
 	uint16_t getFPGAVersion(int32_t *status);
 	uint32_t getFPGARevision(int32_t *status);
-	uint32_t getFPGATime(int32_t *status);
+	uint64_t getFPGATime(int32_t *status);
 
 	bool getFPGAButton(int32_t *status);
 

hal/include/HAL/Notifier.hpp

@@ -4,9 +4,9 @@
 
 extern "C"
 {
-	void* initializeNotifier(void (*process)(uint32_t, void*), void* param, int32_t *status);
+	void* initializeNotifier(void (*process)(uint64_t, void*), void* param, int32_t *status);
 	void cleanNotifier(void* notifier_pointer, int32_t *status);
 	void* getNotifierParam(void* notifier_pointer, int32_t *status);
-	void updateNotifierAlarm(void* notifier_pointer, uint32_t triggerTime, int32_t *status);
+	void updateNotifierAlarm(void* notifier_pointer, uint64_t triggerTime, int32_t *status);
 	void stopNotifierAlarm(void* notifier_pointer, int32_t *status);
 }

hal/lib/Athena/Digital.cpp

@@ -88,7 +88,7 @@ tSPI *spiSystem;
 
 struct SPIAccumulator {
 	void* notifier = nullptr;
-	uint32_t triggerTime;
+	uint64_t triggerTime;
 	uint32_t period;
 
 	int64_t value = 0;
@@ -1499,7 +1499,7 @@ priority_recursive_mutex& spiGetSemaphore(uint8_t port) {
 		return spiMXPSemaphore;
 }
 
-static void spiAccumulatorProcess(uint32_t currentTime, void *param) {
+static void spiAccumulatorProcess(uint64_t currentTime, void *param) {
 	SPIAccumulator* accum = (SPIAccumulator*)param;
 
 	// perform SPI transaction

hal/lib/Athena/HALAthena.cpp

@@ -12,6 +12,7 @@
 #include <cstdlib>
 #include <fstream>
 #include <iostream>
+#include <mutex>
 #include <unistd.h>
 #include <sys/prctl.h>
 #include <signal.h> // linux for kill
@@ -23,6 +24,11 @@ const uint32_t kSystemClockTicksPerMicrosecond = 40;
 static tGlobal *global = nullptr;
 static tSysWatchdog *watchdog = nullptr;
 
+static priority_mutex timeMutex;
+static uint32_t timeEpoch = 0;
+static uint32_t prevFPGATime = 0;
+static void* rolloverNotifier = nullptr;
+
 void* getPort(uint8_t pin)
 {
 	Port* port = new Port();
@@ -184,13 +190,19 @@ uint32_t getFPGARevision(int32_t *status)
  *
  * @return The current time in microseconds according to the FPGA (since FPGA reset).
  */
-uint32_t getFPGATime(int32_t *status)
+uint64_t getFPGATime(int32_t *status)
 {
 	if (!global) {
 		*status = NiFpga_Status_ResourceNotInitialized;
 		return 0;
 	}
-	return global->readLocalTime(status);
+	std::lock_guard<priority_mutex> lock(timeMutex);
+	uint32_t fpgaTime = global->readLocalTime(status);
+	if (*status != 0) return 0;
+	// check for rollover
+	if (fpgaTime < prevFPGATime) ++timeEpoch;
+	prevFPGATime = fpgaTime;
+	return (((uint64_t)timeEpoch) << 32) | ((uint64_t)fpgaTime);
 }
 
 /**
@@ -235,6 +247,12 @@ static void HALCleanupAtExit() {
 	watchdog = nullptr;
 }
 
+static void timerRollover(uint64_t currentTime, void*) {
+	// reschedule timer for next rollover
+	int32_t status = 0;
+	updateNotifierAlarm(rolloverNotifier, currentTime + 0x80000000ULL, &status);
+}
+
 /**
  * Call this to start up HAL. This is required for robot programs.
  */
@@ -256,6 +274,14 @@ int HALInitialize(int mode)
 
 	std::atexit(HALCleanupAtExit);
 
+	if (!rolloverNotifier)
+		rolloverNotifier = initializeNotifier(timerRollover, nullptr, &status);
+	if (status == 0) {
+		uint64_t curTime = getFPGATime(&status);
+		if (status == 0)
+			updateNotifierAlarm(rolloverNotifier, curTime + 0x80000000ULL, &status);
+	}
+
 	// Kill any previous robot programs
 	std::fstream fs;
 	// By making this both in/out, it won't give us an error if it doesnt exist
@@ -302,6 +328,7 @@ int HALInitialize(int mode)
 	pid = getpid();
 	fs << pid << std::endl;
 	fs.close();
+
 	return 1;
 }
 

hal/lib/Athena/Notifier.cpp

@@ -12,12 +12,12 @@ static priority_mutex notifierInterruptMutex;
 static priority_recursive_mutex notifierMutex;
 static tAlarm *notifierAlarm = nullptr;
 static tInterruptManager *notifierManager = nullptr;
-static uint32_t closestTrigger = UINT32_MAX;
+static uint64_t closestTrigger = UINT64_MAX;
 struct Notifier {
 	Notifier *prev, *next;
 	void *param;
-	void (*process)(uint32_t, void*);
-	uint32_t triggerTime = UINT32_MAX;
+	void (*process)(uint64_t, void*);
+	uint64_t triggerTime = UINT64_MAX;
 };
 static Notifier *notifiers = nullptr;
 static std::atomic_flag notifierAtexitRegistered = ATOMIC_FLAG_INIT;
@@ -28,19 +28,19 @@ static void alarmCallback(uint32_t, void*)
 	std::unique_lock<priority_recursive_mutex> sync(notifierMutex);
 
 	int32_t status = 0;
-	uint32_t currentTime = 0;
+	uint64_t currentTime = 0;
 
 	// the hardware disables itself after each alarm
-	closestTrigger = UINT32_MAX;
+	closestTrigger = UINT64_MAX;
 
 	// process all notifiers
 	Notifier *notifier = notifiers;
 	while (notifier) {
-		if (notifier->triggerTime != UINT32_MAX) {
+		if (notifier->triggerTime != UINT64_MAX) {
 			if (currentTime == 0)
 				currentTime = getFPGATime(&status);
 			if (notifier->triggerTime < currentTime) {
-				notifier->triggerTime = UINT32_MAX;
+				notifier->triggerTime = UINT64_MAX;
 				auto process = notifier->process;
 				auto param = notifier->param;
 				sync.unlock();
@@ -59,7 +59,7 @@ static void cleanupNotifierAtExit() {
 	notifierManager = nullptr;
 }
 
-void* initializeNotifier(void (*process)(uint32_t, void*), void *param, int32_t *status)
+void* initializeNotifier(void (*process)(uint64_t, void*), void *param, int32_t *status)
 {
 	if (!process) {
 		*status = NULL_PARAMETER;
@@ -124,22 +124,23 @@ void* getNotifierParam(void* notifier_pointer, int32_t *status)
 	return ((Notifier*)notifier_pointer)->param;
 }
 
-void updateNotifierAlarm(void* notifier_pointer, uint32_t triggerTime, int32_t *status)
+void updateNotifierAlarm(void* notifier_pointer, uint64_t triggerTime, int32_t *status)
 {
 	std::lock_guard<priority_recursive_mutex> sync(notifierMutex);
 
 	Notifier* notifier = (Notifier*)notifier_pointer;
 	notifier->triggerTime = triggerTime;
-	bool wasActive = (closestTrigger != UINT32_MAX);
+	bool wasActive = (closestTrigger != UINT64_MAX);
 
-        if (!notifierInterruptMutex.try_lock() || notifierRefCount == 0 ||
-            !notifierAlarm)
-          return;
+	if (!notifierInterruptMutex.try_lock() || notifierRefCount == 0 ||
+			!notifierAlarm)
+		return;
 
-        // Update alarm time if closer than current.
+	// Update alarm time if closer than current.
 	if (triggerTime < closestTrigger) {
 		closestTrigger = triggerTime;
-		notifierAlarm->writeTriggerTime(triggerTime, status);
+		// Simply truncate the hardware trigger time to 32-bit.
+		notifierAlarm->writeTriggerTime((uint32_t)triggerTime, status);
 	}
 	// Enable the alarm.  The hardware disables itself after each alarm.
 	if (!wasActive) notifierAlarm->writeEnable(true, status);
@@ -151,5 +152,5 @@ void stopNotifierAlarm(void* notifier_pointer, int32_t *status)
 {
 	std::lock_guard<priority_recursive_mutex> sync(notifierMutex);
 	Notifier* notifier = (Notifier*)notifier_pointer;
-	notifier->triggerTime = UINT32_MAX;
+	notifier->triggerTime = UINT64_MAX;
 }