#include "drvLinkNodeSim.h" #define DEBUG 0 #define LN_EVENT_CODE 0 /***************************************************************************************/ void mpsLinkNodeFidTime(void *arg); void mpsLinkNodeReceiveThread(void *arg); void mpsLinkNodeSendThread(void *arg); void mpsLinkNodesCreate(); /***************************************************************************************/ /* wrapper to configure LanIpBasic for the mpsLinkProcessor. Called in st.cmd */ int mpsLanIpBscConfigWrapper(unsigned rxQueueDepth, unsigned rxRingSize, unsigned numRBufs, unsigned txRingSize ) { int status = 0; #ifdef __rtems__ LanIpBscConfigRec cfg; cfg.mask = LANIPCFG_RX_RING | LANIPCFG_TX_RING | LANIPCFG_N_RBUFS | LANIPCFG_SQDEPTH; cfg.rx_queue_depth = rxQueueDepth; cfg.rx_ring_size = rxRingSize; cfg.num_rbufs = numRBufs; cfg.tx_ring_size = txRingSize; status = lanIpBscConfig(&cfg,0); #endif return status; } //end:mpsLanIpBscConfigWrapper void mpsLinkNodeFidTime(void *arg) { epicsTimeStamp timeStamp; evrModifier_ta modifier_a; unsigned long patternStatus; int status; int timeSlot; status = evrTimeGetFromPipeline(&timeStamp,evrTimeCurrent,modifier_a,&patternStatus,0,0,0); if(status) { errlogPrintf("%s:%d: evrTimeFromPipeline failed: %d: %s\n",__func__,(__LINE__-3),status,strerror(status)); return; } timeSlot = TIMESLOT(modifier_a); if( timeSlot == 0 ) { errlogPrintf("Retrieved bad pattern. TIMESLOT is zero!\n"); return; } epicsEventSignal(mpsBSALinkNodeDrv->eventID); return; } //end:mpsLinkNodeFidTime /* Receive UDP packets */ void mpsLinkNodeReceiveThread(void *arg) { epicsThreadSleep(10); UdpCommPkt packet = NULL; epicsInt8 *mlpheader; uint32_t ipAddr; uint16_t port; int lnID; // Receive all data while (1) { // Grab packet packet = udpCommRecvFrom(mpsBSALinkNodeDrv->linkNodeSocket, 400, &ipAddr, &port); // If no data received or if from LinkNodeProcessor-> continue if((packet == NULL) || (ipAddr == -1062731775)) //TODO ip should not be hardcoded { udpCommFreePacket(packet); continue; } else { mlpheader = udpCommBufPtr(packet); if(DEBUG) { struct in_addr ipaddr; ipaddr.s_addr = ipAddr; char* ip_addr_string = inet_ntoa(ipaddr); printf("UDP packet received\tIP addreess: %d\tPort: %d\tstring ip Addr: %s\n",(int)ipAddr,(int)port,ip_addr_string); printf("\t\tProtocolVersion: %d\t MessageType: %d lnID_received: %d\n", LINK_MESSAGE_PROTOCOL_VERSION(mlpheader), LINK_MESSAGE_TYPE(mlpheader), LINK_BSA_LN_LINK_NODE_H_ID(mlpheader)); } } if(LINK_MESSAGE_PROTOCOL_VERSION(mlpheader) != MPSLinkProtocolVersionCurrent) { errlogPrintf("%s %s:%d: Expected different version of MPSLinkProtocol. Expected version: %d, received version is: %d \n", __FILE__,__func__,__LINE__,MPSLinkProtocolVersionCurrent,LINK_MESSAGE_PROTOCOL_VERSION(packet)); udpCommFreePacket(mlpheader); continue; } if (LINK_MESSAGE_TYPE(mlpheader) == MPSBSAMessageTypeEnble) { if(ipAddr != mpsBSALinkNodeDrv->BSAProcessorIP) { if(epicsMutexLock(mpsBSALinkNodeDrv->mutex) != epicsMutexLockOK) { errlogPrintf("%s %s:%d: Could not lock the mpsBSALinkNodeDrv->mutex!\n", __FILE__, __func__, __LINE__); } mpsBSALinkNodeDrv->BSAProcessorIP = ipAddr; epicsMutexUnlock(mpsBSALinkNodeDrv->mutex); } for( lnID = 0; lnID < MPSLinkNodeMaxCount ; lnID++ ) { if(linkNode[lnID].present) { if( LINK_BSA_LN_LINK_NODE_H_ID(mlpheader) == lnID) { if(DEBUG) printf("LinkNode%d: present and active\n",lnID); if(epicsMutexLock(linkNode[lnID].bsaDataMutex) != epicsMutexLockOK) { errlogPrintf("%s %s:%d: Could not lock the bsaDataMutex!\n", __FILE__, __func__, __LINE__); } linkNode[lnID].active = epicsTrue; epicsMutexUnlock(linkNode[lnID].bsaDataMutex); break; } } }//end:for udpCommFreePacket(packet); }//end:if }//end:while return; } //end:mpsLinkNodeReceiveThread /* Link Node send packets thread */ void mpsLinkNodeSendThread(void *arg) { epicsThreadSleep(10); int lnID; epicsInt8 *mlpheader; epicsInt8 *mlpdata; epicsTimeStamp timeStamp; evrModifier_ta modifier_a; unsigned long patternStatus; int time_s, time_n; int status; int i,j; int counter = 0; while(1) { if(epicsEventWait(mpsBSALinkNodeDrv->eventID) != epicsEventWaitOK) { errlogPrintf("epicsEventWait failed\n"); continue; } status = evrTimeGetFromPipeline(&timeStamp,evrTimeCurrent,modifier_a,&patternStatus,0,0,0); for( lnID = 0; lnID < MPSLinkNodeMaxCount ; lnID++ ) { if(linkNode[lnID].active != epicsTrue) //TODO here mutex must be used { continue; } /* Creating PACKET CONTENT */ for(j = 0; j < linkNode[lnID].module; j++) { /* Allocate space for packet */ linkNode[lnID].packet = udpCommAllocPacket(); /* Creating Link Message Header */ mlpheader = (epicsInt8*) udpCommBufPtr(linkNode[lnID].packet); LINK_MESSAGE_PROTOCOL_VERSION(mlpheader) = MPSLinkProtocolVersionCurrent; LINK_MESSAGE_TYPE(mlpheader) = MPSBSAMessageTypeBsaData; /* Creating DATA block for LinkNode packet*/ mlpdata = LINK_BSA_LN_LINK_NODE_DATA_POINTER(mlpheader); /* Time stamp, tims slot and linkNode ID */ LINK_BSA_LN_TIMESTAMP_SECPASTEPOCH(mlpdata) = timeStamp.secPastEpoch; LINK_BSA_LN_TIMESTAMP_NSEC_PULSEID(mlpdata) = timeStamp.nsec; LINK_BSA_LN_TIMESLOT(mlpdata) = TIMESLOT(modifier_a); LINK_BSA_LN_LINK_NODE_ID(mlpdata) = linkNode[lnID].ID; LINK_BSA_LN_LINK_NODE_MODULE(mlpdata) = j + 1; epicsInt8 *chnlPnt; time_s = (int)(timeStamp.secPastEpoch%(100*(timeStamp.secPastEpoch/100))); time_n = (int)(timeStamp.nsec%(1000*(timeStamp.nsec/1000))); for ( i = 0; i < NB_OF_CHANNELS; i++) { chnlPnt = mlpdata + LINK_BSA_LN_METADATA_SIZE + i*32; /*LINK_BSA_LN_BASELINE(chnlPnt) = linkNode[lnID].baseline + i*100000 + time_s*1000 + time_n; LINK_BSA_LN_Q0_BEAM(chnlPnt) = linkNode[lnID].q0_beam + i*100000 + time_s*1000 + time_n; LINK_BSA_LN_Q1_BEAM(chnlPnt) = linkNode[lnID].q1_beam + i*100000 + time_s*1000 + time_n; LINK_BSA_LN_Q10_BEAM(chnlPnt) = linkNode[lnID].q10_beam + i*100000 + time_s*1000 + time_n; LINK_BSA_LN_Q30_BEAM(chnlPnt) = linkNode[lnID].q30_beam + i*100000 + time_s*1000 + time_n; LINK_BSA_LN_Q60_BEAM(chnlPnt) = linkNode[lnID].q60_beam + i*100000 + time_s*1000 + time_n; LINK_BSA_LN_Q0_TEST(chnlPnt) = linkNode[lnID].q0_test + i*100000 + time_s*1000 + time_n; LINK_BSA_LN_Q1_TEST(chnlPnt) = linkNode[lnID].q1_test + i*100000 + time_s*1000 + time_n;*/ LINK_BSA_LN_BASELINE(chnlPnt) = counter; LINK_BSA_LN_Q0_BEAM(chnlPnt) = ((i << 20) | linkNode[lnID].q0_beam); LINK_BSA_LN_Q1_BEAM(chnlPnt) = ((i << 20) | linkNode[lnID].q1_beam); LINK_BSA_LN_Q10_BEAM(chnlPnt) = ((i << 20) | linkNode[lnID].q10_beam); LINK_BSA_LN_Q30_BEAM(chnlPnt) = ((i << 20) | linkNode[lnID].q30_beam); LINK_BSA_LN_Q60_BEAM(chnlPnt) = ((i << 20) | linkNode[lnID].q60_beam); LINK_BSA_LN_Q0_TEST(chnlPnt) = ((i << 20) | linkNode[lnID].q0_test); LINK_BSA_LN_Q1_TEST(chnlPnt) = ((i << 20) | linkNode[lnID].q1_test); } status = udpCommSendPktTo(mpsBSALinkNodeDrv->linkNodeSocket,linkNode[lnID].packet,LINK_BSA_LN_MESSAGE_SIZE, mpsBSALinkNodeDrv->BSAProcessorIP,MPSLinkProtocolPort); if (status < 0) { errlogPrintf("ERROR: Could not send packet: %d - %s \n",abs(status),strerror(abs(status))); } } } //end:for counter++; } // end:while return; } //end:mpsLinkNodeSendThread /* Create LinkNodes and threads */ void mpsLinkNodesCreate() { int lnID; /* Fill linkNode information */ for(lnID=0; lnID < MPSLinkNodeMaxCount; lnID++) { scanIoInit(&(linkNode[lnID].ioscanpvtData)); scanIoInit(&(linkNode[lnID].ioscanpvtStatus)); linkNode[lnID].ID = lnID; linkNode[lnID].active = epicsFalse; linkNode[lnID].present = epicsFalse; linkNode[lnID].module = 1; //default is 1 - this number is initialized from bo record (could be changed in future to have init file) linkNode[lnID].packet = udpCommAllocPacket(); //TODO one packet should be enought. Needs to be put to mpsBSALinkNodeDrv // beginning + linknode ID + qtype /*linkNode[lnID].baseline = (epicsUInt32)(1000000000 + lnID*10000000 + 0000000); linkNode[lnID].q0_beam = (epicsUInt32)(1000000000 + lnID*10000000 + 1000000); linkNode[lnID].q1_beam = (epicsUInt32)(1000000000 + lnID*10000000 + 2000000); linkNode[lnID].q10_beam = (epicsUInt32)(1000000000 + lnID*10000000 + 3000000); linkNode[lnID].q30_beam = (epicsUInt32)(1000000000 + lnID*10000000 + 4000000); linkNode[lnID].q60_beam = (epicsUInt32)(1000000000 + lnID*10000000 + 5000000); linkNode[lnID].q0_test = (epicsUInt32)(1000000000 + lnID*10000000 + 6000000); linkNode[lnID].q1_test = (epicsUInt32)(1000000000 + lnID*10000000 + 7000000);*/ linkNode[lnID].baseline = 0; linkNode[lnID].q0_beam = 0x20; linkNode[lnID].q1_beam = 0x30; linkNode[lnID].q10_beam = 0x40; linkNode[lnID].q30_beam = 0x50; linkNode[lnID].q60_beam = 0x60; linkNode[lnID].q0_test = 0x70; linkNode[lnID].q1_test = 0x80; char linkNodeName[40]; sprintf(linkNodeName, "LINK_NODE_%d", (lnID+1)); strcpy(linkNode[lnID].name, linkNodeName); linkNode[lnID].bsaDataMutex = epicsMutexCreate(); if (linkNode[lnID].bsaDataMutex == 0) { cantProceed("%s %s:%d: Error: %s could not create mutex. \n", __FILE__,__func__,__LINE__, linkNode[lnID].name); } }//end:for mpsBSALinkNodeDrv = (MPSBSALinkNodeDrv *) malloc(sizeof(MPSBSALinkNodeDrv)); if (mpsBSALinkNodeDrv == NULL) { cantProceed("%s %s:%d: ERROR: Failed to allocate memory mpsBSALinkNodeDrv\n",__FILE__,__func__,__LINE__); } mpsBSALinkNodeDrv->BSAProcessorIP = 0; /* Create socket for link node In communication */ mpsBSALinkNodeDrv->linkNodeSocket = udpCommSocket(MPSLinkProtocolPort); if(mpsBSALinkNodeDrv->linkNodeSocket < 0) cantProceed("%s %s:%d: Error: Could not create linkNodeSocket.\n",__FILE__,__func__,__LINE__); /* Create event ID */ mpsBSALinkNodeDrv->eventID = epicsEventCreate(epicsEventEmpty); /* Creating mutex */ mpsBSALinkNodeDrv->mutex = epicsMutexCreate(); if (mpsBSALinkNodeDrv->mutex == 0) { cantProceed("%s %s:%d: Error: Could not create mutex.\n",__FILE__,__func__,__LINE__); } /* Create a thread for receiving data*/ if(epicsThreadCreate("LINK_NODE_RECV", (epicsThreadPriorityMedium-5), 12000, (EPICSTHREADFUNC)mpsLinkNodeReceiveThread, NULL) == 0) { cantProceed("%s %s:%d: Error: Could not create thread LINK_NODE_RECV! \n",__FILE__,__func__,__LINE__); } /* Create a thread for sending data */ if(epicsThreadCreate("LINK_NODE_SEND", epicsThreadPriorityMedium, 12000, (EPICSTHREADFUNC)mpsLinkNodeSendThread, NULL) == 0) { cantProceed("%s %s:%d: Error: Could not create thread LINK_NODE_SEND! \n",__FILE__,__func__,__LINE__); } evrTimeRegister(mpsLinkNodeFidTime,NULL); return; } // end:mpsLinkNodesCreate /***************************************************************************************/ /* EPICS register stuff */ static const iocshFuncDef mpsLinkNodesCreateFuncDef = {"mpsLinkNodeCreate",0,NULL}; static void mpsLinkNodesCreateCallFunc(const iocshArgBuf *args) { mpsLinkNodesCreate(); } static void mpsLinkNodesCreateRegister(void) { iocshRegister(&mpsLinkNodesCreateFuncDef, mpsLinkNodesCreateCallFunc); } epicsExportRegistrar(mpsLinkNodesCreateRegister);