#include <sys/types.h>
#include <sys/socket.h>
#include <stdio.h>
#include <netinet/in.h>
#include <signal.h>
#include <unistd.h>
typedef unsigned char byte;

#pragma pack (1)

typedef struct {
	unsigned short command;
	unsigned short length;
	unsigned long handle;
	unsigned long status;
	byte context[8];
	unsigned long options;
	} _encaps_header;

typedef struct {
	unsigned short count;
	unsigned short type;
	unsigned short length;
	unsigned short version;
	unsigned short flags;
	byte name[16];
	} _services;

typedef struct {
	byte dst;
	byte ctrl;
	byte src;
	byte lsap;
	byte cmd;
	byte sts;
	unsigned short tns;
	byte fnc;
	byte data[250];
	} _df1_cmd;

typedef struct {
	byte dst;
	byte ctrl;
	byte src;
	byte lsap;
	byte cmd;
	byte sts;
	unsigned short tns;
	} _df1_resp;

typedef struct {
	byte version;
	unsigned short id;
	} _ver;

typedef struct {
	byte junk0[6];
	unsigned short data1;
	unsigned short data2;
	unsigned short data3;
	unsigned short data4;
	unsigned short length;
	byte command;
	byte ioilen;
	byte ioiclasscmd;
	byte ioiclass;
	byte ioiinstcmd;
	byte ioiinst;
	_ver version;
	} _cip_cmd;

typedef struct {
	byte junk0[6];
	unsigned short data1;
	unsigned short data2;
	unsigned short data3;
	unsigned short data4;
	unsigned short length;
	byte response;
	byte junk1;
	unsigned short status;
	_ver version;
	} _cip_resp;
	
#pragma pack()

int server_socket, client_socket;
int server_len, client_len;
int recv_len;
struct sockaddr_in server_address, client_address;






byte services_resp[] = {
0x04,0x00,0x1a,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x01,0x00,0x00,0x01,0x14,0x00,0x01,0x00,
0x20,0x00,'C','o','m','m','u','n',
'i','c','a','t','i','o','n','s',
0x00,0x00};
int services_resp_len = 50;

byte register_resp[] = {
0x65,0x00,0x04,0x00,0x00,0x02,0x02,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x01,0x00,0x00,0x00};
int register_resp_len = 28;

byte df1_resp[] = {
0x6f,0x00,0x1f,0x00,0x00,0x02,0x02,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,
0x00,0x01,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x00,
0x00,0x00,0x00,0x00,0xb2,0x00,0x0f,0x00,
0xcb,0x00,0x00,0x00,0x07,0x01,0x00,0x5d,
0x7f,0x06,0x00,0x4f,0x00,0x00,0x00};
int df1_resp_len = 55;



void hexdump(unsigned char *buf, size_t len)
{
  size_t indx;
  size_t ix2;

  for(indx = 0;indx < len;indx++)
  {
    if((indx % 16) == 0)
    {
      printf("%4.4x:",indx);
    }
    printf(" %2.2x",buf[indx]);
    if((indx % 16) == 7)
    {
      printf(" -");
    }
    else if((indx % 16) == 15)
    {
      printf("  ");
      for(ix2 = indx & -16;ix2 <= indx;ix2++)
      {
        printf("%c",isprint(buf[ix2]) ? buf[ix2] : '.');
      }
      printf("\n");
    }
  }

  if((indx % 16) != 0)
  {
    printf("%*s  ",((16 - (indx % 16)) * 3) + ((1 - ((indx % 16) / 8)) * 2),
           "");
    for(ix2 = indx & -16;ix2 < indx;ix2++)
    {
      printf("%c",isprint(buf[ix2]) ? buf[ix2] : '.');
    }
    printf("\n");
  }
}

void fhexdump(FILE *fp, unsigned char *buf, size_t len)
{
  size_t indx;
  size_t ix2;

  for(indx = 0;indx < len;indx++)
  {
    if((indx % 16) == 0)
    {
      fprintf(fp, "%4.4x:",indx);
    }
    fprintf(fp, " %2.2x",buf[indx]);
    if((indx % 16) == 7)
    {
      fprintf(fp, " -");
    }
    else if((indx % 16) == 15)
    {
      fprintf(fp, "  ");
      for(ix2 = indx & -16;ix2 <= indx;ix2++)
      {
        fprintf(fp, "%c",isprint(buf[ix2]) ? buf[ix2] : '.');
      }
      fprintf(fp, "\n");
    }
  }

  if((indx % 16) != 0)
  {
    fprintf(fp, "%*s  ",((16 - (indx % 16)) * 3) + ((1 - ((indx % 16) / 8)) * 2),
           "");
    for(ix2 = indx & -16;ix2 < indx;ix2++)
    {
      fprintf(fp, "%c",isprint(buf[ix2]) ? buf[ix2] : '.');
    }
    fprintf(fp, "\n");
  }
}




int main()
{
int x;
unsigned long counter;
_cip_cmd cip_cmd;
_cip_resp cip_resp;
_encaps_header enet;
_df1_cmd df1_cmd;
_df1_resp df1_resp;
_services services;
unsigned long session_ver;
byte command[512], response[512];

server_socket = socket(AF_INET, SOCK_STREAM,0);
server_address.sin_family = AF_INET;
server_address.sin_addr.s_addr = inet_addr("192.168.1.200");
server_address.sin_port = htons(44818);
server_len = sizeof(server_address);
x = 1;
setsockopt (server_socket, SOL_SOCKET, SO_REUSEADDR, (void *) &x, sizeof(x));
if (bind(server_socket, (struct sockaddr *)&server_address, server_len)<0) exit (0);
listen (server_socket, 5);
signal (SIGCHLD, SIG_IGN);
counter=0;
while (1)
	{
	printf ("Server listening...\n");
	client_socket = accept(server_socket, (struct sockaddr *)&client_address, &client_len);
	while (1) {
	recv_len = recv (client_socket, command, 512, 0);
	if (recv_len < 2)
		exit(-1);
	memcpy (&enet, &command,sizeof(_encaps_header));

		switch (enet.command)
			{
			case 0x04:
				printf ("Received list_services command.\n");
				enet.length = sizeof(services);
				memset (&response,0,512);
				memset (&services,0,sizeof(_services));
				services.count = 1;
				services.type=0x100;
				services.length = 0x14;
				services.version = 0x01;
				services.flags = 0x20;
				strncpy (services.name,"Communications",14);
				printf ("Copying encaps header.\n");
				memcpy ((void *)&response, (void *)&enet, sizeof (_encaps_header));
				printf ("Copying services struct.\n");
				memcpy ((void *)&response + sizeof(_encaps_header),(void *)&services, sizeof(_services));
				
				//hexdump (ch,recv_len);
				//hexdump (services_resp, services_resp_len);
				printf ("Sending response.\n");
				//hexdump (response, sizeof(_encaps_header) + sizeof(_services));
				write (client_socket, response, sizeof(_encaps_header) + sizeof(_services));
				break;
			case 0x65:
				printf ("Received register_session command.\n");
				//hexdump (ch,recv_len);
				memset (&response,0,512);
				enet.handle = 0x1234;
				session_ver=1;
				memcpy ((void *)&response, (void *)&enet, sizeof(_encaps_header));
				memcpy ((void *)&response + sizeof(_encaps_header),(void *)&session_ver, sizeof(long));
				//hexdump (response, sizeof (_encaps_header) + sizeof (long));
				write (client_socket, response, sizeof(_encaps_header)+sizeof(long));
				break;
			case 0x6f:
				if (counter % 64)  printf ("Received CIP command %ld.\n",counter);
				//hexdump (command,recv_len);
				counter++;
				enet.length = sizeof (_cip_resp) + sizeof (_df1_resp);
				memset (&response,0,512);
				memset (&df1_resp,0,sizeof(_df1_resp));
				memset (&cip_resp,0,sizeof(_cip_resp));
				memcpy ((void *)&response,(void *)&enet,sizeof(_encaps_header));
				//hexdump (response, sizeof (_encaps_header));
				
				memcpy ((void *)&cip_cmd,(void *)&command +sizeof(_encaps_header),sizeof(_cip_cmd));
				memcpy ((void *)&cip_resp, (void *)&cip_cmd, sizeof(_cip_resp));
				memcpy ((void *)&cip_resp.version, (void *)&cip_cmd.version, sizeof(_ver));
				cip_resp.length = sizeof(_df1_resp) + 7;
				cip_resp.response = cip_cmd.command + 0x80;
				cip_resp.junk1 = 0;
				cip_resp.status = 0;
				memcpy ((void *)&response + sizeof(_cip_resp)+1,(void *)&cip_resp,sizeof(_cip_resp));
				//hexdump (response, sizeof (_encaps_header) + sizeof (_cip_resp));
				
				memcpy ((void *)&df1_cmd, (void *)&command + sizeof (_encaps_header) + sizeof (_cip_cmd), sizeof (_df1_cmd));
				//memcpy ((void *)&df1_resp,(void *)&df1_cmd,sizeof(&df1_resp));
				df1_resp.src = df1_cmd.src;
				df1_resp.dst = df1_cmd.dst;
				df1_resp.ctrl = df1_cmd.ctrl;
				df1_resp.lsap = df1_cmd.lsap;
				df1_resp.cmd = df1_cmd.cmd + 0x40;
				df1_resp.tns = df1_cmd.tns;
				df1_resp.sts = 0;
				
				memcpy ((void *)&response + sizeof (_encaps_header) + sizeof (_cip_resp), (void *)&df1_resp, sizeof (_df1_resp));
				
				//hexdump (response, sizeof (_encaps_header) + sizeof (_cip_resp) + sizeof (_df1_resp));
				write (client_socket, response, sizeof (_encaps_header) + sizeof(_cip_resp) + sizeof(_df1_resp));
				break;			
			default:
				printf ("Received unknown command.\n");
				for (x=0;x<recv_len;x++)
					printf ("%02X ",command[x]);
				printf ("\n-----------------------\n");
			}
		}
	}
}