diff --git a/datalink814_ge/src/linklayer/rtt.h b/datalink814_ge/src/linklayer/rtt.h
index 9943b6e..3e6627d 100644
--- a/datalink814_ge/src/linklayer/rtt.h
+++ b/datalink814_ge/src/linklayer/rtt.h
@@ -22,51 +22,30 @@
 #include <stdint.h>
 #include "linklayer814.h"
 #include "linklayer814_types.h"
+#include "../time_ctrl/time_ctrl.h"
 
 /************************** Constant Definitions *****************************/
-#define MAX_FREQ_EST_NUM 128 // 32bit word
-#define SVC_FREQ_EST_NUM 512 // byte
-#define MGMT_FREQ_EST_NUM 512 // byte
-#define BISAR_FREQ_EST_NUM 480 // byte
-
-#define CXR_FREQ 3800000000.0       // Hz
-
-#define LIGHT_SPPED_AIR 299702547.0 // m/s
-#define LIGHT_SPPED_VAC 299792458.0 // m/s
-
-#define RCV2FSYNC_CLOCKS 0 // clocks from frame sync time to receive time
-#define PHYRSP_DELAY_CLOCKS 0// clocks from frame sync time to receive time
-//#define PHY_SYNCT2FEND_200K 904052L
-//#define PHY_SYNCT2FEND_200K 924232L
-#define PHY_SYNCT2FEND_200K 905052L
-
-#define RTT_TIME_NONE	UINT32_MAX
-#define RTT_DOPPLER_NONE	UINT32_MAX
-
-
 /***************** Macros (Inline Functions) Definitions *********************/
-
 /**************************** Type Definitions *******************************/
 
 // TODO: Multi-node RTT
 typedef struct {
-	L2_TIME_WORD Tt1;
-	L2_TIME_WORD Tr1;
-	L2_TIME_WORD Tt2;
-	L2_TIME_WORD Tr2;
-	L2_TIME_WORD Tt3;
-	L2_TIME_WORD Tr3;
-	L2_TIME_WORD Tt4;
-	L2_TIME_WORD Tr4;
+	TIME_WORD Tt1;
+	TIME_WORD Tr1;
+	TIME_WORD Tt2;
+	TIME_WORD Tr2;
+	TIME_WORD Tt3;
+	TIME_WORD Tr3;
+	TIME_WORD Tt4;
+	TIME_WORD Tr4;
 	int32_t dopplerR1;
 	int32_t dopplerR2;
 	int32_t dopplerR3;
 	int32_t dopplerR4;
-	L2_TIME_WORD TroundA;
-	L2_TIME_WORD TreplyA;
-	L2_TIME_WORD TroundB;
-	L2_TIME_WORD TreplyB;
-	int32_t rttAdj;
+	TIME_WORD TroundA;
+	TIME_WORD TreplyA;
+	TIME_WORD TroundB;
+	TIME_WORD TreplyB;
 } RTT_RECORD;
 
 typedef enum {
@@ -78,25 +57,15 @@ typedef enum {
 } L2_RTT_SATGE;
 
 typedef struct {
-	L2_RCD rttStatge;
+	L2_RTT_SATGE rttStatge;
 	bool isR1Valid;
 	bool isR2Valid;
 	bool isR3Valid;
 	bool isR4Valid;
 	RTT_RECORD rttRecord;
+	TIME_WORD timeStamp;
+	uint32_t doppler;
 } L2_RTT_FSM;
 
-typedef enum {
-	RTT_NONE = 0,
-	RTT_SEND = 1,
-	RTT_RCV = 2,
-} L2_RTT_TOR;
-
 /************************** Function Prototypes ******************************/
-
-// RTT
-L2_RTT_SATGE RTT_GetLastStage(L2_RTT_FSM* rttFsmPtr);
-L2_RTT_SATGE RTT_GetCurrentStage(L2_RTT_FSM* rttFsmPtr);
-L2_RTT_SATGE RTT_GetNextStage(L2_RTT_FSM* rttFsmPtr);
-
 #endif /* RTT_H_ */
diff --git a/datalink814_ge/src/linklayer/time_sync.c b/datalink814_ge/src/linklayer/time_sync.c
index 3f677b2..3959f98 100644
--- a/datalink814_ge/src/linklayer/time_sync.c
+++ b/datalink814_ge/src/linklayer/time_sync.c
@@ -26,14 +26,22 @@
 #include "../common.h"
 
 /************************** Constant Definitions *****************************/
+#define LIGHT_SPPED_AIR	299702547.0 // m/s
+#define LIGHT_SPPED_VAC	299792458.0 // m/s
+#define RTT_DOPPLER_NONE	UINT32_MAX
+
+const TIME_WORD RTT_TIME_NONE = {
+	.int_part = UINT32_MAX,
+	.frac_part = UINT32_MAX
+};
 /***************** 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);
+static void RTT_GetFrameStart(L2_INST* instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType);
+void RTT_CalculateTimeGap(const TIME_WORD *tmA, const TIME_WORD *tmB, TIME_WORD *tRound);
 
 
 /*****************************************************************************/
@@ -70,48 +78,18 @@ int32_t L2_TimeAlign(L2_INST* instPtr, uint32_t frameNum, uint32_t slotNum){
 
 	timeDiff = (int64_t)(expctTime - currTime);
 
-	slotTimerAdjmet.int_part = (uint32_t)(timeDiff/COUNTE_FRAC_MAX);
-	slotTimerAdjmet.frac_part = (uint32_t)(timeDiff%COUNTE_FRAC_MAX);
+	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.
+ * @param   newSatge    New state.
  *
  * @return  NONE.
  *
@@ -119,49 +97,114 @@ static inline void RTT_ResetRecord(RTT_RECORD* recordPtr){
 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;
+	DBG_INFO("RTT Stage Update: %d -> %d.\n", rttFsmPtr->rttStatge, newSatge);
 }
 
 /*****************************************************************************/
 /**
- * @brief   Reset RTT systerm .
+ * @brief   Reset RTT FSM
  *
- * @param   rttFsmPtr
+ * @param   rttFsmPtr	a pointer to rtt FSM instance.
  *
- * @return  SUCCESS Initialization successful.
- *          EMAC_MEMORY_ALLOC_ERROR Memory allocate error.
+ * @return  NONE
  *
  ******************************************************************************/
 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;
+	memset(&(rttFsmPtr->rttRecord), 0x00, sizeof(RTT_RECORD));
+	rttFsmPtr->rttStatge = RTT_NONE_STAGE;
 	rttFsmPtr->isR1Valid = false;
 	rttFsmPtr->isR2Valid = false;
 	rttFsmPtr->isR3Valid = false;
 	rttFsmPtr->isR4Valid = false;
+	rttFsmPtr->timeStamp.int_part = 0;
+	rttFsmPtr->timeStamp.frac_part = 0;
+	rttFsmPtr->doppler = 0;
 }
 
+/*****************************************************************************/
+/**
+ * @brief   Destroy RTT FSM, free allocated memory.
+ *
+ * @param   rttFsmPtr	a pointer to rtt FSM instance.
+ *
+ * @return  NONE
+ *
+ ******************************************************************************/
 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   Get the frame start time. If torType == L2_TOR_TX, get the next
+ * 			slot start time, if torType == L2_TOR_RX get last frame start 
+ * 			time.
+ *
+ * @param   instPtr	a pointer to linklayer layer instance
+ * @param   tmStamp	a pointer to store frame start time
+ * @param   torType	tx/rx mode
+ *
+ * @return  NONE
+ *
+ ******************************************************************************/
+void RTT_GetFrameStart(L2_INST* instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType){
+	ASSERT(instPtr != NULL);
+	ASSERT(tmStamp != NULL);
+	// Get the next frame start time
+	if(torType == L2_TOR_TX) {
+		tmStamp->int_part  = L2_GetNextFrameNum(instPtr)*L2_SLOTS_PER_FRAME + L2_GetNextSlotNum(instPtr);
+		tmStamp->frac_part = 0 ;
+	}
+	// Get the last reiceive frame start time
+	else if(torType == L2_TOR_RX) {
+		PHY_INST* phyRef = instPtr->config.phyInstPtr;
+		TIME_WORD timeStamp;
+		int64_t timeDiff;
+		Phy_GetFrameSyncTimeStamp(phyRef, &timeStamp);
+		timeDiff = (uint64_t) timeStamp.frac_part;
+		timeDiff += ((uint64_t)timeStamp.int_part) * COUNTE_FRAC_MAX;
+		Phy_GetLastSlotStartTimeStamp(phyRef, tmStamp);
+		timeDiff -= ((uint64_t) tmStamp->int_part) * COUNTE_FRAC_MAX;
+		timeDiff -= (uint64_t) tmStamp->frac_part;
+		timeDiff = (int64_t)Phy_GetTimeComp(L2_L2frame2L1frame(L2_GetLastFrameType(instPtr))) - timeDiff;
+		if (timeDiff < 0) {
+			timeDiff += COUNTE_FRAC_MAX;
+			tmStamp->int_part--;
+			_nop_();
+		}
+		tmStamp->frac_part = (TIME_FRAC)timeDiff;
+	}
+	else {
+		*tmStamp = RTT_TIME_NONE;
+		DBG_ERROR("Unexpected tx/rx mode, setting dummy values.\n");
+	}
+}
+
+
+/*****************************************************************************/
+/**
+ * @brief   Calculate tmA - tmA.
+ *
+ * @param   tmA	time stamp A
+ * @param   tmA	time stamp B
+ * @param   tRound	t round 
+ *
+ * @return  NONE
+ *
+ ******************************************************************************/
+void RTT_CalculateTimeGap(const TIME_WORD *tmA, const TIME_WORD *tmB, TIME_WORD *tRound){
+	ASSERT(tmA != NULL);
+	ASSERT(tmB != NULL);
+	ASSERT(tRound != NULL);
+	uint64_t timeDiff;
+	timeDiff = (uint64_t) tmB->frac_part;
+	timeDiff += ((uint64_t)tmB->int_part) * COUNTE_FRAC_MAX;
+	timeDiff -= ((uint64_t)tmA->int_part) * COUNTE_FRAC_MAX;
+	timeDiff -= (uint64_t) tmA->frac_part;
+	tRound->frac_part = (TIME_FRAC)(timeDiff % COUNTE_FRAC_MAX);
+	tRound->frac_part = (TIME_INT)(timeDiff / COUNTE_FRAC_MAX);
 }
 
 /*****************************************************************************/
@@ -186,338 +229,266 @@ int32_t RTT_Init(L2_RTT_FSM** rttFsmPtr){
 
 /*****************************************************************************/
 /**
- * @brief   Execute a Time-Sync Process.
+ * @brief   Update RTT FSM accordingly.
  *
  * @param   instPtr	a pointer to linklayer layer instance
+ * @param	torType tx/rx mode type
  *
- * @return  SUCCESS Time-Sync Process Done.
- *          FAILURE Time-Sync Process end with error.
+ * @return  SUCCESS 
+ * 			error code
  *
  ******************************************************************************/
-int32_t RTT_Proc(L2_INST* instPtr) {
+int32_t RTT_FsmUpdate(L2_INST* instPtr, L2_TOR_TYPE torType) {
 	ASSERT(instPtr != NULL);
-	L2_RTT_FSM* rttFsmPtr = L2_GetRttDesc(instPtr);
+	L2_RTT_FSM* rttFsmPtr = L2_GetRttFsm(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");
+	DBG_INFO("-------- RTT FSM Update --------\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;
+	if(L2_GetNodeType(instPtr) == HEAD_NODE){
+		switch(rttFsmPtr->rttStatge) {
 		case RTT_REQ1 :
-			if(nextTor == RTT_SEND){
+			if(torType == L2_TOR_TX){
 				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);
+				// Update Tt1
+				RTT_GetFrameStart(instPtr, &(rttRecord->Tt1), torType);
+				DBG_INFO("Tt1-timeL:%u.\n", rttRecord->Tt1.int_part);
+				DBG_INFO("Tt1-timeH:%u.\n", rttRecord->Tt1.frac_part);
+
+				// Prepare data for sending...
+				rttFsmPtr->timeStamp = rttRecord->Tt1;
+				rttFsmPtr->doppler = RTT_DOPPLER_NONE;
 				RTT_StageUpdate(rttFsmPtr, RTT_RSP1);
 			}
 			else{
-				DBG_INFO("Skip RTT slot, [Stage: %u].\n", rttFsmPtr->rttStatge.next);
+				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStatge);
 			}
 			break;
 		case RTT_RSP1 :
-			if(lastTor == RTT_RCV){
+			if(torType == L2_TOR_RX){
 				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);
-
+					// Update doppler
+					rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
+					// Update Tr2
+					RTT_GetFrameStart(instPtr, &(rttRecord->Tr2), torType);
 					// 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);
+					RTT_CalculateTimeGap(&(rttRecord->Tt1), &(rttRecord->Tr2), &(rttRecord->TroundA));
 				}
 				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);
+					rttRecord->Tr2 = RTT_TIME_NONE;
+					rttRecord->TroundA = RTT_TIME_NONE;
+					DBG_WARNING("RTT RSP-1 had not been received [Stage: %d].\n", rttFsmPtr->rttStatge);
+					DBG_WARNING("Set Tr2 dopplerR2 and TroundA to dummy value.\n");
 				}
+				DBG_INFO("dopplerR2:%08X.\n", rttRecord->dopplerR2);
+				DBG_INFO("Tr2-timeL:%u.\n", rttRecord->Tr2.frac_part);
+				DBG_INFO("Tr2-timeH:%u.\n", rttRecord->Tr2.int_part);
+				DBG_INFO("TroundA-timeL:%08X.\n", rttRecord->TroundA.frac_part);
+				DBG_INFO("TroundA-timeH:%08X.\n", rttRecord->TroundA.int_part);
 				RTT_StageUpdate(rttFsmPtr, RTT_REQ2);
 			}
 			else{
-				DBG_INFO("Skip RTT slot, [Stage: %u].\n", rttFsmPtr->rttStatge.next);
+				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStatge);
 			}
 			break;
 		case RTT_REQ2 :
-			if(nextTor == RTT_SEND){
+			if(torType == L2_TOR_TX){
 				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);
+				// Update Tt3
+				RTT_GetFrameStart(instPtr, &(rttRecord->Tt3), torType);
 
 				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);
+					RTT_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr2), &(rttRecord->TreplyA));
 				}
 				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);
+					rttRecord->TreplyA = RTT_TIME_NONE;
+					DBG_WARNING("RTT RSP-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStatge);
+					DBG_WARNING("Set Tt3 and TreplyA to dummy value.\n");
 				}
+				DBG_INFO("Tt3-timeL:%08X.\n", rttRecord->Tt3.frac_part);
+				DBG_INFO("Tt3-timeH:%08X.\n", rttRecord->Tt3.int_part);
+				DBG_INFO("TreplyA-timeL:%08X.\n", rttRecord->TreplyA.frac_part);
+				DBG_INFO("TreplyA-timeH:%08X.\n", rttRecord->TreplyA.int_part);
+
+				// Prepare data for sending.
+				rttFsmPtr->timeStamp = rttRecord->TroundA;
+				rttFsmPtr->doppler = rttRecord->dopplerR2;
 				RTT_StageUpdate(rttFsmPtr, RTT_RSP2);
 			}
 			else{
-				DBG_INFO("Skip RTT slot, [Stage: %u].\n", rttFsmPtr->rttStatge.next);
+				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStatge);
 			}
 			break;
 		case RTT_RSP2 :
-			if(lastTor == RTT_RCV){
+			if(torType == L2_TOR_RX){
 				if(L2_isDataAvail(instPtr) == true){
 					DBG_INFO("RTT RSP-2 have been received.\n");
 					rttFsmPtr->isR4Valid = true;
-					// Calculate doppler
+					// Update 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);
+					// Update Tr4
+					RTT_GetFrameStart(instPtr, &(rttRecord->Tr4), torType);
+					// Calculate TroundA
+					RTT_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr4), &(rttRecord->TroundA));
 				}
 				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);
+					rttRecord->dopplerR4 = RTT_DOPPLER_NONE;
+					rttRecord->Tr4 = RTT_TIME_NONE;
+					rttRecord->TroundA = RTT_TIME_NONE;
+					DBG_WARNING("RTT RSP-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStatge);
+					DBG_WARNING("Set Tr4 dopplerR4 and TroundA to dummy value.\n");
+					
 				}
+				DBG_INFO("dopplerR4:%08X.\n", rttRecord->dopplerR4);
+				DBG_INFO("Tr4-timeL:%08X.\n", rttRecord->Tr4.frac_part);
+				DBG_INFO("Tr4-timeH:%08X.\n", rttRecord->Tr4.int_part);
+				DBG_INFO("TroundA-timeL:%08X.\n", rttRecord->TroundA.frac_part);
+				DBG_INFO("TroundA-timeH:%08X.\n", rttRecord->TroundA.int_part);
 				RTT_StageUpdate(rttFsmPtr, RTT_NONE_STAGE);
 				DBG_INFO("RTT procedure end...\n");
 			}
+			else{
+				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStatge);
+			}
 			break;
 		default :
+			DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStatge);
 			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;
+		switch(rttFsmPtr->rttStatge) {
 		case RTT_REQ1 :
-			if(lastTor == RTT_RCV){
+			if(torType == L2_TOR_RX){
 				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);
+					// Update doppler
+					rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
+					// Update Tr1
+					RTT_GetFrameStart(instPtr, &(rttRecord->Tr1), torType);
 				}
 				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);
+					rttRecord->Tr1 = RTT_TIME_NONE;
+					DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStatge);
 					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);
 				}
+				DBG_INFO("dopplerR1:%08X.\n", rttRecord->dopplerR1);
+				DBG_INFO("Tr1-timeL:%08X.\n", rttRecord->Tr1.frac_part);
+				DBG_INFO("Tr1-timeH:%08X.\n", rttRecord->Tr1.int_part);
 				RTT_StageUpdate(rttFsmPtr, RTT_RSP1);
 			}
+			else{
+				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStatge);
+			}
 			break;
 		case RTT_RSP1 :
-			if(nextTor == RTT_SEND){
+			if(torType == L2_TOR_TX){
 				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);
-
+				// Update Tt2
+				RTT_GetFrameStart(instPtr, &(rttRecord->Tt2), torType);
 				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);
+					RTT_CalculateTimeGap(&(rttRecord->Tr1), &(rttRecord->Tt2), &(rttRecord->TreplyB));
 				}
 				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);
+					rttRecord->TreplyB = RTT_TIME_NONE;
+					DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStatge);
 					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);
 				}
+				DBG_INFO("Tt2-timeL:%u.\n", rttRecord->Tt2.frac_part);
+				DBG_INFO("Tt2-timeH:%u.\n", rttRecord->Tt2.int_part);
+				DBG_INFO("TreplyB-timeL:%08X.\n", rttRecord->TreplyB.frac_part);
+				DBG_INFO("TreplyB-timeH:%08X.\n", rttRecord->TreplyB.int_part);
+
+				// Prepare data for sending.
+				rttFsmPtr->timeStamp = rttRecord->TreplyB;
+				rttFsmPtr->doppler = rttRecord->dopplerR1;
 				RTT_StageUpdate(rttFsmPtr, RTT_REQ2);
 			}
+			else{
+				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStatge);
+			}
 			break;
 		case RTT_REQ2 :
-			if(lastTor == RTT_RCV){
+			if(torType == L2_TOR_RX){
 				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);
-
+					// Update doppler
+					rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
+					// Update Tr3
+					RTT_GetFrameStart(instPtr, &(rttRecord->Tr3), torType);
 					// 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);
+					RTT_CalculateTimeGap(&(rttRecord->Tt2), &(rttRecord->Tr3), &(rttRecord->TroundB));
 				}
 				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);
+					rttRecord->TroundB = RTT_TIME_NONE;
+					DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStatge);
 					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);
 				}
+				DBG_INFO("dopplerR3:%08X.\n", rttRecord->dopplerR3);
+				DBG_INFO("Tr3-timeL:%u.\n", rttRecord->Tr3.frac_part);
+				DBG_INFO("Tr3-timeH:%u.\n", rttRecord->Tr3.int_part);
+				DBG_INFO("TroundB-timeL:%08X.\n", rttRecord->TroundB.frac_part);
+				DBG_INFO("TroundB-timeH:%08X.\n", rttRecord->TroundB.int_part);
 				RTT_StageUpdate(rttFsmPtr, RTT_RSP2);
 			}
+			else{
+				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStatge);
+			}
 			break;
 		case RTT_RSP2 :
-			if(nextTor == RTT_SEND){
+			if(torType == L2_TOR_TX){
 				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);
-
+				// Update Tt4
+				RTT_GetFrameStart(instPtr, &(rttRecord->Tt4), torType);
 				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");
+					RTT_CalculateTimeGap(&(rttRecord->Tr3), &(rttRecord->Tt4), &(rttRecord->TreplyB));
+					// TODO: implement a time adjustment.
 				}
 				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);
+					rttRecord->TreplyB = RTT_TIME_NONE;
+					DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStatge);
 					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);
 				}
+				DBG_INFO("Tt4-timeL:%u.\n", rttRecord->Tt4.frac_part);
+				DBG_INFO("Tt4-timeH:%u.\n", rttRecord->Tt4.int_part);
+				DBG_INFO("TreplyB-timeL:%08X.\n", rttRecord->TreplyB.frac_part);
+				DBG_INFO("TreplyB-timeH:%08X.\n", rttRecord->TreplyB.int_part);
+
+				// Prepare data for sending.
+				rttFsmPtr->timeStamp = rttRecord->TreplyB;
+				rttFsmPtr->doppler = rttRecord->dopplerR3;
 				RTT_StageUpdate(rttFsmPtr, RTT_NONE_STAGE);
+				DBG_INFO("RTT procedure end...\n");
+			}
+			else{
+				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStatge);
 			}
 			break;
-		default :
+		default:
+			DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStatge);
 			break;
 		}
 	}
-	return Status;
+	return SUCCESS;
 }