/* $Date: 2013/07/11 12:43:21 $ */ /* $Id: drvS7plcFW.c,v 1.6 2013/07/11 12:43:21 anicic Exp $ */ /* $Name: $ */ /* $Revision: 1.6 $ */ /* * NOTE: s7plcFWwriteThread -is not used for writting (we write direct), * s7plcFWIOintThread - is used instead, just to trigger output records configured for "I/O intr" */ #include #include #include #include #include #include #include #include #include #if defined(vxWorks) || defined(__vxworks) #include #include #include #include #define in_addr_t unsigned long #else #include #endif #ifdef __rtems__ #include #endif #include #include #include #include #include "drvS7plcFW.h" #ifndef BASE_VERSION /* R3.14 */ #include #include #include #include #include #include #include #include #else /* R3.13 */ #define EPICS_3_13 #include "compat3_13.h" #endif #ifdef __vxworks #define __BYTE_ORDER _BYTE_ORDER #define __LITTLE_ENDIAN _LITTLE_ENDIAN #define __BIG_ENDIAN _BIG_ENDIAN #else #include #endif /* #define STACK_SIZE 20000 */ /* io thread stack size */ #define STACK_SIZE epicsThreadGetStackSize(epicsThreadStackBig) #define CONNECT_TIMEOUT 5.0 /* connect timeout [s] */ #define RECONNECT_DELAY 10.0 /* delay before reconnect [s] */ static char cvsid[] __attribute__((unused)) = "$Id: drvS7plcFW.c,v 1.6 2013/07/11 12:43:21 anicic Exp $"; STATIC long s7plcFWIoReport(int level); STATIC long s7plcFWInit(); STATIC void s7plcFWMain (); STATIC void s7plcFWIOintThread(s7plcFWStation* station); STATIC void s7plcFWFetchThread(s7plcFWStation* station); STATIC int s7plcFWWaitForInput(s7plcFWStation* station, int which, double timeout); STATIC int s7plcFWEstablishConnection(s7plcFWStation* station, int which); STATIC void s7plcFWCloseConnection(s7plcFWStation* station, int which); STATIC void s7plcFWSignal(void* event); STATIC int s7plcFWdoFetch(s7plcFWStation *station, int org, int db, int offs, int size, char *recvBuf); STATIC int s7plcFWdoWrite(s7plcFWStation *station, char *data, int offs, int len); s7plcFWStation* s7plcFWStationList = NULL; static epicsTimerQueueId timerqueue = NULL; struct { long number; long (*report)(); long (*init)(); } s7plcFW = { 2, s7plcFWIoReport, s7plcFWInit }; epicsExportAddress(drvet, s7plcFW); int s7plcFWDebug = 0; epicsExportAddress(int, s7plcFWDebug); #define FOR_FETCH 1 #define FOR_WRITE 2 struct s7plcFWStation { struct s7plcFWStation *next; char name[32]; char serverIP[20]; unsigned short int fetchPort, writePort; unsigned char fetchOrg, writeOrg; unsigned char fetchDb, writeDb; unsigned int fetchOffs, writeOffs; unsigned int fetchSize, writeSize; volatile int fetchConnStatus, writeConnStatus; volatile int fetchSocket, writeSocket; int fetchWriteDb; /* we will fetch write db once, on driver startup */ epicsMutexId fetchMutex, writeMutex; epicsMutexId fetchIo, writeIo; IOSCANPVT fetchScanPvt, writeScanPvt; epicsThreadId iointThread, fetchThread; epicsTimerId timer; epicsEventId outTrigger; char *fetchBuffer, *writeBuffer; int swapBytes; float recvTimeout, recvDelay, outIOintDelay; int writeAll; // flag, when != 0 then always write the whole buffer to DB (needed for SINQ) }; /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ void s7plcFWDebugLog(int level, const char *fmt, ...) { va_list args; if (level > s7plcFWDebug) return; va_start(args, fmt); vfprintf(stderr, fmt, args); va_end(args); } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ STATIC long s7plcFWIoReport(int level) { s7plcFWStation *station; printf("%s\n", cvsid); if (level == 1) { printf("S7plcFW stations:\n"); for (station = s7plcFWStationList; station; station=station->next) { printf(" ** Station %s\n", station->name); printf(" PLC: IP-address=%s\n", station->serverIP); printf(" FETCH: Port=%5d Org=%3d Db=%3d Offs=%5d Size=%5d buffer@%p (%5d bytes)", station->fetchPort, station->fetchOrg, station->fetchDb, station->fetchOffs, station->fetchSize, station->fetchBuffer, station->fetchSize); if (station->fetchConnStatus) { printf(" CONNECTED (fd=%d)\n", station->fetchSocket); } else { printf(" NOT CONNECTED\n"); } printf(" WRITE: Port=%5d Org=%3d Db=%3d Offs=%5d Size=%5d buffer@%p (%5d bytes) %s", station->writePort, station->writeOrg, station->writeDb, station->writeOffs, station->writeSize, station->writeBuffer, station->writeSize, (station->writeAll ? "" : "writeAll")); if (station->writeConnStatus) { printf(" CONNECTED (fd=%d)\n", station->writeSocket); } else { printf(" NOT CONNECTED\n"); } printf(" swap bytes %s\n", station->swapBytes ? #if (__BYTE_ORDER == __LITTLE_ENDIAN) "ioc:intel <-> plc:motorola" : "no (both intel)" #else "ioc:motorola <-> plc:intel" : "no (both motorola)" #endif ); printf(" receive timeout %d ms\n", (int)(station->recvTimeout * 1000.0)); printf(" receive delay %d ms\n", (int)(station->recvDelay * 1000.0)); printf(" outIOint delay %d ms\n", (int)(station->outIOintDelay * 1000.0)); } } return 0; } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ STATIC long s7plcFWInit() { if (!s7plcFWStationList) { errlogSevPrintf(errlogInfo, "s7plcFWInit: no stations configured\n"); return 0; } s7plcFWDebugLog(1, "s7plcFWInit: starting main thread\n"); epicsThreadCreate("s7plcFWMain", epicsThreadPriorityMedium, STACK_SIZE, (EPICSTHREADFUNC) s7plcFWMain, NULL); return 0; } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ STATIC void s7plcFWSignal(void* event) { epicsEventSignal((epicsEventId)event); } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ STATIC void extractPGDOS(char *info, unsigned short int *port, unsigned char *org, unsigned char *db, unsigned int *offs, unsigned int *size) { int status; unsigned int p, g, d, o, s; *port = 0; *org = 0; *db = 0; *offs = 0; *size = 0; if (info == NULL) return; status = sscanf(info, "%u,%u,%u,%u,%u", &p, &g, &d, &o, &s); if (status == 5) { *port = p; *org = g; *db = d; *offs = o; *size = s; } } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ /* strings fetchInfo & writeInfo: "Port,Org,Db,Offs,Size" */ int s7plcFWConfigure(char *name, char* IPaddr, char *fetchInfo, char *writeInfo, int bigEndian, int recvTimeout, int recvDelay, int outIOintDelay) { s7plcFWStation *station; s7plcFWStation **pstation; unsigned long ip; unsigned short int fetchPort, writePort; unsigned char fetchOrg, writeOrg; unsigned char fetchDb, writeDb; unsigned int fetchOffs, writeOffs; unsigned int fetchSize, writeSize; int writeAll; if (!name) { errlogSevPrintf(errlogFatal, "s7plcFWConfigure: missing name\n"); return -1; } if (*name == '\0') { errlogSevPrintf(errlogFatal, "s7plcFWConfigure: missing name\n"); return -1; } if (!IPaddr) { errlogSevPrintf(errlogFatal, "s7plcFWConfigure: missing IP address\n"); return -1; } if (!fetchInfo) { errlogSevPrintf(errlogFatal, "s7plcFWConfigure: missing FETCH info\n"); return -1; } extractPGDOS(fetchInfo, &fetchPort, &fetchOrg, &fetchDb, &fetchOffs, &fetchSize); extractPGDOS(writeInfo, &writePort, &writeOrg, &writeDb, &writeOffs, &writeSize); writeAll = 0; if (strstr(writeInfo, "writeall") != NULL) writeAll = 1; // write the whole buffer to DB if ((fetchPort == 0) || (fetchOrg == 0) || (fetchDb == 0) || ((fetchOffs%2) != 0) || (fetchSize == 0) || ((fetchSize%2) != 0)) { /* size & offs: only even numbers */ fetchPort = fetchOrg = fetchDb = fetchOffs = fetchSize = 0; } if ((writePort == 0) || (writeOrg == 0) || (writeDb == 0) || ((writeOffs%2) != 0) || (writeSize == 0) || ((writeSize%2) != 0)) { /* size & offs: only even numbers */ writePort = writeOrg = writeDb = writeOffs = writeSize = 0; } ip = inet_addr(IPaddr); if (ip == INADDR_NONE) { errlogSevPrintf(errlogFatal, "s7plcFWConfigure: invalid IP address %s\n", IPaddr); return -1; } ip = ntohl(ip); if (fetchPort == 0) { errlogSevPrintf(errlogFatal, "s7plcFWConfigure: invalid FETCH info\n"); return -1; } if (writePort == 0) { errlogSevPrintf(errlogFatal, "s7plcFWConfigure: WRITE will not be supported\n"); /* return -1; */ } /* find last station in list */ for (pstation = &s7plcFWStationList; *pstation; pstation = &(*pstation)->next); station = callocMustSucceed(1, sizeof(s7plcFWStation), "s7plcFWConfigure"); station->next = NULL; sprintf(station->serverIP, "%ld.%ld.%ld.%ld", (ip>>24)&0xff, (ip>>16)&0xff, (ip>>8)&0xff, ip&0xff); station->fetchPort = fetchPort; station->writePort = writePort; station->fetchOrg = fetchOrg; station->writeOrg = writeOrg; station->fetchDb = fetchDb; station->writeDb = writeDb; station->fetchOffs = fetchOffs; station->writeOffs = writeOffs; station->fetchSize = fetchSize; station->writeSize = writeSize; station->writeAll = writeAll; if (fetchSize > 0) station->fetchBuffer = callocMustSucceed(1, fetchSize, "s7plcFWConfigure"); else station->fetchBuffer = NULL; if (writeSize > 0) station->writeBuffer = callocMustSucceed(1, writeSize, "s7plcFWConfigure"); else station->writeBuffer = NULL; sprintf(station->name, "%.31s", name); #if (__BYTE_ORDER == __LITTLE_ENDIAN) station->swapBytes = bigEndian; #elif (__BYTE_ORDER == __BIG_ENDIAN) station->swapBytes = !bigEndian; #else #error Strange byte order on this machine. #endif station->fetchConnStatus = 0; station->writeConnStatus = 0; station->fetchSocket = -1; station->writeSocket = -1; station->fetchWriteDb = 1; /* fetch it (next time when both fetch and write sockets are connected) */ station->fetchMutex = epicsMutexMustCreate(); station->writeMutex = epicsMutexMustCreate(); station->fetchIo = epicsMutexMustCreate(); station->writeIo = epicsMutexMustCreate(); if (station->writeSize) { station->outTrigger = epicsEventMustCreate(epicsEventEmpty); if (!timerqueue) { timerqueue = epicsTimerQueueAllocate(1, epicsThreadPriorityHigh); } station->timer = epicsTimerQueueCreateTimer(timerqueue, s7plcFWSignal, station->outTrigger); } scanIoInit(&station->fetchScanPvt); scanIoInit(&station->writeScanPvt); station->fetchThread = NULL; station->iointThread = NULL; station->recvTimeout = recvTimeout > 0 ? recvTimeout/1000.0 : 2.0; station->recvDelay = recvDelay > 0 ? recvDelay/1000.0 : 1.0; station->outIOintDelay = outIOintDelay > 0 ? outIOintDelay/1000.0 : 1.0; /* append station to list */ *pstation = station; return 0; } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ #ifndef EPICS_3_13 static const iocshArg s7plcFWConfigureArg0 = { "name", iocshArgString }; static const iocshArg s7plcFWConfigureArg1 = { "IPaddr", iocshArgString }; static const iocshArg s7plcFWConfigureArg2 = { "fetchInfo", iocshArgString }; static const iocshArg s7plcFWConfigureArg3 = { "writeInfo", iocshArgString }; static const iocshArg s7plcFWConfigureArg4 = { "bigEndian", iocshArgInt }; static const iocshArg s7plcFWConfigureArg5 = { "recvTimeout", iocshArgInt }; static const iocshArg s7plcFWConfigureArg6 = { "recvDelay", iocshArgInt }; static const iocshArg s7plcFWConfigureArg7 = { "outIOintDelay", iocshArgInt }; static const iocshArg * const s7plcFWConfigureArgs[] = { &s7plcFWConfigureArg0, &s7plcFWConfigureArg1, &s7plcFWConfigureArg2, &s7plcFWConfigureArg3, &s7plcFWConfigureArg4, &s7plcFWConfigureArg5, &s7plcFWConfigureArg6, &s7plcFWConfigureArg7 }; static const iocshFuncDef s7plcFWConfigureDef = { "s7plcFWConfigure", 8, s7plcFWConfigureArgs }; static void s7plcFWConfigureFunc (const iocshArgBuf *args) { (void) s7plcFWConfigure( args[0].sval, args[1].sval, args[2].sval, args[3].sval, args[4].ival, args[5].ival, args[6].ival, args[7].ival); } static void s7plcFWRegister () { iocshRegister(&s7plcFWConfigureDef, s7plcFWConfigureFunc); } epicsExportRegistrar(s7plcFWRegister); #endif /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ s7plcFWStation *s7plcFWOpen(char *name) { s7plcFWStation *station; for (station = s7plcFWStationList; station; station = station->next) { if (strcmp(name, station->name) == 0) { return station; } } errlogSevPrintf(errlogFatal, "s7plcFWOpen: station %s not found\n", name); return NULL; } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ IOSCANPVT s7plcFWGetFetchScanPvt(s7plcFWStation *station) { return station->fetchScanPvt; } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ IOSCANPVT s7plcFWGetWriteScanPvt(s7plcFWStation *station) { return station->writeScanPvt; } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ STATIC void s7plcFWMain () { s7plcFWStation* station; char threadname[20]; s7plcFWDebugLog(1, "s7plcFWMain: main thread started\n"); while (1) { /* watch loop to restart dead threads and reopen sockets*/ for (station = s7plcFWStationList; station; station=station->next) { /* establish connection with server - for fetch */ epicsMutexMustLock(station->fetchIo); if (station->fetchSocket == -1) { /* create station socket */ if ((station->fetchSocket = socket(AF_INET, SOCK_STREAM, 0)) < 0) { fprintf(stderr, "s7plcFWMain %s: FATAL ERROR! fetch socket(AF_INET, SOCK_STREAM, 0) failed: %s\n", station->name, strerror(errno)); abort(); } s7plcFWDebugLog(1, "s7plcFWMain %s: Connect to %s:%d on fetch socket %d\n", station->name, station->serverIP, station->fetchPort, station->fetchSocket); if (s7plcFWEstablishConnection(station, FOR_FETCH) < 0) { s7plcFWDebugLog(1, "s7plcFWMain %s: connect(%d, %s:%d) failed for fetch: %s. Retry in %d ms\n", station->name, station->fetchSocket, station->serverIP, station->fetchPort, strerror(errno), (int)(RECONNECT_DELAY * 1000.0)); if (close(station->fetchSocket) && errno != ENOTCONN) { s7plcFWDebugLog(1, "s7plcFWMain %s: close(%d) failed for fetch (ignored): %s\n", station->name, station->fetchSocket, strerror(errno)); } station->fetchSocket=-1; } else { station->fetchConnStatus = 1; } } epicsMutexUnlock(station->fetchIo); /* establish connection with server - for write */ if (station->writeSize != 0) { epicsMutexMustLock(station->writeIo); if (station->writeSocket == -1) { /* create station socket */ if ((station->writeSocket = socket(AF_INET, SOCK_STREAM, 0)) < 0) { fprintf(stderr, "s7plcFWMain %s: FATAL ERROR! write socket(AF_INET, SOCK_STREAM, 0) failed: %s\n", station->name, strerror(errno)); abort(); } s7plcFWDebugLog(1, "s7plcFWMain %s: Connect to %s:%d on write socket %d\n", station->name, station->serverIP, station->writePort, station->writeSocket); if (s7plcFWEstablishConnection(station, FOR_WRITE) < 0) { s7plcFWDebugLog(1, "s7plcFWMain %s: connect(%d, %s:%d) failed for write: %s. Retry in %d ms\n", station->name, station->writeSocket, station->serverIP, station->writePort, strerror(errno), (int)(RECONNECT_DELAY * 1000.0)); if (close(station->writeSocket) && errno != ENOTCONN) { s7plcFWDebugLog(1, "s7plcFWMain %s: close(%d) failed for write (ignored): %s\n", station->name, station->writeSocket, strerror(errno)); } station->writeSocket=-1; } else { station->writeConnStatus = 1; } } epicsMutexUnlock(station->writeIo); } /* printf("s7plcFWMain: fetchConnStatus=%d fetchSocket=%d writeConnStatus=%d writeSocket=%d\n", station->fetchConnStatus, station->fetchSocket, station->writeConnStatus, station->writeSocket); */ if ((station->fetchSocket >= 0) && (station->writeSocket >= 0) && (station->fetchWriteDb == 1)) { int status; epicsMutexMustLock(station->writeMutex); status = s7plcFWdoFetch(station, station->writeOrg, station->writeDb, station->writeOffs, station->writeSize, station->writeBuffer); epicsMutexUnlock(station->writeMutex); if (status != 0) { printf("s7plcFWMain: %s: FETCH writeDb %d size %d FAILED - will use zeroed writeDb\n", station->name, station->writeDb, station->writeSize); bzero(station->writeBuffer, station->writeSize); } /* else { int i; unsigned short int *ptr = (unsigned short int *) station->writeBuffer; printf("s7plcFWMain: %s: FETCH writeDb %d size %d OK\n", station->name, station->writeDb, station->writeSize); for (i = 0; i < (station->writeSize / 2); i++) { printf(" writeDb[%3d] = 0x%04X\n", i, ptr[i] & 0xFFFF); } } */ station->fetchWriteDb = 0; /* we declare it here fetched (even if it failed to fetch) */ } /* check whether station threads are running */ /* thread for former write - now out I/O intr notification (periodic action) */ sprintf (threadname, "%.15sS", station->name); if (station->iointThread && epicsThreadIsSuspended(station->iointThread)) { /* if suspended delete it */ s7plcFWDebugLog(0, "s7plcFWMain %s: send thread %s %p is dead\n", station->name, threadname, station->iointThread); station->iointThread = 0; } if (!station->iointThread && station->writeSize) { s7plcFWDebugLog(1, "s7plcFWMain %s: starting send thread %s\n", station->name, threadname); station->iointThread = epicsThreadCreate(threadname, epicsThreadPriorityMedium, STACK_SIZE, (EPICSTHREADFUNC) s7plcFWIOintThread, station); if (!station->iointThread) { fprintf(stderr, "s7plcFWMain %s: FATAL ERROR! could not start send thread %s\n", station->name, threadname); abort(); } } /* thread for fetch */ sprintf (threadname, "%.15sR", station->name); if (station->fetchThread && epicsThreadIsSuspended(station->fetchThread)) { /* if suspended delete it */ s7plcFWDebugLog(0, "s7plcFWMain %s: recv thread %s %p is dead\n", station->name, threadname, station->fetchThread); /* maybe we should cleanup the semaphores ? */ s7plcFWCloseConnection(station, FOR_FETCH); station->fetchThread = 0; } if (!station->fetchThread && station->fetchSize) { s7plcFWDebugLog(1, "s7plcFWMain %s: starting recv thread %s\n", station->name, threadname); station->fetchThread = epicsThreadCreate(threadname, epicsThreadPriorityMedium, STACK_SIZE, (EPICSTHREADFUNC) s7plcFWFetchThread, station); if (!station->fetchThread) { fprintf(stderr, "s7plcFWMain %s: FATAL ERROR! could not start recv thread %s\n", station->name, threadname); abort(); } } } epicsThreadSleep(RECONNECT_DELAY); } } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ int s7plcFWReadArray(s7plcFWStation *station, unsigned int offset, unsigned int dlen, unsigned int nelem, void* data) { unsigned int elem, i; unsigned char byte; epicsUInt16 connStatus; /* special case: nelem = 0 & dlen = 0 - we actualy read only connections status */ if ((nelem == 0) && (dlen == 0)) { if (offset == 1) connStatus = station->fetchConnStatus; else connStatus = station->writeConnStatus; if (!connStatus) return S_drv_noConn; return S_drv_OK; } if (offset+dlen > station->fetchSize) { errlogSevPrintf(errlogMajor, "s7plcFWRead %s/%u: offset out of range\n", station->name, offset); return S_drv_badParam; } if (offset+nelem*dlen > station->fetchSize) { errlogSevPrintf(errlogMajor, "s7plcFWRead %s/%u: too many elements (%u)\n", station->name, offset, nelem); return S_drv_badParam; } s7plcFWDebugLog(4, "s7plcFWReadArray (station=%p, offset=%u, dlen=%u, nelem=%u)\n", station, offset, dlen, nelem); epicsMutexMustLock(station->fetchMutex); connStatus = station->fetchConnStatus; for (elem = 0; elem < nelem; elem++) { s7plcFWDebugLog(5, "data in:"); for (i = 0; i < dlen; i++) { if (station->swapBytes) byte = station->fetchBuffer[offset + elem*dlen + dlen - 1 - i]; else byte = station->fetchBuffer[offset + elem*dlen + i]; ((char*)data)[elem*dlen+i] = byte; s7plcFWDebugLog(5, " %02x", byte); } s7plcFWDebugLog(5, "\n"); } epicsMutexUnlock(station->fetchMutex); if (!connStatus) return S_drv_noConn; return S_drv_OK; } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ STATIC int s7plcFWdoFetch(s7plcFWStation *station, int org, int db, int offs, int size, char *recvBuf) { int s7addr = offs / 2; int s7len = size / 2; int status; unsigned char req[16] = {'S','5',16,1,3,5,3,8,0,0,0,0,0,0,0xff,2}; unsigned char ack[16]; int input; int received; if ((station->fetchConnStatus == 0) || (station->fetchSocket < 0)) return (-1); /* this should actually never happen */ req[8] = org; req[9] = db; req[0xa] = s7addr / 0x100; req[0xb] = s7addr % 0x100; req[0xc] = s7len / 0x100; req[0xd] = s7len % 0x100; epicsMutexMustLock(station->fetchIo); status = write(station->fetchSocket, (void *) req, 16); epicsMutexUnlock(station->fetchIo); if (status != 16) { s7plcFWDebugLog(3, "s7plcFWdoFetch: write 16 byte header failed, returned status = %d, errno = %d\n", status, errno); return (-1); } status = s7plcFWWaitForInput(station, FOR_FETCH, station->recvTimeout); if (status <= 0) { s7plcFWDebugLog(3, "s7plcFWdoFetch: s7plcFWWaitForInput timed-out, returned status = %d\n", status); return(-1); } epicsMutexMustLock(station->fetchIo); status = read(station->fetchSocket, (void *) ack, 16); epicsMutexUnlock(station->fetchIo); if(status < 16) { s7plcFWDebugLog(3, "s7plcFWdoFetch: Got too few bytes (%d) ACK from PLC!\n", status); return(-1); } if(ack[8] != 0) { s7plcFWDebugLog(3, "s7plcFWdoFetch:Got error %d from PLC!\n", ack[8]); return(ack[8]); } /* now receive data */ input = 0; while (input < size) { s7plcFWDebugLog(3, "s7plcFWdoFetch: %s: waiting for input for %d ms\n", station->name, (int)(station->recvTimeout * 1000.0)); status = s7plcFWWaitForInput(station, FOR_FETCH, station->recvTimeout); if (status <= 0) { s7plcFWDebugLog(0, "s7plcFWdoFetch: %s: s7plcFWWaitForInput(%d, ..., %d, 0) failed\n", station->name, station->fetchSocket, size - input); return (-1); } /* data available; receive data from server plc */ epicsMutexMustLock(station->fetchIo); received = recv(station->fetchSocket, recvBuf+input, size-input, 0); epicsMutexUnlock(station->fetchIo); s7plcFWDebugLog(3, "s7plcFWdoFetch: %s: received %d bytes\n", station->name, received); if (received <= 0) { s7plcFWDebugLog(0, "s7plcFWdoFetch: %s: recv(%d, ..., %d, 0) failed\n", station->name, station->fetchSocket, size - input); return (-1); } input += received; if (input > size) { /* input complete, check for excess bytes */ s7plcFWDebugLog(0, "s7plcFWdoFetch: %s: %d bytes excess data received\n", station->name, input - size); return (-1); } } return 0; /* OK */ } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ STATIC int s7plcFWdoWrite(s7plcFWStation *station, char *data, int offs, int len) { int s7addr = station->writeOffs / 2 + offs / 2; int s7len = len / 2; int status, dstatus, sstatus; unsigned char req[16] = {'S','5',16,1,3,3,3,8,0,0,0,0,0,0,0xff,2}; unsigned char ack[16]; /* printf("AD84: s7plcFWdoWrite: REQUEST to write %d bytes at offset %d\n", len, offs); */ req[8] = station->writeOrg; req[9] = station->writeDb; req[0xa] = s7addr / 0x100; req[0xb] = s7addr % 0x100; req[0xc] = s7len / 0x100; req[0xd] = s7len % 0x100; epicsMutexMustLock(station->writeIo); status = write(station->writeSocket, (void *) req, 16); dstatus = write(station->writeSocket, data, len); epicsMutexUnlock(station->writeIo); if (status != 16) { s7plcFWDebugLog(3, "s7plcFWdoWrite: Sent too few header bytes (%d), errno =%d\n", status, errno); return (-1); } if (dstatus != len) { s7plcFWDebugLog(3, "s7plcFWdoWrite: Sent too few data bytes (%d), errno =%d\n", dstatus, errno); return (-1); } /* printf("AD84: s7plcFWdoWrite: OK, sent 16 header bytes and %d data bytes\n", len); */ sstatus = s7plcFWWaitForInput(station, FOR_WRITE, station->recvTimeout); /* printf("AD84: s7plcFWdoWrite: s7plcFWWaitForInput returned ssatus=%d\n", sstatus); */ if (sstatus <= 0) { s7plcFWDebugLog(3, "s7plcFWdoWrite: s7plcFWWaitForInput timed-out, returned status = %d\n", sstatus); return(-1); } epicsMutexMustLock(station->writeIo); if((status = read(station->writeSocket, (void *) ack, 16)) < 16) { s7plcFWDebugLog(3, "Got too few bytes (%d) ACK from PLC!\n", status); /* printf("AD84: s7plcFWdoWrite: read too few bytes (%d) ACK from PLC!\n", status); */ epicsMutexUnlock(station->writeIo); return(-1); } epicsMutexUnlock(station->writeIo); /* printf("AD84: s7plcFWdoWrite: OK, received 16 header bytes confirmation\n"); */ if(ack[8] != 0) { s7plcFWDebugLog(3, "Got error %d from PLC!\n", ack[8]); return(ack[8]); } /* printf("AD84: s7plcFWWrite: OK, write %d data bytes succeeded\n", len); */ return 0; /* OK */ } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ int s7plcFWWriteMaskedArray(s7plcFWStation *station, unsigned int offset, unsigned int dlen, unsigned int nelem, void* data, void* mask) { unsigned int elem, i; unsigned char byte; epicsUInt16 connStatus; int wstatus, woffs, wlen; if (station->writeSize == 0) { errlogSevPrintf(errlogMajor, "s7plcFWWriteMaskedArray %s: write buffer size = 0, no writes allowed\n", station->name); return -1; } if (offset+dlen > station->writeSize) { errlogSevPrintf(errlogMajor, "s7plcFWWriteMaskedArray %s/%d: offset out of range\n", station->name, offset); return -1; } if (offset+nelem*dlen > station->writeSize) { errlogSevPrintf(errlogMajor, "s7plcFWWriteMaskedArray %s/%d: too many elements (%u)\n", station->name, offset, nelem); return -1; } s7plcFWDebugLog(4, "s7plcFWWriteMaskedArray (station=%p, offset=%u, dlen=%u, nelem=%u)\n", station, offset, dlen, nelem); epicsMutexMustLock(station->writeMutex); connStatus = station->writeConnStatus; for (elem = 0; elem < nelem; elem++) { s7plcFWDebugLog(5, "data out:"); for (i = 0; i < dlen; i++) { byte = ((unsigned char*)data)[elem*dlen+i]; if (mask) { s7plcFWDebugLog(5, "(%02x & %02x)", byte, ((unsigned char*)mask)[i]); byte &= ((unsigned char*)mask)[i]; } if (station->swapBytes) { if (mask) { s7plcFWDebugLog(5, " | (%02x & %02x) =>", station->writeBuffer[offset + elem*dlen + dlen - 1 - i], ~((unsigned char*)mask)[i]); byte |= station->writeBuffer[offset + elem*dlen + dlen - 1 - i] & ~((unsigned char*)mask)[i]; } s7plcFWDebugLog(5, " %02x", byte); station->writeBuffer[offset + elem*dlen + dlen - 1 - i] = byte; s7plcFWDebugLog(3, "s7plcFWWriteMaskedArray: writeBuffer[%3d] <- 0x%02X\n", offset + elem*dlen + dlen - 1 - i, byte & 0xFF); } else { if (mask) { s7plcFWDebugLog(5, " | (%02x & %02x) =>", station->writeBuffer[offset + elem*dlen + i], ~((unsigned char*)mask)[i]); byte |= station->writeBuffer[offset + elem*dlen + i] & ~((unsigned char*)mask)[i]; } s7plcFWDebugLog(5, " %02x", byte); station->writeBuffer[offset + elem*dlen + i] = byte; s7plcFWDebugLog(3, "s7plcFWWriteMaskedArray: writeBuffer[%3d] <- 0x%02X\n", offset + elem*dlen + i, byte & 0xFF); } } s7plcFWDebugLog(5, "\n"); /* we do write immediately here - not in a writethread (as original s7plc driver does) */ /* warning: we allways have to write even number of bytes - so it can happen that we have to write 1 or 2 bytes more than requested */ woffs = offset & 0xFFFFFFFE; /* we need even byte offset */ wlen = 0; if (woffs != offset) wlen += 1; wlen += nelem*dlen; if ((wlen % 2) != 0) wlen += 1; if (station->writeAll == 0) { wstatus = s7plcFWdoWrite(station, &station->writeBuffer[woffs], woffs, wlen); } else { wstatus = s7plcFWdoWrite(station, &station->writeBuffer[0], 0, station->writeSize); } if (wstatus != 0) { s7plcFWDebugLog(0, "s7plcFWWriteMaskedArray %s: s7plcFWdoWrite(%d, ...) failed\n", station->name, station->writeSocket); s7plcFWCloseConnection(station, FOR_WRITE); break; } } epicsMutexUnlock(station->writeMutex); if (!connStatus) return S_drv_noConn; return S_drv_OK; } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ /* this mimics (partly) I/O int for output records as used in S7plc driver */ STATIC void s7plcFWIOintThread (s7plcFWStation* station) { s7plcFWDebugLog(1, "s7plcFWIOintThread %s: started\n", station->name); while (1) { epicsTimerStartDelay(station->timer, station->outIOintDelay); if (interruptAccept) { scanIoRequest(station->writeScanPvt); /* printf("AD84: out I/O interrupt\n"); */ } epicsEventMustWait(station->outTrigger); } } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ STATIC void s7plcFWFetchThread (s7plcFWStation *station) { char* recvBuf = callocMustSucceed(2, station->fetchSize, "s7plcFWFetchThread"); s7plcFWDebugLog(1, "s7plcFWFetchThread %s: started\n", station->name); while (1) { int status; epicsThreadSleep(station->recvDelay); /* check (with timeout) for data arrival from server */ if ((station->fetchConnStatus != 0) && (station->fetchSocket != -1)) { s7plcFWDebugLog(3, "s7plcFWFetchThread %s: will initiate FETCH\n", station->name); status = s7plcFWdoFetch(station, station->fetchOrg, station->fetchDb, station->fetchOffs, station->fetchSize, recvBuf); if (status != 0) { s7plcFWDebugLog(0, "s7plcFWFetchThread %s: s7plcFWdoFetch(%d, 0, %d, ...) failed\n", station->name, station->fetchSocket, station->fetchSize); s7plcFWCloseConnection(station, FOR_FETCH); continue; /* was break before, but it would exit thread - no need to end thread */ } /* got it */ epicsMutexMustLock(station->fetchMutex); memcpy(station->fetchBuffer, recvBuf, station->fetchSize); epicsMutexUnlock(station->fetchMutex); #if 0 { int i; unsigned short int *ptr = (unsigned short int *) station->fetchBuffer; printf("received data (before swap):\n"); for (i = 0; i < (station->fetchSize/2); i++) { printf(" [%32d] 0x%04X\n", i, ptr[i] & 0xFFFF); } } #endif /* notify all "I/O Intr" input records */ s7plcFWDebugLog(3, "s7plcFWFetchThread %s: receive successful, notify all input records\n", station->name); scanIoRequest(station->fetchScanPvt); } } } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ STATIC int s7plcFWWaitForInput(s7plcFWStation* station, int which, double timeout) { struct timeval to; /* AD84: was static - why? */ int socket; int iSelect; fd_set socklist; if (which != FOR_WRITE) socket = station->fetchSocket; else socket = station->writeSocket; FD_ZERO(&socklist); FD_SET(socket, &socklist); to.tv_sec=(int)timeout; to.tv_usec=(int)((timeout-to.tv_sec)*1000000); /* select returns when either the socket has data or the timeout elapsed */ errno = 0; while ((iSelect=select(socket+1, &socklist, 0, 0, &to)) < 0) { if (errno != EINTR) { s7plcFWDebugLog(0, "s7plcFWWaitForInput %s: select(%d, %d ms) failed: %s\n", station->name, socket, (int)(timeout * 1000.0), strerror(errno)); return -1; } } if (iSelect==0 && timeout > 0) { /* timed out */ s7plcFWDebugLog(0, "s7plcFWWaitForInput %s: select(%d, %d ms) timed out\n", station->name, socket, (int)(timeout * 1000.0)); errno = ETIMEDOUT; } return iSelect; } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ STATIC int s7plcFWEstablishConnection(s7plcFWStation* station, int which) { struct sockaddr_in serverAddr; /* server socket address */ struct timeval to; int socket; unsigned short int port; #ifndef __vxworks long opt; #endif if (which != FOR_WRITE) { socket = station->fetchSocket; port = station->fetchPort; } else { socket = station->writeSocket; port = station->writePort; } s7plcFWDebugLog(1, "s7plcFWEstablishConnection %s: fd=%d, IP=%s port=%d\n", station->name, socket, station->serverIP, port); /* build server socket address */ bzero((char *) &serverAddr, sizeof (serverAddr)); serverAddr.sin_family = AF_INET; serverAddr.sin_port = htons(port); serverAddr.sin_addr.s_addr = inet_addr(station->serverIP); /* connect to server */ to.tv_sec=(int)(CONNECT_TIMEOUT); to.tv_usec=(int)(CONNECT_TIMEOUT-to.tv_sec)*1000000; #ifdef __vxworks if (connectWithTimeout(socket, (struct sockaddr *) &serverAddr, sizeof (serverAddr), &to) < 0) { s7plcFWDebugLog(0, "s7plcFWEstablishConnection %s: connectWithTimeout(%d,...,%d ms) failed: %s\n", station->name, socket, (int)(CONNECT_TIMEOUT * 1000.0), strerror(errno)); return -1; } #else /* connect in non-blocking mode */ if((opt = fcntl(socket, F_GETFL, NULL)) < 0) { s7plcFWDebugLog(0, "s7plcFWEstablishConnection %s: fcntl(%d, F_GETFL, NULL) failed: %s\n", station->name, socket, strerror(errno)); return -1; } opt |= O_NONBLOCK; if(fcntl(socket, F_SETFL, opt) < 0) { s7plcFWDebugLog(0, "s7plcFWEstablishConnection %s: fcntl(%d, F_SETFL, O_NONBLOCK) failed: %s\n", station->name, socket, strerror(errno)); return -1; } if (connect(socket, (struct sockaddr *) &serverAddr, sizeof(serverAddr)) < 0) { if (errno == EINPROGRESS) { /* start timeout */ long status; socklen_t lon = sizeof(status); fd_set fdset; FD_ZERO(&fdset); FD_SET(socket, &fdset); /* wait for connection */ while ((status = select(socket+1, NULL, &fdset, NULL, &to)) < 0) { if (errno != EINTR) { s7plcFWDebugLog(0, "s7plcFWEstablishConnection %s: select(%d, %d ms) failed: %s\n", station->name, socket, (int)(CONNECT_TIMEOUT * 1000.0), strerror(errno)); return -1; } } if (status == 0) { s7plcFWDebugLog(0, "s7plcFWEstablishConnection %s: select(%d, %d ms) timed out\n", station->name, socket, (int)(CONNECT_TIMEOUT * 1000.0)); errno = ETIMEDOUT; return -1; } /* get background error status */ if (getsockopt(socket, SOL_SOCKET, SO_ERROR, &status, &lon) < 0) { s7plcFWDebugLog(0, "s7plcFWEstablishConnection %s: getsockopt(%d,...) failed: %s\n", station->name, socket, strerror(errno)); return -1; } if (status) { errno = status; s7plcFWDebugLog(0, "s7plcFWEstablishConnection %s: background connect(%d,...) failed: %s\n", station->name, socket, strerror(errno)); return -1; } } else { s7plcFWDebugLog(0, "s7plcFWEstablishConnection %s: connect(%d,...) failed: %s\n", station->name, socket, strerror(errno)); return -1; } } /* connected */ opt &= ~O_NONBLOCK; if(fcntl(socket, F_SETFL, opt) < 0) { s7plcFWDebugLog(0, "s7plcFWEstablishConnection %s: fcntl(%d, F_SETFL, ~O_NONBLOCK) failed: %s\n", station->name, socket, strerror(errno)); return -1; } #endif s7plcFWDebugLog(1, "s7plcFWEstablishConnection %s: fd=%d, IP=%s port=%d - ESTABLISHED\n", station->name, socket, station->serverIP, port); return 0; } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/ STATIC void s7plcFWCloseConnection(s7plcFWStation* station, int which) { int socket; unsigned short int port; if (which != FOR_WRITE) { socket = station->fetchSocket; port = station->fetchPort; station->fetchConnStatus = 0; } else { socket = station->writeSocket; port = station->writePort; station->writeConnStatus = 0; } if (socket > 0) { if (shutdown(socket, 2) < 0) { s7plcFWDebugLog(0, "s7plcFWCloseConnection %s: shutdown(%d, 2) failed (ignored): %s\n", station->name, socket, strerror(errno)); } if (close(socket) && errno != ENOTCONN) { s7plcFWDebugLog(0, "s7plcFWCloseConnection %s: close(%d) failed (ignored): %s\n", station->name, socket, strerror(errno)); } if (which != FOR_WRITE) { station->fetchSocket = -1; } else { station->writeSocket = -1; } } /* notify all "I/O Intr" input records */ scanIoRequest(station->fetchScanPvt); s7plcFWDebugLog(1, "s7plcFWCloseConnection %s: fd=%d, IP=%s port=%d - CLOSED\n", station->name, socket, station->serverIP, port); } /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/