modbus_tcp.c.svn-base

linux 平台, modbus tcp 协议的简单实现.

/******************************************************************************
 *
 * Copyright (c) 2008 Shanghai IS Software
 *
 * All rights reserved
 *
 * $Revision$
 *
 * $LastChangedBy$
 *
 * $LastChangedData$
 *
 * Description: modbus tcp server
 *
 * Revision History:
 * 2008/09/18 14:33 by lcj
 * #1.created
 *
 *****************************************************************************/
#include "md/md_manager.h"
#include "md/md_info.h"
#include "md/md_reg.h"


#include "modbus_tcp/modbus_tcp.h"


int  get_value(unsigned char *temp,unsigned char *buff_data)
{
	int result,i,j;
	MD_info* info;
	MD_reg_info *reg_info;
	int port = V_PORT;
	int addr = V_ADDR;
	unsigned short start_addr;
	unsigned short num;
	unsigned short data[16];
	unsigned char *p = temp;

	
	start_addr = (unsigned short)(MAKEWORD(buff_data[2],buff_data[3]));
	num	= (unsigned short)(MAKEWORD(buff_data[4],buff_data[5]));

	
	result = init_MD_manager(2, 5, 500);
	if(result)
	{
		info = get_MD_info(port,addr);
		if(info)
		{
			for( i = 0; i < num; i++ )
			{
				reg_info = (MD_reg_info *)get_reg_info(info,(start_addr + i ));
				if (reg_info)
				{
					if ( MD_STATUS_NORMAL == reg_info->status )
						get_MD_reg_value(info,(start_addr+i),&data[i]);
					else 
					{
						printf("V_modbus Value is not Set!\n");
						exit(0);
					}	
				}
			}
		}
	}
	for ( j = 0, i = 0; i < num; i++,j+=2)
	{
		p[j] = (data[i] & 0xff00) >> 8;
		p[j+1] = data[i] & 0x00ff;
	
	}
	
	return 0;
	
}

int num2str(unsigned char *buff,unsigned char *buf)
{
	unsigned char *p = buf;
	int i,j, len ;
	
	len = p[5] + 6;

	for (j = 0, i = 0; i < len; i++)
	{
		sprintf(&buff[j++],"%x ",((*p & 0xF0) >> 4));
		sprintf(&buff[j++],"%x ",(*p++ & 0xF));
	}

	buff[j] = '\0';
			
	return 0;
}

void sendframe(int fd,unsigned char *buf,struct sockaddr_in cl_addr )
{
	int i,n,len;
	unsigned char *p = buf;
	unsigned char buff[32];

	num2str(buff,buf);

	n = sendto(fd,buff,sizeof(buff),0,(struct sockaddr_in *)&cl_addr,sizeof(cl_addr));
	if ( n < 0 || n > sizeof(buff) )	
	{
		printf("writen error!\n");
		exit(0);
	}
	return;
}

int parseframe(int fd,unsigned char *buf_top,unsigned char *buf_data,struct sockaddr_in cl_addr )
{
	int ret,flag = 0;
	int i;
	unsigned char *ptr = buf_data;
	unsigned char  tmp[MAX_RX] ,rx_buf[MAX_RX];
	unsigned char *p = rx_buf;

	for ( i = 0; i < 5; i++ )  
		rx_buf[i] = buf_top[i];

	rx_buf[6] = ptr[0]; // dev_addr
	rx_buf[7] = ptr[1]; // command
	rx_buf[8] = ((unsigned short)MAKEWORD(buf_data[4],buf_data[5]))*2; // num
	rx_buf[5] = 3 + rx_buf[8]; // len

	for ( i = 0; i < 9; i++ )
		p++;

	switch(ptr[1])
	{
		case 0x03:
			ret = get_value(tmp,buf_data);
			if ( ret < 0 )
			{
				printf("Get Value error!\n");
				exit(-1);
			}
			flag = 1;
			break;
		case 0x06:
			printf("command is write!\n");
			break;
		default:
			printf("command error!\n");
			break;
	}

	if ( flag == 0 )
	{	
		printf("commmand is not supply!\n");
		exit(0);
	}
	
	strncpy(p,tmp,rx_buf[8]);
	
	sendframe(fd,rx_buf,cl_addr);
	
	return 0;
}

 int do_it(int connfd,struct sockaddr_in cl_addr)
 {
 	int i,temp;
	int fd = connfd;
	ssize_t n;
	unsigned char buf_top[6];
	unsigned char buf_data[MAXLINE];
	struct sockaddr_in	addr = cl_addr;
	int addr_len = sizeof(struct sockaddr_in);
		
	n = recvfrom(fd,buf_top,6,0,(struct sockaddr *)&addr,&addr_len);
	if ( n < 0 || n > 6 )
	{
		printf("read error!\n");
		return -1;
	}
	
	if ((MAKEWORD(buf_top[2],buf_top[3]) != 0) || (MAKEWORD(buf_top[4],buf_top[5])) > 0xff )
	{
		printf("modbus tcp data error!\n");
		return -1;
	}
	n = recvfrom(fd, buf_data,(MAKEWORD(buf_top[4],buf_top[5])),0,(struct sockaddr *)&addr,&addr_len);
	if ( n < 0 || n > (MAKEWORD(buf_top[4],buf_top[5])))
	{
		printf("read error!\n");
		return -1;
	}	
	parseframe(fd,buf_top,buf_data,addr);
	
	return ;
}


void sig_chld(int signo)
{
	pid_t 	pid;
	int 	stat;

	while ((pid = waitpid(-1,&stat,WNOHANG)) > 0)
		printf("child %d terminated\n",pid);

	return;
}

int main(int argc, char **argv)
{
	int listenfd,connfd;
	pid_t childpid;
	socklen_t	clilen;
	struct sockaddr_in	cliaddr,servaddr;
	
	listenfd = socket(AF_INET,SOCK_STREAM,IPPROTO_TCP);
	if ( listenfd < 0 )
	{
		printf("socket error!\n");
		exit(0);
	}

	bzero(&servaddr,sizeof(servaddr));
	servaddr.sin_family = AF_INET;
	servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
	servaddr.sin_port = htons(MODBUS_TCP_PORT);
	
	bind(listenfd,(SA *)&servaddr,sizeof(servaddr));

	listen(listenfd,LISTENQ);

	signal(SIGCHLD,sig_chld);

	while (1)
	{
		clilen = sizeof(cliaddr);

		if ((connfd = accept(listenfd,(SA *)&cliaddr,&clilen)) < 0 )
		{
			if ( errno == EINTR )
				continue;
			else 
				printf(" accept error!\n");
		}
		
		if ( 0 == (childpid = fork())) // child process
		{
			close(listenfd);
						
			do_it(connfd,cliaddr);
			
			close(connfd);
			exit(0);
		}
		
		close(connfd);
	}

	return 0;
}