forked from epics_driver_modules/motorBase
Lots of changes to make it robust, and for V1.5.1 of XPS firmware
This commit is contained in:
MarkRivers
@@ -1,8 +1,9 @@
|
||||
/* This script was addapted from the Newport Socket.cpp code
|
||||
/* This script was adapted from the Newport Socket.cpp code
|
||||
to provide TCP/IP sockets for the XPSC8 motion controller
|
||||
using EPICS asynOctetSyncIO.cc
|
||||
|
||||
By Jon Kelly July 2005
|
||||
Re-written by Mark Rivers March 2006
|
||||
*/
|
||||
|
||||
/* includes */
|
||||
@@ -23,288 +24,184 @@ typedef int BOOL;
|
||||
#include <epicsString.h>
|
||||
#include <asynDriver.h>
|
||||
#include <asynOctetSyncIO.h>
|
||||
#include <asynCommonSyncIO.h>
|
||||
#include <drvAsynIPPort.h>
|
||||
|
||||
|
||||
/* defines */
|
||||
/* The maximum number of sockets to XPS controllers. The driver uses
|
||||
* one socket per motor plus one per controller, so a maximum of 9 per controller.
|
||||
* This number is used to create an array because the Newport code uses integers for
|
||||
* the socket ID. */
|
||||
#define MAX_SOCKETS 1000
|
||||
#define PORT_NAME_SIZE 100
|
||||
#define ERROR_STRING_SIZE 100
|
||||
#define DEFAULT_TIMEOUT 0.2
|
||||
|
||||
#define MAX_MSG_SIZE 256
|
||||
#define TIMEOUT 0.2
|
||||
#define MAX_RETRY 5
|
||||
#define CONNREFUSED -1
|
||||
#define CREATESOCKETFAILED -2
|
||||
#define OPTREFUSED -3
|
||||
|
||||
#define MAX_NB_SOCKETS 50
|
||||
|
||||
/* global variables */
|
||||
|
||||
BOOL UsedSocket[MAX_NB_SOCKETS] = { FALSE };
|
||||
double TimeoutSocket[MAX_NB_SOCKETS];
|
||||
int ErrorSocket[MAX_NB_SOCKETS];
|
||||
static int nextSocket = 0;
|
||||
|
||||
/* Pointer to the connection info for each socket
|
||||
the asynUser structure is defined in asynDriver.h */
|
||||
static struct asynUser *pasynUserArray[MAX_NB_SOCKETS];
|
||||
|
||||
asynStatus status;
|
||||
|
||||
/***************************************************************************************/
|
||||
#define DEBUG
|
||||
|
||||
#ifdef __GNUG__
|
||||
#ifdef DEBUG
|
||||
volatile int asynXPSC8Debug = 0;
|
||||
#define Debug(l, f, args...) { if(l<=asynXPSC8Debug) printf(f,## args); }
|
||||
epicsExportAddress(int, asynXPSC8Debug);
|
||||
#else
|
||||
#define Debug(l, f, args...)
|
||||
#endif
|
||||
#else
|
||||
#define Debug()
|
||||
#endif
|
||||
|
||||
typedef struct {
|
||||
asynUser *pasynUser;
|
||||
asynUser *pasynUserCommon;
|
||||
double timeout;
|
||||
char errorString[ERROR_STRING_SIZE];
|
||||
int connected;
|
||||
} socketStruct;
|
||||
static socketStruct socketStructs[MAX_SOCKETS];
|
||||
|
||||
|
||||
/***************************************************************************************/
|
||||
/* The drvXPSC8.cc must assign the aysn port string to the IP field
|
||||
and replace the xps port int with the asyn addr int */
|
||||
int ConnectToServer(char *Ip_Address, int Ip_Port, double TimeOut)
|
||||
int ConnectToServer(char *IpAddress, int IpPort, double timeout)
|
||||
{
|
||||
|
||||
int SocketIndex = 0;
|
||||
const char *asynPort = (const char *)Ip_Address;
|
||||
int asynAddr;
|
||||
asynAddr = Ip_Port;
|
||||
char portName[PORT_NAME_SIZE];
|
||||
char ipString[PORT_NAME_SIZE];
|
||||
asynUser *pasynUser, *pasynUserCommon;
|
||||
socketStruct *psock;
|
||||
int status;
|
||||
|
||||
/* Select a free socket */
|
||||
while ((UsedSocket[SocketIndex] == TRUE) && (SocketIndex < MAX_NB_SOCKETS))
|
||||
{
|
||||
SocketIndex++;
|
||||
}
|
||||
|
||||
if (SocketIndex == MAX_NB_SOCKETS)
|
||||
if (nextSocket >= MAX_SOCKETS) {
|
||||
printf("ConnectToServer: too many open sockets, max=%d\n", MAX_SOCKETS);
|
||||
return -1;
|
||||
}
|
||||
/* Create a new asyn port */
|
||||
epicsSnprintf(ipString, PORT_NAME_SIZE, "%s:%d TCP", IpAddress, IpPort);
|
||||
epicsSnprintf(portName, PORT_NAME_SIZE, "%s:%d:%d", IpAddress, IpPort, nextSocket);
|
||||
drvAsynIPPortConfigure(portName, ipString, 0, 0, 1);
|
||||
|
||||
ErrorSocket[SocketIndex] = 0;
|
||||
/* Connect to driver with asynOctet interface */
|
||||
status = pasynOctetSyncIO->connect(portName, 0, &pasynUser, NULL);
|
||||
if (status != asynSuccess) {
|
||||
printf("ConnectToServer, error calling pasynOctetSyncIO->connect %s\n", pasynUser->errorMessage);
|
||||
return -1;
|
||||
}
|
||||
psock = &socketStructs[nextSocket];
|
||||
psock->pasynUser = pasynUser;
|
||||
|
||||
/* Connect to driver with asynCommon interface */
|
||||
status = pasynCommonSyncIO->connect(portName, 0, &pasynUserCommon, NULL);
|
||||
if (status != asynSuccess) {
|
||||
printf("ConnectToServer, error calling pasynCommonSyncIO->connect %s\n",
|
||||
pasynUserCommon->errorMessage);
|
||||
return -1;
|
||||
}
|
||||
psock->pasynUserCommon = pasynUserCommon;
|
||||
|
||||
/* These asyn functions are defined in asynOctetSyncIO.c */
|
||||
status = pasynOctetSyncIO->connect(asynPort,asynAddr,&pasynUserArray[SocketIndex],NULL);
|
||||
|
||||
if (status != asynSuccess )
|
||||
{
|
||||
ErrorSocket[SocketIndex] = CREATESOCKETFAILED;
|
||||
printf("Error Socket connect failed asynStatus=%i \n",status);
|
||||
/* Connect to controller */
|
||||
status = pasynCommonSyncIO->connectDevice(pasynUserCommon);
|
||||
if (status != asynSuccess) {
|
||||
printf("ConnectToServer, error calling pasynCommonSyncIO->connectDevice %s\n",
|
||||
pasynUserCommon->errorMessage);
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
/* Use default timeout macro if one is not given
|
||||
if (TimeOut > 0) TimeoutSocket[SocketIndex] = TimeOut;
|
||||
else TimeoutSocket[SocketIndex] = TIMEOUT;*/
|
||||
|
||||
UsedSocket[SocketIndex] = TRUE;
|
||||
TimeoutSocket[SocketIndex] = TIMEOUT; /* Ignore the requested timeout */
|
||||
return SocketIndex;
|
||||
psock->timeout = timeout;
|
||||
psock->connected = 1;
|
||||
strcpy(psock->errorString, "");
|
||||
|
||||
nextSocket++;
|
||||
return nextSocket-1;
|
||||
}
|
||||
|
||||
/***************************************************************************************/
|
||||
void SetTCPTimeout(int SocketIndex, double TimeOut)
|
||||
{
|
||||
if ((SocketIndex >= 0) && (SocketIndex < MAX_NB_SOCKETS) && (UsedSocket[SocketIndex] == TRUE))
|
||||
{
|
||||
if (TimeOut > 0) TimeoutSocket[SocketIndex] = TimeOut;
|
||||
if ((SocketIndex < 0) || (SocketIndex >= nextSocket)) {
|
||||
printf("SetTCPTimeout, SocketIndex=%d, must be >=0 and < %d\n", SocketIndex, nextSocket);
|
||||
return;
|
||||
}
|
||||
socketStructs[SocketIndex].timeout = TimeOut;
|
||||
}
|
||||
|
||||
|
||||
/***************************************************************************************/
|
||||
void SendAndReceive (int SocketIndex, char *buffer, char *valueRtrn)
|
||||
void SendAndReceive (int SocketIndex, char *buffer, char *valueRtrn, int returnSize)
|
||||
{
|
||||
size_t nbytesOut = 0;
|
||||
size_t nbytesIn = 0;
|
||||
size_t nbytesOut;
|
||||
size_t nbytesIn;
|
||||
int eomReason;
|
||||
/*clock_t start, finish;
|
||||
double timeEllapse = 0.0;*/
|
||||
char *output, *input;
|
||||
int writeReadStatus = -99;
|
||||
asynUser *pasynUser;
|
||||
int loop = 0, bufferLength;
|
||||
int bufferLength;
|
||||
socketStruct *psock;
|
||||
int status;
|
||||
|
||||
/* Check to see if the Socket is valid! */
|
||||
|
||||
bufferLength = strlen(buffer);
|
||||
if ((SocketIndex < 0) || (SocketIndex >= MAX_NB_SOCKETS) || (UsedSocket[SocketIndex] != TRUE)) {
|
||||
/* Not allowed action socket was not created */
|
||||
printf("Error Socket Invalid SendAndRecieve In=%s Out=%s\n",valueRtrn,buffer);
|
||||
printf("asynStatus=%i ",status);
|
||||
if ((SocketIndex < 0) || (SocketIndex >= nextSocket)) {
|
||||
printf("SendAndReceive: invalid SocketIndex %d\n", SocketIndex);
|
||||
strcpy(valueRtrn,"-22");
|
||||
return;
|
||||
}
|
||||
if (bufferLength > MAX_MSG_SIZE) {
|
||||
/* Error string too long */
|
||||
printf("Error buffer>Max SendAndRecieve Out=%s In=%s\n",valueRtrn,buffer);
|
||||
printf("asynStatus=%i ",status);
|
||||
strcpy(valueRtrn,"-3");
|
||||
psock = &socketStructs[SocketIndex];
|
||||
if (!psock->connected) {
|
||||
printf("SendAndReceive: socket not connected %d\n", SocketIndex);
|
||||
strcpy(valueRtrn,"-22");
|
||||
return;
|
||||
}
|
||||
|
||||
pasynUser = pasynUserArray[SocketIndex];
|
||||
while ((writeReadStatus < 0) && (loop < MAX_RETRY)) {
|
||||
status = pasynOctetSyncIO->writeRead(pasynUser,
|
||||
|
||||
/* If timeout > 0. then we do a write read. If < 0. then write. */
|
||||
|
||||
if (psock->timeout > 0.0) {
|
||||
status = pasynOctetSyncIO->writeRead(psock->pasynUser,
|
||||
(char const *)buffer,
|
||||
bufferLength,
|
||||
valueRtrn,
|
||||
MAX_MSG_SIZE,
|
||||
TimeoutSocket[SocketIndex],
|
||||
returnSize,
|
||||
psock->timeout,
|
||||
&nbytesOut,
|
||||
&nbytesIn,
|
||||
&eomReason);
|
||||
if ( status != asynSuccess ) {
|
||||
/* Asyn command error */
|
||||
asynPrint(pasynUser, ASYN_TRACE_ERROR,
|
||||
"SendAndReceive error calling writeRead, status=%d, %s\n",
|
||||
status, pasynUser->errorMessage);
|
||||
continue; /* Try again */
|
||||
asynPrint(psock->pasynUser, ASYN_TRACE_ERROR,
|
||||
"SendAndReceive error calling writeRead, output=%s status=%d, error=%s\n",
|
||||
buffer, status, psock->pasynUser->errorMessage);
|
||||
}
|
||||
|
||||
Debug(12,"buffer %s\n bufferlen %i\n valueRtrn %s\n", buffer, bufferLength, valueRtrn);
|
||||
Debug(12,"nbytesOut %i nbytesIn %i eomReason %i\n", nbytesOut, nbytesIn, eomReason);
|
||||
output = valueRtrn;
|
||||
if (output != NULL) sscanf (output, "%i", &writeReadStatus);
|
||||
|
||||
/* If the XPS returns a status <0 it is probably still confused from a
|
||||
* sendOnly so re-send the command */
|
||||
|
||||
if (writeReadStatus < 0){
|
||||
asynPrint(pasynUser, ASYN_TRACE_ERROR,
|
||||
"SendAndReceive error writeReadStatus=%d\n",
|
||||
writeReadStatus);
|
||||
Debug(2,"Sock=%i Tout=%g In=%s Out=%s \n Command sent again! loop %i\n",SocketIndex,
|
||||
TimeoutSocket[SocketIndex],valueRtrn,buffer,loop);
|
||||
} else {
|
||||
|
||||
/* If the XPS returns a non error status check to see if the data
|
||||
* is what you expected because it frequently returns the answer
|
||||
* to the previous call! */
|
||||
|
||||
output = valueRtrn;
|
||||
input = buffer;
|
||||
strchr (output, ',');
|
||||
while ( (input=strchr (input, '*')) != NULL ) { /* Number required */
|
||||
input++; /* Move ptr past * */
|
||||
if ((output=strchr (output, ',')) == NULL) { /* No comma after number */
|
||||
asynPrint(pasynUser, ASYN_TRACE_ERROR,
|
||||
"SendAndReceive not enough numbers in %s out=%s\n",
|
||||
valueRtrn, buffer);
|
||||
writeReadStatus = -98;
|
||||
break;
|
||||
}
|
||||
output++; /* move ptr past , */
|
||||
}
|
||||
asynPrint(psock->pasynUser, ASYN_TRACEIO_DRIVER,
|
||||
"SendAndReceive, sent: '%s', received: '%s'\n",
|
||||
buffer, valueRtrn);
|
||||
} else {
|
||||
/* This is typically used for the "Move" commands, and we don't want to wait for the response */
|
||||
/* Fake the response by putting "-1" (for error) or "0" (for success) in the return string */
|
||||
status = pasynOctetSyncIO->write(psock->pasynUser,
|
||||
(char const *)buffer,
|
||||
bufferLength,
|
||||
-psock->timeout,
|
||||
&nbytesOut);
|
||||
if ( status != asynSuccess ) {
|
||||
asynPrint(psock->pasynUser, ASYN_TRACE_ERROR,
|
||||
"SendAndReceive error calling write, output=%s status=%d, error=%s\n",
|
||||
buffer, status, psock->pasynUser->errorMessage);
|
||||
strcpy(valueRtrn, "-1");
|
||||
}
|
||||
loop++;
|
||||
asynPrint(psock->pasynUser, ASYN_TRACEIO_DRIVER,
|
||||
"SendAndReceive, sent: '%s'\n", buffer);
|
||||
strcpy(valueRtrn, "0");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/***************************************************************************************/
|
||||
void SendOnly (int SocketIndex, char *buffer, char *valueRtrn)
|
||||
{
|
||||
char dummyReturn[MAX_MSG_SIZE];
|
||||
size_t nbytesIn = 0;
|
||||
size_t nbytesOut = 0;
|
||||
int eomReason;
|
||||
char const dummyBuffer[40] = "FirmwareVersionGet (char *)";
|
||||
char *output;
|
||||
int writeReadStatus = -99;
|
||||
asynUser *pasynUser;
|
||||
int loop = 0;
|
||||
|
||||
/* Valid Socket? */
|
||||
if ((SocketIndex < 0) || (SocketIndex >= MAX_NB_SOCKETS) || (UsedSocket[SocketIndex] != TRUE)) {
|
||||
/* Not allowed action socket was not created */
|
||||
printf("Error Socket Invalid SendOnly In=%s Out=%s\n",valueRtrn,buffer);
|
||||
printf("asynStatus=%i ",status);
|
||||
strcpy(valueRtrn,"-22");
|
||||
return;
|
||||
}
|
||||
if (strlen(buffer) > MAX_MSG_SIZE) {
|
||||
/* Error string too long */
|
||||
strcpy(valueRtrn,"-3");
|
||||
return;
|
||||
}
|
||||
|
||||
pasynUser = pasynUserArray[SocketIndex];
|
||||
Above_Write:
|
||||
status = pasynOctetSyncIO->write(pasynUser, (char const *)buffer,
|
||||
strlen(buffer), TimeoutSocket[SocketIndex], &nbytesOut);
|
||||
|
||||
if ((status != asynSuccess) || (nbytesOut <=0)) {
|
||||
asynPrint(pasynUser, ASYN_TRACE_ERROR,
|
||||
"SendOnly error calling writs, status=%d, nbytesOut=%d, %s\n",
|
||||
status, nbytesOut, pasynUser->errorMessage);
|
||||
/* Error during write message */
|
||||
strcpy(valueRtrn,"-72");
|
||||
return;
|
||||
}
|
||||
|
||||
/* It seems to be necessary to sleep a little while after sending a command?? */
|
||||
epicsThreadSleep(0.02);
|
||||
|
||||
/* When a send only is performed but the XPS is expecting to give a response it
|
||||
* becomes confused for a period of time. A writeRead is sent and the readback
|
||||
* checked until the XPS returns a sensible value */
|
||||
|
||||
Above_Dummy_writeRead:
|
||||
status = pasynOctetSyncIO->writeRead(pasynUser,
|
||||
(char const *)dummyBuffer,
|
||||
strlen(dummyBuffer),
|
||||
dummyReturn,
|
||||
MAX_MSG_SIZE,
|
||||
TimeoutSocket[SocketIndex],
|
||||
&nbytesOut,
|
||||
&nbytesIn,
|
||||
&eomReason);
|
||||
if ((status != asynSuccess) || (nbytesIn <=0)) {
|
||||
asynPrint(pasynUser, ASYN_TRACE_ERROR,
|
||||
"SendOnly error calling writeRead, status=%d, nbytesIn=%d, %s\n",
|
||||
status, nbytesIn, pasynUser->errorMessage);
|
||||
/* Error during write message */
|
||||
strcpy(valueRtrn,"-72");
|
||||
return;
|
||||
}
|
||||
|
||||
output = dummyReturn;
|
||||
if (output != NULL) sscanf (output, "%i", &writeReadStatus);
|
||||
else Debug(5,"SendOnly Dummywriteread Output Null\n");
|
||||
|
||||
if (writeReadStatus < 0 && output != NULL){
|
||||
asynPrint(pasynUser, ASYN_TRACE_ERROR,
|
||||
"SendOnly error calling writeRead, writeReadStatus = %d, nbytesIn=%d, %s\n",
|
||||
status, nbytesIn, pasynUser->errorMessage);
|
||||
}
|
||||
loop++;
|
||||
if (writeReadStatus == -3 && loop < MAX_RETRY) /* Command Not executed by XPS so retry */
|
||||
goto Above_Write;
|
||||
if (writeReadStatus < 0 && loop < MAX_RETRY) /* The XPS is still confused so re-send*/
|
||||
goto Above_Dummy_writeRead;
|
||||
|
||||
strcpy(valueRtrn,"0");
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
/***************************************************************************************/
|
||||
void CloseSocket(int SocketIndex)
|
||||
{
|
||||
if ((SocketIndex >= 0) && (SocketIndex < MAX_NB_SOCKETS) && (UsedSocket[SocketIndex] == TRUE))
|
||||
{
|
||||
status = pasynOctetSyncIO->disconnect(pasynUserArray[SocketIndex]);
|
||||
TimeoutSocket[SocketIndex] = TIMEOUT;
|
||||
ErrorSocket[SocketIndex] = 0;
|
||||
UsedSocket[SocketIndex] = FALSE;
|
||||
socketStruct *psock;
|
||||
asynUser *pasynUser;
|
||||
int status;
|
||||
|
||||
if ((SocketIndex < 0) || (SocketIndex >= nextSocket)) {
|
||||
printf("CloseSocket: invalid SocketIndex %d\n", SocketIndex);
|
||||
return;
|
||||
}
|
||||
psock = &socketStructs[SocketIndex];
|
||||
pasynUser = psock->pasynUserCommon;
|
||||
status = pasynCommonSyncIO->disconnect(pasynUser);
|
||||
if (status != asynSuccess ) {
|
||||
asynPrint(pasynUser, ASYN_TRACE_ERROR,
|
||||
"CloseSocket: error calling pasynCommonSyncIO->disconnect, status=%d, %s\n",
|
||||
status, pasynUser->errorMessage);
|
||||
return;
|
||||
}
|
||||
psock->connected = 0;
|
||||
}
|
||||
|
||||
/***************************************************************************************/
|
||||
@@ -312,42 +209,35 @@ void CloseAllSockets(void)
|
||||
{
|
||||
int i;
|
||||
|
||||
for (i = 0; i < MAX_NB_SOCKETS; i++)
|
||||
{
|
||||
if (UsedSocket[i] == TRUE)
|
||||
{
|
||||
status = pasynOctetSyncIO->disconnect(pasynUserArray[i]);
|
||||
TimeoutSocket[i] = TIMEOUT;
|
||||
ErrorSocket[i] = 0;
|
||||
UsedSocket[i] = FALSE;
|
||||
}
|
||||
for (i=0; i<nextSocket; i++) {
|
||||
if (socketStructs[i].connected) CloseSocket(i);
|
||||
}
|
||||
}
|
||||
|
||||
/***************************************************************************************/
|
||||
void ResetAllSockets(void)
|
||||
{
|
||||
int i;
|
||||
for (i = 0; i < MAX_NB_SOCKETS; i++)
|
||||
{
|
||||
if (UsedSocket[i] == TRUE)
|
||||
UsedSocket[i] = FALSE;
|
||||
}
|
||||
CloseAllSockets();
|
||||
}
|
||||
|
||||
|
||||
/***************************************************************************************/
|
||||
char * GetError(int SocketIndex)
|
||||
{
|
||||
if ((SocketIndex >= 0) && (SocketIndex < MAX_NB_SOCKETS) && (UsedSocket[SocketIndex] == TRUE))
|
||||
{
|
||||
switch (ErrorSocket[SocketIndex])
|
||||
{
|
||||
case CONNREFUSED: return("The attempt to connect was rejected.");
|
||||
case CREATESOCKETFAILED: return("Create Socket failed.");
|
||||
case OPTREFUSED: return("SetSockOption() Refused.");
|
||||
}
|
||||
if ((SocketIndex < 0) || (SocketIndex >= nextSocket)) {
|
||||
printf("GetError: invalid SocketIndex %d\n", SocketIndex);
|
||||
return "Invalid socket";
|
||||
}
|
||||
return("");
|
||||
return socketStructs[SocketIndex].errorString;
|
||||
}
|
||||
|
||||
void strncpyWithEOS(char * szStringOut, const char * szStringIn, int nNumberOfCharToCopy, int nStringOutSize)
|
||||
{
|
||||
char *eos = "\n";
|
||||
|
||||
if ((nNumberOfCharToCopy + (int)strlen(eos)) > nStringOutSize) {
|
||||
return;
|
||||
}
|
||||
strcpy(szStringOut, szStringIn);
|
||||
strcat(szStringOut, eos);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user