/*****************************************************************************/ /** * @file service.c * @author FanHao (hao_fan_bit_ee@163.com) * @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 #include #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*/