mirror of
https://github.com/netdata-be/libnodave
synced 2025-10-13 00:42:50 +08:00
436 lines
12 KiB
C
436 lines
12 KiB
C
/*
|
|
Part of Libnodave, a free communication libray for Siemens S7 300/400.
|
|
This program simulates a CPx43.
|
|
|
|
(C) Thomas Hergenhahn (thomas.hergenhahn@web.de) 2002.
|
|
|
|
Libnodave is free software; you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation; either version 2, or (at your option)
|
|
any later version.
|
|
|
|
Libnodave is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with Visual; see the file COPYING. If not, write to
|
|
the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
|
|
*/
|
|
|
|
#include <stdio.h>
|
|
#include <unistd.h>
|
|
#include <string.h>
|
|
|
|
#include <pthread.h>
|
|
|
|
#include "log2.h"
|
|
|
|
#define uc unsigned char
|
|
|
|
#include <sys/time.h>
|
|
#include <sys/socket.h>
|
|
|
|
#include <netinet/in.h>
|
|
#include <arpa/inet.h>
|
|
#include <errno.h>
|
|
#define ThisModule __FILE__
|
|
|
|
#include "accepter.c"
|
|
//#include <byteswap.h>
|
|
|
|
#include "nodave.h"
|
|
|
|
#define bSize 1256
|
|
#define us unsigned short
|
|
|
|
#define debug 10
|
|
|
|
/*
|
|
many (1024) bytes. hopefully enough to serve any read request.
|
|
*/
|
|
uc dummyRes[1024];
|
|
|
|
/*
|
|
a read callback function
|
|
*/
|
|
uc * dummyRead (int area, int DBnumber, int start, int len, int * res) {
|
|
dummyRes[3]+=5;
|
|
dummyRes[0]+=5;
|
|
#if debug>1
|
|
printf("User callback should deliver pointer to %d bytes from %s %d beginning at %d.\n",
|
|
len, daveAreaName(area),DBnumber,start);
|
|
#endif
|
|
*res=0;
|
|
|
|
return dummyRes;
|
|
};
|
|
|
|
void dummyWrite(int area, int DBnumber, int start, int len, int * res, uc*buffer) {
|
|
// dummyRes[2]++;
|
|
start/=8;
|
|
#if debug>1
|
|
printf("User write callback should write %d bytes from %s %d to address %d.\n",
|
|
len, daveAreaName(area),DBnumber,start);
|
|
#endif
|
|
*res=0;
|
|
memcpy(dummyRes+start,buffer,len);
|
|
};
|
|
|
|
int handleSystemMessage(PDU * p1,PDU * p2) {
|
|
int number=0x100*(p1->data[4])+p1->data[5];
|
|
int count=0x100*(p1->data[6])+p1->data[7];
|
|
printf("SysMessage number: %d\n",number);
|
|
printf("count: %d\n",count);
|
|
if (number==20) {
|
|
/*
|
|
Info about CPU resources. I return some strange values in order
|
|
to watch which number goes where.
|
|
*/
|
|
uc pa[]={0x00,0x01,0x12,0x08,0x12,0x84,0x01,0x03, 0x00,0x00,0x00,0x00,};
|
|
uc va[]={
|
|
// FF,09,00,50,
|
|
00,0x14,00,00,
|
|
00,0x08,00,07,
|
|
0,1,0,1,0,0x81,00,00, // 129 input bytes
|
|
0,2,0,1,0,0x7f,00,00, // 127 output bytes
|
|
0,3,0,1,0x0a,00,00,0x80, // 10*32 flag bytes
|
|
00,04,00,01,00,0x80,00,00, // 128 Timer
|
|
00,05,00,01,00,0x40,00,0x08, // 64 counter
|
|
|
|
00,06,00,01,05,01,00,00, // 5*256+1=1281 log.Address
|
|
00,07,00,01,06,03,00,00, // 6*256+3=1539 local data
|
|
00,0x08,00,01,01,00,00,0x10,
|
|
00,0x09,00,01,00,01,00,00,
|
|
|
|
};
|
|
_daveInitPDUheader(p2,7);
|
|
_daveAddParam(p2, pa, sizeof(pa));
|
|
// _daveAddData(p2, p1->data, 8);
|
|
// p1->data[3]=4; // 4 bytes have been already copied to result string
|
|
// _daveAddValue(p2, va, sizeof(va));
|
|
_daveAddUserData(p2, va, sizeof(va));
|
|
_daveDumpPDU(p2);
|
|
return 1;
|
|
} else if (number==292) {
|
|
uc pa[]={0x00,0x01,0x12,0x08,0x12,0x84,0x01,0x03, 0x00,0x00,0x00,0x00,};
|
|
uc va[]={
|
|
0x00,0x14,0x00,0x00,0x43,0x02,0xff,0x68, 0xc7,0x00,0x00,0x00,
|
|
0x08,0x10,0x77,0x10,0x03,0x07,0x10,0x13, 0x22,0x26,0x58,0x85,
|
|
|
|
};
|
|
_daveInitPDUheader(p2,7);
|
|
_daveAddParam(p2, pa, sizeof(pa));
|
|
_daveAddData(p2, p1->data, 8);
|
|
p1->data[3]=4; // 4 bytes have been copied to result string
|
|
_daveAddValue(p2, va, sizeof(va));
|
|
_daveDumpPDU(p2);
|
|
return 1;
|
|
} else if (number==306) {
|
|
uc pa[]={0x00,0x01,0x12,0x08,0x12,0x84,0x01,0x01, 0x00,0x00,0x00,0x00,};
|
|
uc va306[]={
|
|
0,40,0,1,0,4,0, 1, 0,0,0,
|
|
1, 0,2,0,0,0,0,86,86,0,0,
|
|
0, 0,0,0,0,0,0, 0, 0,0,0,
|
|
0, 0,0,0,0,0,0, 0, 0,0,0,
|
|
};
|
|
_daveInitPDUheader(p2,7);
|
|
_daveAddParam(p2, pa, sizeof(pa));
|
|
_daveAddData(p2, p1->data, 8);
|
|
p1->data[3]=4; // 4 bytes have been copied to result string
|
|
_daveAddValue(p2, va306, sizeof(va306));
|
|
_daveDumpPDU(p2);
|
|
return 1;
|
|
} else if (number==273) { /* read order code (MLFB) */
|
|
uc pa[]={0x00,0x01,0x12,0x08,0x12,0x84,0x01,0x03, 0x00,0x00,0x00,0x00,};
|
|
uc va273[]={
|
|
0x00,0x1c,0x00,0x01,0x43,0x02,
|
|
'6','E','S',
|
|
// '7',' ','3','1','5','-','2','A','F','0','3','-','0','A','B','0',
|
|
'7',' ','L','I','B','N','O','D','A','V','E','-','T','.','H','.',
|
|
' ',0x00,0xc0,0x00,0x02,0x00,0x00,
|
|
};
|
|
_daveInitPDUheader(p2,7);
|
|
_daveAddParam(p2, pa, sizeof(pa));
|
|
_daveAddData(p2, p1->data, 8);
|
|
p1->data[3]=4; // 4 bytes have already been copied to result string
|
|
_daveAddValue(p2, va273, sizeof(va273));
|
|
_daveDumpPDU(p2);
|
|
return 1;
|
|
} else if (number==305) {
|
|
uc pa[]={0x00,0x01,0x12,0x08,0x12,0x84,0x01,0x01, 0x00,0x00,0x00,0x00,};
|
|
uc va306[]={
|
|
// 0,40,0,1,0,4,0, 1, 0,0,0,
|
|
// 1, 0,2,0,0,0,0,86,86,0,0,
|
|
// 0, 0,0,0,0,0,0, 0, 0,0,0,
|
|
// 0, 0,0,0,0,0,0, 0, 0,0,0,
|
|
0, 40, 0, 1, 0, 2,190,253, 15, 0,
|
|
0, 0, 0, 0, 0, 0, 0, 0, 60, 1,
|
|
0, 0, 0, 0,125, 0, 0, 5, 3, 4,
|
|
0, 0, 0, 0, 0, 12, 0, 10, 0, 0,
|
|
0, 9, 0, 0,
|
|
};
|
|
_daveInitPDUheader(p2,7);
|
|
_daveAddParam(p2, pa, sizeof(pa));
|
|
_daveAddData(p2, p1->data, 8);
|
|
p1->data[3]=4; // 4 bytes have been copied to result string
|
|
_daveAddValue(p2, va306, sizeof(va306));
|
|
_daveDumpPDU(p2);
|
|
return 1;
|
|
} else if (number==0) {
|
|
uc pa[]={0x00,0x01,0x12,0x08,0x12,0x84,0x01,0x01, 0x00,0x00,0x00,0x00,};
|
|
uc va[]={
|
|
// 255,9,0,114,0,0,0,0,
|
|
0,2,0,53,0,0,15,0,0,17,1,17,15,17,0,18,
|
|
1,18,15,18,0,19,0,20,0,21,1,21,0,23,1,23,15,
|
|
23,0,24,1,24,15,24,0,25,15,25,15,26,15,27,0,26,0,
|
|
27,0,33,10,33,15,33,2,34,0,35,15,35,0,36,1,36,
|
|
4,36,5,36,1,49,1,50,2,50,0,116,1,116,15,116,12,
|
|
145,13,145,10,145,0,146,2,146,6,146,15,146,0,177,0,178,0,
|
|
179,0,180,0,160,1,160,12,139,126,
|
|
};
|
|
_daveInitPDUheader(p2,7);
|
|
_daveAddParam(p2, pa, sizeof(pa));
|
|
_daveAddData(p2, p1->data, 8);
|
|
p1->data[3]=4; // 4 bytes have been copied to result string
|
|
_daveAddValue(p2, va, sizeof(va));
|
|
_daveDumpPDU(p2);
|
|
return 1;
|
|
} else if (number==1060) {
|
|
uc pa[]={0x00,0x01,0x12,0x08,0x12,0x84,0x01,0x01, 0x00,0x00,0x00,0x00,};
|
|
uc va[]={
|
|
// 255,9,0,114,0,0,0,0,
|
|
0,20,0,1,81,68,255,8,0,0,0,0,0,0,0,0,148,1,2,22,83,88,104,17,
|
|
};
|
|
_daveInitPDUheader(p2,7);
|
|
_daveAddParam(p2, pa, sizeof(pa));
|
|
_daveAddData(p2, p1->data, 8);
|
|
p1->data[3]=4; // 4 bytes have been copied to result string
|
|
_daveAddValue(p2, va, sizeof(va));
|
|
_daveDumpPDU(p2);
|
|
return 1;
|
|
} else
|
|
return 0;
|
|
};
|
|
|
|
int gpacketNumber=0;
|
|
|
|
typedef struct{
|
|
uc prot;
|
|
uc ch1;
|
|
uc ch2;
|
|
uc len;
|
|
uc xxxx1;
|
|
uc func;
|
|
uc xxxx2;
|
|
} ISOpacket;
|
|
|
|
void analyze(daveConnection * dc) {
|
|
ISOpacket * p,* p2;
|
|
uc resp[2000];
|
|
uc r5[]={
|
|
0x03,0x00,0x00,0x16,
|
|
0x11,0xd0,
|
|
0,0,0,1,0,
|
|
0xc0,1,9,
|
|
0x0c1,2,1,2,
|
|
0x0c2,2,1,0,
|
|
};
|
|
|
|
int haveResp=0;
|
|
PDU p1,pr;
|
|
p= (ISOpacket*) dc->msgIn;
|
|
dc->needAckNumber=-1; // Assume no ack
|
|
#if debug>1
|
|
printf("Protocol: %d\n",p->ch1);
|
|
printf("Channel: %d\n",p->ch1);
|
|
printf("Channel: %d\n",p->ch2);
|
|
printf("Length: %d\n",p->len);
|
|
printf("(MPI) func:%d\n",p->func);
|
|
#endif
|
|
if (p->func==0xf0) {
|
|
#if debug>1
|
|
printf("0xf0:PDU transport\n");
|
|
#endif
|
|
// p1.header=((uc*)p)+sizeof(ISOpacket);
|
|
dc->PDUstartI=sizeof(ISOpacket);
|
|
_daveSetupReceivedPDU(dc, &p1);
|
|
#if debug>1
|
|
_daveDumpPDU(&p1);
|
|
#endif
|
|
pr.header=resp+sizeof(ISOpacket);
|
|
p2= (ISOpacket*) resp;
|
|
p2->ch1=p->ch2;
|
|
p2->ch2=p->ch1;
|
|
if (p1.param[0]==daveFuncRead) {
|
|
_daveHandleRead(&p1,&pr);
|
|
haveResp=1;
|
|
p2->len=pr.hlen+pr.plen+pr.dlen+7;
|
|
} else if (p1.param[0]==daveFuncWrite) {
|
|
_daveHandleWrite(&p1,&pr);
|
|
haveResp=1;
|
|
p2->len=pr.hlen+pr.plen+pr.dlen+7;
|
|
} else if (p1.param[0]==240) {
|
|
printf("PDU function code: %d, negociate PDU len\n",p1.param[0]);
|
|
_daveDump("packet:",dc->msgIn,dc->msgIn[3]);
|
|
memcpy(resp, dc->msgIn, dc->msgIn[3]);
|
|
resp[23]=960 / 0x100;
|
|
resp[24]=960 % 0x100;
|
|
haveResp=1;
|
|
} else if (p1.param[0]==0) {
|
|
printf("PDU function code: %d, system Message ?\n",p1.param[0]);
|
|
haveResp=handleSystemMessage(&p1,&pr);
|
|
pr.header[4]=p1.header[4]; // give the PDU a number
|
|
haveResp=1;
|
|
p2->len=pr.hlen+pr.plen+pr.dlen+7;
|
|
} else {
|
|
printf("Unsupported PDU function code: %d\n",p1.param[0]);
|
|
}
|
|
}
|
|
if (p->func==0xe0) {
|
|
int rack=*(dc->msgIn+17)-1;
|
|
int slot=*(dc->msgIn+18);
|
|
printf("Connect to rack:%d slot:%d \n", rack, slot);
|
|
|
|
*(r5+16)=rack+1;
|
|
*(r5+17)=slot;
|
|
memcpy(resp, r5, sizeof(r5));
|
|
haveResp=1;
|
|
}
|
|
if (haveResp) {
|
|
// simulating CP response delay:
|
|
usleep(10000);
|
|
//#define sim_broken_transport
|
|
#undef sim_broken_transport
|
|
#ifdef sim_broken_transport
|
|
double pr=random();
|
|
pr/=RAND_MAX;
|
|
|
|
if (pr>=0.90) {
|
|
LOG2("faking broken transport: %0.2f >= 0.95 \n",pr);
|
|
} else
|
|
write(dc->iface->fd.wfd,resp,resp[3]);
|
|
#else
|
|
write(dc->iface->fd.wfd,resp,resp[3]);
|
|
#endif
|
|
#if debug >1
|
|
_daveDump("I send:",resp,resp[3]);
|
|
#endif
|
|
}
|
|
};
|
|
|
|
typedef struct _portInfo {
|
|
int fd;
|
|
}portInfo;
|
|
|
|
#define mymemcmp _daveMemcmp
|
|
void *portServer(void *arg)
|
|
{
|
|
_daveOSserialType s;
|
|
daveInterface * di;
|
|
daveConnection * dc;
|
|
int waitCount,res,pcount;
|
|
portInfo * pi=(portInfo *) arg;
|
|
LOG2("portServer: My fd is:%d\n", pi->fd);
|
|
FLUSH;
|
|
waitCount = 0;
|
|
daveSetDebug(0);//daveDebugAll;
|
|
pcount=0;
|
|
|
|
s.rfd=pi->fd;
|
|
s.wfd=pi->fd;
|
|
di =daveNewInterface(s,"IF1",0,daveProtoISOTCP,daveSpeed187k);
|
|
di->timeout=900000;
|
|
dc=daveNewConnection(di,2,0,2);
|
|
while (waitCount < 100) {
|
|
dc->AnswLen=_daveReadISOPacket(dc->iface, dc->msgIn);
|
|
if (dc->AnswLen>0) {
|
|
res=dc->AnswLen;
|
|
#if debug>1
|
|
LOG2( "%d ", pcount);
|
|
_daveDump("packet", dc->msgIn, dc->AnswLen);
|
|
#endif
|
|
waitCount = 0;
|
|
analyze(dc);
|
|
pcount++;
|
|
} else {
|
|
waitCount++;
|
|
}
|
|
}
|
|
LOG1( "portServer: I closed my fd.\n");
|
|
FLUSH;
|
|
return NULL;
|
|
}
|
|
|
|
|
|
/*
|
|
This waits in select for a file descriptor from accepter and starts a new child server
|
|
with this file descriptor.
|
|
*/
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
// int PID=getpid();
|
|
portInfo pi;
|
|
int filedes[2], res, newfd;
|
|
fd_set FDS;
|
|
char * s2;
|
|
pthread_attr_t attr;
|
|
pthread_t ac, ps;
|
|
accepter_info ai;
|
|
|
|
printf("CP-Simulation for ISO over TCP\n");
|
|
if (argc<2) {
|
|
printf("Assuming standard port (102) for ISO over TCP\n");
|
|
printf("If you don't want that do: isotest4 xxx, and I will listen on xxx.\n\n");
|
|
s2 = "102 ";
|
|
} else
|
|
s2 = argv[1];
|
|
printf("Assuming rack 0, slot 2 for simulated device. There's currently no way to change that.\n\n");
|
|
readCallBack=dummyRead;
|
|
writeCallBack=dummyWrite;
|
|
|
|
|
|
pipe(filedes);
|
|
ai.port = atol(s2);
|
|
LOG2( "Main serv: port: %d\n", ai.port);
|
|
ai.fd = filedes[1];
|
|
LOG2( "Main serv: Accepter pipe fd: %d\n", ai.fd);
|
|
|
|
pthread_attr_init(&attr);
|
|
pthread_attr_setdetachstate(&attr,PTHREAD_CREATE_DETACHED);
|
|
res=pthread_create(&ac, &attr, accepter, &ai /*&filedes[1] */ );
|
|
do {
|
|
FD_ZERO(&FDS);
|
|
FD_SET(filedes[0], &FDS);
|
|
|
|
LOG2( "Main serv: about to select on %d\n",
|
|
filedes[0]);
|
|
FLUSH;
|
|
if (select(filedes[0] + 1, &FDS, NULL, &FDS, NULL) > 0) {
|
|
LOG1( "Main serv: about to read\n");
|
|
res = read(filedes[0], &pi.fd, sizeof(pi.fd));
|
|
ps=0;
|
|
pthread_attr_init(&attr);
|
|
pthread_attr_setdetachstate(&attr,PTHREAD_CREATE_DETACHED);
|
|
res=pthread_create(&ps, &attr, portServer, &pi);
|
|
if(res) {
|
|
LOG2( "Main serv: create error:%s\n", strerror(res));
|
|
close(newfd);
|
|
usleep(100000);
|
|
}
|
|
}
|
|
}
|
|
while (1);
|
|
return 0;
|
|
}
|
|
|
|
|
|
/*
|
|
Changes:
|
|
|
|
14/07/2003 give a hint about usage
|
|
08/04/2004 Logging for multithreaded things now uses PID, which is set in thread creation
|
|
*/
|