fservice.c

包括ＥＰＡ协议栈

#define FSERVICE_GLOBALS
#include "fs_module.h"
#undef FSERVICE_GLOBALS
#include "epa_module.h"
#include "tcpip.h"
#include "timer.h"
#include "fb_module.h"
#include <string.h>

static uint16 gCrcMode;
static uint16 gPubESN;
static uint16 gSubESN;
static uint16 gSubLRSN;
static uint32 gPubKey;
static uint32 gSubKey;

static uint8 FSRetryOutput(void *pdata){
	uint32 crc;
	PSock psock;
		
	psock = (PSock)pdata;
	if(GetFSComState() == SAFETYCOM_OPEN) {
		h2n16(GetFSComESN(), psock->payload + 4);
		switch(gCrcMode) {
			case 1:
				crc = crc81_check(psock->payload + 4, psock->length - 4, GetFSComKey());
				break;
			case 2:
				crc = crc81_check(psock->payload + 4, psock->length - 4, GetFSComKey());
				break;
			case 3:
				crc = crc16_check(psock->payload + 4, psock->length - 4, GetFSComKey());
				break;
			case 4:
				crc = crc32_check(psock->payload + 4, psock->length - 4, GetFSComKey());
				break;
		}
		h2n32(crc, psock->payload);
		EPAOutput(psock);
		return (0);
	}
	else {
		FreeSock(psock);
		return (1);
	}
}

/*--------------------------------------------------------------------*
 *- Parameter: DesIPAddr 目的地址, pData 数据                        -*
 *- Description: 通信发起方发起open请求			                     -*
 *--------------------------------------------------------------------*/
void FS_ComOpen_req(uint32 dstip, uint16 srcappid) {
	PSock psock;

	if(GetFSComState() == SAFETYCOM_CLOSE) {
	
		psock = GetOutSock(PROTOCOL_EPASFB);
		
		psock->dstip = dstip;
		psock->dstport = EPA_PORT;
		psock->length = 6;	
		psock->srvid = FSID_COM_OPEN;
		psock->comtype = 1;
		h2n16(srcappid, psock->payload);
		h2n32(dstip, psock->payload + 2);

		SetFSComState(SAFETYCOM_REQUESTING);
		EPAOutput(psock);
	}
}

/*--------------------------------------------------------------------*
 *- Parameter: Smo_Socket_Index 数据索引,由通信栈获得                -*
 *- Description: Communication Open 请求服务处理函数, 根据状态机     -*
 *-   对应的动作                                                     -*
 *--------------------------------------------------------------------*/
void FS_ComOpen_reqhandle(PSock psock) {
	OctetString usrdata;
	uint32 rcvip;
	uint16 rcvappid;
	
	n2h16(psock->payload, &rcvappid);	
	n2h32(psock->payload + 2, &rcvip);
	
	FreeSock(psock);
	if(GetFSComState() == SAFETYCOM_CLOSE) {
		if(psock->srcip == rcvip) {
			SetFSComState(SAFETYCOM_REQUESTED);
			SetFSComRmtIP(rcvip);
			FS_ComOpen_pos(rcvip, psock->srcport, psock->msgid, rcvappid);
		}
		else {
			usrdata[0] = 0;
			usrdata[1] = 0;
			memcpy(usrdata + 4, "IP address check error", 32);
			FS_ComOpen_neg(psock->srcip, psock->srcport, psock->msgid, usrdata, rcvappid);
		}
	}
	else {
		usrdata[0] = 0;
		usrdata[1] = 0;
		memcpy(usrdata + 4, "State not match", 32);
		FS_ComOpen_neg(psock->srcip, psock->srcport, psock->msgid, usrdata, rcvappid);
	}
}

/*--------------------------------------------------------------------*
 *- Parameter: DesIPAddr 目标IP, UdpPort 目标端口, 对应请求的MsgID   -*
 *-   pData 需要发送的数据, 以网络序的Octet串形式提供                -*
 *- Description: 根据参数发送Communication Open正响应报文            -*
 *--------------------------------------------------------------------*/
void FS_ComOpen_pos(uint32 dstip, uint16 dstport, uint16 msgid, uint16 dstappid) {//应该连msgid也不用传下来了
	PSock	psock;
	
	if(GetFSComState() == SAFETYCOM_REQUESTED) {
		psock = GetOutSock(PROTOCOL_EPASFB);
		psock->dstip = dstip;
		psock->dstport = dstport;
		psock->srvid = FSID_COM_OPEN_POS;
		psock->comtype = 1;
		psock->length = 2;
		psock->msgid = msgid;
		h2n16(dstappid, psock->payload);
		SetFSComState(SAFETYCOM_OPEN);
		EPAOutput(psock);
	}
}


/*--------------------------------------------------------------------*
 *- Parameter: DesIPAddr 目标IP, UdpPort 目标端口, 对应请求的MsgID   -*
 *-   pData 需要发送的数据, 以网络序的Octet串形式提供                -*
 *- Description: 根据参数发送Communication Open负响应报文            -*
 *--------------------------------------------------------------------*/
void FS_ComOpen_neg(uint32 dstip, uint16 dstport, uint16 msgid, uint8* pdata, uint16 dstappid) {
	PSock	psock;
	
	psock = GetOutSock(PROTOCOL_EPASFB);		
	psock->dstip = dstip;
	psock->dstport = dstport;
	psock->srvid = FSID_COM_OPEN_NEG;
	psock->comtype = 1;
	psock->length = 40;		
		
	h2n16(dstappid, psock->payload);
	memcpy(psock->payload + 4, pdata, 36);

	SetFSComState(SAFETYCOM_CLOSE);
	EPAOutput(psock);		
}

/*--------------------------------------------------------------------*
 *- Parameter: Smo_Socket_Index 数据索引,由通信栈获得                -*
 *- Description: Communication open postive正响应 请求服务处理函数,        -*
 *-   根据状态机对应的动作                                            -*
 *--------------------------------------------------------------------*/
void FS_ComOpen_poshandle(PSock psock) {
	FreeSock(psock);
	if(GetFSComState() == SAFETYCOM_REQUESTING) {
		SetFSComState(SAFETYCOM_OPEN);
	}
}

/*--------------------------------------------------------------------*
 *- Parameter: Smo_Socket_Index 数据索引,由通信栈获得                -*
 *- Description: Communication open nagtive负响应 请求服务处理函数,        -*
 *-   根据状态机对应的动作                                            -*
 *--------------------------------------------------------------------*/
void FS_ComOpen_neghandle(PSock psock) {
	FreeSock(psock);
	if(GetFSComState() == SAFETYCOM_REQUESTING) {
		SetFSComState(SAFETYCOM_CLOSE);
	}
}

/*--------------------------------------------------------------------*
 *- Parameter: DesIPAddr 目的地址, pData 数据                          -*
 *- Description: 通信发起方发起open请求														   -*
 *--------------------------------------------------------------------*/
void FS_ComClose_req(uint32 dstip, uint16 srcappid) {
	PSock psock;

	if(GetFSComState() == SAFETYCOM_OPEN) {

		psock = GetOutSock(PROTOCOL_EPASFB);
		psock->dstip = dstip;
		psock->dstport = EPA_PORT;
		psock->length = 6;	
		psock->srvid = FSID_COM_CLOSE;
		psock->comtype = 1;
		h2n32(dstip, psock->payload + 2);
		h2n16(srcappid, psock->payload);
	
		EPAOutput(psock);
	}
}

/*--------------------------------------------------------------------*
 *- Parameter: Smo_Socket_Index 数据索引,由通信栈获得                -*
 *- Description: Communication Close 请求服务处理函数, 根据状态机    -*
 *-   对应的动作                                                     -*
 *--------------------------------------------------------------------*/
void FS_ComClose_reqhandle(PSock psock) {
	OctetString usrdata;
	uint32 rcvip;
	uint16 rcvappid;

	n2h16(psock->payload, &rcvappid);
	n2h32(psock->payload + 2, &rcvip);

	if(GetFSComState() == SAFETYCOM_OPEN) {
		if(psock->srcip == rcvip && rcvip == GetFSComRmtIP()) {
			FS_ComClose_pos(rcvip, psock->srcport, psock->msgid, rcvappid);
		}
		else {
			usrdata[0] = 0;
			usrdata[1] = 0;
			memcpy(usrdata + 4, "IP address check error", 32);
			FS_ComClose_neg(rcvip, psock->srcport, psock->msgid, usrdata, rcvappid);
		}
	}
	else {
		usrdata[0] = 0;
		usrdata[1] = 0;
		memcpy(usrdata + 4, "State not match", 32);
		FS_ComClose_neg(rcvip, psock->srcport, psock->msgid, usrdata, rcvappid);
	}
}

/*--------------------------------------------------------------------*
 *- Parameter: DesIPAddr 目标IP, UdpPort 目标端口, 对应请求的MsgID   -*
 *-   pData 需要发送的数据, 以网络序的Octet串形式提供                -*
 *- Description: 根据参数发送Communication Close正响应报文           -*
 *--------------------------------------------------------------------*/
void FS_ComClose_pos(uint32 dstip, uint16 dstport, uint16 msgid, uint16 dstappid) {
	PSock psock;
	
	if(GetFSComState() == SAFETYCOM_OPEN) {
		psock = GetOutSock(PROTOCOL_EPASFB);
		psock->dstip = dstip;
		psock->dstport = dstport;
		psock->srvid = FSID_COM_CLOSE_POS;
		psock->comtype = 1;
		psock->length = 2;
		SetFSComState(SAFETYCOM_OPEN);
		EPAOutput(psock);
	}
}

/*--------------------------------------------------------------------*
 *- Parameter: DesIPAddr 目标IP, UdpPort 目标端口, 对应请求的MsgID   -*
 *-   pData 需要发送的数据, 以网络序的Octet串形式提供                -*
 *- Description: 根据参数发送Communication Close负响应报文           -*
 *--------------------------------------------------------------------*/
void FS_ComClose_neg(uint32 dstip, uint16 dstport, uint16 msgid, uint8* pdata, uint16 dstappid) {
	PSock psock;

	psock = GetOutSock(PROTOCOL_EPASFB);
	psock->dstport = dstport;
	psock->srvid = FSID_COM_CLOSE_NEG;
	psock->comtype = 1;
	psock->length = 40;
	psock->dstip = dstip;	
	h2n16(dstappid, psock->payload);
	memcpy(psock->payload + 4, pdata, 36);
	EPAOutput(psock);
}

/*--------------------------------------------------------------------*
 *- Parameter: Smo_Socket_Index 数据索引,由通信栈获得                -*
 *- Description: Communication close postive正响应 请求服务处理函数,        -*
 *-   根据状态机对应的动作                                            -*
 *--------------------------------------------------------------------*/
void FS_ComClose_poshandle(PSock psock) {
	if(GetFSComState() == SAFETYCOM_OPEN) {
		SetFSComState(SAFETYCOM_CLOSE);
		FreeSock(psock);
	}
}

/*--------------------------------------------------------------------*
 *- Parameter: Smo_Socket_Index 数据索引,由通信栈获得                -*
 *- Description: Communication close nagtive负响应 请求服务处理函数,        -*
 *-   根据状态机对应的动作                                            -*
 *--------------------------------------------------------------------*/
void FS_ComClose_neghandle(PSock psock) {

	FreeSock(psock);
}

//////////////////////////////////////// 信息分发服务 /////////////////////////////////////

/*--------------------------------------------------------------------*
 *- Parameter: DesIPAddr 目标IP, UdpPort 目标端口, 对应请求的MsgID   -*
 *-   pData 需要发送的数据, 以网络序的Octet串形式提供,               -*
 *-   Datelen 数据长度                                               -*
 *- Description: 根据参数发送Safety Distribute 请求报文              -*
 *--------------------------------------------------------------------*/
void FS_Distribute_req(PSock psock) {
	uint32 crc;
	Time ctime; 

	ShiftOutSock(psock, PROTOCOL_EPASSFB, PROTOCOL_EPASFB);
	++gPubESN;
	h2n16(gPubESN, psock->payload + 4);
	psock->payload[6] = 0x0;
	psock->payload[7] = 0x0;
	Getime(&ctime);    
	h2n32(ctime.secs, psock->payload + 8);
	h2n32(ctime.nasecs, psock->payload + 12);
	switch(gCrcMode) {
		case 1:
			crc = crc81_check(psock->payload + 4, psock->length + 16, gPubKey);
			break;
		case 2:
			crc = crc81_check(psock->payload + 4, psock->length + 16, gPubKey);
			break;
		case 3:
			crc = crc16_check(psock->payload + 4, psock->length + 16, gPubKey);
			break;
		case 4:
			crc = crc32_check(psock->payload + 4, psock->length + 16, gPubKey);
			break;
	}
	h2n32(crc, psock->payload);
	EPAOutput(psock);
}


/*--------------------------------------------------------------------*
 *- Parameter: Smo_Socket_Index 数据索引,由通信栈获得                -*
 *- Description: Safety Distribute 服务处理函数, 通过时间戳, CRC, SN -*
 *-   检查后, 传递到用户层处理, 未通过检查则产生对应错误报告         -*
 *--------------------------------------------------------------------*/
void FS_Distribute_reqhandle(PSock psock) {
	uint16 srcappid, srcobjid;
	uint32 rmtcrc, localcrc;
	uint16 rmtsn;
	FSCommunicationFailureObject fscfo;

	
	n2h16(psock->payload, &srcappid);
	n2h16(psock->payload + 2, &srcobjid);

	switch(gCrcMode) {
		case 1:
			localcrc = crc81_check(psock->payload + 4, psock->length - 4, gSubKey);
			break;
		case 2:
			localcrc = crc81_check(psock->payload + 4, psock->length - 4, gSubKey);
			break;
		case 3:
			localcrc = crc16_check(psock->payload + 4, psock->length - 4, gSubKey);
			break;
		case 4:
			localcrc = crc32_check(psock->payload + 4, psock->length - 4, gSubKey);
			break;
		default:
			return;
	}
	n2h32(psock->payload, &rmtcrc);
	
	if(localcrc == rmtcrc) {
		n2h16(psock->payload + 4, &rmtsn);
		if(!(gSubESN | gSubLRSN)) {
			// 链路处于初始化状态,使用发送方的ESN为初值
			gSubESN = rmtsn;
		}
		else {
			// 收到报文,处理之前自增tFSLO.ESN, 保持与发送方相等