/* * Timing.c * mpsLinkProcessor * * Created by Stephen Norum. * Copyright 2007 Stanford Linear Accelerator Center. All rights reserved. * */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "Timing.h" #include "TimingEvr.h" #include "TimingEmulatedEvr.h" #include "MachineStateBuffer.h" #include "MachineRatesBuffer.h" #include "LinkNodeCommunication.h" #include "AlgorithmWrapper.h" #include "Mitigation.h" #include "HistoryCommunication.h" #include "MPSConfiguration.h" #include "Priority.h" #define TIMESTAMP_PULSE_ID(_epicsTimeStamp) ((_epicsTimeStamp).nsec & LOWER_17_BIT_MASK) #define TIMESTAMP_NSEC_WITHOUT_PULSE_ID(_epicsTimeStamp) ((_epicsTimeStamp).nsec & UPPER_15_BIT_MASK) /* No prototype for strtok_r with -ansi in RTEMS. Prototype it here. */ #ifdef __rtems__ char *strtok_r(char *s, const char *sep, char **lasts); #endif /* Mutexes and Events */ static epicsMutexId timingMutex = 0; static epicsMutexId queuedTimingDataMutex = 0; static epicsEventId timingShouldRunEvent = 0; static int timingThreadIsProcessing = epicsFalse; static MPSTimingTimeslots mpsTimeslotConfig = {0, 0, 0}; static unsigned int timingIsStable = epicsFalse; /* Recent Timing Data (info for the last three timeslots) */ static MPSTimingDataNode *recentTimingDataHead; static unsigned int recentTimingDataCount = 0; int (*copyTimingData)(MPSTimingData *timingData); #ifdef __rtems__ #include #include #include static unsigned int ticksBetweenPermitMessagesThen = 0; static unsigned int ticksBetweenPermitMessagesNow = 0; static unsigned int ticksBetweenPermitMessages = 0; static unsigned int ticksBetweenPermitMessagesMin = -1; static unsigned int ticksBetweenPermitMessagesMax = 0; static unsigned int ticksAlgorithmDurationThen = 0; static unsigned int ticksAlgorithmDurationNow = 0; static unsigned int ticksAlgorithmDuration = 0; static unsigned int ticksAlgorithmDurationMin = -1; static unsigned int ticksAlgorithmDurationMax = 0; static unsigned int timingCodeDurationInTicksThen; static unsigned int timingCodeDurationInTicksNow; static unsigned int timingCodeDurationInTicks; static unsigned int timingCodeDurationInTicksMin = -1; static unsigned int timingCodeDurationInTicksMax = 0; void mpsTimingShowProcessInMicroseconds() { double megahertz = (double) BSP_bus_frequency / (double) BSP_time_base_divisor / 1000.0f; printf("Times represented as us, ticks\n"); printf("------------------------------\n"); printf("Cur Timing Processing: %g, %u\n", timingCodeDurationInTicks / megahertz, timingCodeDurationInTicks); printf("Min Timing Processing: %g, %u\n", timingCodeDurationInTicksMin / megahertz, timingCodeDurationInTicksMin); printf("Max Timing Processing: %g, %u\n\n", timingCodeDurationInTicksMax / megahertz, timingCodeDurationInTicksMax); printf("Cur Alg Processing: %g, %u\n", ticksAlgorithmDuration / megahertz, ticksAlgorithmDuration); printf("Min Alg Processing: %g, %u\n", ticksAlgorithmDurationMin / megahertz, ticksAlgorithmDurationMin); printf("Max Alg Processing: %g, %u\n\n", ticksAlgorithmDurationMax / megahertz, ticksAlgorithmDurationMax); printf("Cur Time Between Permit Send Calls: %g, %u\n", ticksBetweenPermitMessages / megahertz, ticksBetweenPermitMessages); printf("Min Time Between Permit Send Calls: %g, %u\n", ticksBetweenPermitMessagesMin / megahertz, ticksBetweenPermitMessagesMin); printf("Max Time Between Permit Send Calls: %g, %u\n\n", ticksBetweenPermitMessagesMax / megahertz, ticksBetweenPermitMessagesMax); } void mpsResetTimingMinMax() { timingCodeDurationInTicksMin = -1; timingCodeDurationInTicksMax = 0; ticksAlgorithmDurationMin = -1; ticksAlgorithmDurationMax = 0; ticksBetweenPermitMessagesNow = 0; ticksBetweenPermitMessagesThen = 0; ticksBetweenPermitMessages = 0; ticksBetweenPermitMessagesMin = -1; ticksBetweenPermitMessagesMax = 0; } #endif int mpsPrintTimingData(MPSTimingData *td, unsigned int n) { int i; const char *yesString = "X"; const char *noString = "-"; printf("\n Seconds:"); for (i = 0; i < n; i++) { printf(" %9d", td[i].timestamp.secPastEpoch); } printf("\nNanoseconds:"); for (i = 0; i < n; i++) { printf(" %9d", TIMESTAMP_NSEC_WITHOUT_PULSE_ID(td[i].timestamp)); } printf("\n Timeslot:"); for (i = 0; i < n; i++) { printf(" %9d", td[i].timeslot); } printf("\n Pulse ID:"); for (i = 0; i < n; i++) { printf(" 0x%05X", td[i].pulseid); } printf("\n Beam Req:"); for (i = 0; i < n; i++) { printf(" %9s", td[i].beamRequested ? yesString : noString); } printf("\nBurst Pulse:"); for (i = 0; i < n; i++) { printf(" %9s", td[i].burstPulseRequested ? yesString : noString); } printf("\nBase 120 Hz:"); for (i = 0; i < n; i++) { printf(" %9s", td[i].baseRate120Hz ? yesString : noString); } printf("\n Base 60 Hz:"); for (i = 0; i < n; i++) { printf(" %9s", td[i].baseRate60Hz ? yesString : noString); } printf("\n Base 30 Hz:"); for (i = 0; i < n; i++) { printf(" %9s", td[i].baseRate30Hz ? yesString : noString); } printf("\n Base 10 Hz:"); for (i = 0; i < n; i++) { printf(" %9s", td[i].baseRate10Hz ? yesString : noString); } printf("\n Base 1 Hz:"); for (i = 0; i < n; i++) { printf(" %9s", td[i].baseRate1Hz ? yesString : noString); } printf("\n"); return MPS_SUCCESS; } int mpsShowTimingStatus() { int i, n; MPSTimingDataNode *mtdn; static MPSTimingData recentTimingDataCopy[MPSTimingMaxTimingDataQueueLength]; if (timingMutex == 0) { return MPS_ERROR; } epicsMutexLock(timingMutex); /* Copy recent timing data */ n = recentTimingDataCount; mtdn = recentTimingDataHead; for (i = 0; i < recentTimingDataCount; i++) { recentTimingDataCopy[i] = mtdn->timingData; mtdn = mtdn->next; } epicsMutexUnlock(timingMutex); /* Print copied info */ printf("Recent timing data (current, previous, before previous):\n"); mpsPrintTimingData(recentTimingDataCopy, n); printf("\n"); printf("Base Rate 120 Hz Timeslot 1: %u\n", mpsTimeslotConfig.baseRate120HzTimeslot1); printf("Base Rate 120 Hz Timeslot 2: %u\n", mpsTimeslotConfig.baseRate120HzTimeslot2); printf("Base Rate 60 Hz Timeslot : %u\n", mpsTimeslotConfig.baseRate60HzTimeslot); return MPS_SUCCESS; } int setTimingDataToWorstCase(MPSTimingData *timingData) { static MPSTimingData MPSTimingWorstCaseTimingData = { {0, 0}, /* timestamp */ PULSEID_INVALID, /* pulseid */ 0, /* timeslot */ epicsTrue, /* beamRequested */ epicsFalse, /* burstPulseRequested */ epicsTrue, /* baseRate120Hz */ epicsFalse, /* baseRate60Hz */ epicsFalse, /* baseRate30Hz */ epicsFalse, /* baseRate10Hz */ epicsFalse /* baseRate1Hz */ }; *timingData = MPSTimingWorstCaseTimingData; return MPS_SUCCESS; } #if 0 #pragma mark - #pragma mark LCLS Timeslots #endif int mpsSetLclsTimeslots(epicsUInt8 baseRate120HzTimeslot1, epicsUInt8 baseRate120HzTimeslot2, epicsUInt8 baseRate60HzTimeslot) { if ((baseRate120HzTimeslot1 < MPSTimingTimeslotNumberMin) || (baseRate120HzTimeslot1 > MPSTimingTimeslotNumberMax) || (baseRate120HzTimeslot2 < MPSTimingTimeslotNumberMin) || (baseRate120HzTimeslot2 > MPSTimingTimeslotNumberMax) ) { return MPS_ERROR; } /* baseRate60HzTimeslot must be one of the 120 Hz base rate timeslots */ if ((baseRate60HzTimeslot != baseRate120HzTimeslot1) && (baseRate60HzTimeslot != baseRate120HzTimeslot2) ) { return MPS_ERROR; } mpsTimeslotConfig.baseRate120HzTimeslot1 = baseRate120HzTimeslot1; mpsTimeslotConfig.baseRate120HzTimeslot2 = baseRate120HzTimeslot2; mpsTimeslotConfig.baseRate60HzTimeslot = baseRate60HzTimeslot; return MPS_SUCCESS; } #if 0 #pragma mark - #pragma mark Accessors #endif /* Must lock timingMutex before calling */ int getCurrentTimingData(MPSTimingData *data) { if (recentTimingDataCount > 0) { *data = recentTimingDataHead->timingData; } else { setTimingDataToWorstCase(data); } return MPS_SUCCESS; } int lockTimingMutexAndGetCurrentTimingData(MPSTimingData *data) { if (timingMutex == 0) { return MPS_ERROR; } epicsMutexLock(timingMutex); getCurrentTimingData(data); epicsMutexUnlock(timingMutex); return MPS_SUCCESS; } #if 0 #pragma mark - #pragma mark Timing Validation #endif int lockTimingMutexAndReturnIsRecentTimestamp(const epicsTimeStamp *aTimestamp) { int i; int status = epicsFalse; MPSTimingDataNode *timingDataNode = recentTimingDataHead; epicsMutexLock(timingMutex); for (i = 0; i < recentTimingDataCount; i++) { if ((timingDataNode->timingData.timestamp.secPastEpoch == aTimestamp->secPastEpoch) && (timingDataNode->timingData.timestamp.nsec == aTimestamp->nsec)) { status = epicsTrue; break; } timingDataNode = timingDataNode->next; } epicsMutexUnlock(timingMutex); return status; } int timingDataIsValid(const MPSTimingData *timingData, int count) { int i; /* Check pulse IDs validity */ for (i = 0; i < count; i++) { if(timingData[i].pulseid > PULSEID_MAX) { return epicsFalse; } } /* Check pulse IDs continuity */ for (i = 0; i < (count - 1); i++) { if ((timingData[i].pulseid + 1) % (PULSEID_MAX + 1) != (timingData[i + 1].pulseid)) { return epicsFalse; } } return epicsTrue; } static int updateRecentTimingData(MPSTimingData *queuedTimingDataArray) { double timeDifference; if (timingMutex == 0) { return MPS_ERROR; } /* * Update the recent timing data */ epicsMutexLock(timingMutex); /* Get the time difference between the current timestamp and the previous timestamp */ if (recentTimingDataCount > 0) { timeDifference = epicsTimeDiffInSeconds(&(queuedTimingDataArray[MPSTimingIndexCurrent].timestamp), &(recentTimingDataHead->timingData.timestamp)); } else { /* Nothing to test against. Set the timeDifference outside the timestamp continuity range */ timeDifference = MPSTimingTimestampContinuityRangeHigh + 1; } if((timeDifference > MPSTimingTimestampContinuityRangeLow ) && (timeDifference < MPSTimingTimestampContinuityRangeHigh)) { /* The current timestamp is about 2.77 ms older than the previous timestamp. * Shift the timestamps down. */ recentTimingDataHead = recentTimingDataHead->prev; } else { /* No recent timestamps stored, or current timestamp is not about 2.78 ms older than the previous timestamp. * Reset the number of recent timestamps to 0. */ recentTimingDataCount = 0; } /* Store the new data */ recentTimingDataHead->timingData = queuedTimingDataArray[MPSTimingIndexCurrent]; if (recentTimingDataCount < MPSTimingMaxNumberOfRelevantTimestamps) { recentTimingDataCount++; } if (recentTimingDataCount == MPSTimingMaxNumberOfRelevantTimestamps) { timingIsStable = epicsTrue; deviceStateMakeOk(LP_TIMING_STABLE__INDEX, queuedTimingDataArray[MPSTimingIndexCurrent].timestamp, queuedTimingDataArray[MPSTimingIndexCurrent].timeslot); } else { timingIsStable = epicsFalse; deviceStateMakeFaulted(LP_TIMING_STABLE__INDEX, queuedTimingDataArray[MPSTimingIndexCurrent].timestamp, queuedTimingDataArray[MPSTimingIndexCurrent].timeslot); } epicsMutexUnlock(timingMutex); return MPS_SUCCESS; } int mpsTimingIsStable(epicsUInt32 *val) { *val = timingIsStable; return MPS_SUCCESS; } int mpsTimingSourceIsEvg(epicsUInt32 *val) { char *envUseEmulatedEvr; envUseEmulatedEvr = getenv(MPS_ENV_TIMING_USE_EMULATED_EVR); if (envUseEmulatedEvr == NULL) { /* Not using emulated EVR (timing source is EVG) */ *val = epicsTrue; } else { /* Using emulated EVR (timing source is internal) */ *val = epicsFalse; } return MPS_SUCCESS; } #if 0 #pragma mark - #pragma mark Timing Thread #endif static inline void sendDestinationChangeHistoryMessage(MPSTimingData *timingData, MPSBeamDestination oldDestination, MPSBeamDestination currDestination) { MPSHistoryMessage message; if (oldDestination == currDestination) { return; } message.id = 0; message.type = MPSHistoryMessageTypeDestination; message.timestampWithPulseId = timingData->timestamp; message.timeslot = timingData->timeslot; message.oldValue = oldDestination; message.newValue = currDestination; mpsHistoryAddMessageToQueue(&message, sizeof(message)); } static inline void sendRateChangeHistoryMessage(MPSTimingData *timingData, MPSBeamRate *oldRates, MPSBeamRate *newRates, unsigned int numberOfRates) { int i; MPSHistoryMessage message; message.type = MPSHistoryMessageTypeRate; message.timestampWithPulseId = timingData->timestamp; message.timeslot = timingData->timeslot; for (i = 0; i < numberOfRates; i++) { if (oldRates[i] == newRates[i]) { continue; } message.id = i; message.oldValue = oldRates[i]; message.newValue = newRates[i]; mpsHistoryAddMessageToQueue(&message, sizeof(message)); } } void timingThread (void *arg) { static int i, err; static int timingHasInitialized = epicsFalse; static epicsEventWaitStatus waitStatus; static MPSTimingData mpsTimingQueuedTimingData[MPSTimingMaxTimingDataQueueLength]; static MPSTimingData *currentTimingData = &(mpsTimingQueuedTimingData[MPSTimingIndexCurrent]); static MPSBeamRate prevMaxRateAfterDevice[MPSMitigationDeviceCount]; static MPSBeamRate currMaxRateAfterDevice[MPSMitigationDeviceCount]; static MPSBeamDestination prevDestination = MPSDestinationInvalid; static MPSBeamDestination currDestination = MPSDestinationInvalid; static MPSLinkPermitMessage permitMessage; for (i = 0; i < MPSMitigationDeviceCount; i++) { prevMaxRateAfterDevice[i] = MPSRate120Hz; currMaxRateAfterDevice[i] = MPSRate120Hz; } while(1) { if (timingShouldRunEvent == 0) { cantProceed("Error: timingThread's timingShouldRunEvent is not configured\n"); return; } waitStatus = epicsEventWaitWithTimeout(timingShouldRunEvent, MPSTimingEvrTimeoutInSeconds); #ifdef __rtems__ /* Get current time from high res clock */ timingCodeDurationInTicksThen = read_timebase(); #endif switch (waitStatus) { case epicsEventWaitOK: break; case epicsEventWaitTimeout: break; default: errlogPrintf("timingThread: epicsEventWaitWithTimeout on timingShouldRunEvent returned %d", waitStatus); break; } copyTimingData(mpsTimingQueuedTimingData); updateRecentTimingData(mpsTimingQueuedTimingData); if ((timingHasInitialized == epicsFalse) && (currentTimingData->timeslot == 0)) { goto FINISH_TIMING_LOOP; } else { timingHasInitialized = epicsTrue; } /* ------------------------------------------------------------ */ /* Check past Link Node responses. Request more. */ /* ------------------------------------------------------------ */ /* 360 Hz Link Node communication cleanup */ err = cleanupLinkNodeCommunication(currentTimingData); /* Request Status Update from Link Nodes */ err = broadcastLinkSynchronizationMessage(currentTimingData); /* ------------------------------------------------------------ */ /* Determine maximum allowed beam rates and send permit message */ /* ------------------------------------------------------------ */ mpsMutexLockMachineRatesBuffer(); mpsMutexLockMachineStateBuffer(); /* Get current beam rates and destination */ getRatesAfterAllDevices(prevMaxRateAfterDevice); prevDestination = getBeamDestination(); /* Set all rates to most restrictive and clear burst modes */ setAllRatesToMostRestrictiveAndClearBurstModes(); /* Set all truth table states to invalid */ setAllTruthTableStatesToInvalid(); /* Run algorithm only if timing is stable */ if (timingIsStable) { #ifdef __rtems__ ticksAlgorithmDurationThen = read_timebase(); #endif algorithmRun(&(mpsTimingQueuedTimingData[MPSTimingIndexCurrent])); #ifdef __rtems__ ticksAlgorithmDurationNow = read_timebase(); ticksAlgorithmDuration = ticksAlgorithmDurationNow - ticksAlgorithmDurationThen; if (ticksAlgorithmDuration < ticksAlgorithmDurationMin) { ticksAlgorithmDurationMin = ticksAlgorithmDuration; } if (ticksAlgorithmDuration > ticksAlgorithmDurationMax) { ticksAlgorithmDurationMax = ticksAlgorithmDuration; } #endif } /* Generate Permit Message -- Must only be called at or less than 360 Hz */ err = mitigateFaultsAndGeneratePermitMessage(mpsTimingQueuedTimingData, sizeof(mpsTimingQueuedTimingData) / sizeof(mpsTimingQueuedTimingData[0]), &permitMessage); /* TODO: Determine beam destination */ setBeamDestination(MPSDestinationInvalid); /* Broadcast permit message */ #ifdef __rtems__ /* Get current time from high res clock */ if (ticksBetweenPermitMessagesNow == 0) { ticksBetweenPermitMessagesNow = read_timebase(); } else { ticksBetweenPermitMessagesThen = ticksBetweenPermitMessagesNow; ticksBetweenPermitMessagesNow = read_timebase(); ticksBetweenPermitMessages = ticksBetweenPermitMessagesNow - ticksBetweenPermitMessagesThen; if (ticksBetweenPermitMessages < ticksBetweenPermitMessagesMin) { ticksBetweenPermitMessagesMin = ticksBetweenPermitMessages; } if (ticksBetweenPermitMessages > ticksBetweenPermitMessagesMax) { ticksBetweenPermitMessagesMax = ticksBetweenPermitMessages; } } #endif err = broadcastPermitMessage(&permitMessage); /* FIXME: have mitigateFaultsAndGeneratePermitMessage return the destination? */ /* TODO: Get current beam rates and destination. If any have changed, send history message. */ getRatesAfterAllDevices(currMaxRateAfterDevice); currDestination = getBeamDestination(); mpsMutexUnlockMachineStateBuffer(); mpsMutexUnlockMachineRatesBuffer(); /* ------------------------------------------------------------ */ /* Cleanup */ /* ------------------------------------------------------------ */ /* Send history messages */ sendDestinationChangeHistoryMessage(currentTimingData, prevDestination, currDestination); sendRateChangeHistoryMessage(currentTimingData, prevMaxRateAfterDevice, currMaxRateAfterDevice, MPSMitigationDeviceCount); /************************************************************************************* * Timing thread has finished processing *************************************************************************************/ FINISH_TIMING_LOOP: epicsMutexLock(queuedTimingDataMutex); timingThreadIsProcessing = epicsFalse; epicsMutexUnlock(queuedTimingDataMutex); #ifdef __rtems__ timingCodeDurationInTicksNow = read_timebase(); timingCodeDurationInTicks = timingCodeDurationInTicksNow - timingCodeDurationInTicksThen; if (timingCodeDurationInTicks < timingCodeDurationInTicksMin) { timingCodeDurationInTicksMin = timingCodeDurationInTicks; } if (timingCodeDurationInTicks > timingCodeDurationInTicksMax) { timingCodeDurationInTicksMax = timingCodeDurationInTicks; } #endif } } #if 0 #pragma mark - #pragma mark Initialization #endif int intializeTiming() { int i; int err; long baseRate120HzTimeslot1; long baseRate120HzTimeslot2; long baseRate60HzTimeslot; char *envString; char *envUseEmulatedEvr; /* Initialize Recent Timedata Ring Buffer */ recentTimingDataHead = (MPSTimingDataNode *) callocMustSucceed(MPSTimingMaxNumberOfRelevantTimestamps, sizeof(MPSTimingDataNode), "intializeTiming"); recentTimingDataHead->prev = recentTimingDataHead + MPSTimingMaxNumberOfRelevantTimestamps - 1; recentTimingDataHead->prev->next = recentTimingDataHead; for(i = 0; i < MPSTimingMaxNumberOfRelevantTimestamps - 1; i++) { recentTimingDataHead->next = recentTimingDataHead + 1; recentTimingDataHead->next->prev = recentTimingDataHead; recentTimingDataHead = recentTimingDataHead->next; } /* Mutexes */ timingMutex = epicsMutexCreate(); if (timingMutex == 0) { errlogPrintf("intializeTiming: unable to create timingMutex\n"); return MPS_ERROR; } queuedTimingDataMutex = epicsMutexCreate(); if (queuedTimingDataMutex == 0) { errlogPrintf("intializeTiming: unable to create queuedTimingDataMutex\n"); return MPS_ERROR; } /* Timeslots */ envString = getenv(MPS_ENV_TIMING_BASE_RATE_120_HZ_TIMESLOT_1); baseRate120HzTimeslot1 = strtol(envString, (char **) NULL, 10); envString = getenv(MPS_ENV_TIMING_BASE_RATE_120_HZ_TIMESLOT_2); baseRate120HzTimeslot2 = strtol(envString, (char **) NULL, 10); envString = getenv(MPS_ENV_TIMING_BASE_RATE_60_HZ_TIMESLOT); baseRate60HzTimeslot = strtol(envString, (char **) NULL, 10); err = mpsSetLclsTimeslots(baseRate120HzTimeslot1, baseRate120HzTimeslot2, baseRate60HzTimeslot); if (err != MPS_SUCCESS) { errlogPrintf("intializeTiming: bad base rate timeslots env vars (%ld, %ld, %ld)\n", baseRate120HzTimeslot1, baseRate120HzTimeslot2, baseRate60HzTimeslot ); return MPS_ERROR; } /* Handle timing using timingThread when timingShouldRunEvent event occurs */ timingShouldRunEvent = epicsEventMustCreate(epicsEventEmpty); err = (int) epicsThreadCreate("mpsTimingThread", MPSThreadPriorityHighest, epicsThreadGetStackSize(epicsThreadStackMedium), (EPICSTHREADFUNC) timingThread, NULL); if (err == 0) { errlogPrintf("Error: initializeAlgorithm could not create algorithmThread\n"); return MPS_ERROR_THREAD_CREATION_FAILED; } envUseEmulatedEvr = getenv(MPS_ENV_TIMING_USE_EMULATED_EVR); if (envUseEmulatedEvr == NULL) { err = initializeEvr(&queuedTimingDataMutex, &timingShouldRunEvent, &timingThreadIsProcessing, &mpsTimeslotConfig); copyTimingData = evrCopyTimingData; } else { errlogPrintf("intializeTiming: Warning! Using emulated timing!\n"); errlogPrintf("intializeTiming: Warning! Using emulated timing!\n"); err = initializeEvrEmulated(&queuedTimingDataMutex, &timingShouldRunEvent, &timingThreadIsProcessing, &mpsTimeslotConfig); copyTimingData = evrCopyTimingDataEmulated; errlogPrintf("intializeTiming: Warning! Using emulated timing!\n"); errlogPrintf("intializeTiming: Warning! Using emulated timing!\n"); } return err; }