/* * LinkNodeCommunication.c * LinkProcessorEmulator * * 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 */ #include #include #include #include #include #include #include "MachineStateBuffer.h" #include "LinkNodeCommunication.h" #include "Timing.h" #include "Priority.h" /* If true, V2 sync, permit, and unlatch messages will be sent and V2 digital input status messages will be handled */ epicsUInt8 handleMlpV2 = epicsFalse; /* Local global variables */ static epicsMutexId responseRegisterMutex; static int linkNodeSocket = -1; static uint32_t linkNodeBroadcastIP = -1; static int canBroadcastMessages = epicsTrue; static int canSendPermitMessages = epicsTrue; static epicsInt8 statusMessageResponseRegister[MPSLinkNodeMaxCount]; static epicsTimeStamp newestReceivedTimestamp[MPSLinkNodeMaxCount]; static int missingStatusMessageCount[MPSLinkNodeMaxCount]; static unsigned int receivedLinkNodeId[MPSLinkNodeMaxCount+1]; #ifdef BSDSOCKET static const char *MPSLinkNodeCommunicationBroadcastIP = "255.255.255.255"; static int linkNodeResponseSocket; static epicsMutexId linkNodeResponseSocketMutex; #else static const char *MPSLinkNodeCommunicationBroadcastIP = "255.255.255.255"; #endif /* Test variables */ #ifdef LINK_NODE_COMM_TIME_DIAG #ifdef __rtems__ static uint32_t sentTime; static uint32_t receivedTime[MPSLinkNodeMaxCount + 1]; static uint32_t doneProcessTime[MPSLinkNodeMaxCount + 1]; static uint32_t sendReceiveDuration[MPSLinkNodeMaxCount + 1]; static uint32_t receiveProcessDuration[MPSLinkNodeMaxCount + 1]; static uint32_t sendProcessDuration[MPSLinkNodeMaxCount + 1]; static epicsMutexId tCommMutex; #endif #endif #ifdef __rtems__ void mpsLinkNodeCommunicationShowProcessInMicroseconds() { double megahertz = (double) BSP_bus_frequency / (double) BSP_time_base_divisor / 1000.0f; int i; static uint32_t sendReceiveDurationCopy[MPSLinkNodeMaxCount + 1]; static uint32_t receiveProcessDurationCopy[MPSLinkNodeMaxCount + 1]; static uint32_t sendProcessDurationCopy[MPSLinkNodeMaxCount + 1]; epicsMutexLock(tCommMutex); memcpy(sendReceiveDurationCopy, sendReceiveDuration, sizeof(sendReceiveDurationCopy)); memcpy(receiveProcessDurationCopy, receiveProcessDuration, sizeof(receiveProcessDurationCopy)); memcpy(sendProcessDurationCopy, sendProcessDuration, sizeof(sendProcessDurationCopy)); epicsMutexUnlock(tCommMutex); printf("Link Node Communication\n"); printf("---------------------------------------------------\n"); for (i = 0; i < sizeof(sendReceiveDurationCopy)/sizeof(sendReceiveDurationCopy[0]); i++) { if(receivedLinkNodeId[i] == epicsTrue) { printf("%3d: Send/Receive Duration (us, ticks): %6.2f, %5lu\n", i, sendReceiveDurationCopy[i] / megahertz, sendReceiveDurationCopy[i]); printf("%3d: Receive/Process Duration (us, ticks): %6.2f, %5lu\n", i, receiveProcessDurationCopy[i] / megahertz, receiveProcessDurationCopy[i]); printf("%3d: Total Duration (us, ticks): %6.2f, %5lu\n\n", i, sendProcessDurationCopy[i] / megahertz, sendProcessDurationCopy[i]); } } printf("Full Send/Receive Duration (us, ticks): %6.2f, %6lu\n", sendReceiveDurationCopy[0] / megahertz, sendReceiveDurationCopy[0]); printf("Full Total Duration (us, ticks): %6.2f, %6lu\n\n", sendProcessDurationCopy[0] / megahertz, sendProcessDurationCopy[0]); } #endif #if 0 #pragma mark - #pragma mark Other #endif /**************************************************************************************** * Other ***************************************************************************************/ void printMissingStatusMessageCount(int index) { printf("Missing messages for Link Node %d is %d\n", index, missingStatusMessageCount[index]); } int cleanupLinkNodeCommunication(const MPSTimingData *currentTimingData) { int i; static int flag120Hz = 0; /* This function is called at 360 Hz. flag120Hz will be 0 every third pulse (120 Hz). */ flag120Hz = (flag120Hz + 1) % 3; epicsMutexLock(responseRegisterMutex); for (i = MPSLinkNodeIdMin; i <= MPSLinkNodeIdMax; i++) { /* Currently there is only 120 Hz cleanup. Exit loop if it isn't time for the cleanup. */ if (flag120Hz != 0) { break; } /* Ignore Link Nodes not currently being used in the MPS. */ if (linkNodeIsActive(i) == epicsFalse) { continue; } /* For each status message type, check to see if status has been received within 8.3 ms */ if (statusMessageResponseRegister[i] == 0) { linkNodeFaultStateMakeFaulted( LN_COM_RESPONSE_BASE__INDEX, i, currentTimingData->timestamp, currentTimingData->timeslot); } else { linkNodeFaultStateMakeOk( LN_COM_RESPONSE_BASE__INDEX, i, currentTimingData->timestamp, currentTimingData->timeslot); } /* Reset response register */ statusMessageResponseRegister[i] = 0; } epicsMutexUnlock(responseRegisterMutex); /* Fault all inputs for Link Nodes that have missing data */ for (i = MPSLinkNodeIdMin; i <= MPSLinkNodeIdMax; i++) { /* Ignore Link Nodes not currently being used in the MPS. */ if(linkNodeIsActive(i) == epicsFalse) { continue; } /* Fault all digital inputs for non-responsive Link Nodes */ if (getLatchedLinkNodeFaultState(LN_COM_RESPONSE_BASE__INDEX, i) != OK) { faultAllInputsForLinkNode(i, currentTimingData->timestamp, currentTimingData->timeslot); } } return MPS_SUCCESS; } void printStatusMessageResponseRegister(unsigned int linkNodeId) { printf("%u\n", statusMessageResponseRegister[linkNodeId]); } #ifdef LINK_NODE_COMM_TIME_DIAG #ifdef __rtems__ void plnid() { int i; int count; count = 0; for (i = 0; i <= MPSLinkNodeMaxCount; i++) { if (receivedLinkNodeId[i]) { printf("%d, ", i); count++; } } printf("\nNumber of Link Nodes: %d\n", count); } void clnid() { memset(receivedLinkNodeId, 0,sizeof(receivedLinkNodeId)); } #endif #endif void setHandleMlpV2(int state) { handleMlpV2 = state ? epicsTrue : epicsFalse; } #if 0 #pragma mark - #pragma mark Message Handlers #endif /**************************************************************************************** * Message Handlers ***************************************************************************************/ static int messageIsValid(const UdpCommPkt packet, epicsUInt8 *linkNodeId) { int err; epicsInt8 *mlpdata; epicsInt8 *mlpheader; epicsTimeStamp timestamp; epicsUInt8 lnid = MPSLinkNodeIdInvalid; epicsUInt8 messageType = MPSMessageTypeInvalid; char timeslot = 0; epicsUInt8 mlpVersion = 0; int mlpVersionIsValid = epicsFalse; /* Assign pointers to MPS Link Protocol (MLP) header section */ mlpheader = udpCommBufPtr(packet); /* Check protocol version */ mlpVersion = LINK_MESSAGE_PROTOCOL_VERSION(mlpheader); mlpVersionIsValid = epicsFalse; switch (mlpVersion) { case 0x02: if (handleMlpV2) { mlpVersionIsValid = epicsTrue; } break; case 0x03: mlpVersionIsValid = epicsTrue; break; default: mlpVersionIsValid = epicsFalse; break; } if (mlpVersionIsValid == epicsFalse) { errlogPrintf("messageIsValid: Received invalid message protocol version (%d)\n", mlpVersion); return epicsFalse; } /* Assign pointers to MPS Link Protocol (MLP) data section */ mlpdata = LINK_MESSAGE_DATA_SECTION_POINTER(mlpheader); /* Check the message type and Link Node ID is valid */ messageType = LINK_MESSAGE_TYPE(mlpheader); switch(messageType) { case MPSLinkNodeMessageTypeStatus: case MPSLinkNodeMessageTypeStatusBlm: case MPSLinkNodeMessageTypeStatusPic: case MPSLinkNodeMessageTypeStatusV2: lnid = LINK_STATUS_COMMON_LINK_NODE_ID(mlpdata); break; default: messageType = MPSMessageTypeInvalid; lnid = MPSLinkNodeIdInvalid; break; } if (linkNodeId != NULL) { *linkNodeId = lnid; } if (messageType == MPSMessageTypeInvalid) { if (handleMlpV2 == epicsFalse) { errlogPrintf("messageIsValid: Received invalid message type (%d)\n", messageType); } return epicsFalse; } if ((lnid < MPSLinkNodeIdMin) || (lnid > MPSLinkNodeIdMax)) { errlogPrintf("messageIsValid: Received invalid Link Node ID (%d)\n", lnid); return epicsFalse; } /* Check if message is outdated */ timestamp.secPastEpoch = ntohl(LINK_STATUS_COMMON_TIMESTAMP_SECPASTEPOCH(mlpdata)); timestamp.nsec = ntohl(LINK_STATUS_COMMON_TIMESTAMP_NSEC_PULSEID(mlpdata)); timeslot = LINK_STATUS_COMMON_TIMESLOT(mlpdata); /* Function call to epicsTimeLessThan may be overkill for this compare */ if (epicsTimeLessThan(×tamp, &(newestReceivedTimestamp[lnid]))) { /* * The Link Processor has already received newer data from this Link Node. * Ignore this message. */ /* Show non-zero outdated message timestamps */ if (timestamp.secPastEpoch != 0) { errlogPrintf("messageIsValid: Already received newer data from Link Node %d. %d.%09d:%u < %d.%09d\n", lnid, timestamp.secPastEpoch, timestamp.nsec, timeslot, newestReceivedTimestamp[lnid].secPastEpoch, newestReceivedTimestamp[lnid].nsec); } return epicsFalse; } /* Update the last accepted timestamp for the Link Node */ newestReceivedTimestamp[lnid] = timestamp; /* Check to see if data is no more than 8.3 ms old */ err = lockTimingMutexAndReturnIsRecentTimestamp(×tamp); if (err != epicsTrue) { errlogPrintf("messageIsValid: Received a message older than 8.3 ms from Link Node %d\n", lnid); } return err; } static int messageHandlerStatusMessages(UdpCommPkt packet, uint32_t diaddr, int port) { int err; epicsInt8 *message; epicsInt8 *mlpdata; epicsInt8 *mlpheader; epicsTimeStamp timestamp; epicsUInt8 linkNodeId; char timeslot; unsigned int unlatchMessageSize; unsigned int numberOfInputCards; /* Setup pointers and get data from packet */ message = udpCommBufPtr(packet); mlpheader = LINK_MESSAGE_HEADER_SECTION_POINTER(message); mlpdata = LINK_MESSAGE_DATA_SECTION_POINTER(mlpheader); timestamp.secPastEpoch = ntohl(LINK_STATUS_COMMON_TIMESTAMP_SECPASTEPOCH(mlpdata)); timestamp.nsec = ntohl(LINK_STATUS_COMMON_TIMESTAMP_NSEC_PULSEID(mlpdata)); timeslot = LINK_STATUS_COMMON_TIMESLOT(mlpdata); linkNodeId = LINK_STATUS_COMMON_LINK_NODE_ID(mlpdata); /* Get the number of input cards and set the unlatch message type and size */ switch(LINK_MESSAGE_TYPE(message)) { case MPSLinkNodeMessageTypeStatus: numberOfInputCards = LINK_STATUS_NUMBER_OF_INPUT_CARDS; unlatchMessageSize = LINK_STATUS_UNLATCH_MESSAGE_SIZE; LINK_MESSAGE_TYPE(mlpheader) = MPSLinkProcessorMessageTypeUnlatchStatus; break; case MPSLinkNodeMessageTypeStatusBlm: numberOfInputCards = LINK_STATUS_BLM_NUMBER_OF_INPUT_CARDS; unlatchMessageSize = LINK_STATUS_BLM_UNLATCH_MESSAGE_SIZE; LINK_MESSAGE_TYPE(mlpheader) = MPSLinkProcessorMessageTypeUnlatchStatusBlm; break; case MPSLinkNodeMessageTypeStatusPic: numberOfInputCards = LINK_STATUS_PIC_NUMBER_OF_INPUT_CARDS; unlatchMessageSize = LINK_STATUS_PIC_UNLATCH_MESSAGE_SIZE; LINK_MESSAGE_TYPE(mlpheader) = MPSLinkProcessorMessageTypeUnlatchStatusPic; break; default: errlogPrintf("messageHandlerStatusMessages cannot handle this message type (%d)\n", LINK_MESSAGE_TYPE(message)); udpCommFreePacket(packet); return MPS_ERROR; } /* Update the device states */ err = explodeDeviceStatesWithAutorecover(linkNodeId, numberOfInputCards, LINK_STATUS_COMMON_NUMBER_OF_CHANNELS_PER_INPUT_CARD, LINK_STATUS_COMMON_INPUT_CARD_DATA_POINTER(mlpdata), timestamp, timeslot); if (err != MPS_SUCCESS) { /* Could not explode the data */ errlogPrintf("messageHandlerStatusMessages could not explode data for Link Node %d\n", linkNodeId); udpCommFreePacket(packet); return MPS_ERROR; } /* * Send the unlatch message back to the Link Node */ /* Return the packet back to the specific Link Node that sent it. This call frees the packet. */ #ifdef BSDSOCKET epicsMutexLock(linkNodeResponseSocketMutex); udpCommConnect(linkNodeResponseSocket, diaddr, port); udpCommSend(linkNodeResponseSocket, message, unlatchMessageSize); epicsMutexUnlock(linkNodeResponseSocketMutex); #else udpCommReturnPacket(packet, unlatchMessageSize); #endif /* * Update communication fault */ /* Increment the Link Node reponse register. */ epicsMutexLock(responseRegisterMutex); statusMessageResponseRegister[linkNodeId]++; epicsMutexUnlock(responseRegisterMutex); return MPS_SUCCESS; } static int messageHandlerStatusMessagesVersion2(UdpCommPkt packet, uint32_t diaddr, int port) { int err; epicsInt8 *message; epicsInt8 *mlpdata; epicsInt8 *mlpheader; epicsTimeStamp timestamp; epicsUInt8 linkNodeId; char timeslot; unsigned int unlatchMessageSize; unsigned int numberOfInputCards; /* Setup pointers and get data from packet */ message = udpCommBufPtr(packet); mlpheader = LINK_MESSAGE_HEADER_SECTION_POINTER(message); mlpdata = LINK_MESSAGE_DATA_SECTION_POINTER(mlpheader); timestamp.secPastEpoch = ntohl(LINK_STATUS_COMMON_TIMESTAMP_SECPASTEPOCH(mlpdata)); timestamp.nsec = ntohl(LINK_STATUS_COMMON_TIMESTAMP_NSEC_PULSEID(mlpdata)); timeslot = LINK_STATUS_COMMON_TIMESLOT(mlpdata); linkNodeId = LINK_STATUS_COMMON_LINK_NODE_ID(mlpdata); /* Get the number of input cards and set the unlatch message type and size */ switch(LINK_MESSAGE_TYPE(message)) { case MPSLinkNodeMessageTypeStatusV2: numberOfInputCards = LINK_STATUS_NUMBER_OF_INPUT_CARDS_V2; unlatchMessageSize = LINK_STATUS_MESSAGE_SIZE_V2; LINK_MESSAGE_TYPE(mlpheader) = MPSLinkProcessorMessageTypeUnlatchStatusV2; break; default: errlogPrintf("messageHandlerStatusMessages cannot handle this message type (%d)\n", LINK_MESSAGE_TYPE(message)); udpCommFreePacket(packet); return MPS_ERROR; } /* Update the device states */ err = explodeDeviceStatesWithAutorecover(linkNodeId, numberOfInputCards, LINK_STATUS_COMMON_NUMBER_OF_CHANNELS_PER_INPUT_CARD, LINK_STATUS_INPUT_CARD_DATA_POINTER_V2(mlpdata), timestamp, timeslot); if (err != MPS_SUCCESS) { /* Could not explode the data */ errlogPrintf("messageHandlerStatusMessages could not explode data for Link Node %d\n", linkNodeId); udpCommFreePacket(packet); return MPS_ERROR; } /* * Send the unlatch message back to the Link Node */ /* Return the packet back to the specific Link Node that sent it. This call frees the packet. */ #ifdef BSDSOCKET epicsMutexLock(linkNodeResponseSocketMutex); udpCommConnect(linkNodeResponseSocket, diaddr, port); udpCommSend(linkNodeResponseSocket, message, unlatchMessageSize); epicsMutexUnlock(linkNodeResponseSocketMutex); #else udpCommReturnPacket(packet, unlatchMessageSize); #endif /* * Update communication fault */ /* Increment the Link Node reponse register. */ epicsMutexLock(responseRegisterMutex); statusMessageResponseRegister[linkNodeId]++; epicsMutexUnlock(responseRegisterMutex); return MPS_SUCCESS; } #if 0 #pragma mark - #pragma mark Message Receiving #endif /**************************************************************************************** * Message Receiving ***************************************************************************************/ static void linkNodeMessageReceiverVersion2(UdpCommPkt packet, uint32_t peerip, int peerport) { int err = MPS_ERROR; int isValid = epicsFalse; epicsUInt8 linkNodeId = MPSLinkNodeIdInvalid; epicsInt8 *header; if (packet == NULL) { return; } /* Determine if the message is valid (correct format, and not too old) * and get the sender's Link Node ID. */ isValid = messageIsValid(packet, &linkNodeId); #ifdef LINK_NODE_COMM_TIME_DIAG #ifdef __rtems__ epicsMutexLock(tCommMutex); receivedTime[linkNodeId] = read_timebase(); receivedLinkNodeId[linkNodeId] = 1; missingStatusMessageCount[linkNodeId]--; #endif #endif if (isValid) { /* Only handle sync version 2 sync messages. * Process received message with its correct message handler. * Packets are free'd within their handlers. */ header = udpCommBufPtr(packet); switch(LINK_MESSAGE_TYPE(header)) { case MPSLinkNodeMessageTypeStatusV2: err = messageHandlerStatusMessagesVersion2(packet, peerip, peerport); break; default: udpCommFreePacket(packet); err = MPS_ERROR; break; } } else { udpCommFreePacket(packet); } #ifdef LINK_NODE_COMM_TIME_DIAG #ifdef __rtems__ doneProcessTime[linkNodeId] = read_timebase(); epicsMutexUnlock(tCommMutex); #endif #endif } static void linkNodeMessageReceiverThread(void *arg) { static int err = MPS_ERROR; static int isValid = epicsFalse; static UdpCommPkt packet; static uint32_t peerip; static uint16_t peerport; static epicsInt8 *header; static MPSTimingData currentTimingData; static epicsUInt8 linkNodeId = MPSLinkNodeIdInvalid; /* Receive data from link nodes */ while(1) { /* Receive the data (blocking with timeout) */ packet = udpCommRecvFrom(linkNodeSocket, MPSLinkNodeCommunicationMessageTimeoutInMs, &peerip, &peerport); if (packet == NULL) { /* Haven't received a packet from any link node in MPSLinkNodeCommunicationMessageTimeoutInMs ms */ lockTimingMutexAndGetCurrentTimingData(¤tTimingData); deviceStateMakeFaulted(LP_LN_COM_TIMEOUT__INDEX, currentTimingData.timestamp, currentTimingData.timeslot); continue; } lockTimingMutexAndGetCurrentTimingData(¤tTimingData); deviceStateMakeOk(LP_LN_COM_TIMEOUT__INDEX, currentTimingData.timestamp, currentTimingData.timeslot); /* If the message is a V2 message, handle it another way */ if (handleMlpV2) { header = udpCommBufPtr(packet); if (LINK_MESSAGE_PROTOCOL_VERSION(header) == MPSLinkProtocolVersion2) { linkNodeMessageReceiverVersion2(packet, peerip, peerport); continue; } } /* Determine if the message is valid (correct format, and not too old) * and get the sender's Link Node ID. */ isValid = messageIsValid(packet, &linkNodeId); if (!(isValid)) { udpCommFreePacket(packet); continue; } #ifdef LINK_NODE_COMM_TIME_DIAG #ifdef __rtems__ epicsMutexLock(tCommMutex); receivedTime[linkNodeId] = read_timebase(); receivedLinkNodeId[linkNodeId] = 1; missingStatusMessageCount[linkNodeId]--; #endif #endif /* Process received message with its correct message handler. * Packets are free'd within their handlers. */ header = udpCommBufPtr(packet); switch(LINK_MESSAGE_TYPE(header)) { case MPSLinkNodeMessageTypeStatus: case MPSLinkNodeMessageTypeStatusBlm: case MPSLinkNodeMessageTypeStatusPic: err = messageHandlerStatusMessages(packet, peerip, peerport); break; default: errlogPrintf("Warning: unknown message type, 0x%02X, received from Link Node.\n", LINK_MESSAGE_TYPE(header)); udpCommFreePacket(packet); err = MPS_ERROR; break; } #ifdef LINK_NODE_COMM_TIME_DIAG #ifdef __rtems__ doneProcessTime[linkNodeId] = read_timebase(); epicsMutexUnlock(tCommMutex); #endif #endif } /* Should never reach this point */ cantProceed("Left linkNodeMessageReceiverThread infinite loop"); } #if 0 #pragma mark - #pragma mark Message Sending #endif /**************************************************************************************** * Message Sending ***************************************************************************************/ #if 0 #pragma mark Control #endif void setCanBroadcastMessages(int state) { canBroadcastMessages = state ? epicsTrue : epicsFalse; } void setCanSendPermitMessages(int state) { canSendPermitMessages = state ? epicsTrue : epicsFalse; } int broadcastMessageToLinkNodes(epicsInt8 *data, unsigned int dataLength) { int err; /* Check size */ if ((dataLength < MPSLinkProtocolDataSizeMin) || (dataLength > MPSLinkProtocolDataSizeMax)) { return MPS_ERROR_ARGUMENT_OUT_OF_RANGE; } /* Check if sending is allowed */ if (canBroadcastMessages == epicsFalse) { return MPS_SUCCESS; } /* Send the message */ err = udpCommSend(linkNodeSocket, data, dataLength); #ifdef BSDSOCKET /* BSDSOCKET Implementation */ if (err != dataLength) { return err; } #else #warning snorum: udpCommSend return value is not consistent with BSDSOCKET implementation. /* udpComm Implementation */ if (err < 0) { return err; } #endif return MPS_SUCCESS; } int broadcastLinkSynchronizationMessage(const MPSTimingData *currentTimingData) { int i; int err; epicsInt8 message[LINK_SYNC_MESSAGE_SIZE]; epicsInt8 *data; /* Get pointer to data section */ data = LINK_MESSAGE_DATA_SECTION_POINTER(message); /* Set the protocol version and message type */ LINK_MESSAGE_PROTOCOL_VERSION(message) = MPSLinkProtocolVersionCurrent; LINK_MESSAGE_TYPE(message) = MPSLinkProcessorMessageTypeSynchronization; /* Fill in the message data */ LINK_SYNC_TIMESTAMP_SECPASTEPOCH(data) = htonl(currentTimingData->timestamp.secPastEpoch); LINK_SYNC_TIMESTAMP_NSEC_PULSEID(data) = htonl(currentTimingData->timestamp.nsec); LINK_SYNC_TIMESLOT(data) = currentTimingData->timeslot; #ifdef LINK_NODE_COMM_TIME_DIAG #ifdef __rtems__ epicsMutexLock(tCommMutex); receivedTime[0] = 0; doneProcessTime[0] = 0; for (i = 0; i < sizeof(sendReceiveDuration)/sizeof(sendReceiveDuration[0]); i++) { if (receivedLinkNodeId[i]) { sendReceiveDuration[i] = receivedTime[i] - sentTime; receiveProcessDuration[i] = doneProcessTime[i] - receivedTime[i]; sendProcessDuration[i] = doneProcessTime[i] - sentTime; if (receivedTime[i] > receivedTime[0] ) { receivedTime[0] = receivedTime[i]; } if (doneProcessTime[i] > doneProcessTime[0]) { doneProcessTime[0] = doneProcessTime[i]; } } } sendReceiveDuration[0] = receivedTime[0] - sentTime; sendProcessDuration[0] = doneProcessTime[0] - sentTime; sentTime = read_timebase(); epicsMutexUnlock(tCommMutex); #endif #endif /* Send the message */ err = broadcastMessageToLinkNodes(message, LINK_SYNC_MESSAGE_SIZE); if (handleMlpV2) { LINK_MESSAGE_PROTOCOL_VERSION(message) = MPSLinkProtocolVersion2; LINK_MESSAGE_TYPE(message) = MPSLinkProcessorMessageTypeSynchronizationV2; err = broadcastMessageToLinkNodes(message, LINK_SYNC_MESSAGE_SIZE); } for (i = 0; i < MPSLinkNodeMaxCount; i++) { missingStatusMessageCount[i]++; } return err; } int broadcastPermitMessage(const MPSLinkPermitMessage *permitMessage) { static epicsTimeStamp lastTimestamp = {0, 0}; int err; int i; int isValidTimestamp; static epicsInt8 message[PERMIT_MESSAGE_SIZE]; epicsInt8 *data; epicsUInt8 *dataPermit; epicsUInt8 *dataRateAfterDevice; if (canSendPermitMessages == epicsFalse) { /* Don't send the message */ return MPS_SUCCESS; } /* Get pointer to data section */ data = LINK_MESSAGE_DATA_SECTION_POINTER(message); dataPermit = PERMIT_MITIGATION_PERMIT_POINTER(data); dataRateAfterDevice = PERMIT_MAX_RATE_AFTER_DEVICE_POINTER(data); /* Set the protocol version and message type */ LINK_MESSAGE_PROTOCOL_VERSION(message) = MPSLinkProtocolVersionCurrent; LINK_MESSAGE_TYPE(message) = MPSLinkProcessorMessageTypePermit; /* Fill in the message data */ PERMIT_TIMESTAMP_SECPASTEPOCH(data) = htonl(permitMessage->timestamp.secPastEpoch); PERMIT_TIMESTAMP_NSEC_PULSEID(data) = htonl(permitMessage->timestamp.nsec); PERMIT_TIMESLOT(data) = permitMessage->timeslot; isValidTimestamp = epicsFalse; if (epicsTimeGreaterThan(&(permitMessage->timestamp), &lastTimestamp)) { isValidTimestamp = epicsTrue; lastTimestamp = permitMessage->timestamp; } if (isValidTimestamp) { /* Copy the desintation, estimated rate, and output control */ PERMIT_DESTINATION(data) = permitMessage->destination; PERMIT_ESTIMATED_RATE(data) = permitMessage->estimatedRate; PERMIT_OUTPUT_CARD_CONTROL(data) = permitMessage->outputControl; /* Copy the permit and rate after device data */ for (i = 0; i < MPSMitigationDeviceCount; i++) { dataPermit[i] = permitMessage->shouldSendPermit[i] ? 1 : 0; dataRateAfterDevice[i] = permitMessage->rateAfterDevice[i]; } } else { /* Set the desintation, estimated rate, and output control to most restrictive but valid */ PERMIT_DESTINATION(data) = MPSDestinationPockelsCell; PERMIT_ESTIMATED_RATE(data) = MPSRate0Hz; PERMIT_OUTPUT_CARD_CONTROL(data) = 0x00; /* Set the permit and rate after device data to most restrictive but valid */ for (i = 0; i < MPSMitigationDeviceCount; i++) { dataPermit[i] = 0; dataRateAfterDevice[i] = MPSRate0Hz; } } /* Broadcast the message */ err = broadcastMessageToLinkNodes(message, PERMIT_MESSAGE_SIZE); if (handleMlpV2) { LINK_MESSAGE_PROTOCOL_VERSION(message) = MPSLinkProtocolVersion2; LINK_MESSAGE_TYPE(message) = MPSLinkProcessorMessageTypePermitV2; err = broadcastMessageToLinkNodes(message, PERMIT_MESSAGE_SIZE); } if (err != MPS_SUCCESS) { /* errlogPrintf("broadcastPermitMessage: broadcastMessageToLinkNodes error %d.\n", err); */ } return err; } #if 0 #pragma mark - #pragma mark Initialization #endif /**************************************************************************************** * Initialization ***************************************************************************************/ int initializeLinkNodeCommunication() { int err; /* Initialize Variables */ responseRegisterMutex = epicsMutexMustCreate(); memset(statusMessageResponseRegister, 0, sizeof(statusMessageResponseRegister)); memset(newestReceivedTimestamp, 0, sizeof(newestReceivedTimestamp)); memset(missingStatusMessageCount, 0, sizeof(missingStatusMessageCount)); memset(receivedLinkNodeId, 0, sizeof(receivedLinkNodeId)); #if __rtems__ memset(sendReceiveDuration, 0, sizeof(sendReceiveDuration)); memset(receiveProcessDuration, 0, sizeof(receiveProcessDuration)); memset(sendProcessDuration, 0, sizeof(sendProcessDuration)); tCommMutex = epicsMutexMustCreate(); #endif /* Create socket for link node communication */ linkNodeSocket = udpCommSocket(MPSLinkProtocolPort); if (linkNodeSocket < 0) { errlogPrintf("Error: initializeLinkNodeCommunication could not create linkNodeSocket.\n"); return linkNodeSocket; } #ifdef BSDSOCKET linkNodeResponseSocketMutex = epicsMutexMustCreate(); linkNodeResponseSocket = udpCommSocket(MPSLinkProtocolResponsePort); if (linkNodeResponseSocket < 0) { errlogPrintf("Error: initializeLinkNodeCommunication could not create linkNodeResponseSocket.\n"); return linkNodeResponseSocket; } #endif /* Get Broadcast IP Address */ linkNodeBroadcastIP = inet_addr(MPSLinkNodeCommunicationBroadcastIP); /* Connect */ err = udpCommConnect(linkNodeSocket, linkNodeBroadcastIP, MPSLinkProtocolPort); /* Start thread to receive data from Link Nodes */ err = (int) epicsThreadCreate("mpsLinkNodeMessageReceiverThread", MPSThreadPriorityHigher, epicsThreadGetStackSize(epicsThreadStackMedium), (EPICSTHREADFUNC) linkNodeMessageReceiverThread, NULL); if (err == 0) { errlogPrintf("Error: initializeLinkNodeCommunication could not create linkNodeMessageReceiverThread\n"); return MPS_ERROR_THREAD_CREATION_FAILED; } return MPS_SUCCESS; }