diff --git a/df_datalink_ge/src/main.c b/df_datalink_ge/src/main.c
index c14754e..cfd2b25 100644
--- a/df_datalink_ge/src/main.c
+++ b/df_datalink_ge/src/main.c
@@ -88,7 +88,8 @@ L2_CONFIG linkConfig =
 	.clustId = 1,
 	.myId = 1,
 	.headId = 1,
-	.netScale = 2,
+	.netScale = 10,
+	.serviceRate = L2_SVC_128K,
 	.phyInstPtr = NULL, // physical layer reference
 	.tmcInstPtr = NULL
 };
diff --git a/df_datalink_sp/src/networking/link_layer/link_layer.c b/df_datalink_sp/src/networking/link_layer/link_layer.c
index e6ff703..3490dad 100644
--- a/df_datalink_sp/src/networking/link_layer/link_layer.c
+++ b/df_datalink_sp/src/networking/link_layer/link_layer.c
@@ -149,33 +149,33 @@ static void L2_FrameStateUpdate(L2_INST *instPtr)
 		switch (recvType)
 		{
 		case L2_FRAME_ADV:
-			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_ADV, L2_SVC_NONE), L2_CRC16_1021);
+			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_ADV, L2_SVC_NONE), L2_CRC8_31);
 			instPtr->log.advRecvCnt++;
 			instPtr->log.advCorrectCnt = isValid ? instPtr->log.advCorrectCnt + 1 : instPtr->log.advCorrectCnt;
 			break;
 		case L2_FRAME_ACK:
-			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_ACK, L2_SVC_NONE), L2_CRC16_1021);
+			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_ACK, L2_SVC_NONE), L2_CRC8_31);
 			instPtr->log.ackRecvCnt++;
 			instPtr->log.ackCorrectCnt = isValid ? instPtr->log.ackCorrectCnt + 1 : instPtr->log.ackCorrectCnt;
 			break;
 		case L2_FRAME_DIS:
-			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_DIS, L2_SVC_NONE), L2_CRC16_1021);
+			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_DIS, L2_SVC_NONE), L2_CRC8_31);
 			instPtr->log.disRecvCnt++;
 			instPtr->log.disCorrectCnt = isValid ? instPtr->log.disCorrectCnt + 1 : instPtr->log.disCorrectCnt;
 			break;
 		case L2_FRAME_MNG:
-			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_MNG, L2_SVC_NONE), L2_CRC16_1021);
+			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_MNG, L2_SVC_NONE), L2_CRC8_31);
 			instPtr->log.mngRecvCnt++;
 			instPtr->log.mngCorrectCnt = isValid ? instPtr->log.mngCorrectCnt + 1 : instPtr->log.mngCorrectCnt;
 			break;
 		case L2_FRAME_RTT:
-			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_RTT, L2_SVC_NONE), L2_CRC16_1021);
+			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_RTT, L2_SVC_NONE), L2_CRC8_31);
 			instPtr->log.rttRecvCnt++;
 			instPtr->log.rttCorrectCnt = isValid ? instPtr->log.rttCorrectCnt + 1 : instPtr->log.rttCorrectCnt;
 			break;
 		case L2_FRAME_SVC:
 #ifndef DATALINK_ENABLE_BERTEST
-			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_SVC, instPtr->config.serviceRate), L2_CRC16_1021);
+			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_SVC, instPtr->config.serviceRate), L2_CRC8_31);
 #else
 			isValid = true;
 #endif
@@ -439,6 +439,56 @@ L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame, L2_SVC_RATE l2SvcRate)
 	}
 }
 
+/*****************************************************************************/
+/**
+ * @brief   Get L2 service packet length based on service rate
+ *
+ * @param	l2SvcRate
+ *
+ * @return  Packet length.
+ *
+ ******************************************************************************/
+size_t L2_GetSvcPacketLen(L2_SVC_RATE l2SvcRate)
+{
+	switch (l2SvcRate)
+	{
+	case L2_SVC_128K:
+		return sizeof(L2_MSG_SVC_128K);
+	case L2_SVC_512K:
+		return sizeof(L2_MSG_SVC_512K);
+	case L2_SVC_2M:
+		return sizeof(L2_MSG_SVC_2M);
+	default:
+		DBG_ERROR("Unsupported service rate: %" PRId8 ".\n", instPtr->config.serviceRate);
+		return EPARAM;
+	}
+}
+
+/*****************************************************************************/
+/**
+ * @brief   Get L2 service payload length based on service rate
+ *
+ * @param	l2SvcRate
+ *
+ * @return  Payload length.
+ *
+ ******************************************************************************/
+size_t L2_GetSvcPayloadLen(L2_SVC_RATE l2SvcRate)
+{
+	switch (l2SvcRate)
+	{
+	case L2_SVC_128K:
+		return L2_PAYLOAD_LEN_SVC_128K;
+	case L2_SVC_512K:
+		return L2_PAYLOAD_LEN_SVC_512K;
+	case L2_SVC_2M:
+		return L2_PAYLOAD_LEN_SVC_2M;
+	default:
+		DBG_ERROR("Unsupported service rate: %" PRId8 ".\n", instPtr->config.serviceRate);
+		return EPARAM;
+	}
+}
+
 /*****************************************************************************/
 /**
  * @brief   Convert antenna broadcast id to antenna index
diff --git a/df_datalink_sp/src/networking/link_layer/link_layer.h b/df_datalink_sp/src/networking/link_layer/link_layer.h
index 5c40d68..51ed6f3 100644
--- a/df_datalink_sp/src/networking/link_layer/link_layer.h
+++ b/df_datalink_sp/src/networking/link_layer/link_layer.h
@@ -224,6 +224,8 @@ uint32_t L2_GetNextSlotNum(L2_INST *instPtr);
 L2_FRAME_TYPE L2_GetNextFrameType(L2_INST *instPtr);
 L2_TOR_TYPE L2_GetNextTor(L2_INST *instPtr);
 L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame, L2_SVC_RATE l2SvcRate);
+size_t L2_GetSvcPacketLen(L2_SVC_RATE l2SvcRate);
+size_t L2_GetSvcPayloadLen(L2_SVC_RATE l2SvcRate);
 PHY_ANT_INDEX L2_AntId2AntIndex(uint8_t antId);
 /** @} LinkLayer_Utility*/
 
diff --git a/df_datalink_sp/src/networking/link_layer/service.c b/df_datalink_sp/src/networking/link_layer/service.c
index 2f83863..18d416a 100644
--- a/df_datalink_sp/src/networking/link_layer/service.c
+++ b/df_datalink_sp/src/networking/link_layer/service.c
@@ -91,68 +91,115 @@ static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
 #ifndef DATALINK_ENABLE_BERTEST
 	ASSERT(instPtr != NULL);
 	int32_t Status;
-	uint32_t dataChunk;
-	uint8_t *buff = instPtr->sendPktData;
 
-	// Common section of link layer package
-	dataChunk = L2_GetNextFrameNum(instPtr);
-	L2_FrameInsertChunk(buff, L2_MSG_FRAME_NUM, L2_MSG_FRAME_NUM_LEN, &dataChunk);
-	dataChunk = L2_GetNextSlotNum(instPtr);
-	L2_FrameInsertChunk(buff, L2_MSG_SLOT_NUM, L2_MSG_SLOT_NUM_LEN, &dataChunk);
-	dataChunk = L2_GetNodeId(instPtr);
-	L2_FrameInsertChunk(buff, L2_MSG_SRC_ID, L2_MSG_SRC_ID_LEN, &dataChunk);
-	dataChunk = dstId;
-	L2_FrameInsertChunk(buff, L2_MSG_DST_ID, L2_MSG_DST_ID_LEN, &dataChunk);
 	switch (type)
 	{
-	case L2_SVC_FRAME:
-	{
-		dataChunk = L2_SVC_FRAME;
-		L2_FrameInsertChunk(buff, L2_MSG_TYPE, L2_MSG_TYPE_LEN, &dataChunk);
+	case L2_FRAME_DIS:
+	case L2_FRAME_MNG:
+	case L2_FRAME_RTT:
+		L2_MSG_SVC_128K *message = (L2_MSG_SVC_128K *)(instPtr->sendPktData);
+
+		// Common section of link layer package
+		message->common->frameNum = L2_GetNextFrameNum(instPtr);
+		message->common->slotNum = L2_GetNextSlotNum(instPtr);
+		message->common->src = L2_GetNodeId(instPtr);
+		message->common->dst = dstId;
+		message->common.msgType = L2_FRAME_ADV;
 
-		SysMonitor_GetSysinfo(&(instPtr->sysInfo));
 		// insert position and speed info
-		L2_FrameInsertChunk(buff, L2_MSG_SVC_SRC_POSX, L2_MSG_SVC_SRC_POSX_LEN, &(instPtr->sysInfo.posX));
-		L2_FrameInsertChunk(buff, L2_MSG_SVC_SRC_POSY, L2_MSG_SVC_SRC_POSY_LEN, &(instPtr->sysInfo.posY));
-		L2_FrameInsertChunk(buff, L2_MSG_SVC_SRC_POSZ, L2_MSG_SVC_SRC_POSZ_LEN, &(instPtr->sysInfo.posZ));
-		L2_FrameInsertChunk(buff, L2_MSG_SVC_SRC_SPEEDX, L2_MSG_SVC_SRC_SPEEDX_LEN, &(instPtr->sysInfo.vX));
-		L2_FrameInsertChunk(buff, L2_MSG_SVC_SRC_SPEEDY, L2_MSG_SVC_SRC_SPEEDY_LEN, &(instPtr->sysInfo.vY));
-		L2_FrameInsertChunk(buff, L2_MSG_SVC_SRC_SPEEDZ, L2_MSG_SVC_SRC_SPEEDZ_LEN, &(instPtr->sysInfo.vZ));
+		SysMonitor_GetSysinfo(&(instPtr->sysInfo));
+		message->srcPosX = instPtr->sysInfo.posX;
+		message->srcPosY = instPtr->sysInfo.posY;
+		message->srcPosZ = instPtr->sysInfo.posZ;
+		message->srcSpeedX = instPtr->sysInfo.vX;
+		message->srcSpeedY = instPtr->sysInfo.vY;
+		message->srcSpeedZ = instPtr->sysInfo.vZ;
 
-		uint32_t dataId;
-		if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
+		// Packing extra payload
+		SysMonitor_GetSendPayload(message->svcData, message->svcDataId, L2_PAYLOAD_LEN_SVC_128K);
+		if (message->svcDataId == DATA_ID_NONE)
 		{
-			SysMonitor_GetSendPayload(buff + L2_MSG_SVC_PAYLOAD, &dataId, PAYLOAD_LEN_FLJ_TX);
-		}
-		else
-		{
-			SysMonitor_GetSendPayload(buff + L2_MSG_SVC_PAYLOAD, &dataId, PAYLOAD_LEN_LJ_TX);
+			memset(message->svcData, 0xAA, L2_PAYLOAD_LEN_SVC_128K);
 		}
 
-		L2_FrameInsertChunk(buff, L2_MSG_SVC_PAYLOAD_ID, L2_MSG_SVC_PAYLOAD_ID_LEN, &dataId);
-		if (dataId == DATA_ID_NONE)
-		{
-			L2_FrameInsertChunk(buff, L2_MSG_SVC_PAYLOAD, L2_MSG_SVC_PAYLOAD_LEN, testData);
-		}
-
-		Status = L2_CrcAppend(buff, L2_SVC_PACK_LEN - L2_MSG_SVC_CRC_LEN, L2_MSG_SVC_CRC, L2_CRC16_1021);
+		// Append CRC
+		uint32_t crcVal;
+		Status = L2_CrcGen(message, sizeof(L2_MSG_SVC_128K) - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal);
 		if (Status != SUCCESS)
 		{
-			DBG_ERROR("Fail to append CRC...\n");
+			DBG_ERROR("Fail to generate CRC, error code: %" PRId32 ".\n", Status);
 			return Status;
 		}
+		message->crc = crcVal;
+
+		// Logging...
+		memcpy(instPtr->log.txLog.data, message, sizeof(L2_MSG_SVC_128K));
+		instPtr->log.txLog.dataId = message->svcDataId;
+
+		break;
+	case L2_FRAME_SVC:
+		// retrieve basic info
+		size_t msgLen = L2_GetSvcPacketLen(instPtr->config.serviceRate);
+		size_t dataLen = L2_GetSvcPayloadLen(instPtr->config.serviceRate);
+		switch (instPtr->config.serviceRate)
+		{
+		case L2_SVC_128K:
+			L2_MSG_SVC_128K *message = (L2_MSG_SVC_128K *)(instPtr->sendPktData);
+			break;
+		case L2_SVC_512K:
+			L2_MSG_SVC_512K *message = (L2_MSG_SVC_512K *)(instPtr->sendPktData);
+			break;
+		case L2_SVC_2M:
+			L2_MSG_SVC_2M *message = (L2_MSG_SVC_2M *)(instPtr->sendPktData);
+			break;
+		default:
+			DBG_ERROR("Unsupported service rate: %" PRId8 ".\n", instPtr->config.serviceRate);
+			return EPARAM;
+		}
+
+		// Common section of link layer package
+		message->common->frameNum = L2_GetNextFrameNum(instPtr);
+		message->common->slotNum = L2_GetNextSlotNum(instPtr);
+		message->common->src = L2_GetNodeId(instPtr);
+		message->common->dst = dstId;
+		message->common.msgType = L2_FRAME_ADV;
+
+		// insert position and speed info
+		SysMonitor_GetSysinfo(&(instPtr->sysInfo));
+		message->srcPosX = instPtr->sysInfo.posX;
+		message->srcPosY = instPtr->sysInfo.posY;
+		message->srcPosZ = instPtr->sysInfo.posZ;
+		message->srcSpeedX = instPtr->sysInfo.vX;
+		message->srcSpeedY = instPtr->sysInfo.vY;
+		message->srcSpeedZ = instPtr->sysInfo.vZ;
+
+		// Packing extra payload
+		SysMonitor_GetSendPayload(message->svcData, message->svcDataId, dataLen);
+		if (message->svcDataId == DATA_ID_NONE)
+		{
+			memset(message->svcData, 0xAA, dataLen);
+		}
+
+		// Append CRC
+		uint32_t crcVal;
+		Status = L2_CrcGen(message, msgLen - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal);
+		if (Status != SUCCESS)
+		{
+			DBG_ERROR("Fail to generate CRC, error code: %" PRId32 ".\n", Status);
+			return Status;
+		}
+		message->crc = crcVal;
+
+		// Logging...
+		memcpy(instPtr->log.txLog.data, message, msgLen);
+		instPtr->log.txLog.dataId = message->svcDataId;
 
-		memcpy(instPtr->log.txLog.data, buff, L2_SVC_PACK_LEN);
-		instPtr->log.txLog.dataId = dataId;
 		break;
-	}
 	default:
-	{
 		DBG_ERROR("Unexpected frame type: %" PRIu8 ".\n", type);
 		return EPARAM;
 		break;
 	}
-	}
 	return SUCCESS;
 #else
 	DBG_ERROR("Unexpected call of Service_Packing in BER test mode.\n");
@@ -178,101 +225,100 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg)
 #ifndef DATALINK_ENABLE_BERTEST
 	L2_FRAME_TYPE frameType = L2_NONE_FRAME;
 	L2_FrameGetChunk(msg, L2_MSG_TYPE, L2_MSG_TYPE_LEN, &frameType);
+	size_t msgLen, dataLen;
+
+	// retrieve basic info
 	switch (frameType)
 	{
-	case L2_SVC_FRAME:
-	{
-		uint8_t nodeId;
-		L2_FrameGetChunk(msg, L2_MSG_SRC_ID, L2_MSG_SRC_ID_LEN, &nodeId);
-		NODE_ENTRY *node;
-		node = L2_GetNodeById(instPtr->nodeList, nodeId);
-		if (node == NULL)
-		{
-			node = L2_AppendNode(instPtr->nodeList, nodeId, L2_NODE_MEMBER);
-			if (node == NULL)
-			{
-				DBG_ERROR("Failed to append new node [id: %" PRIu8 "]", nodeId);
-				Status = ENOMEM;
-				break;
-			}
-		}
-
-		// Update node info.
-		float nodePos;
-		L2_FrameGetChunk(msg, L2_MSG_SVC_SRC_POSX, L2_MSG_SVC_SRC_POSX_LEN, &nodePos);
-		node->nodeInfo.pos.x = (double)nodePos;
-		L2_FrameGetChunk(msg, L2_MSG_SVC_SRC_POSY, L2_MSG_SVC_SRC_POSY_LEN, &nodePos);
-		node->nodeInfo.pos.y = (double)nodePos;
-		L2_FrameGetChunk(msg, L2_MSG_SVC_SRC_POSZ, L2_MSG_SVC_SRC_POSZ_LEN, &nodePos);
-		node->nodeInfo.pos.z = (double)nodePos;
-		float nodeSpeed;
-		L2_FrameGetChunk(msg, L2_MSG_SVC_SRC_SPEEDX, L2_MSG_SVC_SRC_SPEEDX_LEN, &nodeSpeed);
-		node->nodeInfo.speed.x = (double)nodePos;
-		L2_FrameGetChunk(msg, L2_MSG_SVC_SRC_SPEEDY, L2_MSG_SVC_SRC_SPEEDY_LEN, &nodeSpeed);
-		node->nodeInfo.speed.y = (double)nodePos;
-		L2_FrameGetChunk(msg, L2_MSG_SVC_SRC_SPEEDZ, L2_MSG_SVC_SRC_SPEEDZ_LEN, &nodeSpeed);
-		node->nodeInfo.speed.z = (double)nodePos;
-
-		// Reset node ttl
-		node->ttlCnt = 0;
-
-		// Calculate time deviation
-		uint32_t frameNum, slotNum = 0;
-		L2_FrameGetChunk(msg, L2_MSG_FRAME_NUM, L2_MSG_FRAME_NUM_LEN, &frameNum);
-		L2_FrameGetChunk(msg, L2_MSG_SLOT_NUM, L2_MSG_SLOT_NUM_LEN, &slotNum);
-		Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev));
-		if (Status != SUCCESS)
-		{
-			DBG_ERROR("Failed to compute time deviation.\n")
-			break;
-		}
-
-		uint32_t dataId;
-		L2_FrameGetChunk(msg, L2_MSG_SVC_PAYLOAD_ID, L2_MSG_SVC_PAYLOAD_ID_LEN, &dataId);
-
+	case L2_FRAME_DIS:
 		if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
 		{
-			SysMonitor_UpdateRecvPayload(msg + L2_MSG_SVC_PAYLOAD, dataId, PAYLOAD_LEN_FLJ_RX);
+			DBG_ERROR("Unexpected call from head node, only member Node can handle L2_FRAME_DIS.\n");
+			Status = EUNEXP;
+			break;
 		}
-		else
-		{
-			SysMonitor_UpdateRecvPayload(msg + L2_MSG_SVC_PAYLOAD, dataId, PAYLOAD_LEN_LJ_RX);
-		}
-
-		// Logging
-		memcpy(instPtr->log.rxLog.data, msg, L2_SVC_PACK_LEN);
-		instPtr->log.rxLog.dataId = dataId;
-		instPtr->serviceFSM->rxdCnt++;
-
-#ifdef DATALINK_ENABLE_AUTOTIMESYNC
-		// Implement a auto time sync.
-		if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
-		{
-			// Time sync.
-			L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
-		}
-#endif
+	case L2_FRAME_MNG:
+	case L2_FRAME_RTT:
+		L2_MSG_SVC_128K *message = (L2_MSG_SVC_128K *)msg;
+		msgLen = sizeof(L2_MSG_SVC_128K);
+		dataLen = L2_PAYLOAD_LEN_SVC_128K;
+		break;
+	case L2_FRAME_SVC:
+		msgLen = L2_GetSvcPacketLen(instPtr->config.serviceRate);
+		dataLen = L2_GetSvcPayloadLen(instPtr->config.serviceRate);
 		break;
-	}
 	default:
 		DBG_ERROR("Unsupported frame type: %" PRIu8 ".\n", frameType);
+		return EPARAM;
+	}
+
+	// Update node status.
+	uint8_t nodeId = message->common.src;
+	NODE_ENTRY *node;
+	node = L2_GetNodeById(instPtr->nodeList, nodeId);
+	if (node == NULL)
+	{
+		node = L2_AppendNode(instPtr->nodeList, nodeId, L2_NODE_MEMBER);
+		if (node == NULL)
+		{
+			DBG_ERROR("Failed to append new node [id: %" PRId8 "]", nodeId);
+			Status = ENOMEM;
+			break;
+		}
+	}
+
+	// Update node info.
+	node->nodeInfo.pos.x = message->srcPosX;
+	node->nodeInfo.pos.y = message->srcPosY;
+	node->nodeInfo.pos.z = message->srcPosZ;
+	node->nodeInfo.speed.x = message->srcSpeedX;
+	node->nodeInfo.speed.y = message->srcSpeedY;
+	node->nodeInfo.speed.z = message->srcSpeedZ;
+	node->nodeInfo.antIndex = Phy_ReceiveAntIndex(instPtr->config.phyInstPtr);
+
+	// Reset node ttl
+	node->ttlCnt = 0;
+
+	// Calculate time deviation
+	uint32_t frameNum = message->common.frameNum;
+	uint32_t slotNum = (uint32_t)message->common.slotNum;
+	Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev));
+	if (Status != SUCCESS)
+	{
+		DBG_ERROR("Failed to compute time deviation.\n")
 		break;
 	}
-	return Status;
+
+	// Get data payload
+	SysMonitor_UpdateRecvPayload(message->svcData, message->svcDataId, dataLen);
+
+	// Logging...
+	memcpy(instPtr->log.rxLog.data, msg, msgLen);
+	instPtr->log.rxLog.dataId = message->svcDataId;
+
+#ifdef DATALINK_ENABLE_AUTOTIMESYNC
+	// Implement a auto time sync.
+	if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
+	{
+		// Time sync.
+		L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
+	}
+#endif
 #else
 	uint32_t errorBits = 0;
-	for (int i = 0; i < L2_SVC_PACK_LEN; i++)
+	uint32_t pktLen = L2_GetSvcPacketLen(instPtr->config.serviceRate);
+
+	for (int i = 0; i < pktLen; i++)
 	{
 		errorBits += count_ones(msg[i] ^ testData[i]);
 	}
 
 	instPtr->serviceFSM->bitError += errorBits;
-	instPtr->serviceFSM->bitTotal += L2_SVC_PACK_LEN * 8;
+	instPtr->serviceFSM->bitTotal += pktLen * 8;
 
 	// Calculate time deviation
-	uint32_t frameNum, slotNum = 0;
-	L2_FrameGetChunk(msg, L2_MSG_FRAME_NUM, L2_MSG_FRAME_NUM_LEN, &frameNum);
-	L2_FrameGetChunk(msg, L2_MSG_SLOT_NUM, L2_MSG_SLOT_NUM_LEN, &slotNum);
+	uint32_t frameNum = message->common.frameNum;
+	uint32_t slotNum = (uint32_t)message->common.slotNum;
 	Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev));
 	if (Status != SUCCESS)
 	{
@@ -338,7 +384,7 @@ void L2_Service_Reset(L2_SVC_FSM *svcFSMPtr)
 /**
  * @brief  Destroy ServiceNet FSM, free memory
  *
- * @param   scanFsmPtr a pointer to service net finite state machine
+ * @param   serviceFSM a pointer to service net finite state machine
  *
  * @return  NONE
  *
@@ -369,55 +415,157 @@ int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntr
 	}
 
 	PHY_INST *phyRef = instPtr->config.phyInstPtr;
+	PHY_ANT_INDEX antIndex;
 	L2_SVC_FSM *svcFSMPtr = instPtr->serviceFSM;
 	L2_TOR_TYPE torState;
 	const NODE_ENTRY *commNode;
-	if (rtneEntry->src == L2_GetNodeId(instPtr))
-	{
-		torState = L2_TOR_TX;
-		commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->dst);
-	}
-	else if (rtneEntry->dst == L2_GetNodeId(instPtr))
-	{
-		torState = L2_TOR_RX;
-		commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
-	}
-	else
-	{
-		torState = L2_TOR_SUSPEND;
-	}
-
-	// Setup antenna beam
-	if (L2_GetNodeType(instPtr) != L2_NODE_HEAD && commNode != NULL)
-	{
-		Phy_SetBeamWidth(phyRef, PHY_ANT_BEAM_16);
-		Phy_SetBeamNo(phyRef, commNode->nodeInfo.beamNo, PHY_ANT_BEAM_16);
-		instPtr->log.lastBeamWidth = PHY_ANT_BEAM_16;
-	}
-	else
-	{
-		Phy_SetBeamWidth(phyRef, PHY_ANT_BEAM_120);
-		instPtr->log.lastBeamWidth = PHY_ANT_BEAM_120;
-	}
 
 	switch (rtneEntry->slotType)
 	{
-	case L2_SLOT_SERVICE:
-		if (torState == L2_TOR_TX)
+	case L2_SLOT_DISTRIBUTE:
+		if (L2_GetNodeType(instPtr) == L2_NODE_HEAD && rtneEntry->src == L2_GetNodeId(instPtr))
 		{
-			svcFSMPtr->txCnt++;
-#ifndef DATALINK_ENABLE_BERTEST
-			// Packing service data
-			Service_Packing(instPtr, L2_SVC_FRAME, rtneEntry->dst);
-#else
-			memcpy(instPtr->sendPktData, testData, L2_SVC_PACK_LEN);
-#endif
+			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->dst);
+			// antenna selection based on destination id in routing table
+			antIndex = commNode->nodeInfo.antIndex;
+			// head node transmits messages in distribute slot
+			torState = L2_TOR_TX;
+
+			// reset memory
+			memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX);
+			// pack message
+			if (Service_Packing(instPtr, L2_FRAME_DIS, rtneEntry->dst) != SUCCESS)
+			{
+				return FAILURE;
+			}
+		}
+		else if (L2_GetNodeType(instPtr) == L2_NODE_MEMBER && rtneEntry->dst == L2_GetNodeId(instPtr))
+		{
+			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
+			// antenna selection based on node list
+			antIndex = commNode->nodeInfo.antIndex;
+			// member node receives message in advertise slot
+			torState = L2_TOR_RX;
 		}
 		else
 		{
+			// otherwise suspend
+			antIndex = PHY_ANT_INDEX_NONE;
+			torState = L2_TOR_SUSPEND;
+		}
+		(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
+		(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_DIS);
+		break;
+	case L2_SLOT_MANAGE:
+		if (rtneEntry->src == L2_GetNodeId(instPtr))
+		{
+			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->dst);
+			// antenna selection based on destination id in routing table
+			antIndex = commNode->nodeInfo.antIndex;
+			// head node transmits messages in distribute slot
+			torState = L2_TOR_TX;
+
+			// reset memory
+			memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX);
+			// pack message
+			if (Service_Packing(instPtr, L2_FRAME_MNG, rtneEntry->dst) != SUCCESS)
+			{
+				return FAILURE;
+			}
+		}
+		else if (rtneEntry->dst == L2_GetNodeId(instPtr))
+		{
+			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
+			// antenna selection based on node list
+			antIndex = commNode->nodeInfo.antIndex;
+			// member node receives message in advertise slot
+			torState = L2_TOR_RX;
+		}
+		else
+		{
+			// otherwise suspend
+			antIndex = PHY_ANT_INDEX_NONE;
+			torState = L2_TOR_SUSPEND;
+		}
+		(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
+		(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_MNG);
+		break;
+	case L2_SLOT_RTT:
+		if (rtneEntry->src == L2_GetNodeId(instPtr))
+		{
+			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->dst);
+			// antenna selection based on destination id in routing table
+			antIndex = commNode->nodeInfo.antIndex;
+			// head node transmits messages in distribute slot
+			torState = L2_TOR_TX;
+
+			// reset memory
+			memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX);
+			// pack message
+			if (Service_Packing(instPtr, L2_FRAME_RTT, rtneEntry->dst) != SUCCESS)
+			{
+				return FAILURE;
+			}
+		}
+		else if (rtneEntry->dst == L2_GetNodeId(instPtr))
+		{
+			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
+			// antenna selection based on node list
+			antIndex = commNode->nodeInfo.antIndex;
+			// member node receives message in advertise slot
+			torState = L2_TOR_RX;
+		}
+		else
+		{
+			// otherwise suspend
+			antIndex = PHY_ANT_INDEX_NONE;
+			torState = L2_TOR_SUSPEND;
+		}
+		(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
+		(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_RTT);
+		break;
+	case L2_SLOT_SERVICE:
+		if (rtneEntry->src == L2_GetNodeId(instPtr))
+		{
+			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->dst);
+			// antenna selection based on destination id in routing table
+			antIndex = commNode->nodeInfo.antIndex;
+			// head node transmits messages in distribute slot
+			torState = L2_TOR_TX;
+
+			svcFSMPtr->txCnt++;
+
+			// reset memory
+			memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX);
+#ifndef DATALINK_ENABLE_BERTEST
+			// pack message
+			if (Service_Packing(instPtr, L2_FRAME_SVC, rtneEntry->dst) != SUCCESS)
+			{
+				return FAILURE;
+			}
+#else
+			size_t msgLen = L2_GetSvcPacketLen(instPtr->config.serviceRate);
+			memcpy(instPtr->sendPktData, testData, msgLen);
+#endif
+		}
+		else if (rtneEntry->dst == L2_GetNodeId(instPtr))
+		{
+			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
+			// antenna selection based on node list
+			antIndex = commNode->nodeInfo.antIndex;
+			// member node receives message in advertise slot
+			torState = L2_TOR_RX;
+
 			svcFSMPtr->rxCnt++;
 		}
-		L2_SetupNextSlot(instPtr, torState, L2_SVC_FRAME);
+		else
+		{
+			// otherwise suspend
+			antIndex = PHY_ANT_INDEX_NONE;
+			torState = L2_TOR_SUSPEND;
+		}
+		(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
+		(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_SVC);
 		break;
 	default:
 		DBG_ERROR("UNKNOWN slot type [%" PRIu8 "].\n", rtneEntry->slotType);
diff --git a/df_datalink_sp/unit_test/linklayer_emulate_unit_test.c b/df_datalink_sp/unit_test/linklayer_emulate_unit_test.c
index 11644d3..b794e6f 100644
--- a/df_datalink_sp/unit_test/linklayer_emulate_unit_test.c
+++ b/df_datalink_sp/unit_test/linklayer_emulate_unit_test.c
@@ -228,7 +228,7 @@ TEST(linklayer_master_routine)
 
 		if (L2_isDataAvail(linkInstPtr))
 		{
-			if (L2_GetLastOptFrameType(linkInstPtr) == L2_CFM_FRAME)
+			if (L2_GetLastFrameType(linkInstPtr) == L2_CFM_FRAME)
 			{
 				ASSERT_EQUAL(1, L2_GetNodeListLen(linkInstPtr->nodeList));
 				ASSERT_TRUE(L2_isNodeInList(linkInstPtr->nodeList, 2));