fix typo.

datalink814_ge/src/linklayer/linklayer814.c

@@ -20,20 +20,20 @@
 #include <string.h>
 #include "linklayer814.h"
 #include "linklayer814_types.h"
+#include "node_list.h"
 #include "msg_def.h"
 #include "../error.h"
-#include "../time_ctrl/time_ctrl.h"
-#include "../util/util.h"
 #include "../common.h"
+#include "../util/util.h"
+#include "../time_ctrl/time_ctrl.h"
 
 /************************** Constant Definitions *****************************/
 #define NET_START_TIME_INFTY UINT32_MAX
 /************************** Variable Definitions *****************************/
 /***************** Macros (Inline Functions) Definitions *********************/
-
 /**************************** Type Definitions *******************************/
-
 /************************** Function Prototypes ******************************/
+static void L2_RcdShift(L2_RCD *rcd, int32_t netxVal);
 static void UpdateMainSate(L2_INST *instPtr, L2_NODE_STATE newSate);
 static void L2_SetSlotNum(L2_INST *instPtr, uint32_t newSlotNum);
 static void L2_SetFrameNum(L2_INST *instPtr, uint32_t newFrameNum);
@@ -312,7 +312,7 @@ static L2_FRAME_TYPE L2_GetReceiveFrameType(L2_INST *instPtr)
 		return instPtr->frame.curr;
 	}
 
-	switch (instPtr->scanNetFSM->scanStage)
+	switch (instPtr->scanFSM->scanStage)
 	{
 	case L2_SCAN_INIT_STAGE:
 	case L2_SCAN_BMADVT_STAGE:
@@ -350,6 +350,7 @@ void L2_FrameStateUpdate(L2_INST *instPtr)
 	Phy_RetrieveRecvData(phyRef, instPtr->recvPktData);
 	instPtr->log.frameRecv++;
 
+	// Update frame state.
 	switch (L2_GetReceiveFrameType(instPtr))
 	{
 	case L2_INTRA_SCAN_FRAME:
@@ -606,7 +607,7 @@ uint32_t L2_GetFrameLen(L2_FRAME_TYPE l2FrameType)
 /**
  * @brief   Convert L2 frame type to L1 frame type
  *
- * @param   phyInstPtr  A pointer to physical layer control instance.
+ * @param   l2Frame
  *
  * @return  L1 frame type.
  *
@@ -818,24 +819,24 @@ int32_t Linklayer_Init(L2_INST **instPtr, const L2_CONFIG *config)
 		goto error;
 	}
 
-	Status = ScanNet_Init(&((*instPtr)->scanNetFSM));
+	Status = L2_Scan_Init(&((*instPtr)->scanFSM));
 	if (Status != SUCCESS)
 	{
-		DBG_ERROR("ScanNet_Init runs failed, with error code: %d.\n", Status);
+		DBG_ERROR("L2_Scan_Init runs failed, with error code: %d.\n", Status);
 		goto error;
 	}
 
-	Status = MgmtNet_Init(&((*instPtr)->mgmtNetFSM));
+	Status = L2_Mgmt_Init(&((*instPtr)->mgmtFSM));
 	if (Status != SUCCESS)
 	{
-		DBG_ERROR("MgmtNet_Init runs failed, with error code: %d.\n", Status);
+		DBG_ERROR("L2_Mgmt_Init runs failed, with error code: %d.\n", Status);
 		goto error;
 	}
 
-	Status = ServiceNet_Init(&((*instPtr)->serviceNetFSM));
+	Status = L2_Service_Init(&((*instPtr)->serviceFSM));
 	if (Status != SUCCESS)
 	{
-		DBG_ERROR("ServiceNet_Init runs failed, with error code: %d.\n", Status);
+		DBG_ERROR("L2_Service_Init runs failed, with error code: %d.\n", Status);
 		goto error;
 	}
 
@@ -923,9 +924,9 @@ int32_t Linklayer_Reconfig(L2_INST *instPtr, const L2_CONFIG *config)
 	// Reset according instance
 	L2_ClearSchedEventPool(instPtr->schdEventPool);
 	L2_NodeList_Clear(instPtr->nodeList);
-	ScanNet_Reset(instPtr->scanNetFSM);
+	L2_Scan_Reset(instPtr->scanFSM);
-	MgmtNet_Reset(instPtr->mgmtNetFSM);
+	L2_Mgmt_Reset(instPtr->mgmtFSM);
-	ServiceNet_Reset(instPtr->serviceNetFSM);
+	L2_Service_Reset(instPtr->serviceFSM);
 	RTT_Reset(instPtr->rttFSM);
 	return SUCCESS;
 }
@@ -951,9 +952,9 @@ void Linklayer_Destroy(L2_INST *instPtr)
 	// Destroy FSMs and nodeList
 	L2_DestroySchedEventPool(instPtr->schdEventPool);
 	L2_NodeList_Destroy(instPtr->nodeList);
-	ScanNet_Destroy(instPtr->scanNetFSM);
+	L2_Scan_Destroy(instPtr->scanFSM);
-	MgmtNet_Destroy(instPtr->mgmtNetFSM);
+	L2_Mgmt_Destroy(instPtr->mgmtFSM);
-	ServiceNet_Destroy(instPtr->serviceNetFSM);
+	L2_Service_Destroy(instPtr->serviceFSM);
 	RTT_Destroy(instPtr->rttFSM);
 	free(instPtr);
 }
@@ -1030,11 +1031,6 @@ int32_t Linklayer_EventHandler(L2_INST *instPtr, const L2_EVENT *l2Event)
 		// TODO: Clear ACK
 		break;
 	}
-	case L2_EV_MGMT_PROC_DONE:
-	{
-		DBG_INFO("Event L2_EV_MGMT_PROC_DONE asserted....\n");
-		break;
-	}
 	case L2_EV_NET_SETUP_DONE:
 	{
 		DBG_INFO("Event L2_EV_NETSETUP_DONE asserted....\n");
@@ -1066,8 +1062,17 @@ int32_t Linklayer_EventHandler(L2_INST *instPtr, const L2_EVENT *l2Event)
 	case L2_EV_LOST_CXN:
 	{
 		DBG_INFO("Event L2_EV_LOST_CXN asserted....\n");
+		if (instPtr->config.myType == L2_NODE_HEAD)
+		{
+			DBG_ERROR("Illegal event, only None_Head node can assert L2_EV_LOST_CXN.\n");
+			break;
+		}
 		DBG_WARNING("Lost connection with head node [%d].\n", L2_GetHeadId(instPtr));
+		L2_Scan_Reset(instPtr->scanFSM);
+		L2_Mgmt_Reset(instPtr->mgmtFSM);
+		L2_Service_Reset(instPtr->serviceFSM);
 		instPtr->cxn2Net = false;
+		UpdateMainSate(instPtr, L2_INIT_STATE);
 		break;
 	}
 	case L2_EV_LOST_NODE:
@@ -1081,8 +1086,8 @@ int32_t Linklayer_EventHandler(L2_INST *instPtr, const L2_EVENT *l2Event)
 		if (instPtr->nextState == L2_INIT_STATE)
 		{
 			// Reset scan net FMS for next scan process.
-			ScanNet_Reset(instPtr->scanNetFSM);
+			L2_Scan_Reset(instPtr->scanFSM);
-			UpdateMainSate(instPtr, L2_NORMAL_STATE);
+			UpdateMainSate(instPtr, L2_INIT_STATE);
 			instPtr->rtneConfig.rtneTable = normalTable;
 			instPtr->rtneConfig.rtnePrd = ARRAY_SIZE(normalTable);
 		}
@@ -1106,6 +1111,10 @@ int32_t Linklayer_EventHandler(L2_INST *instPtr, const L2_EVENT *l2Event)
 			instPtr->rtneConfig.rtneTable = interSeekTable;
 			instPtr->rtneConfig.rtnePrd = ARRAY_SIZE(interSeekTable);
 		}
+		else
+		{
+			DBG_WARNING("Unsupported routine state [%d], Continue withe previous routine table.\n", instPtr->nextState);
+		}
 		break;
 	}
 	case L2_ROUTINE_UPDATE:
@@ -1135,12 +1144,10 @@ int32_t Linklayer_EventHandler(L2_INST *instPtr, const L2_EVENT *l2Event)
  * @return  Status code.
  *
  ******************************************************************************/
-int mgmtFrameRecv = 0;
-int svcFrameRecv = 0;
 int32_t Linklayer_PktHandler(L2_INST *instPtr, L2_FRAME_TYPE frameType)
 {
 	ASSERT(instPtr != NULL);
-	int32_t Status = SUCCESS;
+	int32_t Status;
 
 	if (instPtr->dataAvail == false)
 	{
@@ -1155,7 +1162,7 @@ int32_t Linklayer_PktHandler(L2_INST *instPtr, L2_FRAME_TYPE frameType)
 	case L2_INTRA_BM_FRAME:
 	case L2_INTER_W2_ACQ_FRAME:
 	case L2_INTRA_W2_ACQ_FRAME:
-		Status = ScanNet_PktHandler(instPtr);
+		Status = L2_Scan_PktHandler(instPtr);
 		break;
 	case L2_INTER_MGMT_FRAME:
 	case L2_INTRA_MGMT_FRAME:
@@ -1176,8 +1183,7 @@ int32_t Linklayer_PktHandler(L2_INST *instPtr, L2_FRAME_TYPE frameType)
 				instPtr->mgmtMsgRecvCnt1++;
 			}
 		}
-		mgmtFrameRecv++;
+		Status = L2_Mgmt_PktHandler(instPtr);
-		Status = MgmtNet_PktHandler(instPtr);
 		break;
 	case L2_INTER_SVC_FRAME:
 	case L2_INTRA_SVC_FRAME:
@@ -1196,8 +1202,7 @@ int32_t Linklayer_PktHandler(L2_INST *instPtr, L2_FRAME_TYPE frameType)
 				instPtr->svcMsgRecvCnt1++;
 			}
 		}
-		svcFrameRecv++;
+		Status = L2_Service_PktHandler(instPtr);
-		Status = ServiceNet_PktHandler(instPtr);
 		break;
 	default:
 		DBG_WARNING("L2FrameHandler: UNKNOWN L2-FrameType %d\n", frameType);
@@ -1257,17 +1262,17 @@ static int32_t Linklayer_Routine(L2_INST *instPtr)
 	case L2_SLOT_INTRA_SCAN_BM:
 	case L2_SLOT_INTER_SCAN_2W:
 	case L2_SLOT_INTRA_SCAN_2W:
-		Status = ScanNet_Routine(instPtr, &rtneEntry);
+		Status = L2_Scan_Routine(instPtr, &rtneEntry);
 		break;
 	case L2_SLOT_INTER_SVC:
 	case L2_SLOT_INTRA_SVC:
-		Status = ServiceNet_Routine(instPtr, &rtneEntry);
+		Status = L2_Service_Routine(instPtr, &rtneEntry);
 		break;
 	case L2_SLOT_INTER_MGMT:
 	case L2_SLOT_INTRA_MGMT:
 	case L2_SLOT_INTER_TSYNC:
 	case L2_SLOT_INTRA_TSYNC:
-		Status = MgmtNet_Routine(instPtr, &rtneEntry);
+		Status = L2_Mgmt_Routine(instPtr, &rtneEntry);
 		break;
 	default:
 		DBG_ERROR("UNKNOWN slot type [%d].\n", rtneEntry.slotType);
@@ -1325,7 +1330,10 @@ int32_t Linklayer_Proc(L2_INST *instPtr)
 	// Handle frame received in last slot.
 	if (L2_isDataAvail(instPtr) == true)
 	{
-		Linklayer_PktHandler(instPtr, L2_GetLastFrameType(instPtr));
+		if (Linklayer_PktHandler(instPtr, L2_GetLastFrameType(instPtr)) != SUCCESS)
+		{
+			DBG_ERROR("Failed to handle last received frame: %ld.\n", instPtr->frame.last);
+		}
 	}
 
 	// Run Link layer routine, setup next slot procedure.
@@ -1344,9 +1352,10 @@ int32_t Linklayer_Proc(L2_INST *instPtr)
 		else
 		{
 			Phy_SetMode(instPtr->config.phyInstPtr, PHY_MODE_LISTEN);
-			Status = ScanNet_Proc(instPtr);
+			Status = L2_Scan_Proc(instPtr);
 		}
 	}
+	
 	// Check if previous runs has error occurred.
 	if (Status != SUCCESS)
 	{

datalink814_ge/src/linklayer/linklayer814.h

@@ -53,7 +53,7 @@ typedef struct
 	uint8_t bkpHeadId;
 	uint8_t netScale;
 	PHY_INST *phyInstPtr;
-	TIMECTRL_INST *tmcInstPtr;
+	TMC_INST *tmcInstPtr;
 } L2_CONFIG;
 
 typedef struct
@@ -106,9 +106,9 @@ typedef struct
 	L2_NODE_STATE mainState;   // the main state of link layer state
 	L2_NODE_STATE nextState;   // next state of link layer state, which will be applied to mainState every second.
 	L2_RTNE_CONFIG rtneConfig; // current routine table configuration and status.
-	L2_SCAN_FSM *scanNetFSM;   // FSM of scan net (link set up)
+	L2_SCAN_FSM *scanFSM;   // FSM of scan net (link set up)
-	L2_MGMT_FSM *mgmtNetFSM;   // FSM of management net (link management and time sync)
+	L2_MGMT_FSM *mgmtFSM;   // FSM of management net (link management and time sync)
-	L2_SVC_FSM *serviceNetFSM; // FSM of service net (data service)
+	L2_SVC_FSM *serviceFSM; // FSM of service net (data service)
 	L2_RTT_FSM *rttFSM;		   // FSM for RTT (time sync algorithm)
 	uint8_t *sendPktData;
 	uint8_t *recvPktData;
@@ -160,26 +160,26 @@ int32_t L2_CrcAppend(uint8_t *dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CR
 bool L2_CrcCheck(uint8_t *dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType);
 
 // Scan Net
-int32_t ScanNet_Init(L2_SCAN_FSM **scanFSMPtr);
+int32_t L2_Scan_Init(L2_SCAN_FSM **scanFSMPtr);
-void ScanNet_Reset(L2_SCAN_FSM *scanFSMPtr);
+void L2_Scan_Reset(L2_SCAN_FSM *scanFSMPtr);
-void ScanNet_Destroy(L2_SCAN_FSM *scanFSMPtr);
+void L2_Scan_Destroy(L2_SCAN_FSM *scanFSMPtr);
-int32_t ScanNet_Proc(L2_INST *instPtr);
+int32_t L2_Scan_Proc(L2_INST *instPtr);
-int32_t ScanNet_PktHandler(L2_INST *instPtr);
+int32_t L2_Scan_PktHandler(L2_INST *instPtr);
-int32_t ScanNet_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry);
+int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry);
 
 // Management Net
-int32_t MgmtNet_Init(L2_MGMT_FSM **mgmtFSM);
+int32_t L2_Mgmt_Init(L2_MGMT_FSM **mgmtFSM);
-void MgmtNet_Reset(L2_MGMT_FSM *mgmtFSM);
+void L2_Mgmt_Reset(L2_MGMT_FSM *mgmtFSM);
-void MgmtNet_Destroy(L2_MGMT_FSM *mgmtFSM);
+void L2_Mgmt_Destroy(L2_MGMT_FSM *mgmtFSM);
-int32_t MgmtNet_PktHandler(L2_INST *instPtr);
+int32_t L2_Mgmt_PktHandler(L2_INST *instPtr);
-int32_t MgmtNet_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry);
+int32_t L2_Mgmt_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry);
 
 // Service net
-int32_t ServiceNet_Init(L2_SVC_FSM **serviceFSM);
+int32_t L2_Service_Init(L2_SVC_FSM **serviceFSM);
-void ServiceNet_Reset(L2_SVC_FSM *serviceFSM);
+void L2_Service_Reset(L2_SVC_FSM *serviceFSM);
-void ServiceNet_Destroy(L2_SVC_FSM *serviceFSM);
+void L2_Service_Destroy(L2_SVC_FSM *serviceFSM);
-int32_t ServiceNet_PktHandler(L2_INST *instPtr);
+int32_t L2_Service_PktHandler(L2_INST *instPtr);
-int32_t ServiceNet_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *schedEntry);
+int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *schedEntry);
 
 // Miscellaneous
 uint8_t L2_GetNodeId(L2_INST *instPtr);
@@ -190,7 +190,6 @@ L2_NODE_STATE L2_GetMainState(L2_INST *instPtr);
 L2_RTT_FSM *L2_GetRttFsm(L2_INST *instPtr);
 bool L2_isDataAvail(L2_INST *instPtr);
 bool L2_isCxn2Net(L2_INST *instPtr);
-void L2_RcdShift(L2_RCD *rcd, int32_t netxVal);
 uint32_t L2_GetFrameLen(L2_FRAME_TYPE l2FrameType);
 uint32_t L2_GetLastFrameNum(L2_INST *instPtr);
 uint32_t L2_GetLastSlotNum(L2_INST *instPtr);

datalink814_ge/src/linklayer/linklayer814_types.h

@@ -19,8 +19,9 @@
 
 /***************************** Include Files *********************************/
 #include <stdbool.h>
-#include <inttypes.h>
+#include <stdint.h>
 #include "geometry/coord.h"
+#include "geometry/quaternion.h"
 #include "../phy_ctrl/phy_ctrl.h"
 /************************** Constant Definitions *****************************/
 // Common node ID definition
@@ -46,6 +47,7 @@
 #define L2_SLOTS_PER_SUBFRAME 20
 
 #define L2_NUM_SLOTS_NONE UINT32_MAX
+#define L2_NUM_SLOTS_100MS 20
 #define L2_NUM_SLOTS_20MS 4
 #define L2_NUM_SLOTS_10MS 2
 #define L2_NUM_SLOTS_5MS 1
@@ -91,6 +93,9 @@ typedef enum
 	L2_SEEK_STATE = 5
 } L2_NODE_STATE;
 
+/** @name Node information type definition
+ * The following types define the detail node info which needed by the link layer.
+ */
 typedef struct
 {
 	uint8_t nodeId;
@@ -100,6 +105,7 @@ typedef struct
 	uint8_t beamNo;
 	COORD pos;
 	COORD speed;
+	QUATERNION attitude;
 } L2_NODE_INFO;
 
 /** @name L2 event enum
@@ -109,18 +115,17 @@ typedef enum
 {
 	L2_EV_NONE = 0,
 	L2_EV_SCAN_PROC_DONE = 1,
-	L2_EV_MGMT_PROC_DONE = 2,
+	L2_EV_NET_SETUP_DONE = 2,
-	L2_EV_NET_SETUP_DONE = 3,
+	L2_EV_TIME_SYNC = 3,
-	L2_EV_TIME_SYNC = 4,
+	L2_EV_RTT_RECV_UPDATE = 4,
-	L2_EV_RTT_RECV_UPDATE = 5,
+	L2_EV_LINK_SET_REQ = 5,
-	L2_EV_LINK_SET_REQ = 6,
+	L2_EV_LOST_CXN = 6,
-	L2_EV_LOST_CXN = 7,
+	L2_EV_LOST_NODE = 7,
-	L2_EV_LOST_NODE = 8,
+	L2_EV_SET_FRAME_NUM = 8,
-	L2_EV_SET_FRAME_NUM = 9,
+	L2_EV_SET_SLOT_NUM = 9,
-	L2_EV_SET_SLOT_NUM = 10,
+	L2_EV_MAIN_STATE_UPDATE = 10,
-	L2_EV_MAIN_STATE_UPDATE = 11,
+	L2_ROUTINE_FINISH = 11,
-	L2_ROUTINE_FINISH = 12,
+	L2_ROUTINE_UPDATE = 12
-	L2_ROUTINE_UPDATE = 13
 } L2_EVENT_TYPE;
 
 /** @name L2 event
@@ -153,17 +158,6 @@ typedef enum
 	L2_SLOT_INTRA_TSYNC = 13,
 } L2_SLOT_TYPE;
 
-/** @name Frame types
- * A definition of different frame types.
- */
-typedef enum
-{
-	L2_CRC_NONE = 0,
-	L2_CRC16_1021 = 1,
-	L2_CRC16_8005 = 2,
-	L2_CRC8_31 = 3
-} L2_CRC_TYPE;
-
 /** @name Frame types
  * A definition of different frame types.
  */
@@ -184,6 +178,17 @@ typedef enum
 	L2_INTRA_SVC_FRAME = 12
 } L2_FRAME_TYPE;
 
+/** @name Frame types
+ * A definition of different frame types.
+ */
+typedef enum
+{
+	L2_CRC_NONE = 0,
+	L2_CRC16_1021 = 1,
+	L2_CRC16_8005 = 2,
+	L2_CRC8_31 = 3
+} L2_CRC_TYPE;
+
 /** @name Schedule subtable entry
  * A definition of Schedule subtable entry.
  */
@@ -244,7 +249,7 @@ typedef enum
 typedef struct
 {
 	bool isLocked;	// init acq done.
-	bool isBmRcved; // bm received.
+	bool isBmRecvd; // bm received.
 	bool isAcked;	// 2way acq done.
 	uint32_t scanCnt;
 	uint32_t bmTimeout;

datalink814_ge/src/linklayer/net_mgmt.c

@@ -550,14 +550,14 @@ failed:
  *          EALLOC Memory allocation error.
  *
  ******************************************************************************/
-int32_t MgmtNet_Init(L2_MGMT_FSM **mgmtFSM)
+int32_t L2_Mgmt_Init(L2_MGMT_FSM **mgmtFSM)
 {
 	(*mgmtFSM) = (L2_MGMT_FSM *)malloc(sizeof(L2_MGMT_FSM));
 	if (*mgmtFSM == NULL)
 	{
 		return EALLOC;
 	}
-	MgmtNet_Reset(*mgmtFSM);
+	L2_Mgmt_Reset(*mgmtFSM);
 	return SUCCESS;
 }
 
@@ -570,7 +570,7 @@ int32_t MgmtNet_Init(L2_MGMT_FSM **mgmtFSM)
  * @return	NONE
  *
  ******************************************************************************/
-void MgmtNet_Reset(L2_MGMT_FSM *mgmtFSM)
+void L2_Mgmt_Reset(L2_MGMT_FSM *mgmtFSM)
 {
 	mgmtFSM->isBisarProc = false;
 	mgmtFSM->isPendSlotAlloc = false;
@@ -607,7 +607,7 @@ void MgmtNet_Reset(L2_MGMT_FSM *mgmtFSM)
  * @return	NONE
  *
  ******************************************************************************/
-void MgmtNet_Destroy(L2_MGMT_FSM *mgmtFSM)
+void L2_Mgmt_Destroy(L2_MGMT_FSM *mgmtFSM)
 {
 	free(mgmtFSM);
 }
@@ -623,15 +623,15 @@ void MgmtNet_Destroy(L2_MGMT_FSM *mgmtFSM)
  *          Other Packed was dumped(not addressed to me, unrecognized frame type)
  *
  ******************************************************************************/
-int32_t MgmtNet_PktHandler(L2_INST *instPtr)
+int32_t L2_Mgmt_PktHandler(L2_INST *instPtr)
 {
 	ASSERT(instPtr != NULL);
 	int32_t Status;
 	uint32_t filedSize = 0;
 	uint64_t dataChunk = 0;
-	L2_MGMT_MSG_RECORD *mgmtMsgLog = &(instPtr->mgmtNetFSM->mgmtMsgLog);
+	L2_MGMT_MSG_RECORD *mgmtMsgLog = &(instPtr->mgmtFSM->mgmtMsgLog);
-	L2_RTT_MSG_RECORD *rttMsgRspLog = &(instPtr->mgmtNetFSM->rttMsgRspLog);
+	L2_RTT_MSG_RECORD *rttMsgRspLog = &(instPtr->mgmtFSM->rttMsgRspLog);
-	L2_RTT_MSG_RECORD *rttMsgReqLog = &(instPtr->mgmtNetFSM->rttMsgReqLog);
+	L2_RTT_MSG_RECORD *rttMsgReqLog = &(instPtr->mgmtFSM->rttMsgReqLog);
 
 	DBG_INFO("Management net handling packet...\n");
 
@@ -936,7 +936,7 @@ static bool IsTTLValid(NODE_ENTRY *entry)
  * @return  Slot has be used.
  *
  ******************************************************************************/
-int32_t MgmtNet_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
+int32_t L2_Mgmt_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
 {
 	ASSERT(instPtr != NULL);
 	int32_t Status;

datalink814_ge/src/linklayer/net_scan.c

@@ -27,7 +27,7 @@
 #include "../bit_array/bit_macros.h"
 
 /************************** Constant Definitions *****************************/
-#define BM_LSTEN_TIMEOUT 40
+#define BM_LISTEN_TIMEOUT 40
 
 #define SQUENCE_SCAN_SLOTS 0
 
@@ -88,8 +88,8 @@ static void BmrecvStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state)
 {
 	ASSERT(scanFsmPtr != NULL);
 	DBG_INFO("Update BmRecvState from [%d] to [%d].\n",
-			 scanFsmPtr->isBmRcved, state);
+			 scanFsmPtr->isBmRecvd, state);
-	scanFsmPtr->isBmRcved = state;
+	scanFsmPtr->isBmRecvd = state;
 	return;
 }
 
@@ -440,7 +440,7 @@ failed:
 static int32_t Scan_InitScanHandler(L2_INST *instPtr)
 {
 	ASSERT(instPtr != NULL);
-	L2_SCAN_FSM *scanFsmPtr = instPtr->scanNetFSM;
+	L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM;
 	DBG_INFO("** Netsetup - InitScan Procedure (Attempts: %u)**\n", scanFsmPtr->scanCnt);
 
 	if (scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE)
@@ -494,11 +494,11 @@ static int32_t Scan_InitScanHandler(L2_INST *instPtr)
  ******************************************************************************/
 static int32_t Scan_BmAdvtHandler(L2_INST *instPtr)
 {
-	L2_SCAN_FSM *scanFsmPtr = instPtr->scanNetFSM;
+	L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM;
 	DBG_INFO("** Netsetup - BmAdvt Procedure (Timeout: %u) **\n", scanFsmPtr->bmTimeout);
 
 	// if received a valid bm frame.
-	if (scanFsmPtr->isBmRcved == true)
+	if (scanFsmPtr->isBmRecvd == true)
 	{
 		scanFsmPtr->bmTimeout = 0; /* reset listen counter. */
 		DBG_INFO("Received Bm frame, skipping 10ms for turn around...\n");
@@ -510,7 +510,7 @@ static int32_t Scan_BmAdvtHandler(L2_INST *instPtr)
 	else
 	{
 		/* if time out reached, go back to init scan stage. */
-		if (scanFsmPtr->bmTimeout > BM_LSTEN_TIMEOUT)
+		if (scanFsmPtr->bmTimeout > BM_LISTEN_TIMEOUT)
 		{
 			if (scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE)
 			{
@@ -528,7 +528,7 @@ static int32_t Scan_BmAdvtHandler(L2_INST *instPtr)
 				return FAILURE;
 			}
 			DBG_WARNING("BM timeout, restart net scan from L2_SCAN_INIT_STAGE.\n");
-			ScanNet_Reset(scanFsmPtr);
+			L2_Scan_Reset(scanFsmPtr);
 		}
 		else
 		{
@@ -564,7 +564,7 @@ static int32_t Scan_BmAdvtHandler(L2_INST *instPtr)
  ******************************************************************************/
 static int32_t Scan_2WAcqHandler(L2_INST *instPtr)
 {
-	L2_SCAN_FSM *scanFsmPtr = instPtr->scanNetFSM;
+	L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM;
 	DBG_INFO("** Netsetup - 2WAcq Procedure (Timeout: %u) **\n", scanFsmPtr->w2acqTimeout);
 	L2_FRAME_TYPE frameType = L2_NONE_FRAME;
 
@@ -586,7 +586,7 @@ static int32_t Scan_2WAcqHandler(L2_INST *instPtr)
 	if (scanFsmPtr->w2acqTimeout >= WAY2_ACQ_SLOT_MAX)
 	{
 		DBG_WARNING("2WAcq timeout, restart net scan from L2_SCAN_INIT_STAGE.\n");
-		ScanNet_Reset(scanFsmPtr);
+		L2_Scan_Reset(scanFsmPtr);
 	}
 	else
 	{
@@ -610,11 +610,11 @@ static int32_t Scan_2WAcqHandler(L2_INST *instPtr)
  * 			error code
  *
  ******************************************************************************/
-int32_t ScanNet_PktHandler(L2_INST *instPtr)
+int32_t L2_Scan_PktHandler(L2_INST *instPtr)
 {
 	ASSERT(instPtr != NULL);
 	uint32_t Status;
-	L2_SCAN_FSM *scanFsmPtr = instPtr->scanNetFSM;
+	L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM;
 	uint64_t dataChunk;
 
 	DBG_INFO("Scan net handling packet...\n");
@@ -865,7 +865,7 @@ failed:
  *          EMAC_MEMORY_ALLOC_ERROR Memory allocate error.
  *
  ******************************************************************************/
-int32_t ScanNet_Init(L2_SCAN_FSM **scanFSMPtr)
+int32_t L2_Scan_Init(L2_SCAN_FSM **scanFSMPtr)
 {
 	*scanFSMPtr = (L2_SCAN_FSM *)malloc(sizeof(L2_SCAN_FSM));
 	if (*scanFSMPtr == NULL)
@@ -873,7 +873,7 @@ int32_t ScanNet_Init(L2_SCAN_FSM **scanFSMPtr)
 		DBG_ERROR("Failed to allocate memory for scanFsmPtr.\n");
 		return EALLOC;
 	}
-	ScanNet_Reset(*scanFSMPtr);
+	L2_Scan_Reset(*scanFSMPtr);
 	return (SUCCESS);
 }
 
@@ -886,11 +886,11 @@ int32_t ScanNet_Init(L2_SCAN_FSM **scanFSMPtr)
  * @return  NONE
  *
  ******************************************************************************/
-void ScanNet_Reset(L2_SCAN_FSM *scanFSMPtr)
+void L2_Scan_Reset(L2_SCAN_FSM *scanFSMPtr)
 {
 	ASSERT(scanFSMPtr != NULL);
 	scanFSMPtr->isLocked = false;
-	scanFSMPtr->isBmRcved = false;
+	scanFSMPtr->isBmRecvd = false;
 	scanFSMPtr->isAcked = false;
 	scanFSMPtr->scanCnt = 0;
 	scanFSMPtr->bmTimeout = 0;
@@ -924,7 +924,7 @@ void ScanNet_Reset(L2_SCAN_FSM *scanFSMPtr)
  * @return  NONE
  *
  ******************************************************************************/
-void ScanNet_Destroy(L2_SCAN_FSM *scanFSMPtr)
+void L2_Scan_Destroy(L2_SCAN_FSM *scanFSMPtr)
 {
 	free(scanFSMPtr);
 }
@@ -938,12 +938,12 @@ void ScanNet_Destroy(L2_SCAN_FSM *scanFSMPtr)
  * @return  used slot (5ms slot).
  *
  ******************************************************************************/
-int32_t ScanNet_Proc(L2_INST *instPtr)
+int32_t L2_Scan_Proc(L2_INST *instPtr)
 {
-	L2_SCAN_FSM *scanFsmPtr = instPtr->scanNetFSM;
+	L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM;
 	if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
 	{
-		DBG_ERROR("Only member node needs to execute ScanNet_Proc. \n");
+		DBG_ERROR("Only member node needs to execute L2_Scan_Proc. \n");
 		return FAILURE;
 	}
 	int32_t Status;
@@ -983,7 +983,7 @@ int32_t ScanNet_Proc(L2_INST *instPtr)
  *
  ******************************************************************************/
 int acqSend = 0;
-int32_t ScanNet_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
+int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
 {
 	ASSERT(instPtr != NULL);
 	L2_TOR_TYPE torState;
@@ -1087,7 +1087,7 @@ int32_t ScanNet_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
 				Status = FAILURE;
 				goto routine_err;
 			}
-			W2ackStateUpdate(instPtr->scanNetFSM, true);
+			W2ackStateUpdate(instPtr->scanFSM, true);
 			acqSend++;
 		}
 		/* Antenna setting (reserved.) */

datalink814_ge/src/linklayer/net_service.c

@@ -179,25 +179,45 @@ static int PduHeader_Packing(L2_INST *instPtr, BIT_ARRAY *bitarr, L2_PDU_HEADER_
  *          Other       Initialize failed.
  *
  ******************************************************************************/
-int32_t ServiceNet_Init(L2_SVC_FSM **serviceFSM)
+int32_t L2_Service_Init(L2_SVC_FSM **serviceFSM)
 {
-	(*serviceFSM) = (L2_SVC_FSM *)malloc(sizeof(L2_SVC_FSM));
+	*serviceFSM = (L2_SVC_FSM *)malloc(sizeof(L2_SVC_FSM));
 	if (*serviceFSM == NULL)
 	{
 		return EALLOC;
 	}
-	ServiceNet_Reset(*serviceFSM);
+	L2_Service_Reset(*serviceFSM);
 	return SUCCESS;
 }
 
-void ServiceNet_Reset(L2_SVC_FSM *serviceFSM)
+/*****************************************************************************/
+/**
+ * @brief  Reset ServiceNet FSM
+ *
+ * @param   scanFsmPtr a pointer to service net finite state machine
+ *
+ * @return  NONE
+ *
+ ******************************************************************************/
+void L2_Service_Reset(L2_SVC_FSM *serviceFSM)
 {
+	ASSERT(serviceFSM != NULL);
 	serviceFSM->isBisar = false;
 	serviceFSM->isBisarEven = false;
 	serviceFSM->bisarDst = L2_NONE_ID;
 }
 
-void ServiceNet_Destroy(L2_SVC_FSM *serviceFSM)
+
+/*****************************************************************************/
+/**
+ * @brief  Destroy ServiceNet FSM, free memory
+ *
+ * @param   scanFsmPtr a pointer to service net finite state machine
+ *
+ * @return  NONE
+ *
+ ******************************************************************************/
+void L2_Service_Destroy(L2_SVC_FSM *serviceFSM)
 {
 	free(serviceFSM);
 }
@@ -211,7 +231,7 @@ void ServiceNet_Destroy(L2_SVC_FSM *serviceFSM)
  *          other       error occurred.
  *
  ******************************************************************************/
-int32_t ServiceNet_PktHandler(L2_INST *instPtr)
+int32_t L2_Service_PktHandler(L2_INST *instPtr)
 {
 	// TODO:
 	DBG_INFO("Scan net handling packet...\n");
@@ -229,7 +249,7 @@ int32_t ServiceNet_PktHandler(L2_INST *instPtr)
  * @return  Slot has been used.
  *
  ******************************************************************************/
-int32_t ServiceNet_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
+int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
 {
 	ASSERT(instPtr != NULL);
 	int32_t Status;

datalink814_ge/src/linklayer/rtt.c

@@ -0,0 +1,472 @@
+/*****************************************************************************/
+/**
+ * Description: RTT algorithm.
+ *
+ * Author: FanHao
+ * Date: 2023-12-10 20:43
+ * LastEditors: FanHao
+ * LastEditTime: 2023-12-10 20:43
+ * Revisions: Rev 0.0
+ * History:
+ * [2023-12-10]: Initial version, split form time_sync.c
+ *
+ * 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 "../phy_ctrl/phy_ctrl.h"
+#include "../error.h"
+#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_STGE newStage);
+static void RTT_GetFrameStart(L2_INST *instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType);
+
+/*****************************************************************************/
+/**
+ * @brief   Update corrent time-sync for RTT stage
+ *
+ * @param   scanFsmPtr  A pointer to scan net fsm.
+ * @param   newSatge    New state.
+ *
+ * @return  NONE.
+ *
+ ******************************************************************************/
+static void RTT_StageUpdate(L2_RTT_FSM *rttFsmPtr, L2_RTT_STGE newStage)
+{
+	ASSERT(rttFsmPtr != NULL);
+	ASSERT(newStage >= RTT_NONE_STAGE && newStage <= RTT_RSP2);
+	DBG_INFO("RTT Stage Update: %d -> %d.\n", rttFsmPtr->rttStage, newStage);
+	rttFsmPtr->rttStage = newStage;
+}
+
+/*****************************************************************************/
+/**
+ * @brief   Reset RTT FSM
+ *
+ * @param   rttFsmPtr	a pointer to rtt FSM instance.
+ *
+ * @return  NONE
+ *
+ ******************************************************************************/
+void RTT_Reset(L2_RTT_FSM *rttFsmPtr)
+{
+	ASSERT(rttFsmPtr != NULL);
+	memset(&(rttFsmPtr->rttRecord), 0x00, sizeof(RTT_RECORD));
+	rttFsmPtr->rttStage = RTT_REQ1;
+	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);
+}
+
+/*****************************************************************************/
+/**
+ * @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_GetLastReceivedFrameSyncTimeStamp(phyRef, &timeStamp);
+		timeDiff = (uint64_t)timeStamp.frac_part;
+		timeDiff += ((uint64_t)timeStamp.int_part) * TIME_FRAC_MAX;
+		Phy_GetLastReceivedSlotStartTimeStamp(phyRef, tmStamp);
+		timeDiff -= ((uint64_t)tmStamp->int_part) * TIME_FRAC_MAX;
+		timeDiff -= (uint64_t)tmStamp->frac_part;
+		timeDiff = (int64_t)Phy_GetTimeComp(L2_L2frame2L1frame(L2_GetLastFrameType(instPtr))) - timeDiff;
+		if (timeDiff < 0)
+		{
+			timeDiff += TIME_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   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   Update RTT FSM accordingly.
+ *
+ * @param   instPtr	a pointer to linklayer layer instance
+ * @param	torType tx/rx mode type
+ *
+ * @return  SUCCESS
+ * 			error code
+ *
+ ******************************************************************************/
+int32_t RTT_FsmUpdate(L2_INST *instPtr, L2_TOR_TYPE torType)
+{
+	ASSERT(instPtr != NULL);
+	L2_RTT_FSM *rttFsmPtr = L2_GetRttFsm(instPtr);
+	RTT_RECORD *rttRecord = &(rttFsmPtr->rttRecord);
+	PHY_INST *phyRef = instPtr->config.phyInstPtr;
+
+	DBG_INFO("-------- RTT FSM Update --------\n");
+	// Stage trans
+	// For Head Node: Tx->Rx->Tx->Rx
+	if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
+	{
+		switch (rttFsmPtr->rttStage)
+		{
+		case RTT_REQ1:
+			if (torType == L2_TOR_TX)
+			{
+				DBG_INFO("Prepare data for RTT REQ-1\n");
+				// 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("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
+			}
+			break;
+		case RTT_RSP1:
+			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;
+					// Update doppler
+					rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
+					// Update Tr2
+					RTT_GetFrameStart(instPtr, &(rttRecord->Tr2), torType);
+					// Calculate TroundA
+					TimeCtrl_CalculateTimeGap(&(rttRecord->Tt1), &(rttRecord->Tr2), &(rttRecord->TroundA));
+				}
+				else
+				{
+					DBG_WARNING("RTT RSP-1 had not been received [Stage: %d].\n", rttFsmPtr->rttStage);
+					DBG_WARNING("Set Tr2 dopplerR2 and TroundA to dummy value.\n");
+					rttFsmPtr->isR2Valid = false;
+					rttRecord->dopplerR2 = RTT_DOPPLER_NONE;
+					rttRecord->Tr2 = RTT_TIME_NONE;
+					rttRecord->TroundA = RTT_TIME_NONE;
+				}
+				DBG_INFO("dopplerR2:%d.\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:%u.\n", rttRecord->TroundA.frac_part);
+				DBG_INFO("TroundA-timeH:%u.\n", rttRecord->TroundA.int_part);
+				RTT_StageUpdate(rttFsmPtr, RTT_REQ2);
+			}
+			else
+			{
+				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
+			}
+			break;
+		case RTT_REQ2:
+			if (torType == L2_TOR_TX)
+			{
+				DBG_INFO("Prepare data for RTT REQ-2\n");
+				// Update Tt3
+				RTT_GetFrameStart(instPtr, &(rttRecord->Tt3), torType);
+
+				if (rttFsmPtr->isR2Valid)
+				{
+					// Calculate reply A
+					TimeCtrl_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr2), &(rttRecord->TreplyA));
+				}
+				else
+				{
+					DBG_WARNING("RTT RSP-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
+					DBG_WARNING("Set TreplyA to dummy value.\n");
+					rttRecord->TreplyA = RTT_TIME_NONE;
+				}
+				DBG_INFO("Tt3-timeL:%u.\n", rttRecord->Tt3.frac_part);
+				DBG_INFO("Tt3-timeH:%u.\n", rttRecord->Tt3.int_part);
+				DBG_INFO("TreplyA-timeL:%u.\n", rttRecord->TreplyA.frac_part);
+				DBG_INFO("TreplyA-timeH:%u.\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("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
+			}
+			break;
+		case RTT_RSP2:
+			if (torType == L2_TOR_RX)
+			{
+				if (L2_isDataAvail(instPtr) == true)
+				{
+					DBG_INFO("RTT RSP-2 have been received.\n");
+					rttFsmPtr->isR4Valid = true;
+					// Update doppler
+					double doppler = Phy_GetCoarseFreqEst(phyRef);
+					rttRecord->dopplerR2 = (int32_t)doppler;
+					// Update Tr4
+					RTT_GetFrameStart(instPtr, &(rttRecord->Tr4), torType);
+					// Calculate TroundA
+					TimeCtrl_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr4), &(rttRecord->TroundA));
+				}
+				else
+				{
+					DBG_WARNING("RTT RSP-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
+					DBG_WARNING("Set Tr4 dopplerR4 and TroundA to dummy value.\n");
+					rttFsmPtr->isR4Valid = false;
+					rttRecord->dopplerR4 = RTT_DOPPLER_NONE;
+					rttRecord->Tr4 = RTT_TIME_NONE;
+					rttRecord->TroundA = RTT_TIME_NONE;
+				}
+				DBG_INFO("dopplerR4:%d.\n", rttRecord->dopplerR4);
+				DBG_INFO("Tr4-timeL:%u.\n", rttRecord->Tr4.frac_part);
+				DBG_INFO("Tr4-timeH:%u.\n", rttRecord->Tr4.int_part);
+				DBG_INFO("TroundA-timeL:%u.\n", rttRecord->TroundA.frac_part);
+				DBG_INFO("TroundA-timeH:%u.\n", rttRecord->TroundA.int_part);
+				RTT_StageUpdate(rttFsmPtr, RTT_REQ1);
+				DBG_INFO("RTT procedure end...\n");
+			}
+			else
+			{
+				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
+			}
+			break;
+		default:
+			DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStage);
+			break;
+		}
+	}
+	// For Member Node: Rx->Tx->Rx->Tx
+	else
+	{
+		switch (rttFsmPtr->rttStage)
+		{
+		case RTT_REQ1:
+			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;
+					// Update doppler
+					rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
+					// Update Tr1
+					RTT_GetFrameStart(instPtr, &(rttRecord->Tr1), torType);
+				}
+				else
+				{
+					DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
+					DBG_WARNING("Set dopplerR1 and Tr1 to dummy value.\n");
+					rttFsmPtr->isR1Valid = false;
+					rttRecord->dopplerR1 = RTT_DOPPLER_NONE;
+					rttRecord->Tr1 = RTT_TIME_NONE;
+				}
+				DBG_INFO("dopplerR1:%d.\n", rttRecord->dopplerR1);
+				DBG_INFO("Tr1-timeL:%u.\n", rttRecord->Tr1.frac_part);
+				DBG_INFO("Tr1-timeH:%u.\n", rttRecord->Tr1.int_part);
+				RTT_StageUpdate(rttFsmPtr, RTT_RSP1);
+			}
+			else
+			{
+				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
+			}
+			break;
+		case RTT_RSP1:
+			if (torType == L2_TOR_TX)
+			{
+				DBG_INFO("Prepare data for RTT RSP-1\n");
+				// Update Tt2
+				RTT_GetFrameStart(instPtr, &(rttRecord->Tt2), torType);
+				if (rttFsmPtr->isR1Valid)
+				{
+					// Calculate reply B
+					TimeCtrl_CalculateTimeGap(&(rttRecord->Tr1), &(rttRecord->Tt2), &(rttRecord->TreplyB));
+				}
+				else
+				{
+					rttRecord->TreplyB = RTT_TIME_NONE;
+					DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
+					DBG_WARNING("Set ReplayB to dummy value.\n");
+				}
+				DBG_INFO("Tt2-timeL:%u.\n", rttRecord->Tt2.frac_part);
+				DBG_INFO("Tt2-timeH:%u.\n", rttRecord->Tt2.int_part);
+				DBG_INFO("TreplyB-timeL:%u.\n", rttRecord->TreplyB.frac_part);
+				DBG_INFO("TreplyB-timeH:%u.\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->rttStage);
+			}
+			break;
+		case RTT_REQ2:
+			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;
+					// Update doppler
+					rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
+					// Update Tr3
+					RTT_GetFrameStart(instPtr, &(rttRecord->Tr3), torType);
+					// Calculate TroundB
+					TimeCtrl_CalculateTimeGap(&(rttRecord->Tt2), &(rttRecord->Tr3), &(rttRecord->TroundB));
+				}
+				else
+				{
+					DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
+					DBG_WARNING("Set dopplerR3 and ReplayB to dummy value.\n");
+					rttFsmPtr->isR3Valid = false;
+					rttRecord->dopplerR3 = RTT_DOPPLER_NONE;
+					rttRecord->TroundB = RTT_TIME_NONE;
+				}
+				DBG_INFO("dopplerR3:%d.\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:%u.\n", rttRecord->TroundB.frac_part);
+				DBG_INFO("TroundB-timeH:%u.\n", rttRecord->TroundB.int_part);
+				RTT_StageUpdate(rttFsmPtr, RTT_RSP2);
+			}
+			else
+			{
+				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
+			}
+			break;
+		case RTT_RSP2:
+			if (torType == L2_TOR_TX)
+			{
+				DBG_INFO("Prepare data for RTT RSP-2\n");
+				// Update Tt4
+				RTT_GetFrameStart(instPtr, &(rttRecord->Tt4), torType);
+				if (rttFsmPtr->isR3Valid)
+				{
+					// Calculate reply B
+					TimeCtrl_CalculateTimeGap(&(rttRecord->Tr3), &(rttRecord->Tt4), &(rttRecord->TreplyB));
+					// TODO: implement a time adjustment.
+				}
+				else
+				{
+					DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
+					DBG_WARNING("Set ReplayB to dummy value.\n");
+					rttRecord->TreplyB = RTT_TIME_NONE;
+				}
+				DBG_INFO("Tt4-timeL:%u.\n", rttRecord->Tt4.frac_part);
+				DBG_INFO("Tt4-timeH:%u.\n", rttRecord->Tt4.int_part);
+				DBG_INFO("TreplyB-timeL:%u.\n", rttRecord->TreplyB.frac_part);
+				DBG_INFO("TreplyB-timeH:%u.\n", rttRecord->TreplyB.int_part);
+
+				// Prepare data for sending.
+				rttFsmPtr->timeStamp = rttRecord->TreplyB;
+				rttFsmPtr->doppler = rttRecord->dopplerR3;
+				RTT_StageUpdate(rttFsmPtr, RTT_REQ1);
+				DBG_INFO("RTT procedure end...\n");
+			}
+			else
+			{
+				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
+			}
+			break;
+		default:
+			DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStage);
+			break;
+		}
+	}
+	return SUCCESS;
+}

datalink814_ge/src/linklayer/time_sync.c

@@ -20,27 +20,17 @@
 #include <string.h>
 #include <stdlib.h>
 #include "linklayer814.h"
-#include "rtt.h"
 #include "../time_ctrl/time_ctrl.h"
+#include "../phy_ctrl/phy_ctrl.h"
 #include "../error.h"
 #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_STGE newStage);
-static void RTT_GetFrameStart(L2_INST *instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType);
-
 /*****************************************************************************/
 /**
  * @brief    a slot alignment, base on physical layer information.
@@ -87,431 +77,3 @@ int32_t L2_TimeAlign(L2_INST *instPtr, uint32_t frameNum, uint32_t slotNum, bool
 	TimeCtrl_TimeAdjust(instPtr->config.tmcInstPtr, &slotTimerAdjmet, TM_ADJ_ADD);
 	return SUCCESS;
 }
-
-/*****************************************************************************/
-/**
- * @brief   Update corrent time-sync for RTT stage
- *
- * @param   scanFsmPtr  A pointer to scan net fsm.
- * @param   newSatge    New state.
- *
- * @return  NONE.
- *
- ******************************************************************************/
-static void RTT_StageUpdate(L2_RTT_FSM *rttFsmPtr, L2_RTT_STGE newStage)
-{
-	ASSERT(rttFsmPtr != NULL);
-	ASSERT(newStage >= RTT_NONE_STAGE && newStage <= RTT_RSP2);
-	DBG_INFO("RTT Stage Update: %d -> %d.\n", rttFsmPtr->rttStage, newStage);
-	rttFsmPtr->rttStage = newStage;
-}
-
-/*****************************************************************************/
-/**
- * @brief   Reset RTT FSM
- *
- * @param   rttFsmPtr	a pointer to rtt FSM instance.
- *
- * @return  NONE
- *
- ******************************************************************************/
-void RTT_Reset(L2_RTT_FSM *rttFsmPtr)
-{
-	ASSERT(rttFsmPtr != NULL);
-	memset(&(rttFsmPtr->rttRecord), 0x00, sizeof(RTT_RECORD));
-	rttFsmPtr->rttStage = RTT_REQ1;
-	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);
-}
-
-/*****************************************************************************/
-/**
- * @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_GetLastReceivedFrameSyncTimeStamp(phyRef, &timeStamp);
-		timeDiff = (uint64_t)timeStamp.frac_part;
-		timeDiff += ((uint64_t)timeStamp.int_part) * TIME_FRAC_MAX;
-		Phy_GetLastReceivedSlotStartTimeStamp(phyRef, tmStamp);
-		timeDiff -= ((uint64_t)tmStamp->int_part) * TIME_FRAC_MAX;
-		timeDiff -= (uint64_t)tmStamp->frac_part;
-		timeDiff = (int64_t)Phy_GetTimeComp(L2_L2frame2L1frame(L2_GetLastFrameType(instPtr))) - timeDiff;
-		if (timeDiff < 0)
-		{
-			timeDiff += TIME_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   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   Update RTT FSM accordingly.
- *
- * @param   instPtr	a pointer to linklayer layer instance
- * @param	torType tx/rx mode type
- *
- * @return  SUCCESS
- * 			error code
- *
- ******************************************************************************/
-int32_t RTT_FsmUpdate(L2_INST *instPtr, L2_TOR_TYPE torType)
-{
-	ASSERT(instPtr != NULL);
-	L2_RTT_FSM *rttFsmPtr = L2_GetRttFsm(instPtr);
-	RTT_RECORD *rttRecord = &(rttFsmPtr->rttRecord);
-	PHY_INST *phyRef = instPtr->config.phyInstPtr;
-
-	DBG_INFO("-------- RTT FSM Update --------\n");
-	// Stage trans
-	// For Head Node: Tx->Rx->Tx->Rx
-	if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
-	{
-		switch (rttFsmPtr->rttStage)
-		{
-		case RTT_REQ1:
-			if (torType == L2_TOR_TX)
-			{
-				DBG_INFO("Prepare data for RTT REQ-1\n");
-				// 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("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
-			}
-			break;
-		case RTT_RSP1:
-			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;
-					// Update doppler
-					rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
-					// Update Tr2
-					RTT_GetFrameStart(instPtr, &(rttRecord->Tr2), torType);
-					// Calculate TroundA
-					TimeCtrl_CalculateTimeGap(&(rttRecord->Tt1), &(rttRecord->Tr2), &(rttRecord->TroundA));
-				}
-				else
-				{
-					DBG_WARNING("RTT RSP-1 had not been received [Stage: %d].\n", rttFsmPtr->rttStage);
-					DBG_WARNING("Set Tr2 dopplerR2 and TroundA to dummy value.\n");
-					rttFsmPtr->isR2Valid = false;
-					rttRecord->dopplerR2 = RTT_DOPPLER_NONE;
-					rttRecord->Tr2 = RTT_TIME_NONE;
-					rttRecord->TroundA = RTT_TIME_NONE;
-				}
-				DBG_INFO("dopplerR2:%d.\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:%u.\n", rttRecord->TroundA.frac_part);
-				DBG_INFO("TroundA-timeH:%u.\n", rttRecord->TroundA.int_part);
-				RTT_StageUpdate(rttFsmPtr, RTT_REQ2);
-			}
-			else
-			{
-				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
-			}
-			break;
-		case RTT_REQ2:
-			if (torType == L2_TOR_TX)
-			{
-				DBG_INFO("Prepare data for RTT REQ-2\n");
-				// Update Tt3
-				RTT_GetFrameStart(instPtr, &(rttRecord->Tt3), torType);
-
-				if (rttFsmPtr->isR2Valid)
-				{
-					// Calculate reply A
-					TimeCtrl_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr2), &(rttRecord->TreplyA));
-				}
-				else
-				{
-					DBG_WARNING("RTT RSP-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
-					DBG_WARNING("Set TreplyA to dummy value.\n");
-					rttRecord->TreplyA = RTT_TIME_NONE;
-				}
-				DBG_INFO("Tt3-timeL:%u.\n", rttRecord->Tt3.frac_part);
-				DBG_INFO("Tt3-timeH:%u.\n", rttRecord->Tt3.int_part);
-				DBG_INFO("TreplyA-timeL:%u.\n", rttRecord->TreplyA.frac_part);
-				DBG_INFO("TreplyA-timeH:%u.\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("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
-			}
-			break;
-		case RTT_RSP2:
-			if (torType == L2_TOR_RX)
-			{
-				if (L2_isDataAvail(instPtr) == true)
-				{
-					DBG_INFO("RTT RSP-2 have been received.\n");
-					rttFsmPtr->isR4Valid = true;
-					// Update doppler
-					double doppler = Phy_GetCoarseFreqEst(phyRef);
-					rttRecord->dopplerR2 = (int32_t)doppler;
-					// Update Tr4
-					RTT_GetFrameStart(instPtr, &(rttRecord->Tr4), torType);
-					// Calculate TroundA
-					TimeCtrl_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr4), &(rttRecord->TroundA));
-				}
-				else
-				{
-					DBG_WARNING("RTT RSP-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
-					DBG_WARNING("Set Tr4 dopplerR4 and TroundA to dummy value.\n");
-					rttFsmPtr->isR4Valid = false;
-					rttRecord->dopplerR4 = RTT_DOPPLER_NONE;
-					rttRecord->Tr4 = RTT_TIME_NONE;
-					rttRecord->TroundA = RTT_TIME_NONE;
-				}
-				DBG_INFO("dopplerR4:%d.\n", rttRecord->dopplerR4);
-				DBG_INFO("Tr4-timeL:%u.\n", rttRecord->Tr4.frac_part);
-				DBG_INFO("Tr4-timeH:%u.\n", rttRecord->Tr4.int_part);
-				DBG_INFO("TroundA-timeL:%u.\n", rttRecord->TroundA.frac_part);
-				DBG_INFO("TroundA-timeH:%u.\n", rttRecord->TroundA.int_part);
-				RTT_StageUpdate(rttFsmPtr, RTT_REQ1);
-				DBG_INFO("RTT procedure end...\n");
-			}
-			else
-			{
-				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
-			}
-			break;
-		default:
-			DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStage);
-			break;
-		}
-	}
-	// For Member Node: Rx->Tx->Rx->Tx
-	else
-	{
-		switch (rttFsmPtr->rttStage)
-		{
-		case RTT_REQ1:
-			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;
-					// Update doppler
-					rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
-					// Update Tr1
-					RTT_GetFrameStart(instPtr, &(rttRecord->Tr1), torType);
-				}
-				else
-				{
-					DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
-					DBG_WARNING("Set dopplerR1 and Tr1 to dummy value.\n");
-					rttFsmPtr->isR1Valid = false;
-					rttRecord->dopplerR1 = RTT_DOPPLER_NONE;
-					rttRecord->Tr1 = RTT_TIME_NONE;
-				}
-				DBG_INFO("dopplerR1:%d.\n", rttRecord->dopplerR1);
-				DBG_INFO("Tr1-timeL:%u.\n", rttRecord->Tr1.frac_part);
-				DBG_INFO("Tr1-timeH:%u.\n", rttRecord->Tr1.int_part);
-				RTT_StageUpdate(rttFsmPtr, RTT_RSP1);
-			}
-			else
-			{
-				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
-			}
-			break;
-		case RTT_RSP1:
-			if (torType == L2_TOR_TX)
-			{
-				DBG_INFO("Prepare data for RTT RSP-1\n");
-				// Update Tt2
-				RTT_GetFrameStart(instPtr, &(rttRecord->Tt2), torType);
-				if (rttFsmPtr->isR1Valid)
-				{
-					// Calculate reply B
-					TimeCtrl_CalculateTimeGap(&(rttRecord->Tr1), &(rttRecord->Tt2), &(rttRecord->TreplyB));
-				}
-				else
-				{
-					rttRecord->TreplyB = RTT_TIME_NONE;
-					DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
-					DBG_WARNING("Set ReplayB to dummy value.\n");
-				}
-				DBG_INFO("Tt2-timeL:%u.\n", rttRecord->Tt2.frac_part);
-				DBG_INFO("Tt2-timeH:%u.\n", rttRecord->Tt2.int_part);
-				DBG_INFO("TreplyB-timeL:%u.\n", rttRecord->TreplyB.frac_part);
-				DBG_INFO("TreplyB-timeH:%u.\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->rttStage);
-			}
-			break;
-		case RTT_REQ2:
-			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;
-					// Update doppler
-					rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef);
-					// Update Tr3
-					RTT_GetFrameStart(instPtr, &(rttRecord->Tr3), torType);
-					// Calculate TroundB
-					TimeCtrl_CalculateTimeGap(&(rttRecord->Tt2), &(rttRecord->Tr3), &(rttRecord->TroundB));
-				}
-				else
-				{
-					DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
-					DBG_WARNING("Set dopplerR3 and ReplayB to dummy value.\n");
-					rttFsmPtr->isR3Valid = false;
-					rttRecord->dopplerR3 = RTT_DOPPLER_NONE;
-					rttRecord->TroundB = RTT_TIME_NONE;
-				}
-				DBG_INFO("dopplerR3:%d.\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:%u.\n", rttRecord->TroundB.frac_part);
-				DBG_INFO("TroundB-timeH:%u.\n", rttRecord->TroundB.int_part);
-				RTT_StageUpdate(rttFsmPtr, RTT_RSP2);
-			}
-			else
-			{
-				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
-			}
-			break;
-		case RTT_RSP2:
-			if (torType == L2_TOR_TX)
-			{
-				DBG_INFO("Prepare data for RTT RSP-2\n");
-				// Update Tt4
-				RTT_GetFrameStart(instPtr, &(rttRecord->Tt4), torType);
-				if (rttFsmPtr->isR3Valid)
-				{
-					// Calculate reply B
-					TimeCtrl_CalculateTimeGap(&(rttRecord->Tr3), &(rttRecord->Tt4), &(rttRecord->TreplyB));
-					// TODO: implement a time adjustment.
-				}
-				else
-				{
-					DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
-					DBG_WARNING("Set ReplayB to dummy value.\n");
-					rttRecord->TreplyB = RTT_TIME_NONE;
-				}
-				DBG_INFO("Tt4-timeL:%u.\n", rttRecord->Tt4.frac_part);
-				DBG_INFO("Tt4-timeH:%u.\n", rttRecord->Tt4.int_part);
-				DBG_INFO("TreplyB-timeL:%u.\n", rttRecord->TreplyB.frac_part);
-				DBG_INFO("TreplyB-timeH:%u.\n", rttRecord->TreplyB.int_part);
-
-				// Prepare data for sending.
-				rttFsmPtr->timeStamp = rttRecord->TreplyB;
-				rttFsmPtr->doppler = rttRecord->dopplerR3;
-				RTT_StageUpdate(rttFsmPtr, RTT_REQ1);
-				DBG_INFO("RTT procedure end...\n");
-			}
-			else
-			{
-				DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
-			}
-			break;
-		default:
-			DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStage);
-			break;
-		}
-	}
-	return SUCCESS;
-}

datalink814_ge/src/main.c

@@ -49,7 +49,7 @@ L2_CONFIG l2Config = {
 	.phyInstPtr = NULL,
 	.tmcInstPtr = NULL};
 
-TIMECTRL_CONFIG tmcConfig = {
+TMC_CONFIG tmcConfig = {
 	.baseAddr = TIME_CONTROLLER_BASEADDR,
 	.reset = 10};
 
@@ -73,7 +73,7 @@ void MPC_IntrHandler(void *callBackRef);
 #endif
 
 /*********************** Driver Instance Definitions **************************/
-TIMECTRL_INST *tmcInstPtr = NULL;
+TMC_INST *tmcInstPtr = NULL;
 PHY_INST *phyInstPtr = NULL;
 L2_INST *linkInstPtr = NULL;
 static XScuGic gicInst; // General interrupt controller intance
@@ -449,12 +449,12 @@ int main(void)
 			if (linkInstPtr->buffSwitch == 0)
 			{
 				MPC_StateUpload(mpcInstPtr, linkInstPtr->mgmtMsgRecvBuff1, linkInstPtr->mgmtMsgRecvCnt1,
-								linkInstPtr->svcMsgRecvBuff1, linkInstPtr->svcMsgRecvCnt1, &(linkInstPtr->mgmtNetFSM->mgmtMsgLog));
+								linkInstPtr->svcMsgRecvBuff1, linkInstPtr->svcMsgRecvCnt1, &(linkInstPtr->mgmtFSM->mgmtMsgLog));
 			}
 			else
 			{
 				MPC_StateUpload(mpcInstPtr, linkInstPtr->mgmtMsgRecvBuff0, linkInstPtr->mgmtMsgRecvCnt0,
-								linkInstPtr->svcMsgRecvBuff0, linkInstPtr->svcMsgRecvCnt0, &(linkInstPtr->mgmtNetFSM->mgmtMsgLog));
+								linkInstPtr->svcMsgRecvBuff0, linkInstPtr->svcMsgRecvCnt0, &(linkInstPtr->mgmtFSM->mgmtMsgLog));
 			}
 		}
 	}

datalink814_ge/src/phy_ctrl/phy_ctrl.c

@@ -556,12 +556,7 @@ int32_t Phy_SetupNextSlot(PHY_INST *instPtr, PHY_TOR_TYPE txrxMode,
 	default:
 		return EPARAM;
 	}
-	if (instPtr->isPresetHop == false)
+	
-	{
-		hopPattern = hopPattern_Config;
-	}
-	else
-	{
 	switch (frameType)
 	{
 	case L1_2KBPS_20MS_FRAME:
@@ -608,6 +603,9 @@ int32_t Phy_SetupNextSlot(PHY_INST *instPtr, PHY_TOR_TYPE txrxMode,
 		return EPARAM;
 		break;
 	}
+	if (instPtr->isPresetHop == false)
+	{
+		hopPattern = hopPattern_Config;
 	}
 
 	PhyWrite(instPtr, PHY_MODE_CTRL, phyModeCtrl | PHY_MODE_CTRL_CRC_EN);
@@ -767,6 +765,21 @@ double Phy_GetCoarseFreqEst(PHY_INST *instPtr)
 	return freqEst;
 }
 
+/*****************************************************************************/
+/**
+ * @brief   Get fine frequency estimation of last valid frame.
+ *
+ * @param   instPtr  a pointer to physical instance
+ *
+ * @return  frequency estimation
+ *
+ ******************************************************************************/
+double Phy_GetFineFreqEst(PHY_INST *instPtr)
+{
+	// TODO:
+	return 0.0;
+}
+
 /*****************************************************************************/
 /**
  * @brief   Setup a frequency hopping pattern.

datalink814_ge/src/time_ctrl/time_ctrl.c

@@ -51,12 +51,12 @@
  *          FAILURE Initialization failed
  *
  ******************************************************************************/
-int32_t TimeCtrl_Init(TIMECTRL_INST **instPtr, TIMECTRL_CONFIG *config)
+int32_t TimeCtrl_Init(TMC_INST **instPtr, TMC_CONFIG *config)
 {
 	ASSERT(config != NULL);
 	ASSERT(instPtr != NULL);
-	(*instPtr) = malloc(sizeof(TIMECTRL_INST));
+	(*instPtr) = malloc(sizeof(TMC_INST));
-	memcpy(&((*instPtr)->config), config, sizeof(TIMECTRL_CONFIG));
+	memcpy(&((*instPtr)->config), config, sizeof(TMC_CONFIG));
 	(*instPtr)->modelVer = TMC_Read((*instPtr), VERSION_REG);
 	return SUCCESS;
 }
@@ -70,7 +70,7 @@ int32_t TimeCtrl_Init(TIMECTRL_INST **instPtr, TIMECTRL_CONFIG *config)
  * @return  NONE
  *
  ******************************************************************************/
-void TimeCtrl_Destroy(TIMECTRL_INST *instPtr)
+void TimeCtrl_Destroy(TMC_INST *instPtr)
 {
 	free(instPtr);
 }
@@ -84,7 +84,7 @@ void TimeCtrl_Destroy(TIMECTRL_INST *instPtr)
  * @return  timer controller version.
  *
  ******************************************************************************/
-uint32_t TimeCtrl_GetVersion(TIMECTRL_INST *instPtr)
+uint32_t TimeCtrl_GetVersion(TMC_INST *instPtr)
 {
 	ASSERT(instPtr != NULL);
 	return TMC_Read(instPtr, VERSION_REG);
@@ -100,7 +100,7 @@ uint32_t TimeCtrl_GetVersion(TIMECTRL_INST *instPtr)
  *          FAILURE Initialization failed
  *
  ******************************************************************************/
-void TimeCtrl_Reset(TIMECTRL_INST *instPtr)
+void TimeCtrl_Reset(TMC_INST *instPtr)
 {
 	ASSERT(instPtr != NULL);
 	TMC_Write(instPtr, CNTSTEP_OPT, TM_RESET_CODE);
@@ -115,7 +115,7 @@ void TimeCtrl_Reset(TIMECTRL_INST *instPtr)
  * @return  NONE
  *
  ******************************************************************************/
-void TimeCtrl_ResetHigh(TIMECTRL_INST *instPtr)
+void TimeCtrl_ResetHigh(TMC_INST *instPtr)
 {
 	ASSERT(instPtr != NULL);
 	// TODO:
@@ -130,7 +130,7 @@ void TimeCtrl_ResetHigh(TIMECTRL_INST *instPtr)
  * @return  NONE
  *
  ******************************************************************************/
-void TimeCtrl_ResetLow(TIMECTRL_INST *instPtr)
+void TimeCtrl_ResetLow(TMC_INST *instPtr)
 {
 	ASSERT(instPtr != NULL);
 	// TODO:
@@ -145,7 +145,7 @@ void TimeCtrl_ResetLow(TIMECTRL_INST *instPtr)
  * @return  integer value
  *
  ******************************************************************************/
-inline TIME_INT TimeCtrl_GetIntCnt(TIMECTRL_INST *instPtr)
+inline TIME_INT TimeCtrl_GetIntCnt(TMC_INST *instPtr)
 {
 	ASSERT(instPtr != NULL);
 	return TMC_Read(instPtr, TIME_INT_REG);
@@ -160,7 +160,7 @@ inline TIME_INT TimeCtrl_GetIntCnt(TIMECTRL_INST *instPtr)
  * @return  fractional value
  *
  ******************************************************************************/
-inline TIME_FRAC TimeCtrl_GetFracCnt(TIMECTRL_INST *instPtr)
+inline TIME_FRAC TimeCtrl_GetFracCnt(TMC_INST *instPtr)
 {
 	ASSERT(instPtr != NULL);
 	return TMC_Read(instPtr, TIME_FRAC_REG);
@@ -176,7 +176,7 @@ inline TIME_FRAC TimeCtrl_GetFracCnt(TIMECTRL_INST *instPtr)
  * @return  NONE
  *
  ******************************************************************************/
-void TimeCtrl_GetCurrentTime(TIMECTRL_INST *instPtr, TIME_WORD *tmWord)
+void TimeCtrl_GetCurrentTime(TMC_INST *instPtr, TIME_WORD *tmWord)
 {
 	ASSERT(instPtr != NULL);
 	ASSERT(tmWord != NULL);
@@ -194,7 +194,7 @@ void TimeCtrl_GetCurrentTime(TIMECTRL_INST *instPtr, TIME_WORD *tmWord)
  * @return  NONE.
  *
  ******************************************************************************/
-void TimeCtrl_GetCurrentStep(TIMECTRL_INST *instPtr, TIME_FRAC *tmFrac)
+void TimeCtrl_GetCurrentStep(TMC_INST *instPtr, TIME_FRAC *tmFrac)
 {
 	ASSERT(instPtr != NULL);
 	ASSERT(tmFrac != NULL);
@@ -212,7 +212,7 @@ void TimeCtrl_GetCurrentStep(TIMECTRL_INST *instPtr, TIME_FRAC *tmFrac)
  * @return  NONE.
  *
  ******************************************************************************/
-void TimeCtrl_TimeAdjust(TIMECTRL_INST *instPtr, const TIME_WORD *tmWord, TIMECTRL_ADJ_TYPE type)
+void TimeCtrl_TimeAdjust(TMC_INST *instPtr, const TIME_WORD *tmWord, TIMECTRL_ADJ_TYPE type)
 {
 	ASSERT(instPtr != NULL);
 	ASSERT(tmWord != NULL);
@@ -243,7 +243,7 @@ void TimeCtrl_TimeAdjust(TIMECTRL_INST *instPtr, const TIME_WORD *tmWord, TIMECT
  * @return  NONE.
  *
  ******************************************************************************/
-void TimeCtrl_SetOptimizeStep(TIMECTRL_INST *instPtr, TIME_FRAC *tmFrac)
+void TimeCtrl_SetOptimizeStep(TMC_INST *instPtr, TIME_FRAC *tmFrac)
 {
 	ASSERT(instPtr != NULL);
 	ASSERT(tmFrac != NULL);

datalink814_ge/src/time_ctrl/time_ctrl.h

@@ -45,30 +45,30 @@ typedef struct
 {
 	uintptr_t baseAddr;
 	uint32_t reset; // time for reset (unit: us)
-} TIMECTRL_CONFIG;
+} TMC_CONFIG;
 
 typedef struct
 {
-	TIMECTRL_CONFIG config;
+	TMC_CONFIG config;
 	uint32_t modelVer;
-} TIMECTRL_INST;
+} TMC_INST;
 
 /************************** Function Prototypes ******************************/
-int32_t TimeCtrl_Init(TIMECTRL_INST **instPtr, TIMECTRL_CONFIG *config);
+int32_t TimeCtrl_Init(TMC_INST **instPtr, TMC_CONFIG *config);
-uint32_t TimeCtrl_GetVersion(TIMECTRL_INST *instPtr);
+uint32_t TimeCtrl_GetVersion(TMC_INST *instPtr);
-void TimeCtrl_Destroy(TIMECTRL_INST *instPtr);
+void TimeCtrl_Destroy(TMC_INST *instPtr);
-void TimeCtrl_Reset(TIMECTRL_INST *instPtr);
+void TimeCtrl_Reset(TMC_INST *instPtr);
-void TimeCtrl_ResetLow(TIMECTRL_INST *instPtr);
+void TimeCtrl_ResetLow(TMC_INST *instPtr);
-void TimeCtrl_ResetHigh(TIMECTRL_INST *instPtr);
+void TimeCtrl_ResetHigh(TMC_INST *instPtr);
 
-TIME_INT TimeCtrl_GetIntCnt(TIMECTRL_INST *instPtr);
+TIME_INT TimeCtrl_GetIntCnt(TMC_INST *instPtr);
-TIME_FRAC TimeCtrl_GetFracCnt(TIMECTRL_INST *instPtr);
+TIME_FRAC TimeCtrl_GetFracCnt(TMC_INST *instPtr);
 
-void TimeCtrl_GetCurrentTime(TIMECTRL_INST *instPtr, TIME_WORD *tmWord);
+void TimeCtrl_GetCurrentTime(TMC_INST *instPtr, TIME_WORD *tmWord);
-void TimeCtrl_GetCurrentStep(TIMECTRL_INST *instPtr, TIME_FRAC *tmFrac);
+void TimeCtrl_GetCurrentStep(TMC_INST *instPtr, TIME_FRAC *tmFrac);
 
-void TimeCtrl_SetOptimizeStep(TIMECTRL_INST *instPtr, TIME_FRAC *tmFrac);
+void TimeCtrl_SetOptimizeStep(TMC_INST *instPtr, TIME_FRAC *tmFrac);
-void TimeCtrl_TimeAdjust(TIMECTRL_INST *instPtr, const TIME_WORD *tmWord, TIMECTRL_ADJ_TYPE type);
+void TimeCtrl_TimeAdjust(TMC_INST *instPtr, const TIME_WORD *tmWord, TIMECTRL_ADJ_TYPE type);
 void TimeCtrl_CalculateTimeGap(const TIME_WORD *a, const TIME_WORD *b, TIME_WORD *gap);
 
 #endif /* TIME_SYNC_H_ */

datalink814_ge/unit_test/linklayer_emulate_unit_test.c

@@ -15,7 +15,7 @@
 #define TIME_ADJ_TRIG (0x00000024 >> 2)
 #define CNTSTEP_OPT (0x00000028 >> 2)
 
-TIMECTRL_INST *tmcInstPtr = NULL;
+TMC_INST *tmcInstPtr = NULL;
 PHY_INST *phyInstPtr = NULL;
 L2_INST *linkInstPtr = NULL;
 
@@ -197,7 +197,7 @@ PHY_CONFIG l1Config =
 	.baseAddr = (uintptr_t)phycReg
 };
 
-TIMECTRL_CONFIG tmcConfig = 
+TMC_CONFIG tmcConfig = 
 {
 	.baseAddr = (uintptr_t)tmcReg,
 	.reset = 10
@@ -384,13 +384,30 @@ void forge_rtt_frame(uint8_t *buff, uint16_t srcId, uint16_t destId,
 	return;
 }
 
-TEST(linklayer_master_routine) {
+TEST(linklayer_reconfig)
+{
+	int32_t Status;
+	Status = Linklayer_Reconfig(linkInstPtr, &l2Config);
+	ASSERT_EQUAL(SUCCESS, Status);
+	ASSERT_EQUAL(false, linkInstPtr->isEnable);
+	ASSERT_EQUAL(false, linkInstPtr->isEnable);
+	ASSERT_EQUAL(0, linkInstPtr->nodeList->len);
+	ASSERT_EQUAL(0, linkInstPtr->schdEventPool->envCnt);
+}
+
+TEST(linklayer_master_routine)
+{
 	int32_t Status;
 	Linklayer_Start(linkInstPtr);
 	TIME_WORD timeStampA, timeStampB, timeGap;
 	uint32_t timeDiff;
+	l2Config.myType = L2_NODE_HEAD;
+	l2Config.myId = 1;
+	Status = Linklayer_Reconfig(linkInstPtr, &l2Config);
+	ASSERT_EQUAL(SUCCESS, Status);
 	// 30 seconds
-	for(int i = 0; i < 200*30; i++) {
+	for (int i = 0; i < 30 * L2_SLOTS_PER_FRAME; i++)
+	{
 		tmcReg[TIME_INT_REG]++;
 
 		if (L2_GetNextTor(linkInstPtr) == L2_TOR_TX)
@@ -401,29 +418,29 @@ TEST(linklayer_master_routine) {
 				// ASSERT_EQUAL(linkInstPtr->rttFSM->rttRecord.Tt1.int_part + linkInstPtr->netStartTime*L2_SLOTS_PER_FRAME,
 				// 				timeStampA.int_part);
 				// ASSERT_EQUAL(linkInstPtr->rttFSM->rttRecord.Tt1.frac_part, timeStampA.frac_part);
-				timeStampA.frac_part = rand()%1000; // 10us deviation.
+				timeStampA.frac_part = rand_range(0, 1000); // up to 10us deviation.
-				
 			}
 		}
 		if (L2_GetCurrentTor(linkInstPtr) == L2_TOR_RX)
 		{
-			if(L2_GetCurrentFrameType(linkInstPtr) == L2_INTRA_W2_ACQ_FRAME) {
+			if (L2_GetCurrentFrameType(linkInstPtr) == L2_INTRA_W2_ACQ_FRAME)
-				forge_w2acq_frame((uint8_t*)phyInstPtr->recvDataAddr, L2_NODE_MEMBER, 2, 1, 1, -1, L2_INIT_STATE);
+			{
+				forge_w2acq_frame((uint8_t *)phyInstPtr->recvDataAddr, L2_NODE_MEMBER, 2, 1, 1, -1, L2_INIT_STATE);
 				PhyWrite(phyInstPtr, TXRX_STATE, 0x01010101);
 				PhyWrite(phyInstPtr, RECVDATA_STATE, INTRA_W2ACQ_FRAME_LENGTH_MAX);
 			}
 
 			if (L2_GetCurrentFrameType(linkInstPtr) == L2_INTRA_MGMT_FRAME)
 			{
-				forge_mgmt_frame((uint8_t*)phyInstPtr->recvDataAddr, L2_NORMAL_STATE);
+				forge_mgmt_frame((uint8_t *)phyInstPtr->recvDataAddr, L2_NORMAL_STATE);
 				PhyWrite(phyInstPtr, TXRX_STATE, 0x01010101);
 				PhyWrite(phyInstPtr, RECVDATA_STATE, INTRA_MGMT_FRAME_LENGTH_MAX);
 			}
 			if (L2_GetCurrentFrameType(linkInstPtr) == L2_INTRA_TSYNC_FRAME)
 			{
-				timeDiff = Phy_GetTimeComp(L2_L2frame2L1frame(L2_INTRA_TSYNC_FRAME)) + (rand()-RAND_MAX)%1000;
+				timeDiff = Phy_GetTimeComp(L2_L2frame2L1frame(L2_INTRA_TSYNC_FRAME)) + (rand() - RAND_MAX) % 1000;
 				TimeCtrl_CalculateTimeGap(&timeStampA, &timeStampB, &timeGap);
-				forge_rtt_frame((uint8_t*)phyInstPtr->recvDataAddr, 2, 1, 1, &timeGap , rand()%200000);
+				forge_rtt_frame((uint8_t *)phyInstPtr->recvDataAddr, 2, 1, 1, &timeGap, rand() % 200000);
 				PhyWrite(phyInstPtr, TXRX_STATE, 0x01010101);
 				PhyWrite(phyInstPtr, FSTIME_H, L2_GetLastFrameNum(linkInstPtr));
 				PhyWrite(phyInstPtr, FSTIME_L, timeDiff);
@@ -432,12 +449,13 @@ TEST(linklayer_master_routine) {
 
 			if (L2_GetCurrentFrameType(linkInstPtr) == L2_INTRA_SVC_FRAME)
 			{
-				forge_mgmt_frame((uint8_t*)phyInstPtr->recvDataAddr, L2_NORMAL_STATE);
+				forge_mgmt_frame((uint8_t *)phyInstPtr->recvDataAddr, L2_NORMAL_STATE);
 				PhyWrite(phyInstPtr, TXRX_STATE, 0x01010101);
 				PhyWrite(phyInstPtr, RECVDATA_STATE, INTRA_MGMT_FRAME_LENGTH_MAX);
 			}
 		}
-		else {
+		else
+		{
 			PhyWrite(phyInstPtr, TXRX_STATE, 0x02020202);
 		}
 
@@ -445,15 +463,19 @@ TEST(linklayer_master_routine) {
 
 		Status = Linklayer_Proc(linkInstPtr);
 
-		if((tmcReg[TIME_INT_REG]+1)/200 < linkInstPtr->netStartTime) {
+		if ((tmcReg[TIME_INT_REG] + 1) / 200 < linkInstPtr->netStartTime)
+		{
 			ASSERT_EQUAL(EIGNORE, Status);
 		}
-		else{
+		else
+		{
 			ASSERT_EQUAL(SUCCESS, Status);
 		}
 
-		if(L2_isDataAvail(linkInstPtr)) {
+		if (L2_isDataAvail(linkInstPtr))
-			if(L2_GetLastFrameType(linkInstPtr) == L2_INTRA_W2_ACQ_FRAME) {
+		{
+			if (L2_GetLastFrameType(linkInstPtr) == L2_INTRA_W2_ACQ_FRAME)
+			{
 				ASSERT_EQUAL(1, L2_GetNodeListLen(linkInstPtr->nodeList));
 				ASSERT_TRUE(L2_isNodeInList(linkInstPtr->nodeList, 2));
 			}
@@ -461,23 +483,126 @@ TEST(linklayer_master_routine) {
 	}
 }
 
-
+TEST(linklayer_slave_netsetup)
-TEST(linklayer_reconfig) {
+{
 	int32_t Status;
+	Linklayer_Start(linkInstPtr);
+	TIME_WORD timeStampA, timeStampB, timeGap;
+	TIME_WORD bmTime;
+	uint32_t netStart, timeDiff;
+	// 30 seconds
+	l2Config.myType = L2_NODE_MEMBER;
+	l2Config.myId = 2;
 	Status = Linklayer_Reconfig(linkInstPtr, &l2Config);
+	tmcReg[TIME_FRAC_REG] = rand_range(0, TIME_FRAC_MAX);
+	tmcReg[TIME_INT_REG] = rand_range(0, 60 * L2_SLOTS_PER_FRAME); // 0~60s
+	bmTime.frac_part = rand_range(0, TIME_FRAC_MAX);
+	bmTime.int_part = rand_range(tmcReg[TIME_INT_REG], tmcReg[TIME_INT_REG] + 5 * L2_SLOTS_PER_FRAME);
+
+	// 74 is the index of bm frame in routine table.
+	netStart = bmTime.int_part % L2_SLOTS_PER_FRAME + rand_range(-12, 12);
+
+	for (int i = 0; i < 12 * L2_SLOTS_PER_FRAME * TIME_FRAC_MAX; i++)
+	{
+		tmcReg[TIME_FRAC_REG]++;
+		if (tmcReg[TIME_FRAC_REG] >= TIME_FRAC_MAX)
+		{
+			tmcReg[TIME_FRAC_REG] = 0;
+			tmcReg[TIME_INT_REG]++;
+		}
+
+		if (tmcReg[TIME_FRAC_REG] == bmTime.frac_part && tmcReg[TIME_INT_REG] == bmTime.int_part)
+		{
+			forge_bm_frame((uint8_t *)phyInstPtr->recvDataAddr, 1, 1, 1, netStart);
+			PhyWrite(phyInstPtr, TXRX_STATE, 0x01010101);
+			PhyWrite(phyInstPtr, RECVDATA_STATE, INTRA_BM_FRAME_LENGTH_MAX);
+		}
+
+		if (L2_GetNextTor(linkInstPtr) == L2_TOR_TX)
+		{
+			if (L2_GetNextFrameType(linkInstPtr) == L2_INTRA_TSYNC_FRAME)
+			{
+				TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStampA);
+				timeStampA.frac_part = rand_range(0, 1000); // up to 10us deviation.
+			}
+		}
+		if (L2_GetCurrentTor(linkInstPtr) == L2_TOR_RX)
+		{
+			if (L2_GetCurrentFrameType(linkInstPtr) == L2_INTRA_W2_ACQ_FRAME)
+			{
+				forge_w2acq_frame((uint8_t *)phyInstPtr->recvDataAddr, L2_NODE_MEMBER, 2, 1, 1, -1, L2_INIT_STATE);
+				PhyWrite(phyInstPtr, TXRX_STATE, 0x01010101);
+				PhyWrite(phyInstPtr, RECVDATA_STATE, INTRA_W2ACQ_FRAME_LENGTH_MAX);
+			}
+
+			if (L2_GetCurrentFrameType(linkInstPtr) == L2_INTRA_MGMT_FRAME)
+			{
+				forge_mgmt_frame((uint8_t *)phyInstPtr->recvDataAddr, L2_NORMAL_STATE);
+				PhyWrite(phyInstPtr, TXRX_STATE, 0x01010101);
+				PhyWrite(phyInstPtr, RECVDATA_STATE, INTRA_MGMT_FRAME_LENGTH_MAX);
+			}
+			if (L2_GetCurrentFrameType(linkInstPtr) == L2_INTRA_TSYNC_FRAME)
+			{
+				timeDiff = Phy_GetTimeComp(L2_L2frame2L1frame(L2_INTRA_TSYNC_FRAME)) + (rand() - RAND_MAX) % 1000;
+				TimeCtrl_CalculateTimeGap(&timeStampA, &timeStampB, &timeGap);
+				forge_rtt_frame((uint8_t *)phyInstPtr->recvDataAddr, 2, 1, 1, &timeGap, rand() % 200000);
+				PhyWrite(phyInstPtr, TXRX_STATE, 0x01010101);
+				PhyWrite(phyInstPtr, FSTIME_H, L2_GetLastFrameNum(linkInstPtr));
+				PhyWrite(phyInstPtr, FSTIME_L, timeDiff);
+				PhyWrite(phyInstPtr, RECVDATA_STATE, INTRA_MGMT_FRAME_LENGTH_MAX);
+			}
+
+			if (L2_GetCurrentFrameType(linkInstPtr) == L2_INTRA_SVC_FRAME)
+			{
+				forge_mgmt_frame((uint8_t *)phyInstPtr->recvDataAddr, L2_NORMAL_STATE);
+				PhyWrite(phyInstPtr, TXRX_STATE, 0x01010101);
+				PhyWrite(phyInstPtr, RECVDATA_STATE, INTRA_MGMT_FRAME_LENGTH_MAX);
+			}
+		}
+		else
+		{
+			PhyWrite(phyInstPtr, TXRX_STATE, 0x02020202);
+		}
+		if (PhyRead(phyInstPtr, TXRX_STATE) != 0x02020202)
+		{
+			Phy_UpdateState(phyInstPtr);
+		}
+
+		if (tmcReg[TIME_FRAC_REG] == 200000)
+		{ // 2ms
+			Status = Linklayer_Proc(linkInstPtr);
+			if ((tmcReg[TIME_INT_REG] + 1) / 200 < linkInstPtr->netStartTime)
+			{
+				ASSERT_EQUAL(EIGNORE, Status);
+			}
+			else
+			{
 				ASSERT_EQUAL(SUCCESS, Status);
+			}
+
+			if (L2_isDataAvail(linkInstPtr))
+			{
+				if (L2_GetLastFrameType(linkInstPtr) == L2_INTRA_W2_ACQ_FRAME)
+				{
+					ASSERT_EQUAL(1, L2_GetNodeListLen(linkInstPtr->nodeList));
+					ASSERT_TRUE(L2_isNodeInList(linkInstPtr->nodeList, 2));
+				}
+			}
+		}
+	}
 }
 
-TEST_SUITE(linklayer_unit_test) {
+TEST_SUITE(linklayer_unit_test)
+{
 	TEST_SUITE_CONFIGURE(linklayer_setup, linklayer_teardown);
-	RUN_TEST(linklayer_master_routine);
 	RUN_TEST(linklayer_reconfig);
+	RUN_TEST(linklayer_slave_netsetup);
+	RUN_TEST(linklayer_master_routine);
 }
 
-int main(void) {
+int main(void)
+{
 	RUN_TEST_SUITE(linklayer_unit_test);
 	TEST_REPORT();
 	return TEST_EXIT_CODE;
 }
-
-

datalink814_ge/unit_test/tmc_board_unit_test.c

@@ -7,11 +7,11 @@
 #define HARDWARE_VERSION 0x00000001
 #define MAX_OPT_DELAY 100 // 1us = 100*10ns
 
-TIMECTRL_CONFIG tmcConfig = {
+TMC_CONFIG tmcConfig = {
 	.baseAddr = 0 /*TODO: fill withe real base address*/,
 	.reset = 10};
 
-TIMECTRL_INST *tmcInstPtr = NULL;
+TMC_INST *tmcInstPtr = NULL;
 
 void tmc_setup(void)
 {

datalink814_ge/unit_test/tmc_emulate_unit_test.c

@@ -11,11 +11,11 @@
 
 uint32_t tmcReg[32] = {0};
 
-TIMECTRL_CONFIG tmcConfig = {
+TMC_CONFIG tmcConfig = {
 	.baseAddr = 0,
 	.reset = 10};
 
-TIMECTRL_INST *tmcInstPtr = NULL;
+TMC_INST *tmcInstPtr = NULL;
 
 void tmc_setup(void)
 {