fumabuildparse.c

用于串口传送的一种 串口协议打包拆包示例

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <errno.h>
#include <unistd.h>
#include <fcntl.h>



/////////////////////////////////////////////////////////////
//|Function:unsigned int cal_crc
//|
//|16位CRC：按位计算，速度最慢，占用空间最少,数据流是高位先行
//|
//|输入：整数以及初始crc值
//|
//|输出：按位计算的16位crc值，余式采用0x1021
//////////////////////////////////////////////////////////////
unsigned int  cal_crc(unsigned int  command, unsigned int crc){
 	
	unsigned int i;
 	
	crc=crc^(command<<8);
 	
	for(i=0; i<=7; i++) {
 		
		if((crc&0x8000)!=0)
    		crc=(crc<<1)^0x1021;
 		else 
    		crc=(crc<<1);
 	}
 	
	//fprintf(stderr,"CRC%d\n",crc);
	return crc;
	
}

///////////////////////////////////////////////////////////////////////
//| Function:	FUMAChangeback()
//|	富马转义逆变换，将输入的数组按照富马协议恢复，也就是				
//|
//|	输入：源字符串，融合起始位置，融合长度，目标数据数组，数据起始位置
//|
//|	返回：int ,正常返回1; 解包错误返回-1;		
//|
//| Example:    
//|             unsigned char str[]={0x23,0x7D,0x5D,0x7D,0x5E}
//|             int  data[5];
//|             BinaryFuse(str,0,5,data,0); //data={0x03,0x23,0x7D,0x7E, ...}           
/////////////////////////////////////////////////////////////////////////

int FUMAChangeback(unsigned char* Str,int SStart, int Len, int* Data,int DStart)
{
	
	int i,j;
	int dec; // length to shorten
 	dec=0;
 	//if(DStart==0)
		Data[0]=Len;
		
 	//else
   	//	Data[0]=Len;
 	if(Len<1)
	return -1 ;
	//no 容错功能,默认对方的包是正确的.
	for(i=0,j=0;i<=Len;i++,j++){
	      if(Str[i]==0x7D)
	      {
	          if(Str[i+1]==0x5E)
		  {
		      Data[1+j]=0x7E;
		      i++;
		      dec++;
		  }
  	          else if(Str[i+1]==0x5D)
		  {
		       Data[1+j]=0x7D;
		       i++;
		       dec++;
		  }
		  else
		  return -1;
		  
	      }
	      else
	      Data[1+j]=Str[i];
  	}
	
	Data[0]=Data[0]-dec;	
	return 1;
  	
}





/////////////////////////////////////////////////////////////////////////
//| Function:	int FUMAParse()	BinaryFuse(Str,0,strlen,Data,0);
//|从源字符串中获取命令包、对命令包进行CRC校验、解析出命令单元的数据保存到目的字符串 Data[0]为转换后数组有效的长度
//|					
//|输入：    源数据组，源数据长度, 目的数据数组
//|				
//|返回：int 解析成功则返回1   ,
//|      如果失败返回负数， -1表示：起始结束标记出错   -2表示：富马转义逆变化出错         -3表示：
//|          }
/////////////////////////////////////////////////////////////////////////

int FUMAParse(unsigned char *Str,int strlen, int *Data){
	
	int i;
	unsigned int  RcvCrc, CalCrc=0;
	//搜索消息字符串的起始标志位0x7E
	if(Str[0]!=0x7E){
 		Str=NULL;
 		Data=NULL;
 	//	fprintf(stderr,"Finding Message String StartFlag Error!\n");
  		return -1;
	}
	Str=Str+1;

	//搜索消息字符串的结束标志位0x7E
	if(Str[strlen-2]!=0x7E){
 		Str=NULL;
 		Data=NULL;
 	//	fprintf(stderr,"Finding Message String EndFlag Error!\n");
  		return -1;
	}

	//对命令单元和检验单元字符串进行二进制合并
	
	strlen=strlen-2;
	if( !FUMAChangeback(Str,0,strlen,Data,0) )
	{
	   return  -2;
	}
	//for(i=0;i<23;i++)
//		fprintf(stderr,"Data Message %x\n",Data[i]);

    	//CRC 校验
	RcvCrc=Data[(Data[0])]*256+Data[(Data[0])-1];
		//fprintf(stderr,"ReceiveCrc %x\n",RcvCrc);
	for(i=1;i<=Data[0]-2;i++)
		CalCrc=cal_crc(Data[i],CalCrc);
	CalCrc=CalCrc&0xffff;
	//fprintf(stderr,"CalCrc %x\n",CalCrc);
	if(RcvCrc!=CalCrc){
		Str=NULL;
		fprintf(stderr,"Check Message String CRC Error!\n");
 		return 0x04;
	}
	Data[0]-=2;

	//命令体长度检验
/*	if(Data[12]!=Data[0]-12){
		Str=NULL;
 		Data=NULL;
 		fprintf(stderr,"Check Command Body Length Error!");
		return 0xA0;
	}
*/	
	//消息命令体处理完毕
	Str=NULL;
	return 1;
}

/////////////////////////////////////////////////////////////////
//| Function:	FUMAChange()
//|转义
//|将源整数数组进行如下处理，从数组的第二位起逐个比较数据，看是否有数据为7E 7D 如果有，
//|                          将相应的数据扩展为7E ---> 7D 5E ; 7D ----> 7D 5D;				
//| 
//|输入：源数据数组，起始位置，拆分的长度，目的字符串			
//|
//|返回：int 有效Str长度
//| Example:      
//| 		int  data[2]={0x2,0x7E,0x34};
//|            	char str[];         
//|             FUMAChange(data,1,2,Str); //str={0x7D,0x5E,0x34}                     
//|          
///////////////////////////////////////////////////////////
//完成，没有测试
int FUMAChange(int* Data,int DStart, int Len, unsigned char* Str){
	int i;
	int add; //新增加的数据数目
	add=0;	
	if(DStart<=0)
		DStart=1;
	if(Len<=0)
		Len=1;
	if(Len>Data[0])
		Len=Data[0];

	for(i=0;i<Len;i++)
	{
		if(Data[DStart+i]==0x7E)
		{
			Str[i+add]=0x7D;
			Str[i+add+1]=0x5E;
			add++;
		}
		else if(Data[DStart+i]==0x7D)
		{
			Str[i+add]=0x7D;
			Str[i+add+1]=0x5D;
			add++;
		}
 		else
 		{
 			Str[i+add]=Data[DStart+i] ;
 		}	

	}
	
	return (Len+add);
}




/////////////////////////////////////////////////////////
//| Function:	BuildFUMACmd()nsigned char 
//|
//|功能: 根据命令单元生成校验单元,然后进行富马转义, 然后添加结束标志（‘\n’）生成最终命令包	
//|				
//|输入: 源数据数组，字符串格式的命令包
//|				
//|输出: int  打完包的长度，除了起始结束标记                
////////////////////////////////////////////////////
//完成，待测试
int BuildFUMACmd(int* Data,unsigned char * Cmd){
	int i;
	unsigned int CalCrc=0;
	
	for(i=1;i<=Data[0];i++)
		CalCrc=cal_crc(Data[i],CalCrc);
	
	CalCrc=CalCrc&0xffff;
		//fprintf(stderr,"CalCRC %x\n",CalCrc);
	
	Data[Data[0]+1]=CalCrc%256;
	Data[Data[0]+2]=CalCrc/256;
	Data[0]+=2;

 return	FUMAChange(Data,1,Data[0],Cmd);
}


//test the function of BuildFUMACmd & FUMAParse
int main(void)
{
    unsigned char  tempCmd[20], Cmd[20];
    Cmd[0]=0x7E;
    int i, BeforeBuildData[15], AfterParseData[15];
    int len,beforelen,newlen;
    
    beforelen=BeforeBuildData[0]=10;
    for(i=1;i<11;i++)
    {
        BeforeBuildData[i]=0x75 +i;
    }
    printf("test the function of BuildFUMACmd & FUMAParse :: \n");
    printf("Befor BuildFUMACmd:: \n");
    for(i=1;i<11;i++)
    {
        printf("*%2X*",BeforeBuildData[i] );
    } 
    len=BuildFUMACmd(BeforeBuildData,tempCmd);
    for(i=0;i<len;i++)
    Cmd[i+1]=tempCmd[i];
    Cmd[len+1]=0x7E;
    len=len+2;
    
    printf("\nAfter BuildFUMACmd:: \n");
    if(len>0)
    {
        for(i=0;i<len;i++)
        printf("*%2X*",Cmd[i]);
    }
    //FUMAParse(unsigned char *Str,int strlen, int *Data)
    FUMAParse(Cmd,len,AfterParseData);
    printf("\nAfter ParseFUMA:: \n");
    newlen=AfterParseData[0];
    if(newlen>0)
    {
    for(i=0;i<newlen;i++)
    printf("*%2X*",AfterParseData[i+1]);
    }
 
     printf("Befor lenth: %d, Later lenth: %d",beforelen, newlen);
    if(beforelen==newlen)
    printf("\n*****BuildFUMACmd && FUMAParse is Successfully********\n")  ; 
    
    return 1;
}