Initial version.

2023-12-20 00:11:03 +08:00
commit 416bd53733
1043 changed files with 293547 additions and 0 deletions
@@ -0,0 +1,523 @@
+/*****************************************************************************/
+/**
+ * Description: Time sync implementation file.
+ * 
+ * Author: Author's name
+ * Date: 2021-12-04 16:32
+ * LastEditors: FanHao
+ * LastEditTime: 2022-07-10 20:43
+ * Revisions: Rev 0.0
+ * History: 
+ * [2022-07-10]: New branch for Dijian.
+ * [2021-12-04]: Initial version.
+ * 
+ * Copyright(c) 2021 Beijing Institute of Technology.
+ * Lab of Communication and Networking
+ * 
+ ******************************************************************************/
+
+/***************************** Include Files *********************************/
+#include <string.h>
+#include <stdlib.h>
+#include "linklayer814.h"
+#include "rtt.h"
+#include "../time_ctrl/time_ctrl.h"
+#include "../error.h"
+#include "../common.h"
+
+/************************** Constant Definitions *****************************/
+/***************** Macros (Inline Functions) Definitions *********************/
+/**************************** Type Definitions *******************************/
+/************************** Variable Definitions *****************************/
+/************************** Function Prototypes ******************************/
+
+static void RTT_StageUpdate(L2_RTT_FSM* rttFsmPtr, L2_RTT_SATGE newSatge);
+static void RTT_ResetRecord(RTT_RECORD* recordPtr);
+static int32_t RTT_GetCounter10nsDeviation(PHY_INST *phyRef);
+
+
+/*****************************************************************************/
+/**
+ * @brief    a slot alignment, base on physical layer information.
+ *
+ * @param   instPtr	a pointer to linklayer layer instance
+ * @param   frameNum	
+ * @param   slotNum	
+ *
+ * @return  SUCCESS Initialize successfully.
+ *          FAILURE Initialization failed, allocate memory fail or initial
+ *          bram controller fail.
+ *
+ ******************************************************************************/
+// TODO: Check
+int32_t L2_TimeAlign(L2_INST* instPtr, uint32_t frameNum, uint32_t slotNum){
+	L1_FRAME_TYPE lastRecvdFrame;
+	lastRecvdFrame = Phy_GetLastRecvFrameType(instPtr->config.phyInstPtr);
+	if(lastRecvdFrame != L1_NONE_FRAME){
+		return EIGNORE;
+	}
+
+	// read nextSendTime from physical layer
+	uint64_t currTime, expctTime;
+	int64_t timeDiff;
+	TIME_WORD timeStamp, slotTimerAdjmet;
+	expctTime = ((uint64_t)(frameNum*L2_SLOTS_PER_FRAME + slotNum))*COUNTE_FRAC_MAX;
+	expctTime += Phy_GetTimeComp(lastRecvdFrame);
+
+	Phy_GetFrameSyncTimeStamp(instPtr->config.phyInstPtr, &timeStamp);
+	currTime = (uint64_t)timeStamp.frac_part;
+	currTime += ((uint64_t)timeStamp.int_part) * COUNTE_FRAC_MAX;
+
+	timeDiff = (int64_t)(expctTime - currTime);
+
+	slotTimerAdjmet.int_part = (uint32_t)(timeDiff/COUNTE_FRAC_MAX);
+	slotTimerAdjmet.frac_part = (uint32_t)(timeDiff%COUNTE_FRAC_MAX);
+	TimeCtrl_TimeAdjust(instPtr->config.tmcInstPtr, &slotTimerAdjmet, TM_ADJ_ADD);
+	return SUCCESS;
+}
+
+/**
+ * @brief   Get last RTT stage of a specified RTT descriptor.
+ */
+inline L2_RTT_SATGE RTT_GetLastStage(L2_RTT_FSM* rttFsmPtr){
+	ASSERT(rttFsmPtr != NULL);
+	return rttFsmPtr->rttStatge.last;
+}
+/**
+ * @brief   Get current RTT stage of a specified RTT descriptor.
+ */
+inline L2_RTT_SATGE RTT_GetCurrentStage(L2_RTT_FSM* rttFsmPtr){
+	ASSERT(rttFsmPtr != NULL);
+	return rttFsmPtr->rttStatge.curr;
+}
+/**
+ * @brief   Get next RTT stage of a specified RTT descriptor.
+ */
+inline L2_RTT_SATGE RTT_GetNextStage(L2_RTT_FSM* rttFsmPtr){
+	ASSERT(rttFsmPtr != NULL);
+	return rttFsmPtr->rttStatge.next;
+}
+/**
+ * @brief   Get next RTT stage of a specified RTT descriptor.
+ */
+static inline void RTT_ResetRecord(RTT_RECORD* recordPtr){
+	ASSERT(recordPtr != NULL);
+	memset(recordPtr, 0x00, sizeof(RTT_RECORD));
+	return;
+}
+
+/*****************************************************************************/
+/**
+ * @brief   Update corrent time-sync for RTT stage
+ *
+ * @param   scanFsmPtr  A pointer to scan net fsm.
+ *          newSatge    New state.
+ *
+ * @return  NONE.
+ *
+ ******************************************************************************/
+static void RTT_StageUpdate(L2_RTT_FSM* rttFsmPtr, L2_RTT_SATGE newSatge) {
+	ASSERT(rttFsmPtr != NULL);
+	ASSERT(newSatge >= RTT_NONE_STAGE && newSatge <= RTT_RSP2);
+	DBG_INFO("RTT current Stage: %u -> RTT new Stage: %u, Stage: arranged.%d.\n",
+			rttFsmPtr->rttStatge.curr, rttFsmPtr->rttStatge.next, newSatge);
+	L2_RcdShift(&(rttFsmPtr->rttStatge), newSatge);
+	return;
+}
+
+/*****************************************************************************/
+/**
+ * @brief   Reset RTT systerm .
+ *
+ * @param   rttFsmPtr
+ *
+ * @return  SUCCESS Initialization successful.
+ *          EMAC_MEMORY_ALLOC_ERROR Memory allocate error.
+ *
+ ******************************************************************************/
+void RTT_Reset(L2_RTT_FSM* rttFsmPtr){
+	ASSERT(rttFsmPtr != NULL);
+	RTT_ResetRecord(&(rttFsmPtr->rttRecord));
+	rttFsmPtr->rttStatge.last = RTT_NONE_STAGE;
+	rttFsmPtr->rttStatge.curr = RTT_NONE_STAGE;
+	rttFsmPtr->rttStatge.next = RTT_NONE_STAGE;
+	rttFsmPtr->isR1Valid = false;
+	rttFsmPtr->isR2Valid = false;
+	rttFsmPtr->isR3Valid = false;
+	rttFsmPtr->isR4Valid = false;
+}
+
+void RTT_Destroy(L2_RTT_FSM* rttFsmPtr){
+	free(rttFsmPtr);
+}
+
+
+static int32_t RTT_GetCounter10nsDeviation(PHY_INST *phyRef) {
+	TIME_WORD timeStamp;
+	int64_t lastFrameSyncT, lastSlotStartT;
+	Phy_GetFrameSyncTimeStamp(phyRef, &timeStamp);
+	lastFrameSyncT = (uint64_t) timeStamp.frac_part;
+	lastFrameSyncT += ((uint64_t)timeStamp.int_part) * COUNTE_FRAC_MAX;
+	Phy_GetLastSlotStartTimeStamp(phyRef, &timeStamp);
+	lastSlotStartT = (uint64_t) timeStamp.frac_part;
+	lastSlotStartT += ((uint64_t)timeStamp.int_part) * COUNTE_FRAC_MAX;
+	return (PHY_SYNCT2FEND_200K - (lastSlotStartT - lastFrameSyncT));
+}
+
+/*****************************************************************************/
+/**
+ * @brief   Initialize RTT FSM, allocate memory for FSM instance.
+ *
+ * @param   rttFsmPtr
+ *
+ * @return  SUCCESS Initialization successful.
+ *          EMAC_MEMORY_ALLOC_ERROR Memory allocate error.
+ *
+ ******************************************************************************/
+int32_t RTT_Init(L2_RTT_FSM** rttFsmPtr){
+	(*rttFsmPtr) = (L2_RTT_FSM*) malloc(sizeof(L2_RTT_FSM));
+	if (*rttFsmPtr == NULL) {
+		free(*rttFsmPtr);
+		return EALLOC;
+	}
+	RTT_Reset(*rttFsmPtr);
+	return SUCCESS;
+}
+
+/*****************************************************************************/
+/**
+ * @brief   Execute a Time-Sync Process.
+ *
+ * @param   instPtr	a pointer to linklayer layer instance
+ *
+ * @return  SUCCESS Time-Sync Process Done.
+ *          FAILURE Time-Sync Process end with error.
+ *
+ ******************************************************************************/
+int32_t RTT_Proc(L2_INST* instPtr) {
+	ASSERT(instPtr != NULL);
+	L2_RTT_FSM* rttFsmPtr = L2_GetRttDesc(instPtr);
+	RTT_RECORD* rttRecord = &(rttFsmPtr->rttRecord);
+	PHY_INST *phyRef = instPtr->config.phyInstPtr;
+	int32_t Status = SUCCESS;
+	L2_RTT_TOR lastTor, nextTor;
+	if(instPtr->rtneConfig.last != NULL && instPtr->rtneConfig.last->slotType == SLOT_INTRA_TSYNC){
+		// Get last tor state
+		if(instPtr->rtneConfig.last->src0 == instPtr->myInfo.nodeId){
+			lastTor = RTT_SEND;
+		}
+		else if(instPtr->rtneConfig.last->dst0 == instPtr->myInfo.nodeId){
+			lastTor = RTT_RCV;
+		}
+		else{
+			lastTor = RTT_NONE;
+		}
+	}
+	else{
+		lastTor = RTT_NONE;
+	}
+
+	if(instPtr->rtneConfig.next != NULL && instPtr->rtneConfig.next->slotType == SLOT_INTRA_TSYNC){
+		// Get next tor state
+		if(instPtr->rtneConfig.next->src0 == instPtr->myInfo.nodeId){
+			nextTor = RTT_SEND;
+		}
+		else if(instPtr->rtneConfig.next->dst0 == instPtr->myInfo.nodeId){
+			nextTor = RTT_RCV;
+		}
+		else{
+			nextTor = RTT_NONE;
+		}
+	}
+	else{
+		nextTor = RTT_NONE;
+	}
+
+	DBG_INFO("-------- RTT Procedure --------\n");
+	// Stage trans
+	// For Head Node: Tx->Rx->Tx->Rx
+	if(instPtr->myInfo.nodeType == HEAD_NODE){
+		switch(rttFsmPtr->rttStatge.next) {
+		case RTT_NONE_STAGE :
+			DBG_INFO("RTT procedure start...\n");
+			// TODO
+//			RTT_ResetRecord(rttRecord);
+			RTT_StageUpdate(rttFsmPtr, RTT_REQ1);
+			break;
+		case RTT_REQ1 :
+			if(nextTor == RTT_SEND){
+				DBG_INFO("Prepare data for RTT REQ-1\n");
+				rttRecord->Tt1.timeH  = L2_GetNextFrameNum(instPtr)*L2_SLOTS_PER_FRAME + L2_GetNextSlotNum(instPtr);
+				rttRecord->Tt1.timeL = 0 ;
+				DBG_INFO("Tt1-timeL:%u.\n", rttRecord->Tt1.timeL);
+				DBG_INFO("Tt1-timeH:%u.\n", rttRecord->Tt1.timeH);
+				RTT_StageUpdate(rttFsmPtr, RTT_RSP1);
+			}
+			else{
+				DBG_INFO("Skip RTT slot, [Stage: %u].\n", rttFsmPtr->rttStatge.next);
+			}
+			break;
+		case RTT_RSP1 :
+			if(lastTor == RTT_RCV){
+				if(L2_isDataAvail(instPtr) == true){
+					DBG_INFO("RTT RSP-1 have been received.\n");
+					DBG_INFO("Preparing data for RTT REQ-2.\n");
+					rttFsmPtr->isR2Valid = true;
+					// Calculate doppler
+					double doppler = Phy_GetCoarseFreqEst(phyRef);
+					rttRecord->dopplerR2 = (int32_t)doppler;
+					DBG_INFO("rttRecord->dopplerR2: %u.\n", rttRecord->dopplerR2);
+
+					// Calculate last frame receive time.
+					int32_t counter10ns = RTT_GetCounter10nsDeviation(phyRef);
+					// response for rtt Adj
+					counter10ns += 10000;
+					counter10ns -= rttRecord->rttAdj;
+					if ( counter10ns < 0) {
+						counter10ns += COUNTE_FRAC_MAX;
+						rttRecord->Tr2.timeH -- ;
+						_nop_();
+					}
+					rttRecord->Tr2.timeH = L2_GetLastFrameNum(instPtr)*L2_SLOTS_PER_FRAME + L2_GetLastSlotNum(instPtr);
+					rttRecord->Tr2.timeL = (uint32_t)counter10ns;
+					DBG_INFO("Tr2-timeL:%u.\n", rttRecord->Tr2.timeL);
+					DBG_INFO("Tr2-timeH:%u.\n", rttRecord->Tr2.timeH);
+
+					// Calculate TroundA
+					rttRecord->TroundA.timeL = rttRecord->Tr2.timeL;
+					rttRecord->TroundA.timeH = rttRecord->Tr2.timeH - rttRecord->Tt1.timeH;
+					DBG_INFO("TroundA-timeL:%u.\n", rttRecord->TroundA.timeL);
+					DBG_INFO("TroundA-timeH:%u.\n", rttRecord->TroundA.timeH);
+				}
+				else{
+					rttFsmPtr->isR2Valid = false;
+					rttRecord->dopplerR2 = RTT_DOPPLER_NONE;
+					rttRecord->TroundA.timeL = RTT_TIME_NONE;
+					rttRecord->TroundA.timeH = RTT_TIME_NONE;
+					DBG_WARNING("RTT RSP-1 have not been received [Stage: %u].\n", rttFsmPtr->rttStatge.next);
+					DBG_WARNING("Set dopplerR2 and TroundA to dummy value.\n");
+					DBG_INFO("dopplerR2:%08X.\n", rttRecord->dopplerR2);
+					DBG_INFO("TroundA-timeL:%08X.\n", rttRecord->TroundA.timeL);
+					DBG_INFO("TroundA-timeH:%08X.\n", rttRecord->TroundA.timeH);
+				}
+				RTT_StageUpdate(rttFsmPtr, RTT_REQ2);
+			}
+			else{
+				DBG_INFO("Skip RTT slot, [Stage: %u].\n", rttFsmPtr->rttStatge.next);
+			}
+			break;
+		case RTT_REQ2 :
+			if(nextTor == RTT_SEND){
+				DBG_INFO("Prepare data for RTT REQ-2\n");
+				rttRecord->Tt3.timeH  = L2_GetNextFrameNum(instPtr)*L2_SLOTS_PER_FRAME + L2_GetNextSlotNum(instPtr);
+				rttRecord->Tt3.timeL = 0 ;
+				DBG_INFO("Tt3-timeL:%u.\n", rttRecord->Tt3.timeL);
+				DBG_INFO("Tt3-timeH:%u.\n", rttRecord->Tt3.timeH);
+
+				if ( rttFsmPtr->isR2Valid ) {
+					// Calculate reply A
+					rttRecord->TreplyA.timeL = COUNTE_FRAC_MAX - rttRecord->Tr2.timeL;
+					rttRecord->TreplyA.timeH = rttRecord->Tt3.timeH - rttRecord->Tr2.timeH - 1;
+					DBG_INFO("TreplyA-timeL:%u.\n", rttRecord->TreplyA.timeL);
+					DBG_INFO("TreplyA-timeH:%u.\n", rttRecord->TreplyA.timeH);
+				}
+				else {
+					rttRecord->TreplyA.timeL = RTT_TIME_NONE;
+					rttRecord->TreplyA.timeH = RTT_TIME_NONE;
+					DBG_WARNING("RTT RSP-1 have not been received [Stage: %u].\n", rttFsmPtr->rttStatge.next);
+					DBG_WARNING("Set TreplyA to dummy value.\n");
+					DBG_INFO("TreplyA-timeL:%08X.\n", rttRecord->TreplyA.timeL);
+					DBG_INFO("TreplyA-timeH:%08X.\n", rttRecord->TreplyA.timeH);
+				}
+				RTT_StageUpdate(rttFsmPtr, RTT_RSP2);
+			}
+			else{
+				DBG_INFO("Skip RTT slot, [Stage: %u].\n", rttFsmPtr->rttStatge.next);
+			}
+			break;
+		case RTT_RSP2 :
+			if(lastTor == RTT_RCV){
+				if(L2_isDataAvail(instPtr) == true){
+					DBG_INFO("RTT RSP-2 have been received.\n");
+					rttFsmPtr->isR4Valid = true;
+					// Calculate doppler
+					double doppler = Phy_GetCoarseFreqEst(phyRef);
+					rttRecord->dopplerR2 = (int32_t)doppler;
+					DBG_INFO("rttRecord->dopplerR2: %d.\n", rttRecord->dopplerR2);
+
+					// Calculate last frame receive time.
+					int32_t counter10ns = RTT_GetCounter10nsDeviation(phyRef);
+					if ( counter10ns < 0) {
+						counter10ns += COUNTE_FRAC_MAX;
+						rttRecord->Tr4.timeH -- ;
+					}
+					rttRecord->Tr2.timeH = L2_GetLastFrameNum(instPtr)*L2_SLOTS_PER_FRAME + L2_GetLastSlotNum(instPtr);
+					// Get a cnt with clk of 192MHz
+					rttRecord->Tr4.timeL = (uint32_t)counter10ns;
+					DBG_INFO("Tr4-timeL:%u.\n", rttRecord->Tr4.timeL);
+					DBG_INFO("Tr4-timeH:%u.\n", rttRecord->Tr4.timeH);
+				}
+				else{
+					rttFsmPtr->isR4Valid = false;
+					rttRecord->Tr4.timeL = RTT_TIME_NONE;
+					rttRecord->Tr4.timeH = RTT_TIME_NONE;
+					DBG_WARNING("RTT RSP-2 have not been received [Stage: %u].\n", rttFsmPtr->rttStatge.next);
+					DBG_WARNING("Set Tr4 to dummy value.\n");
+					DBG_INFO("Tr4-timeL:%08X.\n", rttRecord->Tr4.timeL);
+					DBG_INFO("Tr4-timeH:%08X.\n", rttRecord->Tr4.timeH);
+				}
+				RTT_StageUpdate(rttFsmPtr, RTT_NONE_STAGE);
+				DBG_INFO("RTT procedure end...\n");
+			}
+			break;
+		default :
+			break;
+		}
+	}
+	// For Member Node: Rx->Tx->Rx->Tx
+	else{
+		switch (rttFsmPtr->rttStatge.next) {
+		case RTT_NONE_STAGE :
+			DBG_INFO("RTT procedure start...\n");
+			// TODO
+//			RTT_ResetRecord(rttRecord);
+			RTT_StageUpdate(rttFsmPtr, RTT_REQ1);
+			break;
+		case RTT_REQ1 :
+			if(lastTor == RTT_RCV){
+				if(L2_isDataAvail(instPtr) == true){
+					DBG_INFO("RTT REQ-1 have been received.\n");
+					DBG_INFO("Preparing data for RTT RSP-1.\n");
+					rttFsmPtr->isR2Valid = true;
+					// Calculate doppler
+					double doppler = Phy_GetCoarseFreqEst(phyRef);
+					rttRecord->dopplerR2 = (int32_t)doppler;
+					DBG_INFO("rttRecord->dopplerR2: %d.\n", rttRecord->dopplerR2);
+
+					// Calculate last frame receive time.
+					int32_t counter10ns = RTT_GetCounter10nsDeviation(phyRef);
+					if ( counter10ns < 0) {
+						counter10ns += COUNTE_FRAC_MAX;
+						rttRecord->Tr1.timeH -- ;
+					}
+					rttRecord->Tr1.timeH = L2_GetLastFrameNum(instPtr)*L2_SLOTS_PER_FRAME + L2_GetLastSlotNum(instPtr);
+					rttRecord->Tr1.timeL = (uint32_t)counter10ns;
+					DBG_INFO("Tr1-timeL:%u.\n", rttRecord->Tr1.timeL);
+					DBG_INFO("Tr1-timeH:%u.\n", rttRecord->Tr1.timeH);
+				}
+				else{
+					rttFsmPtr->isR1Valid = false;
+					rttRecord->dopplerR1 = RTT_DOPPLER_NONE;
+					rttRecord->Tr1.timeL = RTT_TIME_NONE;
+					rttRecord->Tr1.timeH = RTT_TIME_NONE;
+					DBG_WARNING("RTT REQ-1 have not been received [Stage: %u].\n", rttFsmPtr->rttStatge.next);
+					DBG_WARNING("Set dopplerR1 and Tr1 to dummy value.\n");
+					DBG_INFO("dopplerR1:%08X.\n", rttRecord->dopplerR1);
+					DBG_INFO("Tr1-timeL:%08X.\n", rttRecord->Tr1.timeL);
+					DBG_INFO("Tr1-timeH:%08X.\n", rttRecord->Tr1.timeH);
+				}
+				RTT_StageUpdate(rttFsmPtr, RTT_RSP1);
+			}
+			break;
+		case RTT_RSP1 :
+			if(nextTor == RTT_SEND){
+				DBG_INFO("Prepare data for RTT RSP-1\n");
+				rttRecord->Tt2.timeH  = L2_GetNextFrameNum(instPtr)*L2_SLOTS_PER_FRAME + L2_GetNextSlotNum(instPtr);
+				rttRecord->Tt2.timeL = 0 ;
+				DBG_INFO("Tt2-timeL:%u.\n", rttRecord->Tt2.timeL);
+				DBG_INFO("Tt2-timeH:%u.\n", rttRecord->Tt2.timeH);
+
+				if ( rttFsmPtr->isR1Valid ) {
+					// Calculate reply B
+					rttRecord->TreplyB.timeL = COUNTE_FRAC_MAX - rttRecord->Tr1.timeL;
+					rttRecord->TreplyB.timeH = rttRecord->Tt2.timeH - rttRecord->Tr1.timeH - 1;
+					DBG_INFO("TreplyB-timeL:%u.\n", rttRecord->TreplyB.timeL);
+					DBG_INFO("TreplyB-timeH:%u.\n", rttRecord->TreplyB.timeH);
+				}
+				else {
+					rttRecord->TreplyB.timeL = RTT_TIME_NONE;
+					rttRecord->TreplyB.timeH = RTT_TIME_NONE;
+					DBG_WARNING("RTT REQ-1 have not been received [Stage: %u].\n", rttFsmPtr->rttStatge.next);
+					DBG_WARNING("Set ReplayB to dummy value.\n");
+					DBG_INFO("TreplyB-timeL:%08X.\n", rttRecord->TreplyB.timeL);
+					DBG_INFO("TreplyB-timeH:%08X.\n", rttRecord->TreplyB.timeH);
+				}
+				RTT_StageUpdate(rttFsmPtr, RTT_REQ2);
+			}
+			break;
+		case RTT_REQ2 :
+			if(lastTor == RTT_RCV){
+				if(L2_isDataAvail(instPtr) == true){
+					DBG_INFO("RTT REQ-2 have been received.\n");
+					DBG_INFO("Preparing data for RTT RSP-2.\n");
+					rttFsmPtr->isR3Valid = true;
+
+					// Calculate doppler
+					double doppler = Phy_GetCoarseFreqEst(phyRef);
+					rttRecord->dopplerR2 = (int32_t)doppler;
+					DBG_INFO("rttRecord->dopplerR2: %d.\n", rttRecord->dopplerR2);
+
+					// Calculate last frame receive time.
+					int32_t counter10ns = RTT_GetCounter10nsDeviation(phyRef);
+					if ( counter10ns < 0) {
+						counter10ns += COUNTE_FRAC_MAX;
+						rttRecord->Tr3.timeH -- ;
+					}
+					rttRecord->Tr3.timeL = (uint32_t)counter10ns;
+					DBG_INFO("Tr3-timeL:%u.\n", rttRecord->Tr3.timeL);
+					DBG_INFO("Tr3-timeH:%u.\n", rttRecord->Tr3.timeH);
+
+					// Calculate TroundB
+					rttRecord->TroundB.timeL = rttRecord->Tr3.timeL;
+					rttRecord->TroundB.timeH = rttRecord->Tr3.timeH - rttRecord->Tt2.timeH;
+					DBG_INFO("TroundB-timeL:%u.\n", rttRecord->TroundB.timeL);
+					DBG_INFO("TroundB-timeH:%u.\n", rttRecord->TroundB.timeH);
+				}
+				else{
+					rttFsmPtr->isR3Valid = false;
+					rttRecord->dopplerR3 = RTT_DOPPLER_NONE;
+					rttRecord->TroundB.timeL = RTT_TIME_NONE;
+					rttRecord->TroundB.timeH = RTT_TIME_NONE;
+					DBG_WARNING("RTT REQ-2 have not been received [Stage: %u].\n", rttFsmPtr->rttStatge.next);
+					DBG_WARNING("Set dopplerR3 and ReplayB to dummy value.\n");
+					DBG_INFO("dopplerR3:%08X.\n", rttRecord->dopplerR3);
+					DBG_INFO("TroundB-timeL:%08X.\n", rttRecord->TroundB.timeL);
+					DBG_INFO("TroundB-timeH:%08X.\n", rttRecord->TroundB.timeH);
+				}
+				RTT_StageUpdate(rttFsmPtr, RTT_RSP2);
+			}
+			break;
+		case RTT_RSP2 :
+			if(nextTor == RTT_SEND){
+				DBG_INFO("Prepare data for RTT RSP-2\n");
+				rttRecord->Tt4.timeH  = L2_GetNextFrameNum(instPtr)*L2_SLOTS_PER_FRAME + L2_GetNextSlotNum(instPtr);
+				rttRecord->Tt4.timeL = 0;
+				DBG_INFO("Tt4-timeL:%u.\n", rttRecord->Tt4.timeL);
+				DBG_INFO("Tt4-timeH:%u.\n", rttRecord->Tt4.timeH);
+
+				if ( rttFsmPtr->isR3Valid ) {
+					// Calculate reply B
+					rttRecord->TreplyB.timeL = COUNTE_FRAC_MAX - rttRecord->Tr3.timeL;
+					rttRecord->TreplyB.timeH = rttRecord->Tt4.timeH - rttRecord->Tr3.timeH - 1;
+					DBG_INFO("TreplyB-timeL:%u.\n", rttRecord->TreplyB.timeL);
+					DBG_INFO("TreplyB-timeH:%u.\n", rttRecord->TreplyB.timeH);
+					// TODO implement a time adjustment.
+					DBG_INFO("RTT procedure end...\n");
+				}
+				else {
+					rttRecord->TreplyB.timeL = RTT_TIME_NONE;
+					rttRecord->TreplyB.timeH = RTT_TIME_NONE;
+					DBG_WARNING("RTT REQ-2 have not been received [Stage: %u].\n", rttFsmPtr->rttStatge.next);
+					DBG_WARNING("Set ReplayB to dummy value.\n");
+					DBG_INFO("TreplyB-timeL:%08X.\n", rttRecord->TreplyB.timeL);
+					DBG_INFO("TreplyB-timeH:%08X.\n", rttRecord->TreplyB.timeH);
+				}
+				RTT_StageUpdate(rttFsmPtr, RTT_NONE_STAGE);
+			}
+			break;
+		default :
+			break;
+		}
+	}
+	return Status;
+}