1072 lines
40 KiB
C
1072 lines
40 KiB
C
/* $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 <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <stdarg.h>
|
|
#include <string.h>
|
|
#include <unistd.h>
|
|
#include <errno.h>
|
|
#include <sys/socket.h>
|
|
#include <netinet/in.h>
|
|
#include <arpa/inet.h>
|
|
|
|
#if defined(vxWorks) || defined(__vxworks)
|
|
#include <sockLib.h>
|
|
#include <taskLib.h>
|
|
#include <selectLib.h>
|
|
#include <taskHookLib.h>
|
|
#define in_addr_t unsigned long
|
|
#else
|
|
#include <fcntl.h>
|
|
#endif
|
|
|
|
#ifdef __rtems__
|
|
#include <sys/select.h>
|
|
#endif
|
|
|
|
#include <drvSup.h>
|
|
#include <devLib.h>
|
|
#include <epicsVersion.h>
|
|
#include <epicsPrint.h>
|
|
|
|
#include "drvS7plcFW.h"
|
|
|
|
#ifndef BASE_VERSION
|
|
/* R3.14 */
|
|
#include <dbAccess.h>
|
|
#include <iocsh.h>
|
|
#include <cantProceed.h>
|
|
#include <epicsMutex.h>
|
|
#include <epicsThread.h>
|
|
#include <epicsTimer.h>
|
|
#include <epicsEvent.h>
|
|
#include <epicsExport.h>
|
|
#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 <endian.h>
|
|
#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);
|
|
|
|
}
|
|
|
|
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
|
|
|