KY-XTZ/src/networking/link_layer/link_layer.c

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

/**
 * @addtogroup LinkLayer
 * @{
 */

/***************************** Include Files *********************************/
#include <stdlib.h>
#include <string.h>
#include "link_layer.h"
#include "link_layer_types.h"
#include "node_list.h"
#include "msg_def.h"
#include "error.h"
#include "common.h"
#include "util.h"
#include "circ_queue.h"
#include "time_ctrl/time_ctrl.h"
/************************** Constant Definitions *****************************/
#define NET_START_TIME_INFTY UINT32_MAX
#define LINK_BROCK_THRESHOLD L2_SLOTS_PER_FRAME // 0.5 secods

/************************** Variable Definitions *****************************/
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
/************************** Function Prototypes ******************************/
static void L2_RcdShift(L2_RCD *rcd, uint32_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);
static void L2_UpdateSlotNum(L2_INST *instPtr);
static void L2_DataAvailUpdate(L2_INST *instPtr, bool status);
static void L2_FrameStateUpdate(L2_INST *instPtr);

/**
 * @brief   Shift L2 status record.(newval->next->curr->last)
 */
inline void L2_RcdShift(L2_RCD *rcd, uint32_t netxVal)
{
	ASSERT(rcd != NULL);
	rcd->last = rcd->curr;
	rcd->curr = rcd->next;
	rcd->next = netxVal;
}

/**
 * @brief	helper function to update linkelayer state
 */
static inline void UpdateMainSate(L2_INST *instPtr, L2_NODE_STATE newSate)
{
	ASSERT(instPtr != NULL);
	DBG_INFO("MainStateUpdate: %" PRId8 " -> %" PRId8 "\n", instPtr->mainState, newSate);
	instPtr->nextState = newSate;
}

/**
 * @brief	helper function to set slot number.
 */
static void L2_SetSlotNum(L2_INST *instPtr, uint32_t newSlotNum)
{
	ASSERT(instPtr != NULL);
	DBG_INFO("Set new slot number to %" PRIu32 "\n", newSlotNum);
	instPtr->slotNum.next = newSlotNum;
}

/**
 * @brief	helper function to set frame number
 */
static void L2_SetFrameNum(L2_INST *instPtr, uint32_t newFrameNum)
{
	ASSERT(instPtr != NULL);
	DBG_INFO("Set new frame number to %" PRIu32 "\n", newFrameNum);
	instPtr->frameNum.next = newFrameNum;
}

/**
 * @brief   Update frame number
 */
static inline void L2_UpdateSlotNum(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	uint32_t slotCnt;
	// Shift record
	L2_RcdShift(&(instPtr->frameNum), instPtr->frameNum.next);
	L2_RcdShift(&(instPtr->slotNum), instPtr->slotNum.next);

	// FIXME: Consider time wrapping
	// Compute and set new slot and frame num
	slotCnt = TimeCtrl_GetIntCnt(instPtr->config.tmcInstPtr) + 1;
	instPtr->frameNum.next = slotCnt / L2_SLOTS_PER_FRAME - instPtr->netStartTime;
	instPtr->slotNum.next = slotCnt % L2_SLOTS_PER_FRAME;
}

/**
 * @brief   Update l2 frame buffer state
 * 			infer that if there is a new valid l2 frame up.
 */
static inline void L2_DataAvailUpdate(L2_INST *instPtr, bool status)
{
	ASSERT(instPtr != NULL);
	instPtr->dataAvail = status;
}

/*****************************************************************************/
/**
 * @brief   Update L2 frame state, check if there is any valid L2 frame data in
 * 			data buffer, update receive frame status.
 *
 * @param   instPtr		a pointer to linklayer instance
 *
 * @return	NONE.
 *
 ******************************************************************************/
static void L2_FrameStateUpdate(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	PHY_INST *phyRef = instPtr->config.phyInstPtr;
	bool isValid;
	L2_FRAME_TYPE recvType = L2_GetLastOptFrameType(instPtr);
	isValid = false;
	// Update frame state.
	if (Phy_IsFrameValid(phyRef) == false)
	{
		L2_DataAvailUpdate(instPtr, false);
	}
	else
	{
		DBG_INFO("A frame received, checking data validation...\n");
		DBG_INFO("Last frame type is: %" PRId32 ".\n", instPtr->frame.last);
		Phy_RetrieveRecvData(phyRef, instPtr->recvPktData);
		instPtr->log.frameRecv++;

		// Update frame state.
		switch (recvType)
		{
		case L2_SCAN_FRAME:
		case L2_ADV_FRAME:
			isValid = L2_CrcCheck(instPtr->recvPktData, L2_ADV_PACK_LEN, L2_CRC16_1021);
			instPtr->log.advRecvCnt++;
			instPtr->log.advCorrectCnt = isValid ? instPtr->log.advCorrectCnt + 1 : instPtr->log.advCorrectCnt;
			break;
		case L2_CFM_FRAME:
			isValid = L2_CrcCheck(instPtr->recvPktData, L2_CMF_PACK_LEN, L2_CRC16_1021);
			instPtr->log.cfmRecvCnt++;
			instPtr->log.cfmCorrectCnt = isValid ? instPtr->log.cfmCorrectCnt + 1 : instPtr->log.cfmCorrectCnt;
			break;
		case L2_SVC_2K_FRAME:
			isValid = L2_CrcCheck(instPtr->recvPktData, L2_SVC_PACK_2K_LEN, L2_CRC16_1021);
			instPtr->log.svcRecvCnt++;
			instPtr->log.svcCorrectCnt = isValid ? instPtr->log.svcCorrectCnt + 1 : instPtr->log.svcCorrectCnt;
			break;
		case L2_SVC_128K_FRAME:
			isValid = L2_CrcCheck(instPtr->recvPktData, L2_SVC_PACK_128K_LEN, L2_CRC16_1021);
			instPtr->log.svcRecvCnt++;
			instPtr->log.svcCorrectCnt = isValid ? instPtr->log.svcCorrectCnt + 1 : instPtr->log.svcCorrectCnt;
			break;
		case L2_SVC_256K_FRAME:
			isValid = L2_CrcCheck(instPtr->recvPktData, L2_SVC_PACK_256K_LEN, L2_CRC16_1021);
			instPtr->log.svcRecvCnt++;
			instPtr->log.svcCorrectCnt = isValid ? instPtr->log.svcCorrectCnt + 1 : instPtr->log.svcCorrectCnt;
			break;

		default:
			isValid = false;
			break;
		}
		if (isValid)
		{
			instPtr->log.frameCorrect++;
			L2_DataAvailUpdate(instPtr, true);
		}
		else
		{
			L2_DataAvailUpdate(instPtr, false);
		}
	}
}

/**
 * @addtogroup LinkLayer_Utility
 * @{
 */

/**
 * @brief   Get the current node ID.
 */
inline uint8_t L2_GetNodeId(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->myInfo.nodeId;
}

/**
 * @brief   Get the current cluster ID.
 */
inline uint8_t L2_GetClusterId(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->myInfo.clustId;
}

/**
 * @brief   Get the head node ID.
 */
uint8_t L2_GetHeadId(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->myInfo.headId;
}

/**
 * @brief   Get the current node type.
 */
inline L2_NODE_TYPE L2_GetNodeType(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->myInfo.nodeType;
}

/**
 * @brief   Get the current linklayer state.
 */
inline L2_NODE_STATE L2_GetMainState(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->mainState;
}

/**
 * @brief   Check if there is a valid l2 frame.
 */
inline bool L2_isStarted(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->isEnable;
}

/**
 * @brief   Check if there is a valid l2 frame.
 */
inline bool L2_isDataAvail(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->dataAvail;
}

/**
 * @brief   Check if node is connected to the network
 */
inline bool L2_isCxn2Net(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->cxn2Net;
}

/**
 * @brief   Get corresponding frame length in byte of L2 frame.
 */
inline uint32_t L2_GetFrameLen(L2_FRAME_TYPE l2FrameType)
{
	return Phy_GetFrameLen(L2_L2frame2L1frame(l2FrameType));
}

/**
 * @brief   Get last frame number.
 */
inline uint32_t L2_GetLastFrameNum(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->frameNum.last;
}

/**
 * @brief   Get last slot number.
 */
inline uint32_t L2_GetLastSlotNum(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->slotNum.last;
}

/**
 * @brief   Get the frame type of last slot.
 */
inline L2_FRAME_TYPE L2_GetLastFrameType(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->frame.last;
}

/**
 * @brief    Get the tx/rx state of last slot.
 */
inline L2_TOR_TYPE L2_GetLastTor(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->tor.last;
}

/**
 * @brief   Get current frame number.
 */
inline uint32_t L2_GetCurrentFrameNum(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->frameNum.curr;
}

/**
 * @brief   Get current slot number.
 */
inline uint32_t L2_GetCurrentSlotNum(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->slotNum.curr;
}

/**
 * @brief    Get the frame type of current slot.
 */
inline L2_FRAME_TYPE L2_GetCurrentFrameType(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->frame.curr;
}

/**
 * @brief    Get the current working speed mode.
 */
inline L2_SPEED_MODE L2_GetCurrentSpeedMode(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->speedMode;
}

/**
 * @brief    Get the tx/rx state of current slot.
 */
inline L2_TOR_TYPE L2_GetCurrentTor(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->tor.curr;
}

/**
 * @brief   Get next frame number.
 */
inline uint32_t L2_GetNextFrameNum(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->frameNum.next;
}
/**
 * @brief   Get next slot number.
 */
inline uint32_t L2_GetNextSlotNum(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->slotNum.next;
}

/**
 * @brief    Get the frame type of next slot.
 */
inline L2_FRAME_TYPE L2_GetNextFrameType(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->frame.next;
}

/**
 * @brief    Get the tx/rx state of next slot.
 */
inline L2_TOR_TYPE L2_GetNextTor(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	return instPtr->tor.next;
}

/*****************************************************************************/
/**
 * @brief   Infer last received frame type.
 *
 * @param   instPtr  A pointer to physical layer control instance.
 *
 * @return  L2_FRAME_TYPE.
 *
 ******************************************************************************/
L2_FRAME_TYPE L2_GetLastOptFrameType(L2_INST *instPtr)
{
	if (instPtr->myInfo.nodeType == L2_NODE_HEAD || L2_isCxn2Net(instPtr))
	{
		return instPtr->frame.curr;
	}
	switch (instPtr->scanFSM->scanStage)
	{
	case L2_SCAN_INIT_STAGE:
		return L2_SCAN_FRAME;
	case L2_SCAN_ADV_STAGE:
		return L2_ADV_FRAME;
	case L2_SCAN_CFM_STAGE:
		return L2_CFM_FRAME;
	default:
		return L2_NONE_FRAME;
	}
}
/*****************************************************************************/
/**
 * @brief   Convert L2 frame type to L1 frame type
 *
 * @param   l2Frame
 *
 * @return  L1 frame type.
 *
 ******************************************************************************/
L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame)
{
	switch (l2Frame)
	{
	case L2_SCAN_FRAME:
		return L1_10KBPS_10MS_FRAME;
	case L2_ADV_FRAME:
		return L1_10KBPS_20MS_FRAME;
	case L2_CFM_FRAME:
	case L2_SVC_2K_FRAME:
		return L1_40KBPS_20MS_FRAME;
	case L2_SVC_128K_FRAME:
		return L1_200KBPS_5MS_FRAME;
	case L2_SVC_256K_FRAME:
		return L1_1M2BPS_5MS_FRAME;
	default:
		return L1_NONE_FRAME;
	}
}
/** @} LinkLayer_Utility*/

/**
 * @defgroup LinkLayer_Main
 * @{
 */

/*****************************************************************************/
/**
 * @brief   Initialize link layer fsm instance, allocate memory to storage fsm
 *          data. Apply the default values to fsm.
 *
 * @param   instPtr	a pointer to physical layer controller instance
 * @param	config a pointer to configurations
 *
 * @return  SUCCESS
 *          Other error code.
 *
 ******************************************************************************/
int32_t Linklayer_Init(L2_INST **instPtr, const L2_CONFIG *config)
{
	ASSERT(instPtr != NULL);
	ASSERT(config != NULL);

	int32_t Status;

	if (config->phyInstPtr == NULL)
	{
		return EDINVAL;
	}
	if (config->tmcInstPtr == NULL)
	{
		return EDINVAL;
	}
	(*instPtr) = (L2_INST *)malloc(sizeof(L2_INST));
	if (*instPtr == NULL)
	{
		DBG_ERROR("Allocate memory for L2_InstPtr failed.\n");
		return ENOMEM;
	}

	// Clear random content
	memset(*instPtr, 0x00, sizeof(L2_INST));
	(*instPtr)->speedMode = config->speedMode;
	(*instPtr)->sendPktData = NULL;
	(*instPtr)->recvPktData = NULL;
	(*instPtr)->schdEventPool = NULL;
	(*instPtr)->nodeList = NULL;
	(*instPtr)->scanFSM = NULL;
	(*instPtr)->serviceFSM = NULL;
	memcpy(&((*instPtr)->config), config, sizeof(L2_CONFIG));

	// Packet buffer
	(*instPtr)->sendPktData = calloc(L2_PACK_LEN_MAX / sizeof(uint64_t), sizeof(uint64_t));
	if ((*instPtr)->sendPktData == NULL)
	{
		DBG_ERROR("Allocate memory for sendPktData failed.\n");
		Linklayer_Destroy(*instPtr);
		return ENOMEM;
	}
	(*instPtr)->recvPktData = calloc(L2_PACK_LEN_MAX / sizeof(uint64_t), sizeof(uint64_t));
	if ((*instPtr)->recvPktData == NULL)
	{
		DBG_ERROR("Allocate memory for recvPktData failed.\n");
		Linklayer_Destroy(*instPtr);
		return ENOMEM;
	}
	// Reset basic info
	(*instPtr)->netStartTime = NET_START_TIME_INFTY;
	(*instPtr)->rtneConfig.last = NULL;
	(*instPtr)->rtneConfig.curr = NULL;
	(*instPtr)->rtneConfig.next = NULL;
	(*instPtr)->isEnable = false;
	(*instPtr)->cxn2Net = (config->myType == L2_NODE_HEAD) ? true : false;
	(*instPtr)->dataAvail = false;
	(*instPtr)->frameNum.last = 0;
	(*instPtr)->frameNum.curr = 0;
	(*instPtr)->frameNum.next = 0;
	(*instPtr)->slotNum.last = 0;
	(*instPtr)->slotNum.curr = 0;
	(*instPtr)->slotNum.next = 0;
	(*instPtr)->frame.last = L2_NONE_FRAME;
	(*instPtr)->frame.curr = L2_NONE_FRAME;
	(*instPtr)->frame.next = L2_NONE_FRAME;
	(*instPtr)->tor.last = L2_TOR_SUSPEND;
	(*instPtr)->tor.curr = L2_TOR_SUSPEND;
	(*instPtr)->tor.next = L2_TOR_SUSPEND;
	(*instPtr)->rtneConfig.rtneTable = initTable;
	(*instPtr)->rtneConfig.rtnePrd = ARRAY_SIZE(initTable);
	(*instPtr)->mainState = L2_INIT_STATE;
	(*instPtr)->nextState = L2_INIT_STATE;
	(*instPtr)->myInfo.nodeId = config->myId;
	(*instPtr)->myInfo.clustId = config->clustId;
	(*instPtr)->myInfo.headId = config->headId;
	(*instPtr)->myInfo.nodeType = config->myType;
	(*instPtr)->myInfo.pos.x = 0;
	(*instPtr)->myInfo.pos.y = 0;
	(*instPtr)->myInfo.pos.z = 0;
	(*instPtr)->myInfo.speed.x = 0;
	(*instPtr)->myInfo.speed.y = 0;
	(*instPtr)->myInfo.speed.z = 0;
	(*instPtr)->schdEventPool = NULL;
	(*instPtr)->nodeList = NULL;

	Status = L2_CreateSchedEventPool(&((*instPtr)->schdEventPool));
	if (Status != SUCCESS)
	{
		DBG_ERROR("L2_CreateSchedEventPool runs failed, with error code: %" PRId32 ".\n", Status);
		Linklayer_Destroy(*instPtr);
		return Status;
	}

	Status = L2_NodeList_Init(&((*instPtr)->nodeList));
	if (Status != SUCCESS)
	{
		DBG_ERROR("NodeList_Init runs failed, with error code: %" PRId32 ".\n", Status);
		Linklayer_Destroy(*instPtr);
		return Status;
	}

	Status = L2_Scan_Init(&((*instPtr)->scanFSM));
	if (Status != SUCCESS)
	{
		Linklayer_Destroy(*instPtr);
		return Status;
	}

	Status = L2_Service_Init(&((*instPtr)->serviceFSM));
	if (Status != SUCCESS)
	{
		Linklayer_Destroy(*instPtr);
		return Status;
	}

	return SUCCESS;
}

/*****************************************************************************/
/**
 * @brief   Reconfigure link layer instance, reset all segment and reconfigure
 * 			accordingly.
 *
 * @param   instPtr	a pointer to physical layer controller instance
 * @param	config a pointer to configurations
 *
 * @return  SUCCESS
 *          Other error code.
 *
 ******************************************************************************/
int32_t Linklayer_Reconfig(L2_INST *instPtr, const L2_CONFIG *config)
{
	ASSERT(instPtr != NULL);
	ASSERT(config != NULL);

	if (config->phyInstPtr == NULL)
	{
		return EDINVAL;
	}
	if (config->tmcInstPtr == NULL)
	{
		return EDINVAL;
	}
	memcpy(&(instPtr->config), config, sizeof(L2_CONFIG));

	// Reset basic info
	memset(&(instPtr->log), 0x00, sizeof(L2_LOG));
	instPtr->netStartTime = UINT32_MAX;
	instPtr->rtneConfig.last = NULL;
	instPtr->rtneConfig.curr = NULL;
	instPtr->rtneConfig.next = NULL;
	instPtr->cxn2Net = (config->myType == L2_NODE_HEAD) ? true : false;
	instPtr->dataAvail = false;
	instPtr->frameNum.last = 0;
	instPtr->frameNum.curr = 0;
	instPtr->frameNum.next = 0;
	instPtr->slotNum.last = 0;
	instPtr->slotNum.curr = 0;
	instPtr->slotNum.next = 0;
	instPtr->frame.last = L2_NONE_FRAME;
	instPtr->frame.curr = L2_NONE_FRAME;
	instPtr->frame.next = L2_NONE_FRAME;
	instPtr->tor.last = L2_TOR_SUSPEND;
	instPtr->tor.curr = L2_TOR_SUSPEND;
	instPtr->tor.next = L2_TOR_SUSPEND;
	instPtr->rtneConfig.rtneTable = initTable;
	instPtr->rtneConfig.rtnePrd = ARRAY_SIZE(initTable);
	instPtr->mainState = L2_INIT_STATE;
	instPtr->nextState = L2_INIT_STATE;
	instPtr->myInfo.nodeId = config->myId;
	instPtr->myInfo.clustId = config->clustId;
	instPtr->myInfo.headId = config->headId;
	instPtr->myInfo.nodeType = config->myType;
	instPtr->myInfo.pos.x = 0;
	instPtr->myInfo.pos.y = 0;
	instPtr->myInfo.pos.z = 0;
	instPtr->myInfo.speed.x = 0;
	instPtr->myInfo.speed.y = 0;
	instPtr->myInfo.speed.z = 0;

	// Reset according instance
	L2_ClearSchedEventPool(instPtr->schdEventPool);
	L2_NodeList_Clear(instPtr->nodeList);
	L2_Scan_Reset(instPtr->scanFSM);
	L2_Service_Reset(instPtr->serviceFSM);
	return SUCCESS;
}

/*****************************************************************************/
/**
 * @brief   Completely destroy link layer instance, free all allocated memory.
 *
 * @param   instPtr	a pointer to physical layer controller instance
 *
 * @return  NONE
 *
 ******************************************************************************/
void Linklayer_Destroy(L2_INST *instPtr)
{
	if (instPtr == NULL)
	{
		return;
	}
	free(instPtr->sendPktData);
	free(instPtr->recvPktData);
	// Destroy FSMs and nodeList
	L2_DestroySchedEventPool(instPtr->schdEventPool);
	L2_NodeList_Destroy(instPtr->nodeList);
	L2_Scan_Destroy(instPtr->scanFSM);
	L2_Service_Destroy(instPtr->serviceFSM);
	free(instPtr);
}

/*****************************************************************************/
/**
 * @brief   Start link layer processing.
 *
 * @param   instPtr	a pointer to physical layer controller instance
 *
 * @return  NONE
 *
 ******************************************************************************/
int32_t Linklayer_Start(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	// Set netStartTime at first call.
	if (instPtr->netStartTime == NET_START_TIME_INFTY)
	{
		instPtr->netStartTime = TimeCtrl_GetIntCnt(instPtr->config.tmcInstPtr) / L2_SLOTS_PER_FRAME + 1;
	}
	instPtr->isEnable = true;
	return SUCCESS;
}

/*****************************************************************************/
/**
 * @brief   Halt link layer.
 *
 * @param   instPtr	a pointer to physical layer controller instance
 *
 * @return  NONE
 *
 ******************************************************************************/
int32_t Linklayer_Halt(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	instPtr->isEnable = false;
	return SUCCESS;
}

/*****************************************************************************/
/**
 * @brief   Handle l2 event called by other model.
 *
 * @param   instPtr		a pointer to physical layer controller instance
 * @param   l2Event
 *
 * @return  SUCCESS - Event has been successfully handled.
 *          FAIL    - Event cannot be recognized or error occurred while process.
 *
 ******************************************************************************/
int32_t Linklayer_EventHandler(L2_INST *instPtr, const L2_EVENT *l2Event)
{
	switch (l2Event->event)
	{
	case L2_EV_SET_FRAME_NUM:
	{
		DBG_INFO("Event L2_EV_FRAME_NO_UPDATE asserted....\n");
		L2_SetFrameNum(instPtr, *((uint32_t *)l2Event->msg));
		break;
	}
	case L2_EV_SET_SLOT_NUM:
	{
		DBG_INFO("Event L2_EV_SLOT_NO_UPDATE asserted....\n");
		L2_SetSlotNum(instPtr, *((uint32_t *)l2Event->msg));
		break;
	}
	case L2_EV_MAIN_STATE_UPDATE:
	{
		DBG_INFO("Event L2_EV_MAIN_STATE_UPDATE asserted....\n");
		UpdateMainSate(instPtr, *((L2_NODE_STATE *)l2Event->msg));
		break;
	}
	case L2_EV_SCAN_PROC_DONE:
	{
		DBG_INFO("Event L2_EV_SCAN_PROC_DONE asserted....\n");
		break;
	}
	case L2_EV_NET_ADV_UPDATE:
	{
		DBG_INFO("Event L2_EV_NET_ADV_UPDATE asserted....\n");
		if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
		{
			instPtr->netStartTime = *((uint32_t *)l2Event->msg);
		}
		else
		{
			DBG_WARNING("Invalid call from head node.");
		}
		break;
	}
	case L2_EV_NET_SETUP_DONE:
	{
		DBG_INFO("Event L2_EV_NET_SETUP_DONE asserted....\n");
		if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
		{
			instPtr->cxn2Net = true;
			UpdateMainSate(instPtr, L2_NORMAL_STATE);
		}
		break;
	}
	case L2_EV_TIME_SYNC:
	{
		DBG_INFO("Event L2_EV_TIME_SYNC asserted....\n");
		break;
	}
	case L2_EV_LOST_CXN:
	{
		DBG_INFO("Event L2_EV_LOST_CXN asserted....\n");
		if (L2_GetNodeType(instPtr) == 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 [%" PRId8 "].\n", L2_GetHeadId(instPtr));
		L2_Scan_Reset(instPtr->scanFSM);
		L2_Service_Reset(instPtr->serviceFSM);
		instPtr->cxn2Net = false;
		UpdateMainSate(instPtr, L2_INIT_STATE);
		instPtr->log.reconnectCnt++;
		break;
	}
	case L2_EV_LOST_NODE:
	{
		DBG_INFO("Event L2_EV_LOST_NODE asserted....\n");
		L2_Scan_Reset(instPtr->scanFSM);
		L2_Service_Reset(instPtr->serviceFSM);
		UpdateMainSate(instPtr, L2_INIT_STATE);
		// Log reconnect
		instPtr->log.reconnectCnt++;
		break;
	}
	case L2_ROUTINE_FINISH:
	{
		DBG_INFO("Event L2_ROUTINE_FINISH asserted....\n");
		if (instPtr->nextState == L2_INIT_STATE)
		{
			// Reset scan net FMS for next scan process.
			L2_Scan_Reset(instPtr->scanFSM);
			UpdateMainSate(instPtr, L2_INIT_STATE);
			instPtr->rtneConfig.rtneTable = initTable;
			instPtr->rtneConfig.rtnePrd = ARRAY_SIZE(initTable);
		}
		else if (instPtr->nextState == L2_NORMAL_STATE)
		{
			UpdateMainSate(instPtr, L2_NORMAL_STATE);
			switch(L2_GetCurrentSpeedMode(instPtr)){
			case L2_SPEED_NONE :
				DBG_WARNING("Speed mode not configured, fallback to 2Kbps mode.\n");
			case L2_SPEED_2KBPS :
				instPtr->rtneConfig.rtneTable = normalTable2K;
				instPtr->rtneConfig.rtnePrd = ARRAY_SIZE(normalTable2K);
				break;
			case L2_SPEED_128KBPS :
				instPtr->rtneConfig.rtneTable = normalTable128K;
				instPtr->rtneConfig.rtnePrd = ARRAY_SIZE(normalTable128K);
				break;
			case L2_SPEED_256KBPS :
				instPtr->rtneConfig.rtneTable = normalTable256K;
				instPtr->rtneConfig.rtnePrd = ARRAY_SIZE(normalTable256K);
				break;
			default:
				DBG_ERROR("Unknown speed mode %d.\n", instPtr->speedMode);
				ASSERT(true);
			}
		}
		else
		{
			DBG_WARNING("Unsupported routine state [%" PRId8 "], "
					"Continue with previous routine table.\n", instPtr->nextState);
		}
		break;
	}
	case L2_ROUTINE_UPDATE:
	{
		DBG_INFO("Event L2_ROUTINE_UPDATE asserted....\n");
		L2_RTNE_CONFIG *newRtneConfig = (L2_RTNE_CONFIG *)l2Event->msg;
		instPtr->rtneConfig.rtneTable = newRtneConfig->rtneTable;
		instPtr->rtneConfig.rtnePrd = newRtneConfig->rtnePrd;
		break;
	}
	default:
	{
		DBG_WARNING("UNKNOWN Event %" PRId8 " asserted.\n", l2Event->event);
		break;
	}
	}
	return SUCCESS;
}

/*****************************************************************************/
/**
 * @brief   Handle l2 packet correctly received.
 *
 * @param   instPtr		a pointer to physical layer controller instance
 * @param   frameType
 *
 * @return  Status code.
 *
 ******************************************************************************/
int32_t Linklayer_PktHandler(L2_INST *instPtr, L2_FRAME_TYPE frameType)
{
	ASSERT(instPtr != NULL);
	int32_t Status;

	if (instPtr->dataAvail == false)
	{
		return FAILURE;
	}

	switch (frameType)
	{
	case L2_SCAN_FRAME:
	case L2_ADV_FRAME:
	case L2_CFM_FRAME:
		Status = L2_Scan_PktHandler(instPtr, instPtr->recvPktData);
		break;
	case L2_SVC_2K_FRAME:
	case L2_SVC_128K_FRAME:
	case L2_SVC_256K_FRAME:
		Status = L2_Service_PktHandler(instPtr, instPtr->recvPktData);
		break;
	default:
		DBG_WARNING("L2FrameHandler: UNKNOWN L2-FrameType %" PRId8 "\n", frameType);
		Status = FAILURE;
		break;
	}
	return Status;
}

/**
 * @brief    Utility function to update node ttl.
 */
static int32_t IntcNodeTTL(NODE_ENTRY *node, const void *param)
{
	ASSERT(node != NULL);
	node->ttlCnt += *((uint32_t *)param);
	return SUCCESS;
}

/**
 * @brief    Utility function to check connection to node is still valid.
 */
static bool IsNodeConnected(NODE_ENTRY *node)
{
	ASSERT(node != NULL);
	DBG_INFO("Node: %" PRIu8 ", TTL: %" PRIu32 ".\n", node->nodeInfo.nodeId, node->ttlCnt);
	if (node->ttlCnt < LINK_BROCK_THRESHOLD)
	{
		return true;
	}
	else
	{
		DBG_WARNING("Lost Connection to NODE %" PRIu8 ".\n", node->nodeInfo.nodeId);
		return false;
	}
}

/*****************************************************************************/
/**
 * @brief   Schedule next slot based on time allocate slice.
 *
 * @param   instPtr		a pointer to physical layer controller instance
 *
 * @return  SUCCESS
 *          Other error code.
 *
 ******************************************************************************/
static int32_t Linklayer_Routine(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	int32_t Status;
	uint32_t tableIndx, entryIndx;

	// Determine which routine entry is going to be performed.
	tableIndx = L2_GetNextFrameNum(instPtr) % instPtr->rtneConfig.rtnePrd;
	entryIndx = L2_GetNextSlotNum(instPtr);
	L2_RTNE_TABLE_ENTRY rtneEntry = {0};

	// Shift routine entry record.
	instPtr->rtneConfig.last = instPtr->rtneConfig.curr;
	instPtr->rtneConfig.curr = instPtr->rtneConfig.next;
	instPtr->rtneConfig.next = &(instPtr->rtneConfig.rtneTable[tableIndx][entryIndx]);
	rtneEntry = instPtr->rtneConfig.rtneTable[tableIndx][entryIndx];
	DBG_INFO("## Routine: %" PRIu32 "-%" PRIu32 ", Routine period: %" PRIu32 ".\n", tableIndx, entryIndx, instPtr->rtneConfig.rtnePrd);

	switch (rtneEntry.slotType)
	{
	case L2_SLOT_IDLE:
		// Skip a 5ms slot.
		DBG_INFO("Skipping idle slot (5ms)...\n");
		Status = SUCCESS;
		break;
	case L2_SLOT_SCAN:
	case L2_SLOT_ADVERTISE:
	case L2_SLOT_CONFIRM:
		Status = L2_Scan_Routine(instPtr, &rtneEntry);
		break;
	case L2_SLOT_SERVICE_2K:
	case L2_SLOT_SERVICE_128K:
	case L2_SLOT_SERVICE_256K:
		Status = L2_Service_Routine(instPtr, &rtneEntry);
		break;
	default:
		DBG_ERROR("UNKNOWN slot type [%" PRId8 "].\n", rtneEntry.slotType);
		return EPARAM;
	}

	// check if routine is finished.
	if (entryIndx >= L2_SLOTS_PER_FRAME - 1 && tableIndx >= instPtr->rtneConfig.rtnePrd - 1)
	{
		L2_EVENT rtneFinishEvent = {L2_ROUTINE_FINISH, &(instPtr->rtneConfig)};
		AssertL2Event(instPtr, &rtneFinishEvent);
	}

	// Update TTL
	uint32_t increment = 1;
	L2_IterEditNode(instPtr->nodeList, &increment, IntcNodeTTL);

//#ifdef DATALINK_ENABLE_AUTORECONNECT
	if (L2_GetMainState(instPtr) != L2_INIT_STATE)
	{
		L2_IterDeleteNode(instPtr->nodeList, IsNodeConnected);
		if (L2_GetNodeListLen(instPtr->nodeList) < instPtr->config.netScale - 1)
		{
			// reset scan process
			L2_EVENT l2Event;
			// Assert Net-Setup Done.
			l2Event.event = (L2_GetNodeType(instPtr) == L2_NODE_HEAD) ? L2_EV_LOST_NODE : L2_EV_LOST_CXN;
			l2Event.msg = NULL;
			AssertL2Event(instPtr, &l2Event);
		}
	}
//#endif
	return Status;
}

/*****************************************************************************/
/**
 * @brief   Execute a link layer fsm process.
 *
 * @param   instPtr	a pointer to physical layer controller instance
 *
 * @return  SUCCESS
 *          Other error code.
 *
 ******************************************************************************/
int32_t Linklayer_Proc(L2_INST *instPtr)
{
	ASSERT(instPtr != NULL);
	int32_t Status;
	PHY_INST *phyRef = instPtr->config.phyInstPtr;
	TMC_INST *tmcRef = instPtr->config.tmcInstPtr;

	if (instPtr->isEnable == false)
	{
		DBG_INFO("######## Link Layer is not Enabled ########\n");
		return SUCCESS;
	}

	// FIXME: Consider time wrapping.
	uint32_t nextTimeUnit = (TimeCtrl_GetIntCnt(tmcRef) + 1) / L2_SLOTS_PER_FRAME;
	if (nextTimeUnit < instPtr->netStartTime)
	{
		DBG_WARNING("Linklayer not start yet, next time unit: %" PRIu32 ", start time unit: %" PRIu32 ".\n",
					nextTimeUnit, instPtr->netStartTime);
		return EIGNORE;
	}

	// Update slot number.
	L2_UpdateSlotNum(instPtr);

	DBG_INFO("######## Current frameNum: %" PRIu32 ", SlotNum: %" PRIu32 " ########\n",
			 instPtr->frameNum.curr, instPtr->slotNum.curr);

	// Sync System info
	SysMonitor_GetSysinfo(&(instPtr->sysInfo));

	// Update frame status.
	L2_FrameStateUpdate(instPtr);

	// Handle frame received in last slot.
	if (L2_isDataAvail(instPtr) == true)
	{
		if (Linklayer_PktHandler(instPtr, L2_GetLastOptFrameType(instPtr)) != SUCCESS)
		{
			DBG_ERROR("Failed to handle last received frame: %" PRId8 ".\n", L2_GetLastOptFrameType(instPtr));
		}
	}

	// Run Link layer routine, setup next slot procedure.
	if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
	{
		Phy_SetMode(phyRef, PHY_MODE_PULL);
		Status = Linklayer_Routine(instPtr);
	}
	else
	{ // L2_NODE_MEMBER
		if (L2_isCxn2Net(instPtr))
		{
			Phy_SetMode(phyRef, PHY_MODE_PULL);
			Status = Linklayer_Routine(instPtr);
		}
		else
		{
			Phy_SetMode(phyRef, PHY_MODE_LISTEN);
			Status = L2_Scan_Proc(instPtr);
		}
	}

	// Check if previous runs has error occurred.
	if (Status != SUCCESS)
	{
		return Status;
	}

	// Check and execute scheduled event.
	L2_EventPoolHandler(instPtr);

	// Update link layer state.
	instPtr->mainState = instPtr->nextState;
	instPtr->linkLog.tuNum = instPtr->frameNum.curr;
	instPtr->linkLog.slotNum = (uint8_t)instPtr->slotNum.curr;
	instPtr->linkLog.nodeType = (L2_GetNodeType(instPtr) == L2_NODE_HEAD) ? 0xAA : 0xCC;
	instPtr->linkLog.cxnStatus = (L2_isCxn2Net(instPtr)) ? 0x11 : 0x00;
	instPtr->linkLog.linkStage = (instPtr->mainState == L2_INIT_STATE) ? 0x11 : 0x22;
	instPtr->linkLog.recxnCnt = instPtr->log.reconnectCnt;
	instPtr->linkLog.beamNo = instPtr->scanFSM->beamNo;
	instPtr->linkLog.beamWidth = instPtr->log.lastBeamWidth;
	instPtr->linkLog.beamAz = phyRef->antNrrwPattern[instPtr->scanFSM->beamNo].oaxis;
	instPtr->linkLog.beamEl = phyRef->antNrrwPattern[instPtr->scanFSM->beamNo].raxis;
	instPtr->linkLog.advTxCnt = (uint16_t)instPtr->log.advTxCnt;
	instPtr->linkLog.advRecvCnt = (uint16_t)instPtr->log.advRecvCnt;
	instPtr->linkLog.advCorrectCnt = (uint16_t)instPtr->log.advCorrectCnt;
	instPtr->linkLog.cfmTxCnt = instPtr->log.cfmTxCnt;
	instPtr->linkLog.cfmRecvCnt = instPtr->log.cfmRecvCnt;
	instPtr->linkLog.cfmCorrectCnt = instPtr->log.cfmCorrectCnt;
	instPtr->linkLog.svcTxCnt = instPtr->log.svcTxCnt;
	instPtr->linkLog.svcRecvCnt = instPtr->log.svcRecvCnt;
	instPtr->linkLog.svcCorrectCnt = instPtr->log.svcCorrectCnt;
	instPtr->linkLog.timeDev = (instPtr->log.timeDev > INT32_MAX)
								? INT32_MAX : ((instPtr->log.timeDev < INT32_MIN) ? INT32_MIN : (int32_t)instPtr->log.timeDev);
	instPtr->linkLog.sendId = instPtr->log.txLog.dataId;
	instPtr->linkLog.recvId = instPtr->log.rxLog.dataId;

	SysMonitor_UpdateLinklayerLog(&(instPtr->linkLog));
	return SUCCESS;
}

/*****************************************************************************/
/**
 * @brief   Steer antenna beam to specified node.
 *
 * @param   instPtr	a pointer to link layer descriptor instance
 * @param   nodeId		destination node
 *
 * @return Corresponding frame length
 *         L2_FRAME_NONE_LEN if the frame type code cannot be recognized.
 *
 ******************************************************************************/
int32_t L2_SteerBeam2Node(L2_INST *instPtr, uint8_t nodeId)
{
	ASSERT(instPtr != NULL);
	int32_t Status;

	COORD myPos, dstPos, beamDir;
	Status = L2_GetNodePos(instPtr->nodeList, nodeId, &dstPos);
	if (Status != SUCCESS)
	{
		DBG_ERROR("Failed to retrieve position info of node %" PRIu8 ", remain last configuration.\n", nodeId);
		return EPARAM;
	}
	myPos.x = instPtr->sysInfo.posX;
	myPos.y = instPtr->sysInfo.posY;
	myPos.z = instPtr->sysInfo.posZ;

	// TODO Check data format
	EULERANGLES attitudeEuler, antAttitudeEuler;
	attitudeEuler.alpha = DEG2RAD(instPtr->sysInfo.pitch);
	attitudeEuler.beta = DEG2RAD(instPtr->sysInfo.yaw);
	attitudeEuler.gamma = DEG2RAD(instPtr->sysInfo.roll);

	// FIXME: confirm date source.
	antAttitudeEuler.alpha = DEG2RAD(0);
	antAttitudeEuler.beta = DEG2RAD(0);
	antAttitudeEuler.gamma = DEG2RAD(0);

	// TODO Check data format
	QUATERNION attitude, antAttitude;
	attitude = Quaternion_FromeEulerAngles(attitudeEuler, EULERANGLES_XYZ);
	antAttitude = Quaternion_FromeEulerAngles(antAttitudeEuler, EULERANGLES_XYZ);

	// --- --- Algorithm --- ---
	// TODO Check result validation
	// Compute beam direction vector
	beamDir = Coord_Sub(dstPos, myPos);

	// Compute antenna attitude respect to ECEF
	antAttitude = Quaternion_Mult(attitude, antAttitude);

	// Convert beam direction vector to antenna coordinate
	beamDir = Quaternion_Rotate(Quaternion_GeInverse(antAttitude), beamDir);
	// Normalize
	Coord_Normalize(&beamDir);

	double oAxis, rAxis;
	oAxis = RAD2DEG(atan2(beamDir.z, beamDir.y));
	rAxis = RAD2DEG(acos(Coord_Dot(beamDir, COORD_X_AXIS)));
	Status = Phy_SetBeamAngle(instPtr->config.phyInstPtr, oAxis, rAxis);
	return Status;
}

/*****************************************************************************/
/**
 * @brief   Layer 2 setup next slot operation, update current operating frame.
 *
 * @param   instPtr		a pointer to linklayer instance
 * @param   txrxMode	tx/rx state for next slot
 * @param   frameType	frame type of next slot.
 *
 * @return SUCCESS/FAILURE
 *
 ******************************************************************************/
int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE frameType)
{
	ASSERT(instPtr != NULL);
	PHY_INST *phyRef = instPtr->config.phyInstPtr;
	int32_t Status;
	if (frameType < L2_SCAN_FRAME || frameType > L2_SVC_256K_FRAME)
	{
		return FAILURE;
	}

	// logging
	if (txrxMode == L2_TOR_TX)
	{
		instPtr->log.frameTx++;
		switch (frameType)
		{
		case L2_ADV_FRAME:
			instPtr->log.advTxCnt++;
			break;
		case L2_CFM_FRAME:
			instPtr->log.cfmTxCnt++;
			break;
		case L2_SVC_2K_FRAME:
		case L2_SVC_128K_FRAME:
		case L2_SVC_256K_FRAME:
			instPtr->log.svcTxCnt++;
			break;
		default:
			break;
		}
	}
	else if (txrxMode == L2_TOR_RX)
	{
		instPtr->log.frameRx++;
		switch (frameType)
		{
		case L2_ADV_FRAME:
			instPtr->log.advRxCnt++;
			break;
		case L2_CFM_FRAME:
			instPtr->log.cfmRxCnt++;
			break;
		case L2_SVC_2K_FRAME:
		case L2_SVC_128K_FRAME:
		case L2_SVC_256K_FRAME:
			instPtr->log.svcRxCnt++;
			break;
		default:
			break;
		}
	}

#if _DEBUG_LEVEL >= 3
	char *torStr = NULL;
	char *frameStr = NULL;
	switch (txrxMode)
	{
	case L2_TOR_SUSPEND:
		torStr = "SUSPEND";
		break;
	case L2_TOR_TX:
		torStr = "TX";
		break;
	case L2_TOR_RX:
		torStr = "RX";
		break;
	}
	switch (frameType)
	{
	case L2_SCAN_FRAME:
		frameStr = "Scan";
		break;
	case L2_ADV_FRAME:
		frameStr = "Advertisement";
		break;
	case L2_CFM_FRAME:
		frameStr = "Confirm";
		break;
	case L2_SVC_FRAME:
		frameStr = "Service";
		break;
	default:
		frameStr = "UNKNOWN Frame";
		break;
	}
	DBG_INFO("# Setup Next Slot # [%" PRIu32 "-%" PRIu32 "], %s, %s.\n",
			 instPtr->frameNum.next,
			 instPtr->slotNum.next,
			 torStr, frameStr);
#endif

	// Update frame and tx/rx mode record.
	// Frame types shift (last <- curr <- next)
	L2_RcdShift(&(instPtr->frame), frameType);
	L2_RcdShift(&(instPtr->tor), txrxMode);

	TIME_WORD pullTime = {0, 0};
	pullTime.int_part = TimeCtrl_GetIntCnt(instPtr->config.tmcInstPtr) + 1;
	Status = Phy_SetTimeWord(phyRef, &pullTime, PHY_TIME_WORD_POLL);
	if (Status != SUCCESS)
	{
		DBG_ERROR("Physical set pull time failed, with error code: %" PRId32 "\n", Status);
	}

	PHY_TOR_TYPE tor;
	switch (txrxMode)
	{
	case L2_TOR_SUSPEND:
		tor = PHY_TOR_SUSPEND;
		break;
	case L2_TOR_TX:
		tor = PHY_TOR_TX;
		break;
	case L2_TOR_RX:
		tor = PHY_TOR_RX;
		break;
	default:
		DBG_ERROR("Invalid txrx mode :%" PRId8 ".\n", txrxMode);
		return EPARAM;
	}

	Status = Phy_SetupNextSlot(phyRef, tor, L2_L2frame2L1frame(frameType), instPtr->sendPktData);
	if (Status != SUCCESS)
	{
		DBG_ERROR("Physical setup failed, with error code: %" PRId32 "\n", Status);
	}
	return Status;
}

int32_t Linklayer_UpdateSpeedMode(L2_INST *instPtr, L2_SPEED_MODE mode) {
	ASSERT(instPtr != NULL);
	if(mode < L2_SPEED_2KBPS || mode > L2_SPEED_256KBPS){
		DBG_ERROR("Unsurpport speed mode, %d.\n", mode);
		return EIGNORE;
	}
	instPtr->speedMode = mode;
	return SUCCESS;
}

/** @} LinkLayer_Main*/
/** @} LinkLayer*/