Initial version.

2023-12-20 00:11:03 +08:00
commit 416bd53733
1043 changed files with 293547 additions and 0 deletions
@@ -0,0 +1,991 @@
+/*****************************************************************************/
+/**
+ * Description: Implementations of 814 linklayer management net.
+ * 
+ * Date: 2021-11-18 17:27
+ * LastEditors: Fan Hao
+ * LastEditTime: 2022-01-16 15:52
+ * Revisions: Rev 0.1
+ * History: 
+ * [2022-07-16]: New branch for DiJian.
+ * [2021-11-18]: Initial version.
+ * 
+ * Copyright(c) 2022 Beijing Institute of Technology.
+ * Lab of Communication and Networking
+ * 
+ ******************************************************************************/
+
+/***************************** Include Files *********************************/
+#include <string.h>
+#include "linklayer814.h"
+#include "linklayer814_types.h"
+#include "msg_def.h"
+#include "rtt.h"
+#include "../error.h"
+#include "../common.h"
+#include "node_list.h"
+/************************** Constant Definitions *****************************/
+#define SQUENCE_MGMT_SLOTS  0
+#define INTER_MGMT_SLOTS	2
+#define INTRA_MGMT_SLOTS	1
+#define NONE_MGMT_SLOTS		-1
+#define CONN_LOST_TH 		10      // 10 count of mgmt msg no response to delete
+#define TIME2QUIT 			80		// time to send quit message
+
+#define SENDRTT 0
+#define RCVDRTT 1
+
+/***************** Macros (Inline Functions) Definitions *********************/
+
+/**************************** Type Definitions *******************************/
+typedef enum {
+	MGMTPKT_NONE = 0,
+	MGMTPKT_INTER_MGMT = 1,
+	MGMTPKT_INTRA_MGMT = 2,
+	MGMTPKT_INTER_RTTREQ = 3,
+	MGMTPKT_INTER_RTTRSP = 4,
+	MGMTPKT_INTRA_RTTREQ = 5,
+	MGMTPKT_INTRA_RTTRSP = 6
+} MGMTPKT_TYPE;
+/************************** Variable Definitions *****************************/
+
+/************************** Function Prototypes ******************************/
+static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE msgType);
+static int32_t MgmtPkt_Packing(L2_INST* instPtr, MGMTPKT_TYPE pktType);
+/*****************************************************************************/
+/**
+ * @brief   Packing management message to a bit array.
+ *
+ * @param   bitarr	pointer to bit array buffer.
+ *          msgType	message type.
+ *
+ * @return	EPARAM
+ * 			SUCCESS
+ *
+ ******************************************************************************/
+static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE msgType) {
+	ASSERT(instPtr != NULL);
+	ASSERT(bitarr != NULL);
+	if (bitarr->num_of_bits < MGMT_MSG_LEN || msgType < L2_MSG_TK_INIT
+			|| msgType > L2_MSG_BCU_INIT) {
+		return EPARAM;
+	}
+	uint64_t dataChunk = 0;
+	memset(bitarr->words, 0x99, (size_t) (MGMT_MSG_LEN / 8));
+	switch (msgType) {
+	case L2_MSG_TK_INIT:
+		if (bitarr->num_of_bits < MGMT_MSG_LEN) {
+			return EPARAM;
+		}
+		// Series tag
+		dataChunk = 0;
+		FrameInsertChunk(bitarr, TK_MSG_INI_SER_TAG_OFFSET, TK_MSG_INI_SER_TAG_LEN, dataChunk);
+
+		// word tag
+		dataChunk = 3;
+		FrameInsertChunk(bitarr, TK_MSG_INI_WTAG_OFFSET, TK_MSG_INI_WTAG_LEN, dataChunk);
+
+		// message tag
+		dataChunk = 28;
+		FrameInsertChunk(bitarr, TK_MSG_INI_TAG_OFFSET, TK_MSG_INI_TAG_LEN, dataChunk);
+
+		// message sub tag
+		dataChunk = 1;
+		FrameInsertChunk(bitarr, TK_MSG_INI_STAG_OFFSET, TK_MSG_INI_STAG_LEN, dataChunk);
+
+		// message length tag
+		dataChunk = 4;
+		FrameInsertChunk(bitarr, TK_MSG_INI_OFFSET, TK_MSG_INI_LEN, dataChunk);
+
+		// Next net stage
+		if (instPtr->myInfo.nodeType == HEAD_NODE) {
+			switch (instPtr->nextState) {
+			case INIT_STATE:
+				dataChunk = NET_INI_STAGE;
+				break;
+			case NORMAL_STATE:
+				dataChunk = NET_NORMAL_STAGE;
+				break;
+			case INTRA_SEEK_STATE:
+				dataChunk = NET_INTRA_SEEK_STAGE;
+				break;
+			case INTER_SEEK_STATE:
+				dataChunk = NET_INTER_SEEK_STAGE;
+				break;
+			case SEEK_STATE:
+				dataChunk = NET_SEEK_STAGE;
+				break;
+			default:
+				dataChunk = NET_ERROR_STAGE;
+				break;
+			}
+			FrameInsertChunk(bitarr, TK_MSG_INI_NET_STATE_OFFSET, TK_MSG_INI_NET_STATE_LEN, dataChunk);
+		}
+		break;
+	case L2_MSG_TK_EXTD:
+		if (bitarr->num_of_bits < MGMT_MSG_LEN) {
+			return EPARAM;
+		}
+		// Word tag
+		dataChunk = 1;
+		FrameInsertChunk(bitarr, TK_MSG_EXTEND_WTAG_OFFSET, TK_MSG_EXTEND_WTAG_LEN, dataChunk);
+
+		// Time tag
+		dataChunk = 0xAAAAAAAAAAAA;
+		FrameInsertChunk(bitarr, TK_MSG_EXTEND_TIME_TAG_OFFSET, TK_MSG_EXTEND_TIME_TAG_LEN, dataChunk);
+
+		// East speed
+		dataChunk = 0xBBBB;
+		FrameInsertChunk(bitarr, TK_MSG_EXTEND_EAST_SPEED_OFFSET, TK_MSG_EXTEND_EAST_SPEED_LEN, dataChunk);
+		break;
+	case L2_MSG_TK_CONT_1:
+		// Word tag
+		dataChunk = 2;
+		FrameInsertChunk(bitarr, TK_MSG_CONT_1_WTAG_OFFSET, TK_MSG_CONT_1_WTAG_LEN, dataChunk);
+
+		// Continue word identification.
+		dataChunk = 1;
+		FrameInsertChunk(bitarr, TK_MSG_CONT_1_CONTID_OFFSET, TK_MSG_CONT_1_CONTID_LEN, dataChunk);
+
+		// North speed
+		dataChunk = 0xCCCC;
+		FrameInsertChunk(bitarr, TK_MSG_CONT_1_NORTH_SPEED_OFFSET, TK_MSG_CONT_1_NORTH_SPEED_LEN, dataChunk);
+
+		// Upper speed
+		dataChunk = 0xDDDD;
+		FrameInsertChunk(bitarr, TK_MSG_CONT_1_UPPER_SPEED_OFFSET,
+		TK_MSG_CONT_1_UPPER_SPEED_LEN, dataChunk);
+
+		// Height
+		dataChunk = 0xEEEEEE;
+		FrameInsertChunk(bitarr, TK_MSG_CONT_1_HEIGHT_OFFSET, TK_MSG_CONT_1_HEIGHT_LEN, dataChunk);
+		break;
+
+	case L2_MSG_TK_CONT_2:
+		// Word tag
+		dataChunk = 2;
+		FrameInsertChunk(bitarr, TK_MSG_CONT_2_WTAG_OFFSET, TK_MSG_CONT_2_WTAG_LEN, dataChunk);
+
+		// Continue word identification.
+		dataChunk = 2;
+		FrameInsertChunk(bitarr, TK_MSG_CONT_2_CONTID_OFFSET, TK_MSG_CONT_2_CONTID_LEN, dataChunk);
+
+		// Longitude
+		dataChunk = 0xAAAAAAAA;
+		FrameInsertChunk(bitarr, TK_MSG_CONT_2_LONGITUDE_OFFSET, TK_MSG_CONT_2_LONGITUDE_LEN, dataChunk);
+
+		// Latitude
+		dataChunk = 0xBBBBBBBB;
+		FrameInsertChunk(bitarr, TK_MSG_CONT_2_LATITUDE_OFFSET, TK_MSG_CONT_2_LATITUDE_LEN, dataChunk);
+		break;
+	case L2_MSG_TK_CONT_3:
+
+		// Word tag
+		dataChunk = 2;
+		FrameInsertChunk(bitarr, TK_MSG_CONT_3_WTAG_OFFSET, TK_MSG_CONT_3_WTAG_LEN, dataChunk);
+
+		// Continue word identification.
+		dataChunk = 3;
+		FrameInsertChunk(bitarr, TK_MSG_CONT_3_CONTID_OFFSET, TK_MSG_CONT_3_CONTID_LEN, dataChunk);
+
+		// source node id
+		dataChunk = (instPtr->myInfo.nodeId & 0x1F)
+				| (((uint16_t) (instPtr->myInfo.clustId & 0x1F)) << 5);
+		FrameInsertChunk(bitarr, TK_MSG_CONT_3_MEMBER_ADDR_OFFSET, TK_MSG_CONT_3_MEMBER_ADDR_LEN, dataChunk);
+
+		// East speed
+		dataChunk = 0xBBBB;
+		FrameInsertChunk(bitarr, TK_MSG_CONT_3_EAST_ACC_OFFSET, TK_MSG_CONT_3_EAST_ACC_LEN, dataChunk);
+
+		// North speed
+		dataChunk = 0xCCCC;
+		FrameInsertChunk(bitarr, TK_MSG_CONT_3_NORTH_ACC_OFFSET, TK_MSG_CONT_3_NORTH_ACC_LEN, dataChunk);
+
+		// Upper speed
+		dataChunk = 0xDDDD;
+		FrameInsertChunk(bitarr, TK_MSG_CONT_3_UPPER_ACC_OFFSET, TK_MSG_CONT_3_UPPER_ACC_LEN, dataChunk);
+
+		break;
+
+	case L2_MSG_TA_INIT:
+
+		// Word tag
+		dataChunk = 3;
+		FrameInsertChunk(bitarr, TA_MSG_WTAG_OFFSET, TA_MSG_WTAG_LEN, dataChunk);
+
+		// Msg tag
+		dataChunk = 29;
+		FrameInsertChunk(bitarr, TA_MSG_MSG_TAG_OFFSET, TA_MSG_MSG_TAG_LEN, dataChunk);
+
+		// Sub msg tag
+		dataChunk = 0;
+		FrameInsertChunk(bitarr, TA_MSG_MSG_STAG_OFFSET, TA_MSG_MSG_STAG_LEN, dataChunk);
+
+		// Source node id
+		dataChunk = (instPtr->myInfo.nodeId & 0x1F)
+				| (((uint16_t) (instPtr->myInfo.clustId & 0x1F)) << 5);
+		FrameInsertChunk(bitarr, TA_MSG_SRC_ADDR_OFFSET, TA_MSG_SRC_ADDR_LEN, dataChunk);
+
+		// Height
+		dataChunk = 0xEEEEEE;
+		FrameInsertChunk(bitarr, TA_MSG_HEIGHT_OFFSET, TA_MSG_HEIGHT_LEN, dataChunk);
+
+		break;
+	case L2_MSG_TA_EXTD:
+
+		// Longitude
+		dataChunk = 0xAAAAAAAA;
+		FrameInsertChunk(bitarr, TA_MSG_EX_LONGITUDE_OFFSET, TA_MSG_EX_LONGITUDE_LEN, dataChunk);
+
+		// Latitude
+		dataChunk = 0xBBBBBBBB;
+		FrameInsertChunk(bitarr, TA_MSG_EX_LATITUDE_OFFSET, TA_MSG_EX_LATITUDE_LEN, dataChunk);
+
+		break;
+	case L2_MSG_TA_CONT_1:
+
+		// Time tag
+		dataChunk = 0xCCCCCCCCCCCC;
+		FrameInsertChunk(bitarr, TA_MSG_CONT_1_TIME_TAG_OFFSET, TA_MSG_CONT_1_TIME_TAG_LEN, dataChunk);
+
+		break;
+	case L2_MSG_TA_CONT_2:
+
+		// East speed
+		dataChunk = 0xBBBB;
+		FrameInsertChunk(bitarr, TA_MSG_CONT_2_EAST_OFFSET, TA_MSG_CONT_2_EAST_LEN, dataChunk);
+
+		// North speed
+		dataChunk = 0xCCCC;
+		FrameInsertChunk(bitarr, TA_MSG_CONT_2_NORTH_OFFSET, TA_MSG_CONT_2_NORTH_LEN, dataChunk);
+
+		// Upper speed
+		dataChunk = 0xDDDD;
+		FrameInsertChunk(bitarr, TA_MSG_CONT_2_UPPER_OFFSET, TA_MSG_CONT_2_UPPER_LEN, dataChunk);
+
+		break;
+	case L2_MSG_MADJ_INIT:
+		break;
+	case L2_MSG_MADJ_EXTD:
+		break;
+	case L2_MSG_RTT_REQ_INIT:
+		// Series tag
+		dataChunk = 0;
+		FrameInsertChunk(bitarr, RTT_REQ_SER_TAG_OFFSET, RTT_REQ_SER_TAG_LEN, dataChunk);
+
+		// Word tag
+		dataChunk = 3;
+		FrameInsertChunk(bitarr, RTT_REQ_WTAG_OFFSET, RTT_REQ_WTAG_LEN, dataChunk);
+
+		// message identification.
+		dataChunk = 29;
+		FrameInsertChunk(bitarr, RTT_REQ_MSG_TAG_OFFSET, RTT_REQ_MSG_TAG_LEN, dataChunk);
+
+		// message identification.
+		dataChunk = 1;
+		FrameInsertChunk(bitarr, RTT_REQ_MSG_STAG_OFFSET, RTT_REQ_MSG_STAG_LEN, dataChunk);
+
+		// message length.
+		dataChunk = 2;		//request
+		FrameInsertChunk(bitarr, RTT_REQ_MSG_OFFSET, RTT_REQ_MSG_LEN, dataChunk);
+
+		// source node address length.
+		dataChunk = (instPtr->myInfo.nodeId & 0x1F)
+				| (((uint16_t) (instPtr->myInfo.clustId & 0x1F)) << 5);
+		FrameInsertChunk(bitarr, RTT_REQ_SRC_ADDR_OFFSET, RTT_REQ_SRC_ADDR_LEN, dataChunk);
+
+		// destination node address length.
+		dataChunk = (0x02) | (((uint16_t) (instPtr->myInfo.clustId & 0x1F)) << 5);
+		FrameInsertChunk(bitarr, RTT_REQ_DST_ADDR_OFFSET, RTT_REQ_DST_ADDR_LEN, dataChunk);
+		break;
+
+	case L2_MSG_RTT_REQ_EXTD:
+		// Word tag
+		dataChunk = 1;
+		FrameInsertChunk(bitarr, RTT_REQ_EX_WTAG_OFFSET, RTT_REQ_EX_WTAG_LEN, dataChunk);
+
+		// Time tag Low.
+		dataChunk = instPtr->mgmtNetFSM->rttData.rttTime.timeL;
+		FrameInsertChunk(bitarr, RTT_REQ_EX_TTAGL_OFFSET, RTT_REQ_EX_TTAGL_LEN, dataChunk);
+
+		// Time tag High.
+		dataChunk = instPtr->mgmtNetFSM->rttData.rttTime.timeH;
+		FrameInsertChunk(bitarr, RTT_REQ_EX_TTAGH_OFFSET, RTT_REQ_EX_TTAGH_LEN, dataChunk);
+		break;
+	case L2_MSG_RTT_REQ_CONT:
+		// Doppler
+		dataChunk = instPtr->mgmtNetFSM->rttData.rttDoppler;
+		FrameInsertChunk(bitarr, RTT_REQ_CONT_DOP_OFFSET, RTT_REQ_CONT_DOP_LEN, dataChunk);
+
+		////////////////////////////////////////////////////////////////////////////////////////////
+		break;
+	case L2_MSG_RTT_RSP_INIT:
+		// Series tag
+		dataChunk = 0;
+		FrameInsertChunk(bitarr, RTT_RSP_SER_TAG_OFFSET, RTT_RSP_SER_TAG_LEN, dataChunk);
+
+		// Word tag
+		dataChunk = 3;
+		FrameInsertChunk(bitarr, RTT_RSP_WTAG_OFFSET, RTT_RSP_WTAG_LEN, dataChunk);
+
+		// message identification.
+		dataChunk = 29;
+		FrameInsertChunk(bitarr, RTT_RSP_MSG_TAG_OFFSET,
+		RTT_RSP_MSG_TAG_LEN, dataChunk);
+
+		// message identification.
+		dataChunk = 2;		// response
+		FrameInsertChunk(bitarr, RTT_RSP_MSG_STAG_OFFSET, RTT_RSP_MSG_STAG_LEN, dataChunk);
+
+		// message length.
+		dataChunk = 2;
+		FrameInsertChunk(bitarr, RTT_RSP_MSG_OFFSET, RTT_RSP_MSG_LEN, dataChunk);
+
+		// source node address length.
+		dataChunk = (instPtr->myInfo.nodeId & 0x1F)
+				| (((uint16_t) (instPtr->myInfo.clustId & 0x1F)) << 5);
+		FrameInsertChunk(bitarr, RTT_RSP_SRC_ADDR_OFFSET, RTT_RSP_SRC_ADDR_LEN, dataChunk);
+
+		// destination node address length.
+		dataChunk = (0x01) | (((uint16_t) (instPtr->myInfo.clustId & 0x1F)) << 5);
+		FrameInsertChunk(bitarr, RTT_RSP_DST_ADDR_OFFSET, RTT_RSP_DST_ADDR_LEN, dataChunk);
+		break;
+	case L2_MSG_RTT_RSP_EXTD:
+
+		// Word tag
+		dataChunk = 1;
+		FrameInsertChunk(bitarr, RTT_RSP_EX_WTAG_OFFSET, RTT_RSP_EX_WTAG_LEN, dataChunk);
+
+		// Time tag Low.
+		dataChunk = instPtr->mgmtNetFSM->rttData.rttTime.timeL;
+		FrameInsertChunk(bitarr, RTT_RSP_EX_TTAGL_OFFSET, RTT_RSP_EX_TTAGL_LEN, dataChunk);
+
+		// Time tag High.
+		dataChunk = instPtr->mgmtNetFSM->rttData.rttTime.timeH;
+		FrameInsertChunk(bitarr, RTT_RSP_EX_TTAGH_OFFSET, RTT_RSP_EX_TTAGH_LEN, dataChunk);
+		break;
+	case L2_MSG_RTT_RSP_CONT:
+		// Doppler
+		dataChunk = instPtr->mgmtNetFSM->rttData.rttDoppler;
+		FrameInsertChunk(bitarr, RTT_RSP_CONT_DOP_OFFSET, RTT_RSP_CONT_DOP_LEN, dataChunk);
+		break;
+	case L2_MSG_NQT_INIT:
+//		if (priorInfo.myType == MEMBER_NODE && ( L2_GetCurrentFrameNo() == ( myInfo->netScanTime + TIME2QUIT ) )) {
+//			// word tag
+//			dataChunk = 3;
+//			FrameInsertChunk(bitarr, NQT_MSG_WTAG_OFFSET,
+//			NQT_MSG_WTAG_LEN, dataChunk);
+//
+//			// msg tag
+//			dataChunk = 30;
+//			FrameInsertChunk(bitarr, NQT_MSG_MSG_TAG_OFFSET,
+//			NQT_MSG_MSG_TAG_LEN, dataChunk);
+//
+//			// msg sub tag
+//			dataChunk = 1;
+//			FrameInsertChunk(bitarr, NQT_MSG_MSG_STAG_OFFSET,
+//			NQT_MSG_MSG_STAG_LEN, dataChunk);
+//
+//			// msg length
+//			dataChunk = 0;
+//			FrameInsertChunk(bitarr, NQT_MSG_MSG_OFFSET,
+//			NQT_MSG_MSG_LEN, dataChunk);
+//
+//			// Ctrol Word
+//			dataChunk = 1;
+//			FrameInsertChunk(bitarr, NQT_MSG_CTRL_MUTE_OFFSET,
+//			NQT_MSG_CTRL_MUTE_LEN, dataChunk);
+//
+//			// Quit object
+//			dataChunk = (priorInfo.myId & 0x1F)
+//					| (((uint16_t) (priorInfo.clustId & 0x1F)) << 5);
+//			FrameInsertChunk(bitarr, NQT_MSG_OBJ_OFFSET,
+//			NQT_MSG_OBJ_LEN, dataChunk);
+//			L2_NODE_STATE newState = INIT_STATE;
+//			L2_EVENT stateEvent = { 
+//				.event = L2_EV_MAIN_STATE_UPDATE, 
+//				.msg = &newState 
+//			};
+//			AssertL2Event(&stateEvent);
+//			L2_EVENT lostCxnEvent = { 
+//				.event = L2_EV_LOST_CXN, 
+//				.msg = NULL
+//			};
+//			AssertL2Event(&lostCxnEvent);
+//		}
+		break;
+	case L2_MSG_CMG_INIT:
+		break;
+	case L2_MSG_BCU_INIT:
+		break;
+	default:
+		return EPARAM;
+		break;
+	}
+	return SUCCESS;
+}
+
+
+
+static int32_t MgmtPkt_Packing(L2_INST* instPtr, MGMTPKT_TYPE pktType) {
+	ASSERT(instPtr != NULL);
+	uint8_t* buff = instPtr->sendPktData;
+	uint32_t filedSize = 0;
+	BIT_ARRAY* mgmtPkt = bit_array_create(L2_MSG_MAX);
+	if (mgmtPkt == NULL) {
+		DBG_ERROR("Failed to allocate memory for scanMsg.\n");
+		return NUM_SLOTS_NONE;
+	}
+	switch (pktType) {
+	case MGMTPKT_INTRA_MGMT:
+		for (int i = 0; i < ARRAY_SIZE(intraMgmtFrameMsgs); i++) {
+			if (MgmtMsg_Packing(instPtr, mgmtPkt, intraMgmtFrameMsgs[i]) == SUCCESS) {
+				memcpy((uint8_t*)buff + (size_t) (i * (MGMT_MSG_LEN / 8)),
+						mgmtPkt->words, (size_t) (MGMT_MSG_LEN / 8));
+			}
+			else {
+				DBG_ERROR("Failed to packing intraMgmtFrameMsgs[%d].\n", i);
+				goto failed;
+			}
+			filedSize += MGMT_MSG_LEN;
+			if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX) {
+				DBG_ERROR("Message is too long to be properly packed.\n");
+				goto failed;
+			}
+		}
+		break;
+	case MGMTPKT_INTRA_RTTREQ:
+		for(int i = 0; i < ARRAY_SIZE(rttReqFrameMsgs); i++) {
+			if (MgmtMsg_Packing(instPtr, mgmtPkt, rttReqFrameMsgs[i]) == SUCCESS) {
+				memcpy(buff + (size_t)(i * (MGMT_MSG_LEN / 8)),
+						mgmtPkt->words,
+						(size_t)(MGMT_MSG_LEN / 8));
+			}
+			else {
+				DBG_ERROR("Failed to packing rttReqFrameMsgs[%d].\n", i);
+				goto failed;
+			}
+			filedSize += MGMT_MSG_LEN;
+			if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX) {
+				DBG_ERROR("Message is too long to be properly packed.\n");
+				goto failed;
+			}
+		}
+		break;
+	case MGMTPKT_INTRA_RTTRSP:
+		for (int i = 0; i < ARRAY_SIZE(rttRspFrameMsgs); i++) {
+			if (MgmtMsg_Packing(instPtr, mgmtPkt, rttRspFrameMsgs[i]) == SUCCESS) {
+				memcpy(buff + (size_t) (i * (MGMT_MSG_LEN / 8)),
+						mgmtPkt->words,
+						(size_t) (MGMT_MSG_LEN / 8));
+			}
+			else {
+				DBG_ERROR("Failed to packing rttRspFrameMsgs[%d].\n", i);
+				goto failed;
+			}
+			filedSize += MGMT_MSG_LEN;
+			if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX) {
+				DBG_ERROR("Message is too long to be properly packed.\n");
+				goto failed;
+			}
+		}
+		break;
+	default:
+		DBG_ERROR("Scan Packet type [%d] is not surpported yet.\n", pktType);
+		goto failed;
+	}
+	bit_array_free(mgmtPkt);
+	return SUCCESS;
+failed:
+	bit_array_free(mgmtPkt);
+	return FAILURE;
+}
+
+/*****************************************************************************/
+/**
+ * @brief   initialize the management FSM instance.
+ * 
+ * @param   mgmtFSM  A second rank pointer to management FSM instance.
+ * 
+ * @return  SUCCESS Initialize successfully
+ *          EALLOC Memory allocation error.
+ * 
+ ******************************************************************************/
+int32_t MgmtNet_Init(L2_MGMT_FSM** mgmtFSM) {
+	(*mgmtFSM) = (L2_MGMT_FSM*) malloc(sizeof(L2_MGMT_FSM));
+	if (*mgmtFSM == NULL) {
+		return EALLOC;
+	}
+	MgmtNet_Reset(*mgmtFSM);
+	return SUCCESS;
+}
+
+void MgmtNet_Reset(L2_MGMT_FSM* mgmtFSM) {
+	mgmtFSM->isBisarProc = false;
+	mgmtFSM->isPendSlotAlloc = false;
+	mgmtFSM->isPendTimeAlloc = false;
+
+	mgmtFSM->mgmtMsgLog.nextNetState = NONE_STATE;
+	mgmtFSM->mgmtMsgLog.isValid = false;
+	mgmtFSM->mgmtMsgLog.quitCtrlWord = 0;
+	mgmtFSM->mgmtMsgLog.quitNodeId = 0;
+	mgmtFSM->mgmtMsgLog.msgSubTag = 0 ;
+	mgmtFSM->mgmtMsgLog.msgTag = 0;
+	mgmtFSM->mgmtMsgLog.wordTag = 0;
+	mgmtFSM->mgmtMsgLog.srcNodeId = 0;
+
+	// Position value
+	mgmtFSM->mgmtMsgLog.EastSpeed  = 0 ;
+	mgmtFSM->mgmtMsgLog.NorthSpeed = 0 ;
+	mgmtFSM->mgmtMsgLog.UpperSpeed = 0 ;
+	mgmtFSM->mgmtMsgLog.Height     = 0 ;
+	mgmtFSM->mgmtMsgLog.Longitude  = 0 ;
+	mgmtFSM->mgmtMsgLog.Latitude   = 0 ;
+
+	// clear buffer.
+	memset(&(mgmtFSM->rttMsgRspLog), 0x00, sizeof(RTT_MSG_RECORD));
+	memset(&(mgmtFSM->rttMsgReqLog), 0x00, sizeof(RTT_MSG_RECORD));
+	memset(&(mgmtFSM->rttData), 0x00, sizeof(RTT_DATA));
+}
+
+void MgmtNet_Destroy(L2_MGMT_FSM* mgmtFSM){
+	free(mgmtFSM);
+}
+/*****************************************************************************/
+/**
+ * @brief   Handles management net packet, apply time sync and other management
+ *          net functions.
+ * 
+ * @param   msg A pointer to packet data buffer.
+ * 
+ * @return  SUCCESS Packet was successfully handled.
+ *          Other Packed was dumped(not addressed to me, unrecognized frame type)
+ * 
+ ******************************************************************************/
+int32_t MgmtNet_PktHandler(L2_INST* instPtr) {
+	ASSERT(instPtr != NULL);
+	int32_t Status;
+	uint32_t filedSize = 0;
+	uint64_t dataChunk = 0;
+	MGMT_MSG_RECORD *mgmtMsgLog = &(instPtr->mgmtNetFSM->mgmtMsgLog);
+	RTT_MSG_RECORD *rttMsgRspLog = &(instPtr->mgmtNetFSM->rttMsgRspLog);
+	RTT_MSG_RECORD *rttMsgReqLog = &(instPtr->mgmtNetFSM->rttMsgReqLog);
+
+	BIT_ARRAY* mgmtMsg = bit_array_create(L2_MSG_MAX);
+	if (mgmtMsg == NULL) {
+		free(mgmtMsg);
+		return EALLOC;
+	}
+	switch (L2_GetLastRcvFrameType(instPtr)) {
+	case L2_INTER_MGMT_FRAME:
+	case L2_INTRA_MGMT_FRAME: {
+		for (int i = 0; i < ARRAY_SIZE(intraMgmtFrameMsgs); i++) {
+			memcpy(mgmtMsg->words,
+					instPtr->recvPktData + (size_t) (i * (MGMT_MSG_LEN / 8)),
+					(size_t) (MGMT_MSG_LEN / 8));
+			switch (intraMgmtFrameMsgs[i]) {
+			case L2_MSG_TK_INIT:
+				FrameGetChunk(mgmtMsg, TK_MSG_INI_NET_STATE_OFFSET, TK_MSG_INI_NET_STATE_LEN, &dataChunk);
+				mgmtMsgLog->nextNetState = (uint8_t) (dataChunk);
+
+				FrameGetChunk(mgmtMsg, TK_MSG_INI_WTAG_OFFSET, TK_MSG_INI_WTAG_LEN, &dataChunk);
+				mgmtMsgLog->wordTag = (uint8_t) (dataChunk);
+
+				FrameGetChunk(mgmtMsg, TK_MSG_INI_TAG_OFFSET, TK_MSG_INI_TAG_LEN, &dataChunk);
+				mgmtMsgLog->msgTag = (uint8_t) (dataChunk);
+
+				FrameGetChunk(mgmtMsg, TK_MSG_INI_STAG_OFFSET, TK_MSG_INI_STAG_LEN, &dataChunk);
+				mgmtMsgLog->msgSubTag = (uint8_t) (dataChunk);
+
+				if ( mgmtMsgLog->wordTag == 3 
+					&& mgmtMsgLog->msgTag == 28 
+					&& mgmtMsgLog->msgSubTag == 1 ) {
+					mgmtMsgLog->isValid = true;
+				}
+				else {
+					mgmtMsgLog->isValid = false ;
+				}
+				break;
+			case L2_MSG_TK_EXTD :
+				FrameGetChunk(mgmtMsg, TK_MSG_EXTEND_EAST_SPEED_OFFSET, TK_MSG_EXTEND_EAST_SPEED_LEN, &dataChunk);
+				mgmtMsgLog->EastSpeed = (uint16_t) (dataChunk);
+
+				break;
+			case L2_MSG_TK_CONT_1:
+				FrameGetChunk(mgmtMsg, TK_MSG_EXTEND_EAST_SPEED_OFFSET, TK_MSG_EXTEND_EAST_SPEED_LEN, &dataChunk);
+				mgmtMsgLog->EastSpeed = (uint16_t) (dataChunk);
+
+				FrameGetChunk(mgmtMsg, TK_MSG_CONT_1_NORTH_SPEED_OFFSET, TK_MSG_CONT_1_NORTH_SPEED_LEN, &dataChunk);
+				mgmtMsgLog->NorthSpeed = (uint16_t) (dataChunk);
+
+				FrameGetChunk(mgmtMsg, TK_MSG_CONT_1_UPPER_SPEED_OFFSET, TK_MSG_CONT_1_UPPER_SPEED_LEN, &dataChunk);
+				mgmtMsgLog->UpperSpeed = (uint16_t) (dataChunk);
+
+				FrameGetChunk(mgmtMsg, TK_MSG_CONT_1_HEIGHT_OFFSET, TK_MSG_CONT_1_HEIGHT_LEN, &dataChunk);
+				mgmtMsgLog->Height = (uint32_t) (dataChunk);
+
+				break;
+			case L2_MSG_TK_CONT_2:
+				FrameGetChunk(mgmtMsg, TK_MSG_CONT_2_LONGITUDE_OFFSET, TK_MSG_CONT_2_LONGITUDE_LEN, &dataChunk);
+				mgmtMsgLog->Longitude = (uint32_t) (dataChunk);
+
+				FrameGetChunk(mgmtMsg, TK_MSG_CONT_2_LATITUDE_OFFSET, TK_MSG_CONT_2_LATITUDE_LEN, &dataChunk);
+				mgmtMsgLog->Latitude = (uint32_t) (dataChunk);
+
+				break;
+			case L2_MSG_TK_CONT_3:
+				FrameGetChunk(mgmtMsg, TK_MSG_CONT_3_MEMBER_ADDR_OFFSET, TK_MSG_CONT_3_MEMBER_ADDR_LEN, &dataChunk);
+				mgmtMsgLog->srcNodeId = (uint8_t) (dataChunk & 0x1F);
+
+				break;
+			case L2_MSG_NQT_INIT:
+				FrameGetChunk(mgmtMsg, NQT_MSG_OBJ_OFFSET, NQT_MSG_OBJ_LEN, &dataChunk);
+				mgmtMsgLog->quitNodeId = (uint8_t) (dataChunk & 0x1F);
+				if (mgmtMsgLog->quitNodeId != instPtr->myInfo.nodeId) {
+					FrameGetChunk(mgmtMsg, NQT_MSG_CTRL_MUTE_OFFSET, NQT_MSG_CTRL_MUTE_LEN, &dataChunk);
+					mgmtMsgLog->quitCtrlWord = (uint8_t)(dataChunk & 0x7F);
+					if (mgmtMsgLog->quitCtrlWord == 0x01) {
+						DeletNode(instPtr->nodeList, mgmtMsgLog->quitNodeId);
+					}
+				}
+				else {
+					DBG_WARNING("Ignore node quit request(ID not mine), target node id is [%d]\n", mgmtMsgLog->quitNodeId);
+				}
+				break;
+			default:
+				DBG_ERROR("Cannot decapsulate message type [%d] in L2_INTRA_MGMT_FRAME.\n", intraMgmtFrameMsgs[i]);
+				Status = EPARAM;
+				goto failed;
+			}
+			filedSize += MGMT_MSG_LEN;
+			if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX) {
+				DBG_ERROR("Message decapsulate exceeds frame length.\n");
+				Status = EPARAM;
+				goto failed;
+			}
+		}
+		// Member Node update state with received message
+		if(instPtr->myInfo.nodeId != HEAD_NODE && mgmtMsgLog->isValid == true ){
+			L2_NODE_STATE newState = NONE_STATE;
+			// FIXME :
+			switch (mgmtMsgLog->nextNetState) {
+			case NET_INI_STAGE:
+				newState = INIT_STATE;
+				break;
+			case NET_NORMAL_STAGE:
+				newState = NORMAL_STATE;
+				break;
+			case NET_INTRA_SEEK_STAGE:
+				newState = INTRA_SEEK_STATE;
+				break;
+			case NET_INTER_SEEK_STAGE:
+				newState = INTER_SEEK_STATE;
+				break;
+			case NET_SEEK_STAGE:
+				newState = SEEK_STATE;
+				break;
+			default:
+				DBG_WARNING("Unknown Head node state [%d].\n", mgmtMsgLog->nextNetState);
+				newState = L2_GetMainState(instPtr);
+				break;
+			}
+			L2_EVENT stateUpEvent = {
+				.event = L2_EV_MAIN_STATE_UPDATE,
+				.msg = &newState
+			};
+			AssertL2Event(instPtr, &stateUpEvent);
+		}
+		// TODO: Reset of tracking message src
+		RstNodeTTL(instPtr->nodeList, mgmtMsgLog->srcNodeId);
+
+		if(mgmtMsgLog->srcNodeId == instPtr->config.headId){
+			L2_TimeAlign(instPtr, L2_GetCurrentFrameNum(instPtr), L2_GetCurrentSlotNum(instPtr));
+		}
+		break;
+	}
+	case L2_INTER_TSYNC_FRAME:
+	case L2_INTRA_TSYNC_FRAME: {
+		L2_RTT_FSM* rttFsmPtr = L2_GetRttDesc(instPtr);
+		L2_RTT_SATGE rttStage = RTT_GetCurrentStage(rttFsmPtr);
+		if(rttStage == RTT_REQ1 || rttStage == RTT_REQ2){
+			for (int i = 0; i < ARRAY_SIZE(rttReqFrameMsgs); i++) {
+				memcpy(mgmtMsg->words,
+						instPtr->recvPktData + (size_t) (i * (RTT_MSG_LEN / 8)),
+						(size_t) (RTT_MSG_LEN / 8));
+				switch (rttReqFrameMsgs[i]) {
+				case L2_MSG_RTT_REQ_INIT:
+					FrameGetChunk(mgmtMsg, RTT_REQ_WTAG_OFFSET, RTT_REQ_WTAG_LEN, &dataChunk);
+					rttMsgReqLog->wordTag = (uint16_t) (dataChunk);
+
+					FrameGetChunk(mgmtMsg, RTT_REQ_MSG_TAG_OFFSET, RTT_REQ_MSG_TAG_LEN, &dataChunk);
+					rttMsgReqLog->msgTag = (uint16_t) (dataChunk);
+
+					FrameGetChunk(mgmtMsg, RTT_REQ_MSG_STAG_OFFSET, RTT_REQ_MSG_STAG_LEN, &dataChunk);
+					rttMsgReqLog->msgSubTag = (uint16_t) (dataChunk);
+
+					FrameGetChunk(mgmtMsg, RTT_REQ_MSG_OFFSET, RTT_REQ_MSG_LEN, &dataChunk);
+					rttMsgReqLog->msgLen = (uint16_t) (dataChunk);
+
+					FrameGetChunk(mgmtMsg, RTT_REQ_SRC_ADDR_OFFSET, RTT_REQ_SRC_ADDR_LEN, &dataChunk);
+					rttMsgReqLog->srcAddr = (uint16_t) (dataChunk);
+
+					FrameGetChunk(mgmtMsg, RTT_REQ_DST_ADDR_OFFSET, RTT_REQ_DST_ADDR_LEN, &dataChunk);
+					rttMsgReqLog->dstAddr = (uint16_t) (dataChunk);
+
+					break;
+				case L2_MSG_RTT_REQ_EXTD:
+					FrameGetChunk(mgmtMsg, RTT_REQ_EX_TTAGL_OFFSET, RTT_REQ_EX_TTAGL_LEN, &dataChunk);
+					rttMsgReqLog->timeWordL = (uint32_t) (dataChunk);
+
+					FrameGetChunk(mgmtMsg, RTT_REQ_EX_TTAGH_OFFSET, RTT_REQ_EX_TTAGH_LEN, &dataChunk);
+					rttMsgReqLog->timeWordH = (uint32_t) (dataChunk);
+
+					break;
+				case L2_MSG_RTT_REQ_CONT:
+					FrameGetChunk(mgmtMsg, RTT_REQ_CONT_DOP_OFFSET, RTT_REQ_CONT_DOP_LEN, &dataChunk);
+					rttMsgReqLog->doppler = (int32_t) (dataChunk);
+
+					break;
+				default:
+					DBG_ERROR("Cannot decapsulate message type [%d] in L2_INTRA_TSYNC_FRAME.\n", rttReqFrameMsgs[i]);
+					Status = EPARAM;
+					goto failed;
+				}
+				filedSize += RTT_MSG_LEN;
+				if (filedSize / 8 >= INTRA_RTT_FRAME_LENGTH_MAX) {
+					DBG_ERROR("Message decapsulate exceeds frame length.\n");
+					Status = EPARAM;
+					goto failed;
+				}
+			}
+		}
+		else if(rttStage == RTT_RSP1 || rttStage == RTT_RSP2){
+			for (int i = 0; i < ARRAY_SIZE(rttRspFrameMsgs); i++) {
+				memcpy(mgmtMsg->words,
+						instPtr->recvPktData + (size_t) (i * (RTT_MSG_LEN / 8)),
+						(size_t) (RTT_MSG_LEN / 8));
+				switch (rttRspFrameMsgs[i]) {
+				case L2_MSG_RTT_RSP_INIT:
+					FrameGetChunk(mgmtMsg, RTT_RSP_WTAG_OFFSET, RTT_RSP_WTAG_LEN, &dataChunk);
+					rttMsgRspLog->wordTag = (uint16_t) (dataChunk);
+
+					FrameGetChunk(mgmtMsg, RTT_RSP_MSG_TAG_OFFSET, RTT_RSP_MSG_TAG_LEN, &dataChunk);
+					rttMsgRspLog->msgTag = (uint16_t) (dataChunk);
+
+					FrameGetChunk(mgmtMsg, RTT_RSP_MSG_STAG_OFFSET, RTT_RSP_MSG_STAG_LEN, &dataChunk);
+					rttMsgRspLog->msgSubTag = (uint16_t) (dataChunk);
+
+					FrameGetChunk(mgmtMsg, RTT_RSP_MSG_OFFSET, RTT_RSP_MSG_LEN, &dataChunk);
+					rttMsgRspLog->msgLen = (uint16_t) (dataChunk);
+
+					FrameGetChunk(mgmtMsg, RTT_RSP_SRC_ADDR_OFFSET, RTT_RSP_SRC_ADDR_LEN, &dataChunk);
+					rttMsgRspLog->srcAddr = (uint16_t) (dataChunk);
+
+					FrameGetChunk(mgmtMsg, RTT_RSP_DST_ADDR_OFFSET, RTT_RSP_DST_ADDR_LEN, &dataChunk);
+					rttMsgRspLog->dstAddr = (uint16_t) (dataChunk);
+
+					break;
+				case L2_MSG_RTT_RSP_EXTD:
+					FrameGetChunk(mgmtMsg, RTT_RSP_EX_TTAGL_OFFSET, RTT_RSP_EX_TTAGL_LEN, &dataChunk);
+					rttMsgRspLog->timeWordL = (uint32_t) (dataChunk);
+
+					FrameGetChunk(mgmtMsg, RTT_RSP_EX_TTAGH_OFFSET, RTT_RSP_EX_TTAGH_LEN, &dataChunk);
+					rttMsgRspLog->timeWordH = (uint32_t) (dataChunk);
+
+					break;
+				case L2_MSG_RTT_RSP_CONT:
+					FrameGetChunk(mgmtMsg, RTT_RSP_CONT_DOP_OFFSET,
+					RTT_RSP_CONT_DOP_LEN, &dataChunk);
+					rttMsgRspLog->doppler = (int32_t) (dataChunk);
+
+					break;
+				default:
+					DBG_ERROR("Cannot decapsulate message type [%d] in L2_INTRA_TSYNC_FRAME.\n", rttRspFrameMsgs[i]);
+					Status = EPARAM;
+					goto failed;
+				}
+				filedSize += RTT_MSG_LEN;
+				if (filedSize / 8 >= INTRA_RTT_FRAME_LENGTH_MAX) {
+					DBG_ERROR("Message decapsulate exceeds frame length.\n");
+					Status = EPARAM;
+					goto failed;
+				}
+			}
+		}
+		// TODO: Reset of tracking message src
+		RstNodeTTL(instPtr->nodeList, rttMsgRspLog->srcAddr & 0x001F);
+		if((rttMsgRspLog->srcAddr&0x001F) == instPtr->config.headId){
+			L2_TimeAlign(instPtr, L2_GetCurrentFrameNum(instPtr), L2_GetCurrentSlotNum(instPtr));
+		}
+		break;
+	}
+	default:
+		DBG_ERROR("Cannot handle frame type [%d] properly.\n", L2_GetLastRcvFrameType(instPtr));
+		Status = EPARAM;
+		goto failed;
+	}
+	bit_array_free(mgmtMsg);
+	return SUCCESS;
+failed:
+	bit_array_free(mgmtMsg);
+	return Status;
+}
+
+// helper function
+static bool IsTTLValid(NODE_ENTRY* entry) {
+	ASSERT(entry != NULL);
+	return (entry->ttlCnt <= CONN_LOST_TH);
+}
+/*****************************************************************************/
+/**
+ * @brief   Management net FSM procedure.
+ * 
+ * @param   mgmtFSM 
+ *          mainState
+ * 
+ * @return  Slot has be used.
+ * 
+ ******************************************************************************/
+int32_t MgmtNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry){
+	ASSERT(instPtr != NULL);
+	int32_t Status;
+	RTT_DATA *rttData = &(instPtr->mgmtNetFSM->rttData);
+	RTT_RECORD *rttRecord = &(L2_GetRttDesc(instPtr)->rttRecord);
+	DBG_INFO("-------- MgmtNet Routine --------\n");
+
+	if(rtneEntry->slotSeq != 0){
+		return SUCCESS;
+	}
+
+	// Check if there is any node exceeds the ttl threshold.
+	IterDeleteNode(instPtr->nodeList, IsTTLValid);
+
+	// Determine next slot TOR state.
+	L2_TOR_TYPE torState = L2_TOR_SUSPEND;
+	if ((rtneEntry->src0 == instPtr->myInfo.nodeId)
+			|| (rtneEntry->src1 == instPtr->myInfo.nodeId)) {
+		torState = L2_TOR_TX;
+	}
+	else if ((rtneEntry->dst0 == instPtr->myInfo.nodeId)
+			|| (rtneEntry->dst1 == instPtr->myInfo.nodeId)) {
+		torState = L2_TOR_RX;
+	}
+	else {
+		torState = L2_TOR_SUSPEND;
+	}
+
+	switch (rtneEntry->slotType) {
+	case SLOT_INTER_MGMT:
+		// TODO: 
+		L2_SetupNextSlot(instPtr, torState, L2_INTER_MGMT_FRAME);
+		break;
+	case SLOT_INTRA_MGMT:
+		memset(instPtr->sendPktData, 0xAA, INTRA_MGMT_FRAME_LENGTH_MAX);
+		if (torState == L2_TOR_TX) {
+			if(MgmtPkt_Packing(instPtr, MGMTPKT_INTRA_MGMT) != SUCCESS) {
+				Status = FAILURE;
+				goto routine_err;
+			}
+		}
+		L2_SetupNextSlot(instPtr, torState, L2_INTRA_MGMT_FRAME);
+		break;
+	case SLOT_INTER_TSYNC:
+		// TODO: 
+		L2_SetupNextSlot(instPtr, torState, L2_INTER_TSYNC_FRAME);
+		break;
+	case SLOT_INTRA_TSYNC:
+		if (torState == L2_TOR_TX) {
+			// TODO: 
+			memset(instPtr->sendPktData, 0x00, INTRA_RTT_FRAME_LENGTH_MAX); // FIXME
+			L2_RTT_FSM* rttFsmPtr = L2_GetRttDesc(instPtr);
+			L2_RTT_SATGE rttStage = RTT_GetCurrentStage(rttFsmPtr);
+			if(rttStage == RTT_REQ1 || rttStage == RTT_REQ2){
+				if(rttStage == RTT_REQ1){
+					DBG_INFO("MGMT Sending RTT REQ-1\n");
+					rttData->rttTime.timeH = rttRecord->Tt1.timeH;
+					rttData->rttTime.timeL = rttRecord->Tt1.timeL;
+					rttData->rttDoppler = RTT_DOPPLER_NONE;
+				}
+				else{ //RTT_REQ2
+					DBG_INFO("MGMT Sending RTT REQ-2\n");
+					rttData->rttTime.timeH = (rttFsmPtr->isR2Valid)? rttRecord->TroundA.timeH : RTT_TIME_NONE;
+					rttData->rttTime.timeL = (rttFsmPtr->isR2Valid)? rttRecord->TroundA.timeL : RTT_TIME_NONE;
+					rttData->rttDoppler = (rttFsmPtr->isR2Valid)? rttRecord->dopplerR2:RTT_DOPPLER_NONE;
+				}
+				if(MgmtPkt_Packing(instPtr, MGMTPKT_INTRA_RTTREQ) != SUCCESS) {
+					Status = FAILURE;
+					goto routine_err;
+				}
+			}
+			else if(rttStage == RTT_RSP1 || rttStage == RTT_RSP2){
+				if(rttStage == RTT_RSP1){
+					DBG_INFO("MGMT Sending RTT RSP-1\n");
+					rttData->rttTime.timeH = (rttFsmPtr->isR1Valid) ? rttRecord->TreplyB.timeH : RTT_TIME_NONE;
+					rttData->rttTime.timeL = (rttFsmPtr->isR1Valid) ? rttRecord->TreplyB.timeL : RTT_TIME_NONE;
+					rttData->rttDoppler = (rttFsmPtr->isR1Valid) ? rttRecord->dopplerR1 : RTT_DOPPLER_NONE;
+				}
+				else{ //RTT_RSP2
+					DBG_INFO("MGMT Sending RTT RSP-2\n");
+					rttData->rttTime.timeH = (rttFsmPtr->isR3Valid) ? rttRecord->TreplyB.timeH : RTT_TIME_NONE;
+					rttData->rttTime.timeL = (rttFsmPtr->isR3Valid) ? rttRecord->TreplyB.timeL : RTT_TIME_NONE;
+					rttData->rttDoppler = (rttFsmPtr->isR3Valid) ? rttRecord->dopplerR3 : RTT_DOPPLER_NONE;
+				}
+				// Packing message.
+				if(MgmtPkt_Packing(instPtr, MGMTPKT_INTRA_RTTRSP) != SUCCESS) {
+					Status = FAILURE;
+					goto routine_err;
+				}
+
+			}
+			else{
+				DBG_ERROR("Unexpected RTT Stage: %d.\n", rttStage);
+				torState = L2_TOR_SUSPEND;
+			}
+		}
+		L2_SetupNextSlot(instPtr, torState, L2_INTRA_TSYNC_FRAME);
+		break;
+	default:
+		DBG_ERROR("UNKNOWN slot type [%d].\n", rtneEntry->slotType);
+		Status = FAILURE;
+		goto routine_err;
+	}
+
+	/*************************************************/
+	// Update link layer state...
+	if (instPtr->myInfo.nodeType == HEAD_NODE) {
+		L2_NODE_STATE newState = NONE_STATE;
+		L2_EVENT stateEvent = {
+			.event = L2_EV_MAIN_STATE_UPDATE,
+			.msg = &newState
+		};
+		if (instPtr->nodeList->nodeNum < instPtr->config.netScale
+				&& L2_GetMainState(instPtr) != INTRA_SEEK_STATE) {
+			newState = INTRA_SEEK_STATE;
+			AssertL2Event(instPtr, &stateEvent);
+		}
+		else if(instPtr->nodeList->nodeNum >= instPtr->config.netScale &&
+				L2_GetMainState(instPtr) != NORMAL_STATE){
+			newState = NORMAL_STATE;
+			AssertL2Event(instPtr, &stateEvent);
+		}
+	}
+	else{ // MEMBER_NODE or BACKUP_HEAD
+		if(IsNodeInList(instPtr->nodeList, instPtr->config.headId) == false){
+			L2_NODE_STATE newState = INIT_STATE;
+			L2_EVENT stateEvent = {
+				.event = L2_EV_MAIN_STATE_UPDATE,
+				.msg = &newState
+			};
+			AssertL2Event(instPtr, &stateEvent);
+			L2_EVENT lostCxnEvent = {
+				.event = L2_EV_LOST_CXN,
+				.msg = NULL
+			};
+			AssertL2Event(instPtr, &lostCxnEvent);
+			DBG_WARNING("Lost connection with head node [%d].\n", instPtr->config.headId);
+		}
+	}
+	return SUCCESS;
+routine_err:
+	return Status;
+}