df_datalink/src/networking/link_layer/service.c

/*****************************************************************************/
/**
 * @file service.c
 * @author FanHao ([email protected])
 * @brief DF Linklayer service implementation file.
 * @version 0.0
 * @date 2023-05-06
 *
 * @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
 * Lab of Communication and Network
 *
 ******************************************************************************/
/**
 * @addtogroup LinkLayer
 * @{
 */

/**
 * @addtogroup LinkLayer_Service
 * @{
 */
/***************************** Include Files *********************************/
#include <string.h>
#include <stdint.h>
#include "link_layer.h"
#include "link_layer_types.h"
#include "msg_def.h"
#include "error.h"
#include "common.h"
#include "circ_queue.h"
#ifndef EMULATION
#include "../config/app_config.h"
#endif
/************************** Constant Definitions *****************************/
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
/************************** Function Prototypes ******************************/
static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
static void Service_PackingCommon(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
static int32_t Service_Packing128K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
static int32_t Service_Packing512K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
static int32_t Service_Packing2M(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
static int32_t Service_PktHandler128K(L2_INST *instPtr, uint8_t *msg);
static int32_t Service_PktHandler512K(L2_INST *instPtr, uint8_t *msg);
static int32_t Service_PktHandler2M(L2_INST *instPtr, uint8_t *msg);
/************************** Variable Definitions *****************************/

static uint8_t testData[L2_PACK_LEN_MAX] =
{
	0x39, 0x85, 0x02, 0xF0, 0xC7, 0x6A, 0xE8, 0xB4, 0xE7, 0xAB, 0xC9, 0xDC, 0x0B, 0x41, 0x42, 0xE0,
	0xED, 0x21, 0x07, 0x62, 0x4E, 0x80, 0xA0, 0xE1, 0xAC, 0x77, 0x9B, 0x1F, 0x48, 0x0B, 0x58, 0xB9,
	0x45, 0xA5, 0x24, 0xEF, 0xF3, 0x71, 0x2E, 0x84, 0x6C, 0x22, 0xB3, 0x05, 0xEB, 0x75, 0xF2, 0x0B,
	0x45, 0x73, 0x01, 0xA8, 0x1E, 0x00, 0x21, 0x3E, 0x03, 0x86, 0x55, 0x59, 0x98, 0x5C, 0x61, 0xA2,
	0xE8, 0x20, 0x0E, 0x7A, 0xD5, 0x72, 0xAF, 0xF4, 0xBC, 0xB3, 0x6F, 0xFE, 0xB3, 0x78, 0x6A, 0x59,
	0x73, 0xBD, 0x97, 0xBE, 0x44, 0xA4, 0x9B, 0x71, 0x11, 0x45, 0x37, 0xFE, 0xB3, 0x71, 0xEF, 0x1E,
	0x3A, 0x2B, 0x58, 0x07, 0xF6, 0x01, 0x1C, 0x73, 0x28, 0x11, 0xA8, 0x15, 0xA2, 0x58, 0x32, 0x2E,
	0x7D, 0x35, 0x9C, 0xE4, 0x98, 0x7F, 0x9C, 0x81, 0x32, 0xAA, 0xC4, 0x87, 0x16, 0x26, 0x23, 0x95,
	0x57, 0xC9, 0xD4, 0x34, 0xCE, 0x63, 0x78, 0x7E, 0xD4, 0x3C, 0x16, 0x1D, 0x80, 0xC0, 0x55, 0xBF,
	0xB5, 0xD6, 0x0C, 0xA5, 0xB9, 0x0A, 0x14, 0x5B, 0x4D, 0x52, 0xF2, 0xA9, 0x7A, 0x08, 0x67, 0x8E,
	0xEB, 0x67, 0x34, 0x7F, 0x7A, 0x35, 0xAF, 0xD9, 0x07, 0x52, 0x30, 0x38, 0x70, 0xC9, 0x41, 0xF8,
	0x36, 0x61, 0x32, 0x72, 0xA4, 0x7A, 0xEA, 0x21, 0xB4, 0xBF, 0xAA, 0xCF, 0x93, 0x4F, 0xE1, 0x5C,
	0xA0, 0xFB, 0xD0, 0x2B, 0xA3, 0x82, 0x2E, 0x69, 0x7D, 0xB7, 0x76, 0x88, 0xF7, 0x5C, 0xF3, 0x82,
	0xC6, 0x6D, 0x49, 0x92, 0x64, 0x88, 0x9A, 0x43, 0xEE, 0xE9, 0x93, 0xC3, 0x70, 0x50, 0x22, 0xF1,
	0xD3, 0x2B, 0xCF, 0xA9, 0xE5, 0x69, 0x22, 0xF9, 0xBF, 0x78, 0x9A, 0xA4, 0x5A, 0x3D, 0x2E, 0x2A,
	0x23, 0xE3, 0x51, 0x8F, 0x1A, 0xC1, 0xAF, 0x47, 0x9C, 0x14, 0xCE, 0xB9, 0xCD, 0xD0, 0x02, 0xB7,
	0xB8, 0x81, 0xCC, 0x44, 0xE7, 0xA2, 0x6F, 0x32, 0x8C, 0x78, 0x79, 0x10, 0xF3, 0xE1, 0xB0, 0x7C,
	0xAB, 0x40, 0xDB, 0xF9, 0x99, 0xE7, 0x27, 0x30, 0x53, 0x67, 0x00, 0xE8, 0x33, 0x6D, 0xFC, 0x4D,
	0xDC, 0xB0, 0xFC, 0x3D, 0xEF, 0x8D, 0x43, 0xAF, 0x7E, 0x41, 0x69, 0xDA, 0xFD, 0xDF, 0x2F, 0xF8,
	0xEC, 0xFB, 0x6A, 0x65, 0xE7, 0xEC, 0xC7, 0x53, 0x1B, 0x14, 0xDF, 0xBD, 0x0C, 0x54, 0x57, 0xD2,
	0x56, 0x15, 0xF4, 0x34, 0xE2, 0xFB, 0x12, 0xA0, 0xA2, 0xDE, 0x82, 0xF5, 0x04, 0x81, 0x9B, 0x08,
	0xB6, 0x8C, 0x7B, 0xE0, 0xAA, 0xDF, 0xC9, 0x92, 0x9D, 0x5F, 0x2D, 0xF7, 0x4A, 0x0B, 0xFF, 0xA5,
	0xAA, 0xDC, 0xD6, 0x3F, 0x56, 0xB2, 0x6A, 0x3C, 0x6F, 0xAA, 0xAC, 0x84, 0xAC, 0x3F, 0x3D, 0x05,
	0x96, 0x12, 0x44, 0x39, 0x1C, 0xD1, 0xFA, 0xBE, 0x6B, 0x1A, 0xCD, 0x31, 0x29, 0x41, 0x31, 0xA3,
	0xA8, 0x0D, 0x87, 0xEB, 0x96, 0x21, 0x39, 0xD6, 0x3B, 0x11, 0x50, 0xB5, 0xFE, 0x44, 0x0E, 0x74,
	0x31, 0x12, 0xA1, 0x66, 0x49, 0xFD, 0x2D, 0x75, 0x72, 0xF9, 0xC8, 0xD9, 0x6C, 0xC9, 0x11, 0x74,
	0x2E, 0xEB, 0x20, 0x33, 0x3D, 0x2C, 0x5E, 0xEA, 0x39, 0x31, 0x88, 0xC7, 0xD1, 0x93, 0x95, 0xD8,
	0x9E, 0x87, 0x93, 0xBC, 0x09, 0x17, 0xC9, 0xA2, 0x59, 0x18, 0x51, 0x8F, 0xBF, 0x55, 0x45, 0x81,
	0x8D, 0xFD, 0xAF, 0xB2, 0x21, 0xB7, 0xD8, 0xA8, 0x00, 0x2E, 0x70, 0x73, 0x2F, 0x74, 0xBC, 0x61,
	0x0A, 0x8E, 0xE3, 0x0A, 0x92, 0x82, 0xD2, 0x82, 0xBA, 0xE4, 0x3E, 0x8E, 0xBB, 0xC7, 0xA2, 0x8F,
	0x68, 0xE1, 0x29, 0x34, 0x7C, 0xB4, 0x22, 0xCF, 0x36, 0x8F, 0x9D, 0x63, 0x9D, 0x04, 0x8F, 0x4A,
	0xFB, 0x84, 0xBB, 0x6B, 0x97, 0x2C, 0x15, 0xC9, 0x37, 0x4F, 0x40, 0xD7, 0x47, 0x50, 0x39, 0x97
};

/*****************************************************************************/
/**
 * @brief   Packing Service message
 *
 * @param   instPtr		pointer to linklayer instance
 *          type		message type.
 *          dstId		destination node address.
 *
 * @return	EPARAM
 * 			SUCCESS
 *
 ******************************************************************************/
static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
{
#ifndef DATALINK_ENABLE_BERTEST
	ASSERT(instPtr != NULL);
	int32_t Status;

	switch (type)
	{
	case L2_FRAME_DIS:
	case L2_FRAME_MNG:
	case L2_FRAME_RTT:
	{
		Status = Service_Packing128K(instPtr, type, dstId);
		break;
	}
	case L2_FRAME_SVC:
	{
		switch (instPtr->config.serviceRate)
		{
		case L2_SVC_128K:
			Status = Service_Packing128K(instPtr, type, dstId);
			break;
		case L2_SVC_512K:
			Status = Service_Packing512K(instPtr, type, dstId);
			break;
		case L2_SVC_2M:
			Status = Service_Packing2M(instPtr, type, dstId);
			break;
		default:
			DBG_ERROR("Unexpected service rate: %" PRIu8 ".\n", instPtr->config.serviceRate);
			return EPARAM;
		}
		break;
	}
	default:
		DBG_ERROR("Unexpected frame type: %" PRIu8 ".\n", type);
		return EPARAM;
	}
	return Status;
#else
	DBG_ERROR("Unexpected call of Service_Packing in BER test mode.\n");
	return EUNEXP;
#endif
}

/*****************************************************************************/
/**
 * @brief   Packing Service message (common part)
 *
 * @param   instPtr		pointer to linklayer instance
 *          type		message type.
 *          dstId		destination node address.
 *
 * @return	EPARAM
 * 			SUCCESS
 *
 ******************************************************************************/
static void Service_PackingCommon(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
{
	ASSERT(instPtr != NULL);

	L2_MSG_COMMON *message = (L2_MSG_COMMON *)(instPtr->sendPktData);

	// Common section of link layer package
	message->frameNum = L2_GetNextFrameNum(instPtr);
	message->slotNum = L2_GetNextSlotNum(instPtr);
	message->src = L2_GetNodeId(instPtr);
	message->dst = dstId;
	message->msgType = type;
}

/*****************************************************************************/
/**
 * @brief   Packing Service message (128 kbps)
 *
 * @param   instPtr		pointer to linklayer instance
 *          type		message type.
 *          dstId		destination node address.
 *
 * @return	EPARAM
 * 			SUCCESS
 *
 ******************************************************************************/
static int32_t Service_Packing128K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
{
	ASSERT(instPtr != NULL);
	int32_t Status;

	Service_PackingCommon(instPtr, type, dstId);

	L2_MSG_SVC_128K *message = (L2_MSG_SVC_128K *)(instPtr->sendPktData);
	size_t msgLen = sizeof(L2_MSG_SVC_128K);
	size_t dataLen = L2_PAYLOAD_LEN_SVC_128K;

	// 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((uint8_t *)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;

	return SUCCESS;
}

/*****************************************************************************/
/**
 * @brief   Packing Service message (512 kbps)
 *
 * @param   instPtr		pointer to linklayer instance
 *          type		message type.
 *          dstId		destination node address.
 *
 * @return	EPARAM
 * 			SUCCESS
 *
 ******************************************************************************/
static int32_t Service_Packing512K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
{
	ASSERT(instPtr != NULL);
	int32_t Status;

	Service_PackingCommon(instPtr, type, dstId);

	L2_MSG_SVC_512K *message = (L2_MSG_SVC_512K *)(instPtr->sendPktData);
	size_t msgLen = sizeof(L2_MSG_SVC_512K);
	size_t dataLen = L2_PAYLOAD_LEN_SVC_512K;

	// 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((uint8_t *)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;


	return SUCCESS;
}

/*****************************************************************************/
/**
 * @brief   Packing Service message (2 Mbps)
 *
 * @param   instPtr		pointer to linklayer instance
 *          type		message type.
 *          dstId		destination node address.
 *
 * @return	EPARAM
 * 			SUCCESS
 *
 ******************************************************************************/
static int32_t Service_Packing2M(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
{
	ASSERT(instPtr != NULL);
	int32_t Status;

	Service_PackingCommon(instPtr, type, dstId);

	L2_MSG_SVC_2M *message = (L2_MSG_SVC_2M *)(instPtr->sendPktData);
	size_t msgLen = sizeof(L2_MSG_SVC_2M);
	size_t dataLen = L2_PAYLOAD_LEN_SVC_2M;

	// 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((uint8_t *)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;


	return SUCCESS;
}

/*****************************************************************************/
/**
 * @brief   Handle service received packet. Send data up to MPC
 *
 * @param   msg A point32_ter the the message frame.
 *
 * @return  SUCCESS message has been successfully handled.
 *          other       error occurred.
 *
 ******************************************************************************/
int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg)
{
	ASSERT(instPtr != NULL);
	ASSERT(msg != NULL);
	int32_t Status;
#ifndef DATALINK_ENABLE_BERTEST
	L2_FRAME_TYPE frameType = L2_FRAME_NONE;
	L2_FrameGetChunk(msg, L2_MSG_TYPE, L2_MSG_TYPE_LEN, &frameType);

	// retrieve basic info
	switch (frameType)
	{
	case L2_FRAME_DIS:
		if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
		{
			DBG_ERROR("Unexpected call from head node, only member Node can handle L2_FRAME_DIS.\n");
			return EUNEXP;
		}
	case L2_FRAME_MNG:
	case L2_FRAME_RTT:
		Status = Service_PktHandler128K(instPtr, msg);
		break;
	case L2_FRAME_SVC:
		switch (instPtr->config.serviceRate)
		{
		case L2_SVC_128K:
			Status = Service_PktHandler128K(instPtr, msg);
			break;
		case L2_SVC_512K:
			Status = Service_PktHandler512K(instPtr, msg);
			break;
		case L2_SVC_2M:
			Status = Service_PktHandler2M(instPtr, msg);
			break;
		default:
			DBG_ERROR("Unexpected service rate: %" PRIu8 ".\n", instPtr->config.serviceRate);
			return EPARAM;
		}
		break;
	default:
		DBG_ERROR("Unsupported frame type: %" PRIu8 ".\n", frameType);
		return EPARAM;
	}
	return Status;

#else
	uint32_t errorBits = 0;
	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 += pktLen * 8;

	// 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")
		return Status;
	}
	// Implement a auto time sync.
	if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
	{
		// Time sync.
		L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
	}
	return SUCCESS;
#endif
}

/*****************************************************************************/
/**
 * @brief   Handle service received packet. Send data up to MPC. (128 kbps)
 *
 * @param   msg A point32_ter the the message frame.
 *
 * @return  SUCCESS message has been successfully handled.
 *          other       error occurred.
 *
 ******************************************************************************/
static int32_t Service_PktHandler128K(L2_INST *instPtr, uint8_t *msg)
{
	ASSERT(instPtr != NULL);
	ASSERT(msg != NULL);
	int32_t Status;

	L2_MSG_SVC_128K *message = (L2_MSG_SVC_128K *)msg;
	size_t msgLen = sizeof(L2_MSG_SVC_128K);
	size_t dataLen = L2_PAYLOAD_LEN_SVC_128K;

	// 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);
			return ENOMEM;
		}
	}

	// Update node info.
	node->nodeInfo.pos.x = (double)message->srcPosX;
	node->nodeInfo.pos.y = (double)message->srcPosY;
	node->nodeInfo.pos.z = (double)message->srcPosZ;
	node->nodeInfo.speed.x = (double)message->srcSpeedX;
	node->nodeInfo.speed.y = (double)message->srcSpeedY;
	node->nodeInfo.speed.z = (double)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");
		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

	return Status;
}

/*****************************************************************************/
/**
 * @brief   Handle service received packet. Send data up to MPC. (512 kbps)
 *
 * @param   msg A point32_ter the the message frame.
 *
 * @return  SUCCESS message has been successfully handled.
 *          other       error occurred.
 *
 ******************************************************************************/
static int32_t Service_PktHandler512K(L2_INST *instPtr, uint8_t *msg)
{
	ASSERT(instPtr != NULL);
	ASSERT(msg != NULL);
	int32_t Status;

	L2_MSG_SVC_512K *message = (L2_MSG_SVC_512K *)msg;
	size_t msgLen = sizeof(L2_MSG_SVC_512K);
	size_t dataLen = L2_PAYLOAD_LEN_SVC_512K;

	// 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);
			return ENOMEM;
		}
	}

	// Update node info.
	node->nodeInfo.pos.x = (double)message->srcPosX;
	node->nodeInfo.pos.y = (double)message->srcPosY;
	node->nodeInfo.pos.z = (double)message->srcPosZ;
	node->nodeInfo.speed.x = (double)message->srcSpeedX;
	node->nodeInfo.speed.y = (double)message->srcSpeedY;
	node->nodeInfo.speed.z = (double)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");
		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

	return Status;
}

/*****************************************************************************/
/**
 * @brief   Handle service received packet. Send data up to MPC. (2 Mbps)
 *
 * @param   msg A point32_ter the the message frame.
 *
 * @return  SUCCESS message has been successfully handled.
 *          other       error occurred.
 *
 ******************************************************************************/
static int32_t Service_PktHandler2M(L2_INST *instPtr, uint8_t *msg)
{
	ASSERT(instPtr != NULL);
	ASSERT(msg != NULL);
	int32_t Status;

	L2_MSG_SVC_2M *message = (L2_MSG_SVC_2M *)msg;
	size_t msgLen = sizeof(L2_MSG_SVC_2M);
	size_t dataLen = L2_PAYLOAD_LEN_SVC_2M;

	// 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);
			return ENOMEM;
		}
	}

	// Update node info.
	node->nodeInfo.pos.x = (double)message->srcPosX;
	node->nodeInfo.pos.y = (double)message->srcPosY;
	node->nodeInfo.pos.z = (double)message->srcPosZ;
	node->nodeInfo.speed.x = (double)message->srcSpeedX;
	node->nodeInfo.speed.y = (double)message->srcSpeedY;
	node->nodeInfo.speed.z = (double)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");
		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

	return Status;
}

/*****************************************************************************/
/**
 * @brief   Initialize service net FSM
 *
 * @param   svcFSMPtr  A pointer to service FSM instance.
 *
 * @return  SUCCESS Initialize successfully.
 *          Other       Initialize failed.
 *
 ******************************************************************************/
int32_t L2_Service_Init(L2_SVC_FSM **svcFSMPtr)
{
	ASSERT(svcFSMPtr != NULL);
	*svcFSMPtr = (L2_SVC_FSM *)malloc(sizeof(L2_SVC_FSM));
	if (*svcFSMPtr == NULL)
	{
		return ENOMEM;
	}
	L2_Service_Reset(*svcFSMPtr);
	return SUCCESS;
}

/*****************************************************************************/
/**
 * @brief  Reset ServiceNet FSM
 *
 * @param   svcFSMPtr a pointer to service net finite state machine
 *
 * @return  NONE
 *
 ******************************************************************************/
void L2_Service_Reset(L2_SVC_FSM *svcFSMPtr)
{
	ASSERT(svcFSMPtr != NULL);
	svcFSMPtr->txCnt = 0;
	svcFSMPtr->rxCnt = 0;
	svcFSMPtr->rxdCnt = 0;
#ifdef DATALINK_ENABLE_BERTEST
	svcFSMPtr->bitTotal = 0;
	svcFSMPtr->bitError = 0;
	svcFSMPtr->lossRate = 0;
	svcFSMPtr->errorRate = 0;
#endif
}

/*****************************************************************************/
/**
 * @brief  Destroy ServiceNet FSM, free memory
 *
 * @param   serviceFSM a pointer to service net finite state machine
 *
 * @return  NONE
 *
 ******************************************************************************/
void L2_Service_Destroy(L2_SVC_FSM *serviceFSM)
{
	free(serviceFSM);
}

/*****************************************************************************/
/**
 * @brief   Service FSM Routine.
 *
 * @param   instPtr
 * @param   schedEntry
 *
 * @return  Slot has been used.
 *
 ******************************************************************************/
int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
{
	ASSERT(instPtr != NULL);
	DBG_INFO("-------- ServiceNet Routine --------\n");

	if (rtneEntry->slotSeq != 0)
	{
		return SUCCESS; // skip
	}

	PHY_INST *phyRef = instPtr->config.phyInstPtr;
	PHY_ANT_INDEX antIndex;
	L2_SVC_FSM *svcFSMPtr = instPtr->serviceFSM;
	L2_TOR_TYPE torState;
	const NODE_ENTRY *commNode;

	switch (rtneEntry->slotType)
	{
	case L2_SLOT_DISTRIBUTE:
		if (L2_GetNodeType(instPtr) == L2_NODE_HEAD && 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_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++;
		}
		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);
		return FAILURE;
	}
#ifdef DATALINK_ENABLE_BERTEST
	uint32_t pktLoss;
	pktLoss = svcFSMPtr->rxCnt - svcFSMPtr->rxdCnt;
	svcFSMPtr->lossRate = (svcFSMPtr->rxCnt == 0) ? 0 : (double)pktLoss / (double)svcFSMPtr->rxCnt;
	svcFSMPtr->errorRate = (svcFSMPtr->bitTotal == 0) ? 0 : (double)svcFSMPtr->bitError / (double)svcFSMPtr->bitTotal;
	DBG_PRINT("Packet Loss Rate -- Total: %lu, Loss: %lu.\n", instPtr->log.svcRxCnt, pktLoss);
	DBG_PRINT("Bit Error Rate -- Total:%lu, Error:%lu.\n", svcFSMPtr->bitTotal, svcFSMPtr->bitError);
#endif
	return SUCCESS;
}

/** @} LinkLayer_Service*/
/** @} LinkLayer*/