From e2c4733a934b72725dccf0bea5ef2dd295ecd562 Mon Sep 17 00:00:00 2001 From: Josh Gao Date: Thu, 22 May 2025 22:14:10 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E4=BF=AE=E6=94=B9=E9=80=9A=E8=BF=87SDK?= =?UTF-8?q?=E7=BC=96=E8=AF=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- df_datalink_sp/src/main.c | 1 - .../src/networking/link_layer/framing.c | 2 +- .../src/networking/link_layer/link_layer.c | 3 +- .../src/networking/link_layer/link_layer.h | 15 +- .../src/networking/link_layer/msg_def.h | 12 +- .../src/networking/link_layer/node_list.c | 5 +- .../src/networking/link_layer/scan.c | 69 +- .../src/networking/link_layer/service.c | 606 +++++++++++++----- .../src/networking/phy_layer/phy_layer.c | 4 +- df_datalink_sp/src/system_monitor.h | 6 +- df_datalink_sp/src/systembus/systembus.c | 70 +- 11 files changed, 535 insertions(+), 258 deletions(-) diff --git a/df_datalink_sp/src/main.c b/df_datalink_sp/src/main.c index 31319f6..3f75267 100644 --- a/df_datalink_sp/src/main.c +++ b/df_datalink_sp/src/main.c @@ -94,7 +94,6 @@ static PHY_CONFIG phyConfig = { .devId = PHY_DEVICE_ID, .baseAddr = PHY_BASEADDR, .antBramId = PHY_ANTCONFIG_BRAM_DEVICE_ID, - .antInfoBramId = PHY_ANTINFO_BRAM_DEVICE_ID, .txBramId = PHY_TXBUFF_BRAM_DEVICE_ID, .rxBramId = PHY_RXBUFF_BRAM_DEVICE_ID, .hwXadcId = PHY_HW_XADC_DEVICE_ID, diff --git a/df_datalink_sp/src/networking/link_layer/framing.c b/df_datalink_sp/src/networking/link_layer/framing.c index f926b1b..79a6211 100644 --- a/df_datalink_sp/src/networking/link_layer/framing.c +++ b/df_datalink_sp/src/networking/link_layer/framing.c @@ -81,7 +81,7 @@ void L2_FrameGetChunk(uint8_t *frame, size_t offset, size_t bytes, void *chunk) * SUCCESS * ******************************************************************************/ -static int32_t L2_CrcGen(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t *crc) +int32_t L2_CrcGen(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t *crc) { ASSERT(dataBuff != NULL); int32_t Status; 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 3490dad..9d78807 100644 --- a/df_datalink_sp/src/networking/link_layer/link_layer.c +++ b/df_datalink_sp/src/networking/link_layer/link_layer.c @@ -1014,9 +1014,8 @@ static int32_t Linklayer_Routine(L2_INST *instPtr) case L2_SLOT_DISTRIBUTE: case L2_SLOT_MANAGE: case L2_SLOT_RTT: - Status = L2_Service_Routine(instPtr, &rtneEntry, L2_SVC_128K); case L2_SLOT_SERVICE: - Status = L2_Service_Routine(instPtr, &rtneEntry, L2_GetServiceRate(instPtr)); + Status = L2_Service_Routine(instPtr, &rtneEntry); break; default: DBG_ERROR("UNKNOWN slot type [%" PRId8 "].\n", rtneEntry.slotType); 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 51ed6f3..79b94a6 100644 --- a/df_datalink_sp/src/networking/link_layer/link_layer.h +++ b/df_datalink_sp/src/networking/link_layer/link_layer.h @@ -69,10 +69,10 @@ typedef struct uint32_t ackRecvCnt; ///< acknowledgement received frame count uint32_t ackCorrectCnt; ///< acknowledgement correctly received frame count - uint32_t distRxCnt; ///< distribution rx frame count - uint32_t distTxCnt; ///< distribution tx frame count - uint32_t distRecvCnt; ///< distribution received frame count - uint32_t distCorrectCnt;///< distribution correctly received frame count + uint32_t disRxCnt; ///< distribution rx frame count + uint32_t disTxCnt; ///< distribution tx frame count + uint32_t disRecvCnt; ///< distribution received frame count + uint32_t disCorrectCnt;///< distribution correctly received frame count uint32_t mngRxCnt; ///< management rx frame count uint32_t mngTxCnt; ///< management tx frame count @@ -152,7 +152,7 @@ int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE f void L2_FrameInsertChunk(uint8_t *frame, size_t offset, size_t bytes, const void *chunk); void L2_FrameGetChunk(uint8_t *frame, size_t offset, size_t bytes, void *chunk); int32_t L2_CrcAppend(uint8_t *dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType); -static int32_t L2_CrcGen(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t *crc); +int32_t L2_CrcGen(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t *crc); bool L2_CrcCheck(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType); /** @} LinkLayer_Framing*/ @@ -171,9 +171,6 @@ int32_t L2_RescheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint * @defgroup LinkLayer_Scan * @{ */ -void L2_Scan_RefreashScanTable(L2_INST *instPtr); -void L2_Scan_Reset(L2_SCAN_FSM *scanFSMPtr); -void L2_Scan_Destroy(L2_SCAN_FSM *scanFSMPtr); int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg); int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *schedEntry); /** @} LinkLayer_Scan*/ @@ -231,4 +228,4 @@ PHY_ANT_INDEX L2_AntId2AntIndex(uint8_t antId); #endif /* __LINK_LAYER_H_ */ -/** @} LinkLayer*/ \ No newline at end of file +/** @} LinkLayer*/ diff --git a/df_datalink_sp/src/networking/link_layer/msg_def.h b/df_datalink_sp/src/networking/link_layer/msg_def.h index 63dadc7..19c3121 100644 --- a/df_datalink_sp/src/networking/link_layer/msg_def.h +++ b/df_datalink_sp/src/networking/link_layer/msg_def.h @@ -88,7 +88,7 @@ typedef struct */ typedef struct { - L2_MSG_COMMON *common; ///< common message header + L2_MSG_COMMON common; ///< common message header double srcPosX; ///< position x double srcPosY; ///< position y double srcPosZ; ///< position z @@ -104,7 +104,7 @@ typedef struct */ typedef struct { - L2_MSG_COMMON *common; ///< common message header + L2_MSG_COMMON common; ///< common message header double srcPosX; ///< position x double srcPosY; ///< position y double srcPosZ; ///< position z @@ -122,7 +122,7 @@ typedef struct */ typedef struct { - L2_MSG_COMMON *common; ///< common message header + L2_MSG_COMMON common; ///< common message header double srcPosX; ///< position x double srcPosY; ///< position y double srcPosZ; ///< position z @@ -140,7 +140,7 @@ typedef struct */ typedef struct { - L2_MSG_COMMON *common; ///< common message header + L2_MSG_COMMON common; ///< common message header double srcPosX; ///< position x double srcPosY; ///< position y double srcPosZ; ///< position z @@ -158,7 +158,7 @@ typedef struct */ typedef struct { - L2_MSG_COMMON *common; ///< common message header + L2_MSG_COMMON common; ///< common message header double srcPosX; ///< position x double srcPosY; ///< position y double srcPosZ; ///< position z @@ -176,4 +176,4 @@ typedef struct /** @} LinkLayer_MSG_Constants*/ -/** @} LinkLayer*/ \ No newline at end of file +/** @} LinkLayer*/ diff --git a/df_datalink_sp/src/networking/link_layer/node_list.c b/df_datalink_sp/src/networking/link_layer/node_list.c index d156dc7..e1c962c 100644 --- a/df_datalink_sp/src/networking/link_layer/node_list.c +++ b/df_datalink_sp/src/networking/link_layer/node_list.c @@ -140,7 +140,8 @@ void L2_NodeList_Destroy(NODE_LIST *list) NODE_ENTRY *L2_GetNodeById(NODE_LIST *list, uint8_t nodeId) { ASSERT(list != NULL); - if (nodeId == L2_BROADCAST_ID || nodeId == L2_NONE_ID) + if (nodeId == L2_BROADCAST_ID1 || nodeId == L2_BROADCAST_ID2 || + nodeId == L2_BROADCAST_ID3 || nodeId == L2_NONE_ID) { return NULL; } @@ -558,4 +559,4 @@ int32_t L2_IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *)) /** @} LinkLayer_NodeList*/ -/** @} LinkLayer*/ \ No newline at end of file +/** @} LinkLayer*/ diff --git a/df_datalink_sp/src/networking/link_layer/scan.c b/df_datalink_sp/src/networking/link_layer/scan.c index a2853fe..a271ac9 100644 --- a/df_datalink_sp/src/networking/link_layer/scan.c +++ b/df_datalink_sp/src/networking/link_layer/scan.c @@ -60,14 +60,15 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst switch (type) { case L2_FRAME_ADV: + { L2_MSG_ADV *message = (L2_MSG_ADV *)(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; + 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)); @@ -76,10 +77,10 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst message->srcPosZ = instPtr->sysInfo.posZ; // Insert net start time - message->srcNetStatTime = &(instPtr->netStartTime); + message->srcNetStatTime = instPtr->netStartTime; // Packing extra payload - SysMonitor_GetSendPayload(message->advData, message->advDataId, L2_PAYLOAD_LEN_ADV); + SysMonitor_GetSendPayload(message->advData, &(message->advDataId), L2_PAYLOAD_LEN_ADV); if (message->advDataId == DATA_ID_NONE) { memset(message->advData, 0xAA, L2_PAYLOAD_LEN_ADV); @@ -87,7 +88,7 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst // Append CRC uint32_t crcVal; - Status = L2_CrcGen(message, sizeof(L2_MSG_ADV) - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal); + Status = L2_CrcGen((uint8_t *)message, sizeof(L2_MSG_ADV) - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal); if (Status != SUCCESS) { DBG_ERROR("Fail to generate CRC, error code: %" PRId32 ".\n", Status); @@ -100,14 +101,16 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst instPtr->log.txLog.dataId = message->advDataId; break; + } case L2_FRAME_ACK: + { L2_MSG_ACK *message = (L2_MSG_ACK *)(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.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 @@ -120,7 +123,7 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst message->srcSpeedZ = instPtr->sysInfo.vZ; // Packing extra payload - SysMonitor_GetSendPayload(message->ackData, message->ackDataId, L2_PAYLOAD_LEN_ACK); + SysMonitor_GetSendPayload(message->ackData, &(message->ackDataId), L2_PAYLOAD_LEN_ACK); if (message->ackDataId == DATA_ID_NONE) { memset(message->ackData, 0xAA, L2_PAYLOAD_LEN_ACK); @@ -128,7 +131,7 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst // Append CRC uint32_t crcVal; - Status = L2_CrcGen(message, sizeof(L2_MSG_ACK) - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal); + Status = L2_CrcGen((uint8_t *)message, sizeof(L2_MSG_ACK) - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal); if (Status != SUCCESS) { DBG_ERROR("Fail to generate CRC, error code: %" PRId32 ".\n", Status); @@ -141,6 +144,7 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst instPtr->log.txLog.dataId = message->ackDataId; break; + } default: DBG_ERROR("Unexpected frame type: %" PRId8 ".\n", type); return EPARAM; @@ -305,8 +309,8 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) } // Get data payload - uint32_t dataId = message->advDataId; - SysMonitor_UpdateRecvPayload(message->advData, dataId, L2_PAYLOAD_LEN_ADV); + uint32_t dataId = message->ackDataId; + SysMonitor_UpdateRecvPayload(message->ackData, dataId, L2_PAYLOAD_LEN_ACK); // Logging... memcpy(instPtr->log.rxLog.data, msg, sizeof(L2_MSG_ACK)); @@ -323,39 +327,6 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) return Status; } -/*****************************************************************************/ -/** - * @brief Reset ScanNet FSM - * - * @param scanFsmPtr a pointer to scan net finite state machine - * - * @return NONE - * - ******************************************************************************/ -void L2_Scan_Reset(L2_SCAN_FSM *scanFSMPtr) -{ - ASSERT(scanFSMPtr != NULL); - scanFSMPtr->isLocked = false; - scanFSMPtr->isAdvRecvd = false; - scanFSMPtr->isAckPending = false; - scanFSMPtr->advCnt = 0; - scanFSMPtr->cfmTimeout = 0; -} - -/*****************************************************************************/ -/** - * @brief Destroy ScanNet FSM, free memory - * - * @param scanFsmPtr a pointer to scan net finite state machine - * - * @return NONE - * - ******************************************************************************/ -void L2_Scan_Destroy(L2_SCAN_FSM *scanFSMPtr) -{ - free(scanFSMPtr); -} - /*****************************************************************************/ /** diff --git a/df_datalink_sp/src/networking/link_layer/service.c b/df_datalink_sp/src/networking/link_layer/service.c index 18d416a..5c57513 100644 --- a/df_datalink_sp/src/networking/link_layer/service.c +++ b/df_datalink_sp/src/networking/link_layer/service.c @@ -36,6 +36,13 @@ /**************************** 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] = @@ -97,116 +104,236 @@ static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst 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; - - // 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, L2_PAYLOAD_LEN_SVC_128K); - if (message->svcDataId == DATA_ID_NONE) - { - memset(message->svcData, 0xAA, L2_PAYLOAD_LEN_SVC_128K); - } - - // 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 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; - + { + Status = Service_Packing128K(instPtr, type, dstId); 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); + Status = Service_Packing128K(instPtr, type, dstId); break; case L2_SVC_512K: - L2_MSG_SVC_512K *message = (L2_MSG_SVC_512K *)(instPtr->sendPktData); + Status = Service_Packing512K(instPtr, type, dstId); break; case L2_SVC_2M: - L2_MSG_SVC_2M *message = (L2_MSG_SVC_2M *)(instPtr->sendPktData); + Status = Service_Packing2M(instPtr, type, dstId); break; default: - DBG_ERROR("Unsupported service rate: %" PRId8 ".\n", instPtr->config.serviceRate); + DBG_ERROR("Unexpected service rate: %" PRIu8 ".\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; - break; + } default: DBG_ERROR("Unexpected frame type: %" PRIu8 ".\n", type); return EPARAM; - break; } - return SUCCESS; + 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 @@ -223,9 +350,8 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) ASSERT(msg != NULL); int32_t Status; #ifndef DATALINK_ENABLE_BERTEST - L2_FRAME_TYPE frameType = L2_NONE_FRAME; + L2_FRAME_TYPE frameType = L2_FRAME_NONE; L2_FrameGetChunk(msg, L2_MSG_TYPE, L2_MSG_TYPE_LEN, &frameType); - size_t msgLen, dataLen; // retrieve basic info switch (frameType) @@ -234,76 +360,35 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) if (L2_GetNodeType(instPtr) == L2_NODE_HEAD) { DBG_ERROR("Unexpected call from head node, only member Node can handle L2_FRAME_DIS.\n"); - Status = EUNEXP; - break; + return EUNEXP; } 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; + Status = Service_PktHandler128K(instPtr, msg); break; case L2_FRAME_SVC: - msgLen = L2_GetSvcPacketLen(instPtr->config.serviceRate); - dataLen = L2_GetSvcPayloadLen(instPtr->config.serviceRate); + 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; - // 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; - } - - // 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; uint32_t pktLen = L2_GetSvcPacketLen(instPtr->config.serviceRate); @@ -335,6 +420,231 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) #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 = 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") + 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 = 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") + 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 = 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") + 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 @@ -583,4 +893,4 @@ int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntr } /** @} LinkLayer_Service*/ -/** @} LinkLayer*/ \ No newline at end of file +/** @} LinkLayer*/ diff --git a/df_datalink_sp/src/networking/phy_layer/phy_layer.c b/df_datalink_sp/src/networking/phy_layer/phy_layer.c index 899e503..c2ba1f3 100644 --- a/df_datalink_sp/src/networking/phy_layer/phy_layer.c +++ b/df_datalink_sp/src/networking/phy_layer/phy_layer.c @@ -64,7 +64,7 @@ /************************** Variable Definitions *****************************/ extern const uint16_t freqTable[L1_FRAME_MAX_HOPS]; -extern const int16_t antCalTable[L1_FRAME_MAX_HOPS][L1_ANT_CH_NUM]; +extern const int16_t antCalTable[L1_ANT_NUM][L1_FRAME_MAX_HOPS][L1_ANT_CH_NUM]; extern const uint8_t hopPattern_ADV_24MS[L1_FRAME_ADV_24MS_HOPS]; extern const uint8_t hopPattern_ACK_8MS[L1_FRAME_ACK_8MS_HOPS]; extern const uint8_t hopPattern_128KBPS_4MS[L1_FRAME_128KBPS_4MS_HOPS]; @@ -388,7 +388,7 @@ int32_t Phy_Init(PHY_INST **instPtr, PHY_CONFIG *phyConfig) Phy_AntConfigInit(&((*instPtr)->antConfig)); memset(&((*instPtr))->tor, PHY_TOR_SUSPEND, sizeof(PHY_RCD)); - memset(&((*instPtr)->frameType), L1_NONE_FRAME, sizeof(PHY_RCD)); + memset(&((*instPtr)->frameType), L1_FRAME_NONE, sizeof(PHY_RCD)); memset(&((*instPtr)->antIndex), (int32_t)L1_INIT_ANT_IDX, sizeof(PHY_RCD)); memset(&((*instPtr)->hwSate), 0x00, sizeof(HWSTATE)); memset(&((*instPtr)->phyLog), 0x00, sizeof(PHYLOG)); diff --git a/df_datalink_sp/src/system_monitor.h b/df_datalink_sp/src/system_monitor.h index 19c4188..756488a 100644 --- a/df_datalink_sp/src/system_monitor.h +++ b/df_datalink_sp/src/system_monitor.h @@ -148,9 +148,9 @@ typedef struct __attribute__((packed)) uint32_t ackTxCnt; ///< ack frame send count uint32_t ackRecvCnt; ///< ack frame received count uint32_t ackCorrectCnt; ///< ack frame received correctly count - uint32_t distTxCnt; ///< dist frame send count - uint32_t distRecvCnt; ///< dist frame received count - uint32_t distCorrectCnt;///< dist frame received correctly count + uint32_t disTxCnt; ///< dist frame send count + uint32_t disRecvCnt; ///< dist frame received count + uint32_t disCorrectCnt;///< dist frame received correctly count uint32_t mngTxCnt; ///< mng frame send count uint32_t mngRecvCnt; ///< mng frame received count uint32_t mngCorrectCnt; ///< mng frame received correctly count diff --git a/df_datalink_sp/src/systembus/systembus.c b/df_datalink_sp/src/systembus/systembus.c index 6eda8db..3f6c132 100644 --- a/df_datalink_sp/src/systembus/systembus.c +++ b/df_datalink_sp/src/systembus/systembus.c @@ -310,40 +310,40 @@ static int32_t SystemBus_SystemConfig(SYSBUS *instPtr, uint8_t *msg) return EPARAM; } - if (msg[WD_EN_CMD] == EN_CMD_ON_HIGH_POWER) - { - // full power config - Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_FULL); - if (reconfig || !L2_isStarted(linkInstPtr)) - { - Linklayer_Reconfig(linkInstPtr, &linkConfig); - } - Linklayer_Start(linkInstPtr); - } - else if (msg[WD_EN_CMD] == EN_CMD_ON_LOW_POWER) - { - // low power config - Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_LOW); - if (reconfig || !L2_isStarted(linkInstPtr)) - { - Linklayer_Reconfig(linkInstPtr, &linkConfig); - } - Linklayer_Start(linkInstPtr); - } - else if (msg[WD_EN_CMD] == EN_CMD_SILENT) - { - // silence config - // TODO: better approach? - Linklayer_Halt(linkInstPtr); - Phy_AntConfigMode(phyRef, PHY_ANT_MODE_STANDBY); - TIME_WORD pullTime = {0, 0}; - pullTime.int_part = TimeCtrl_GetIntCnt(tmcRef) + 1; - if (Phy_SetTimeWord(phyRef, &pullTime, PHY_TIME_WORD_POLL) != SUCCESS) - { - DBG_ERROR("Physical set pull time failed.\n"); - } - Phy_SetupNextSlot(phyRef, PHY_TOR_RX, L1_10KBPS_10MS_FRAME, NULL); - } +// if (msg[WD_EN_CMD] == EN_CMD_ON_HIGH_POWER) +// { +// // full power config +// Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_FULL); +// if (reconfig || !L2_isStarted(linkInstPtr)) +// { +// Linklayer_Reconfig(linkInstPtr, &linkConfig); +// } +// Linklayer_Start(linkInstPtr); +// } +// else if (msg[WD_EN_CMD] == EN_CMD_ON_LOW_POWER) +// { +// // low power config +// Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_LOW); +// if (reconfig || !L2_isStarted(linkInstPtr)) +// { +// Linklayer_Reconfig(linkInstPtr, &linkConfig); +// } +// Linklayer_Start(linkInstPtr); +// } +// else if (msg[WD_EN_CMD] == EN_CMD_SILENT) +// { +// // silence config +// // TODO: better approach? +// Linklayer_Halt(linkInstPtr); +// Phy_AntConfigMode(phyRef, PHY_ANT_MODE_STANDBY); +// TIME_WORD pullTime = {0, 0}; +// pullTime.int_part = TimeCtrl_GetIntCnt(tmcRef) + 1; +// if (Phy_SetTimeWord(phyRef, &pullTime, PHY_TIME_WORD_POLL) != SUCCESS) +// { +// DBG_ERROR("Physical set pull time failed.\n"); +// } +// Phy_SetupNextSlot(phyRef, PHY_TOR_RX, L1_10KBPS_10MS_FRAME, NULL); +// } else { // Ignore current command @@ -777,4 +777,4 @@ int32_t SystemBus_RxIntrHandler(SYSBUS *instPtr) return SUCCESS; } -/** @} SystemBus*/ \ No newline at end of file +/** @} SystemBus*/