ieee1588.c

包括ＥＰＡ协议栈

void PTP_Send_PTP_FOLLOW_MESSAGE()
{

    PTP_Follow_Up* msg;
    POutSock posock;
   
    posock = GetOutSock(PROTOCOL_UDP_OR_ICMP);
    posock->dstip = PTPIPADDR;
    posock->dstport = PTP_EVENT_PORT;
    posock->srcport = PTP_EVENT_PORT;
    posock->length = sizeof(PTP_Follow_Up);
	memcpy(posock->payload,gPTP_EVENT_MESSAGE + 42,posock->length);	
    msg = (PTP_Follow_Up* )posock->payload;
	
    msg->Header.sequenceId = ++  gTime_Para.sequenceIdCreator;
    msg->Header.control = PTP_FOLLOWUP_MESSAGE;
    msg->Header.messageType = 0x02;
    msg->Body.associatedSequenceId =  gTime_Para.BackupsequenceId;
	//msg->Header.flags[0]=0x00;
	//msg->Header.flags[1]=0x08;
    memcpy(&(msg->Body.preciseOriginTimestamp),&gTime_Para.BackTorigin,sizeof(TimeRepresentation));
    
    FOLLOWUP_MSG_VERSER_ORDER(msg);
    IPOutput(posock, UDP_PROTOCOL); 
    //udp_send((unsigned char * )msg, gTime_Para.PTPIP , PTP_GERNERAL_PORT ,sizeof(PTP_Follow_Up));
    
}

void PTP_MASTER_SEND_MSG()
{
	if(MasterInterval == 100)
	{
		PTP_Send_PTP_SYNC_MESSAGE();
		PTP_Send_PTP_FOLLOW_MESSAGE();
		MasterInterval = 0;	
	}
}

void PTP_Send_PTP_REQ_RESP(Octet*  requestingSourceUuid,uint16 requestingSourceSequenceId)
{
   
	PTP_Delay_Resp* msg;
    POutSock posock;
   
    posock = GetOutSock(PROTOCOL_UDP_OR_ICMP);
    posock->dstip = PTPIPADDR;
    posock->dstport = PTP_EVENT_PORT;
    posock->srcport = PTP_EVENT_PORT;
    posock->length = sizeof(PTP_Delay_Resp);
	memcpy(posock->payload,gPTP_EVENT_MESSAGE + 42,posock->length);	
    msg = (PTP_Delay_Resp*)posock->payload;

	//PTP_Header_Construct((PTP_Header*) msg,0x02,PTP_GERNERAL_PORT,PTP_DELAY_RESP_MESSAGE,gTime_Para.Subdomain);    
    //gTime_Para.BackupsequenceId  = msg->Header.sequenceId;
    msg->Header.sequenceId = ++  gTime_Para.sequenceIdCreator;
    msg->Header.control = PTP_DELAY_RESP_MESSAGE;
    msg->Header.messageType = 0x02;
    //msg->Header.flags[0]=0x00;
	//msg->Header.flags[1]=0x08;

	memcpy(&msg->Body.delayReceiptTimestamp,&gTime_Para.CurTime,sizeof(TimeRepresentation));
	memcpy(msg->Body.requestingSourceUuid,requestingSourceUuid,6);
	msg->Body.requestingSourceSequenceId = requestingSourceSequenceId;
	DELAY_RESP_VERSER_ORDER(msg); 
	IPOutput(posock, UDP_PROTOCOL); 	
	//udp_send((unsigned char * )msg, gTime_Para.PTPIP , PTP_GERNERAL_PORT ,sizeof(PTP_Delay_Resp));
}


void	PTP_RECV_DELAY_RESP(uint8* pPTPData)
{
	PTP_Delay_Resp* pMessage;
	

	pMessage = (PTP_Delay_Resp*)pPTPData;

	#if	0	
		if ( gTime_Para.State != PTP_SLAVE)
		{
			return;  
		}//end  if ( gTime_Para.State != PTP_PRE_MASTER)
	#endif
   
   	
   	//memcmp(pMessage->Header.sourceUuid,gTime_Para.Parent_set.parent_uuid,6) == 0 &&
	if( memcmp(pMessage->Body.requestingSourceUuid,gLOCALUUID,6) == 0)
	{
		INT32UCOPY(&pMessage->Body.delayReceiptTimestamp.seconds,&gTime_Para.Tdelay_resp.seconds);
		INT32SCOPY(&pMessage->Body.delayReceiptTimestamp.nanoseconds,&gTime_Para.Tdelay_resp.nanoseconds);
		//memcpy(&gTime_Para.Tdelay_resp,&(pMessage->Body.delayReceiptTimestamp),sizeof(TimeRepresentation));
		PTP_Sync_algorithm(&gTime_Para.Tsync_recv,&gTime_Para.Tsync_origin,&gTime_Para.Tdelay_resp,&gTime_Para.Tdelay_req);
		
	}// else if( memcpy(pMessage->Header.sourceUuid,gTime_Para.LocalUuid,6) == 0)
  
	return;

}//end funcion void	PTP_RECV_DELAY_RESP(uint8* pPTP


void PTP_Send_Delay_Req()
{
	
	PTP_Sync_Or_Delay_Req* pReq_Message;
    POutSock posock;
   
    posock = GetOutSock(PROTOCOL_UDP_OR_ICMP);
    posock->dstip = PTPIPADDR;
    posock->dstport = PTP_EVENT_PORT;
    posock->srcport = PTP_EVENT_PORT;
    posock->length = sizeof(PTP_Sync_Or_Delay_Req);
	memcpy(posock->payload,gPTP_EVENT_MESSAGE + 42,posock->length);	
    pReq_Message = (PTP_Sync_Or_Delay_Req*)posock->payload;	
	

	//pReq_Message = (PTP_Sync_Or_Delay_Req*)(gPTP_EVENT_MESSAGE );
	//PTP_Header_Construct((PTP_Header*) pReq_Message,0x01,PTP_EVENT_PORT,PTP_DELAY_REQ_MESSAGE,gTime_Para.Parent_set.parent_subdomain);
	pReq_Message->Header.sequenceId = ++  gTime_Para.sequenceIdCreator;
    pReq_Message->Header.control = PTP_DELAY_REQ_MESSAGE;
    pReq_Message->Header.messageType = 0x01;
	pReq_Message->Header.flags[0]=0x00;
	pReq_Message->Header.flags[1]=0x08;
	
	SYNC__OR_DELAY_REQ_MSG_VERSER_ORDER(pReq_Message);	
	//pReq_Message->Header.sequenceId = EXCHANGEU16(pReq_Message->Header.sequenceId);
	
	//udp_send((unsigned char *)pReq_Message, gTime_Para.PTPIP , PTP_EVENT_PORT,sizeof(PTP_Sync_Or_Delay_Req));
	IPOutput(posock, UDP_PROTOCOL); 
	gTime_Para.Tdelay_req.seconds = gTime_Para.CurTime.seconds;
	gTime_Para.Tdelay_req.nanoseconds = gTime_Para.CurTime.nanoseconds;
	//memcpy(&gTime_Para.Tdelay_req,&gTime_Para.CurTime,sizeof(TimeRepresentation));      
}

//同步算发计算
//它不进行符号调整
void	TimeRepresentationSub(TimeRepresentation * T1, TimeRepresentation * T2, TimeDiff * Result)
{
	Result->seconds = T1->seconds - T2->seconds;
	Result->nanoseconds = T1->nanoseconds - T2->nanoseconds;
}

//它不进行符号调整
void	TimeRepresentationAdd(TimeRepresentation * T1, TimeRepresentation * T2, TimeDiff * Result)
{
	Result->seconds = T1->seconds + T2->seconds;
	Result->nanoseconds = T1->nanoseconds + T2->nanoseconds;
}

//它不进行符号调整
void	TimeDiffSub(TimeDiff * T1, TimeDiff * T2, TimeDiff * Result)
{
	Result->seconds = T1->seconds - T2->seconds;
	Result->nanoseconds = T1->nanoseconds - T2->nanoseconds;
}

//它不进行符号调整
void	TimeDiffAdd(TimeDiff * T1, TimeDiff * T2, TimeDiff * Result)
{
	Result->seconds = T1->seconds + T2->seconds;
	Result->nanoseconds = T1->nanoseconds + T2->nanoseconds;
}

void		TimeDiffSignedJustify(TimeDiff * pTvalue)
{
	int32	seconds = pTvalue->seconds;
	int32	nanoseconds = pTvalue->nanoseconds;
	
	if(seconds > 0 && nanoseconds < 0)
	{
				seconds--;
				nanoseconds += 1000000000;
	}

	if(seconds < 0 && nanoseconds > 0)
	{
				seconds++;
				nanoseconds -= 1000000000;	
	}
	
	if(seconds > 0 || nanoseconds > 0)
	{
		if(nanoseconds >= 1000000000)
		{
			seconds ++;
			nanoseconds -= 1000000000;
		}
	}

	if(seconds < 0 || nanoseconds < 0)
	{
		if(nanoseconds <= -1000000000)
		{
			seconds --;
			nanoseconds += 1000000000;
		}
	}

	pTvalue->seconds = seconds;
	pTvalue->nanoseconds = nanoseconds;
}


void TimeDiffDiveTwo(TimeDiff * pTvalue)
{
	int32		seconds ;
	int32		i;
	int32		nanoseconds;

	seconds = pTvalue->seconds ;
	nanoseconds = pTvalue->nanoseconds;

	i = seconds & 0x00000001;
	
	seconds = seconds >> 1 ;
	nanoseconds = nanoseconds >> 1;
	
	if(i)
	{
		if(seconds < 0 || nanoseconds < 0)
		{
			nanoseconds	+= -500000000;
		}
		
		if(seconds > 0 || nanoseconds > 0)
		{
			nanoseconds	+= 500000000;
		}
	}
	
	pTvalue->seconds = seconds;
	pTvalue->nanoseconds = nanoseconds;
	
}


void PTP_Sync_Local_By_TOffset(TimeDiff * pToffset)
{
		//int32	nanoseconds_1 = pToffset->nanoseconds;
		//int32	nanoseconds_2 ;	

		//nanoseconds_2 = nanoseconds_1 % SYNC_TIMER_PERIOD;//
		//nanoseconds_1 -= nanoseconds_2;	//此时,纳妙10ms的整数倍
		//nanoseconds_2 /= SYNC_TIMER_STEP;//步长偏差数
}

//add by sunpan 10-18-15

INT32S Toffset_s_avg=0;			//修正的平均值(前10次不予考虑)
INT32S Toffset_ns_avg=0;
INT32S Modify_Times=0;			//修正的次数

void PTP_Sync_algorithm(TimeRepresentation * pTrecv, TimeRepresentation * pTorig, TimeRepresentation * pTresp, TimeRepresentation * pTreq)
{
	TimeDiff	Tslavefrommaster;
	TimeDiff	Tmasterfromslave;
	TimeDiff	Toffset;
	TimeDiff	Tdelay;

	TimeRepresentationSub(pTrecv,pTorig,&Tslavefrommaster);
	TimeRepresentationSub(pTresp,pTreq, &Tmasterfromslave);
	TimeDiffSignedJustify(&Tslavefrommaster);
	TimeDiffSignedJustify(&Tmasterfromslave);

	TimeDiffAdd(&Tslavefrommaster,&Tmasterfromslave,&Tdelay);
	TimeDiffSub(&Tslavefrommaster,&Tmasterfromslave,&Toffset);
	
	TimeDiffSignedJustify(&Tdelay);
	TimeDiffSignedJustify(&Toffset);
	
	TimeDiffDiveTwo(&Tdelay);
	TimeDiffDiveTwo(&Toffset);

#if 1
	Modify_Times ++;
	//add by sunpan 10-18-15
	if(Modify_Times > 5){
		if(Toffset.nanoseconds>10000000 || Toffset.nanoseconds<-10000000){
			return;
		}
		/*
		Toffset_s_avg += Toffset.seconds / (Modify_Times - 10);
		Toffset_ns_avg += Toffset.nanoseconds / (Modify_Times - 10);
		Toffset.seconds = Toffset_s_avg;
		Toffset.nanoseconds = Toffset_ns_avg;*/
	}
#endif
	//调用定时器中的时间函数
	UpdateTime((PTime)&Toffset);
	
	if((Toffset.nanoseconds >= gPresionToffset || Toffset.nanoseconds <= -gPresionToffset) || (Toffset.seconds != 0)){
		gSysStatus |= SYS_BPTPSTABLE;
	}else
	{
		gSysStatus &= ~SYS_BPTPSTABLE;
	}

}

void PTP_HEADER_VERSER_ORDER(PPTP_Header pPTP_header) {
	uint16		U16Tempt;
	
#if	1//进行优化处理
	U16Tempt = pPTP_header->versionPTP;
	pPTP_header->versionPTP =  U16Tempt >> 8 |  U16Tempt << 8 ;

	U16Tempt = pPTP_header->versionNetwork;
	pPTP_header->versionNetwork = U16Tempt >> 8 |  U16Tempt << 8 ;

	U16Tempt = pPTP_header->sourcePortId ;
	pPTP_header->sourcePortId = U16Tempt >> 8 |  U16Tempt << 8 ;
#endif

	U16Tempt = pPTP_header->sequenceId ;
	pPTP_header->sequenceId = U16Tempt >> 8 |  U16Tempt << 8;			
}

void SYNC__OR_DELAY_REQ_MSG_VERSER_ORDER(PPTP_Sync_Or_Delay_Req p_sync_or_delay_req) {
	Octet* U32Tempt1;
	Octet U32Tempt2[4];
	uint16 U16Tempt;

#if	0
	PTP_HEADER_VERSER_ORDER(&p_sync_or_delay_req->Header);//为了优化,去掉此局,增加下面的2条语句.
#endif
			
	U16Tempt = p_sync_or_delay_req->Header.sequenceId ;
	p_sync_or_delay_req->Header.sequenceId = U16Tempt >> 8 |  U16Tempt << 8;			

	U32Tempt1 = (Octet *)&p_sync_or_delay_req->Body.originTimestamp.seconds;
	U32Tempt2[0] = U32Tempt1[3];
	U32Tempt2[1] = U32Tempt1[2];
	U32Tempt2[2] = U32Tempt1[1];
	U32Tempt2[3] = U32Tempt1[0];

	U32Tempt1[0] = U32Tempt2[0];
	U32Tempt1[1] = U32Tempt2[1];
	U32Tempt1[2] = U32Tempt2[2];
	U32Tempt1[3] = U32Tempt2[3];			

	U32Tempt1 = (Octet *)&p_sync_or_delay_req->Body.originTimestamp.nanoseconds;
	U32Tempt2[0] = U32Tempt1[3];
	U32Tempt2[1] = U32Tempt1[2];
	U32Tempt2[2] = U32Tempt1[1];
	U32Tempt2[3] = U32Tempt1[0];

	U32Tempt1[0] = U32Tempt2[0];
	U32Tempt1[1] = U32Tempt2[1];
	U32Tempt1[2] = U32Tempt2[2];
	U32Tempt1[3] = U32Tempt2[3];	

#if	1//为了优化,去掉此局,增加下面的2条语句.
	U16Tempt = p_sync_or_delay_req->Body.epochNumber;
	p_sync_or_delay_req->Body.epochNumber = U16Tempt >> 8 |  U16Tempt << 8 ;
	U16Tempt = p_sync_or_delay_req->Body.currentUTCOffset;
	p_sync_or_delay_req->Body.currentUTCOffset = U16Tempt >> 8 |  U16Tempt << 8;

	U16Tempt = p_sync_or_delay_req->Body.grandmasterPortId;
	p_sync_or_delay_req->Body.grandmasterPortId = U16Tempt >> 8 |  U16Tempt << 8;

	U16Tempt = p_sync_or_delay_req->Body.grandmasterSequenceId;
	p_sync_or_delay_req->Body.grandmasterSequenceId = U16Tempt >> 8 |  U16Tempt << 8;

	U16Tempt = p_sync_or_delay_req->Body.grandmasterClockVariance;
	p_sync_or_delay_req->Body.grandmasterClockVariance = U16Tempt >> 8 |  U16Tempt << 8;

	U16Tempt = p_sync_or_delay_req->Body.localClockVariance;
	p_sync_or_delay_req->Body.localClockVariance = U16Tempt >> 8 |  U16Tempt << 8;

	U16Tempt = p_sync_or_delay_req->Body.localStepsRemoved;
	p_sync_or_delay_req->Body.localStepsRemoved = U16Tempt >> 8 |  U16Tempt << 8;

	U16Tempt = p_sync_or_delay_req->Body.parentPortField;
	p_sync_or_delay_req->Body.parentPortField = U16Tempt >> 8 |  U16Tempt << 8;

	U16Tempt = p_sync_or_delay_req->Body.estimatedMasterVariance;
	p_sync_or_delay_req->Body.estimatedMasterVariance = U16Tempt >> 8 |  U16Tempt << 8;			
			
	U32Tempt1 = (Octet *)&p_sync_or_delay_req->Body.estimatedMasterDrift;
	U32Tempt2[0] = U32Tempt1[3];
	U32Tempt2[1] = U32Tempt1[2];
	U32Tempt2[2] = U32Tempt1[1];
	U32Tempt2[3] = U32Tempt1[0];

	U32Tempt1[0] = U32Tempt2[0];
	U32Tempt1[1] = U32Tempt2[1];
	U32Tempt1[2] = U32Tempt2[2];
	U32Tempt1[3] = U32Tempt2[3];
#endif	

}
		
void FOLLOWUP_MSG_VERSER_ORDER(PPTP_Follow_Up p_follow_up) {
	Octet *U32Tempt1;
	Octet U32Tempt2[4];
	uint16 U16Tempt;

#if	0//为了优化
	PTP_HEADER_VERSER_ORDER(&p_follow_up->Header);
#endif
		
	U16Tempt = p_follow_up->Header.sequenceId ;
	p_follow_up->Header.sequenceId = U16Tempt >> 8 |  U16Tempt << 8;	

	U32Tempt1 = (Octet *)&p_follow_up->Body.preciseOriginTimestamp.seconds;
	U32Tempt2[0] = U32Tempt1[3];
	U32Tempt2[1] = U32Tempt1[2];
	U32Tempt2[2] = U32Tempt1[1];
	U32Tempt2[3] = U32Tempt1[0];

	U32Tempt1[0] = U32Tempt2[0];
	U32Tempt1[1] = U32Tempt2[1];
	U32Tempt1[2] = U32Tempt2[2];
	U32Tempt1[3] = U32Tempt2[3];	

	U32Tempt1 = (Octet *)&p_follow_up->Body.preciseOriginTimestamp.nanoseconds;
	U32Tempt2[0] = U32Tempt1[3];
	U32Tempt2[1] = U32Tempt1[2];
	U32Tempt2[2] = U32Tempt1[1];
	U32Tempt2[3] = U32Tempt1[0];

	U32Tempt1[0] = U32Tempt2[0];
	U32Tempt1[1] = U32Tempt2[1];
	U32Tempt1[2] = U32Tempt2[2];
	U32Tempt1[3] = U32Tempt2[3];	
	U16Tempt = p_follow_up->Body.associatedSequenceId;
	p_follow_up->Body.associatedSequenceId = U16Tempt >> 8 |  U16Tempt << 8;
			
}

		
void DELAY_RESP_VERSER_ORDER(PPTP_Delay_Resp p_delay_resp) {
	Octet			*U32Tempt1;
	Octet			U32Tempt2[4];
	uint16		U16Tempt;
		
#if	0	
	PTP_HEADER_VERSER_ORDER(&p_delay_resp->Header);
#endif

	U16Tempt = p_delay_resp->Header.sequenceId ;
	p_delay_resp->Header.sequenceId = U16Tempt >> 8 |  U16Tempt << 8;

	U32Tempt1 = (Octet *)&p_delay_resp->Body.delayReceiptTimestamp.seconds;
	U32Tempt2[0] = U32Tempt1[3];
	U32Tempt2[1] = U32Tempt1[2];
	U32Tempt2[2] = U32Tempt1[1];
	U32Tempt2[3] = U32Tempt1[0];

	U32Tempt1[0] = U32Tempt2[0];
	U32Tempt1[1] = U32Tempt2[1];
	U32Tempt1[2] = U32Tempt2[2];
	U32Tempt1[3] = U32Tempt2[3];
			
	U32Tempt1 = (Octet *)&p_delay_resp->Body.delayReceiptTimestamp.nanoseconds;
	U32Tempt2[0] = U32Tempt1[3];
	U32Tempt2[1] = U32Tempt1[2];
	U32Tempt2[2] = U32Tempt1[1];
	U32Tempt2[3] = U32Tempt1[0];
			
	U32Tempt1[0] = U32Tempt2[0];
	U32Tempt1[1] = U32Tempt2[1];
	U32Tempt1[2] = U32Tempt2[2];
	U32Tempt1[3] = U32Tempt2[3];	

#if	1
	U16Tempt = p_delay_resp->Body.requestingSourcePortId;
	p_delay_resp->Body.requestingSourcePortId = U16Tempt >> 8 |  U16Tempt << 8;
#endif
		
	U16Tempt = p_delay_resp->Body.requestingSourceSequenceId;
	p_delay_resp->Body.requestingSourceSequenceId = U16Tempt >> 8 |  U16Tempt << 8;						
}