modbus_tcp.c

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

/******************************************************************************
 *
 * Copyright (c) 2008 Shanghai IS Software
 *
 * All rights reserved
 *
 * $Revision$
 *
 * $LastChangedBy$
 *1. lcj
 *
 * $LastChangedData$
 * 2008/09/25
 *
 * 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"

//#define DEBUG
#define MODBUS_TCP_LEN		12
#define MODBUS_TCP_HEAD	6
#define PORT					0x00
#define ADDR 				0x01

#include "modbus_tcp/modbus_tcp.h"

#ifdef DEBUG
#define pr_debug(fmt,arg...) \
		printf(fmt,##arg)
#else
#define pr_debug(fmt,arg...) \
		NULL
#endif

/**
*	read modbus register value .
*
**/
int  get_value(unsigned char *temp,unsigned char *buff_data)
{
	int result,i,j;
	MD_info* info;
	MD_reg_info *reg_info;
	int port = PORT;
	int addr = buff_data[MODBUS_TCP_LEN - 6];
	unsigned short start_addr;
	unsigned short num;
	unsigned short data[32];
	unsigned char *p = temp;

	memset(data,0,sizeof(data));
	
	start_addr = (unsigned short)(MAKEWORD(buff_data[MODBUS_TCP_LEN - 4],buff_data[MODBUS_TCP_LEN - 3]));
	num	= (unsigned short)(MAKEWORD(buff_data[MODBUS_TCP_LEN - 2],buff_data[MODBUS_TCP_LEN - 1]));

	
	pr_debug("\naddr = %d,num = %d\n",start_addr,num);

	result = init_MD_manager(2, 5, 500);
	if(result)
	{
		info = get_MD_info(port,addr);
		if(info)
		{
			if ( num > 0x01 )
				get_MD_regs_value(info,start_addr+1,num,(short *)data);
			else 
				get_MD_reg_value(info,start_addr+1,(short *)&data[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;
	
}


/**
*	send tcp data
* 
**/
void sendframe(int fd,unsigned char *buf,int size)
{
	int i,n,len;
	unsigned char *p = buf;
	unsigned char buff[32];

	pr_debug("Send Date:");
	for ( i = 0; i < size; i++ )
		pr_debug("[%d]",buf[i]);

	
	n = send(fd,buf,size,0);
	if ( n < 0 || n > size )	
	{
		
		pr_debug("writen error!\n");
		
		exit(0);
	}
	return;
}

/**
*	parse the recv tcp data;
*
**/
int parseframe(int fd,unsigned char *buf_data )
{
	int ret,flag = 0;
	int i,j;
	unsigned char ch[16];
	unsigned char  tmp[MAX_RX] ,rx_buf[MAX_RX];
	unsigned char *p = rx_buf;
	int size;

	
	for ( i = 0; i < MODBUS_TCP_HEAD-1; i++ ) 
	{
		rx_buf[i] = buf_data[i];
		
	}
	
	rx_buf[6] = buf_data[MODBUS_TCP_LEN - 6]; // dev_addr
	rx_buf[7] = buf_data[MODBUS_TCP_LEN - 5]; // command
	rx_buf[8] = ((unsigned short)MAKEWORD(buf_data[MODBUS_TCP_LEN - 2],buf_data[MODBUS_TCP_LEN - 1]))*2; // num
	rx_buf[5] = 3 + rx_buf[8]; // len

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

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

	if ( flag == 0 )
	{	
		
		pr_debug("commmand is not supply!\n");
	
		exit(0);
	}
	
	for ( i = 0,j = 9; i < rx_buf[8];i++,j++ )
	{
		rx_buf[j] = tmp[i];
	
	}
	
	size =  rx_buf[8] + 9;

	sendframe(fd,rx_buf,size);
	
	return 0;
}

 /**
 *	 recv the tcp client data.
 *
 **/
 int do_it(int connfd)
 {
 	int i,temp;
	int fd = connfd;
	ssize_t n;
	unsigned char buf_top[MODBUS_TCP_HEAD];
	unsigned char buf_data[MAXLINE];

	while(1)
	{
		n = recv(fd,buf_data,MODBUS_TCP_LEN,0);
		if ( n < 0 || n > MODBUS_TCP_LEN )
		{
			#ifdef DEBUG
			fprintf(stderr,"read error!\n");
			#endif
			return -1;
		}

		
		pr_debug("Recv TCP Data:");
		for ( i = 0; i < MODBUS_TCP_LEN; i++ )
			pr_debug("[%d]",buf_data[i]);
		pr_debug("\n");
	
		
		if ((MAKEWORD(buf_data[2],buf_data[3]) != 0) || (MAKEWORD(buf_data[4],buf_data[5])) > 0xff )
		{
		
			pr_debug("modbus tcp data error!\n");
	
			return -1;
		}
		
		parseframe(fd,buf_data);

	}
	return ;
}


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

	while ((pid = waitpid(-1,&stat,WNOHANG)) > 0)
	{
		pr_debug("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 )
	{
		
		pr_debug("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
				{
					pr_debug(" accept error!\n");
				}
			}
			
			if ( 0 == (childpid = fork())) // child process
			{
				close(listenfd);
							
				do_it(connfd);

			}
				
			close(connfd);
		}
		return 0;
}