From 8dedb125aee83411165df5a1ff38434010ddf05b Mon Sep 17 00:00:00 2001 From: Josh Gao Date: Mon, 26 May 2025 14:10:01 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E4=BF=AE=E6=94=B9=E8=81=86=E5=90=AC?= =?UTF-8?q?=E5=BB=BA=E7=BD=91=E9=80=BB=E8=BE=91,=20=E5=A2=9E=E5=8A=A0?= =?UTF-8?q?=E5=BB=BA=E7=BD=91=E5=B8=A7=E6=8E=A5=E6=94=B6=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/networking/link_layer/link_layer.c | 45 +++++-- src/networking/link_layer/link_layer.h | 6 +- src/networking/link_layer/link_layer_types.h | 14 ++- src/networking/link_layer/scan.c | 123 +++++++++++++++++++ src/networking/phy_layer/phy_antenna.c | 8 +- src/networking/phy_layer/phy_layer.h | 2 +- 6 files changed, 178 insertions(+), 20 deletions(-) diff --git a/src/networking/link_layer/link_layer.c b/src/networking/link_layer/link_layer.c index 7f93b60..ef6bce1 100644 --- a/src/networking/link_layer/link_layer.c +++ b/src/networking/link_layer/link_layer.c @@ -33,7 +33,8 @@ #endif /************************** Constant Definitions *****************************/ #define NET_START_TIME_INFTY UINT32_MAX -#define LINK_BROCK_THRESHOLD L2_SLOTS_PER_FRAME // 1 seconds +#define LINK_BROCK_THRESHOLD L2_SLOTS_PER_FRAME // 0.5 seconds +#define LISTEN_TIMEOUT_INT L2_SLOTS_PER_FRAME // number of 2 ms slots /************************** Variable Definitions *****************************/ /***************** Macros (Inline Functions) Definitions *********************/ @@ -562,6 +563,7 @@ int32_t Linklayer_Init(L2_INST **instPtr, const L2_CONFIG *config) (*instPtr)->recvPktData = NULL; (*instPtr)->schdEventPool = NULL; (*instPtr)->nodeList = NULL; + (*instPtr)->scanFSM = NULL; (*instPtr)->serviceFSM = NULL; memcpy(&((*instPtr)->config), config, sizeof(L2_CONFIG)); @@ -633,6 +635,13 @@ int32_t Linklayer_Init(L2_INST **instPtr, const L2_CONFIG *config) 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) { @@ -708,6 +717,7 @@ int32_t Linklayer_Reconfig(L2_INST *instPtr, const L2_CONFIG *config) // Reset according instance L2_ClearSchedEventPool(instPtr->schdEventPool); L2_NodeList_Clear(instPtr->nodeList); + L2_Scan_Reset(instPtr->scanFSM); L2_Service_Reset(instPtr->serviceFSM); return SUCCESS; } @@ -732,6 +742,7 @@ void Linklayer_Destroy(L2_INST *instPtr) // 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); } @@ -848,6 +859,7 @@ int32_t Linklayer_EventHandler(L2_INST *instPtr, const L2_EVENT *l2Event) 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); @@ -857,6 +869,7 @@ int32_t Linklayer_EventHandler(L2_INST *instPtr, const L2_EVENT *l2Event) 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 @@ -869,6 +882,7 @@ int32_t Linklayer_EventHandler(L2_INST *instPtr, const L2_EVENT *l2Event) 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); @@ -1122,10 +1136,7 @@ int32_t Linklayer_Proc(L2_INST *instPtr) else { Phy_SetMode(phyRef, PHY_MODE_LISTEN); - - // Trigger physical configuration. - // Indicate physical layer that all the configurations were done. - PhyWrite(phyRef, SLOT_CMD, SLOT_CMD_KEY); + Status = L2_Scan_Proc(instPtr); } } @@ -1146,6 +1157,7 @@ int32_t Linklayer_Proc(L2_INST *instPtr) 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.antIndex = instPtr->config.phyInstPtr->antIndex.curr; instPtr->linkLog.advTxCnt = (uint16_t)instPtr->log.advTxCnt; instPtr->linkLog.advRecvCnt = (uint16_t)instPtr->log.advRecvCnt; instPtr->linkLog.advCorrectCnt = (uint16_t)instPtr->log.advCorrectCnt; @@ -1301,12 +1313,25 @@ int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE f 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) + if (L2_isCxn2Net(instPtr) == false) { - DBG_ERROR("Physical set pull time failed, with error code: %" PRId32 "\n", Status); + // NOTE: LISTENING mode is assumed. + TIME_WORD listenTime = {LISTEN_TIMEOUT_INT, 0}; + Status = Phy_SetTimeWord(phyRef, &listenTime, PHY_TIME_WORD_LISTEN_TIMEOUT); + if (Status != SUCCESS) + { + DBG_ERROR("Physical set listening timeout failed, with error code: %" PRId32 "\n", Status); + } + } + else + { + TIME_WORD pollTime = {0, 0}; + pollTime.int_part = TimeCtrl_GetIntCnt(instPtr->config.tmcInstPtr) + 1; + Status = Phy_SetTimeWord(phyRef, &pollTime, PHY_TIME_WORD_POLL); + if (Status != SUCCESS) + { + DBG_ERROR("Physical set poll time failed, with error code: %" PRId32 "\n", Status); + } } PHY_TOR_TYPE tor; diff --git a/src/networking/link_layer/link_layer.h b/src/networking/link_layer/link_layer.h index 1830ddd..240c8be 100644 --- a/src/networking/link_layer/link_layer.h +++ b/src/networking/link_layer/link_layer.h @@ -26,7 +26,6 @@ #include "node_list.h" #include "circ_queue.h" #include "../phy_layer/phy_layer.h" -#include "../phy_layer/phy_reg.h" #include "../../system_monitor.h" /************************** Constant Definitions *****************************/ /***************** Macros (Inline Functions) Definitions *********************/ @@ -118,6 +117,7 @@ typedef struct L2_NODE_STATE mainState; ///< the main state of link layer state L2_NODE_STATE nextState; ///< next state of link layer state, which will be applied to mainState every second. L2_RTNE_CONFIG rtneConfig; ///< current routine table configuration and status. + L2_SCAN_FSM *scanFSM; ///< FSM of scan net (link set up) L2_SVC_FSM *serviceFSM; ///< FSM of service net (data service) uint8_t *sendPktData; ///< send data buffer uint8_t *recvPktData; ///< received data buffer @@ -172,6 +172,10 @@ int32_t L2_RescheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint * @defgroup LinkLayer_Scan * @{ */ +int32_t L2_Scan_Init(L2_SCAN_FSM **scanFSMPtr); +void L2_Scan_Reset(L2_SCAN_FSM *scanFSMPtr); +void L2_Scan_Destroy(L2_SCAN_FSM *scanFSMPtr); +int32_t L2_Scan_Proc(L2_INST *instPtr); 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*/ diff --git a/src/networking/link_layer/link_layer_types.h b/src/networking/link_layer/link_layer_types.h index 18d12f9..93b33b9 100644 --- a/src/networking/link_layer/link_layer_types.h +++ b/src/networking/link_layer/link_layer_types.h @@ -206,14 +206,16 @@ typedef struct uint8_t dst; ///< destination node ID } L2_RTNE_TABLE_ENTRY; + /** - * @enum SCAN_STAGE + * @struct L2_SCAN_FSM + * @brief Linklayer scan process FSM instance */ -typedef enum +typedef struct { - L2_SCAN_STAGE_NONE = 0, - L2_SCAN_STAGE_ADV = 1 -} SCAN_STAGE; + bool isAdvRecvd; ///< indicates if advertisement frame done(antenna locked). + uint8_t advCnt; ///< adv receive try count +} L2_SCAN_FSM; /** * @struct L2_SVC_FSM @@ -249,4 +251,4 @@ typedef struct #endif /* __LINK_LAYER_DEF_H_ */ -/** @} LinkLayer*/ \ No newline at end of file +/** @} LinkLayer*/ diff --git a/src/networking/link_layer/scan.c b/src/networking/link_layer/scan.c index d98f059..0704c41 100644 --- a/src/networking/link_layer/scan.c +++ b/src/networking/link_layer/scan.c @@ -39,8 +39,52 @@ /************************** Variable Definitions *****************************/ /************************** Function Prototypes ******************************/ +static void AdvRecvStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state); +static int32_t Scan_AdvHandler(L2_INST *instPtr); static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId); +/** + * @brief Update Base-info Advertisement state. + */ +static void AdvRecvStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state) +{ + ASSERT(scanFsmPtr != NULL); + DBG_INFO("Update AdvRecvState from [%" PRId8 "] to [%" PRId8 "].\n", + scanFsmPtr->isAdvRecvd, state); + scanFsmPtr->isAdvRecvd = state; + return; +} + +/*****************************************************************************/ +/** + * @brief Member node scan net BmAdvt stage handler. + * + * @param instPtr Pointer to linklayer layer instance. + * + * @return used slot (2 ms slot). + * + ******************************************************************************/ +static int32_t Scan_AdvHandler(L2_INST *instPtr) +{ + ASSERT(instPtr != NULL); + L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM; + DBG_INFO("** Net-Setup - Advertisement Procedure (Attempts: %" PRIu8 ") **\n", scanFsmPtr->advCnt); + + // Network status is updated in L2_Scan_PktHandler() if valid ADV frame is received. + if (scanFsmPtr->isAdvRecvd == true) + { + DBG_ERROR("Only unconnected node can execute Scan_AdvHandler().\n"); + return EUNEXP; + } + // no valid bm frame was received. + else + { + (void)L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_FRAME_ADV); + scanFsmPtr->advCnt++; + } + return SUCCESS; +} + /*****************************************************************************/ /** * @brief Packing Scan message to a bit array. @@ -167,6 +211,7 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) ASSERT(instPtr != NULL); ASSERT(msg != NULL); int32_t Status; + L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM; L2_FRAME_TYPE frameType = L2_FRAME_NONE; L2_FrameGetChunk(msg, L2_MSG_TYPE, L2_MSG_TYPE_LEN, &frameType); @@ -247,6 +292,8 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) memcpy(instPtr->log.rxLog.data, msg, sizeof(L2_MSG_ADV)); instPtr->log.rxLog.dataId = dataId; + // trigger flag + AdvRecvStateUpdate(scanFsmPtr, true); Status = SUCCESS; break; } @@ -327,6 +374,82 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) return Status; } +/*****************************************************************************/ +/** + * @brief Initialize scan FSM. + * + * @param instPtr + * + * @return + * + ******************************************************************************/ +int32_t L2_Scan_Init(L2_SCAN_FSM **scanFSMPtr) +{ + ASSERT(scanFSMPtr != NULL); + *scanFSMPtr = (L2_SCAN_FSM *)malloc(sizeof(L2_SCAN_FSM)); + if (*scanFSMPtr == NULL) + { + DBG_ERROR("Failed to allocate memory for scanFsmPtr.\n"); + return ENOMEM; + } + L2_Scan_Reset(*scanFSMPtr); + return SUCCESS; +} + +/*****************************************************************************/ +/** + * @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->isAdvRecvd = false; + scanFSMPtr->advCnt = 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); +} + +/*****************************************************************************/ +/** + * @brief Execute a member node scan net procedure. + * + * @param instPtr a pointer to linklayer layer instance + * + * @return used slot (2 ms slot). + * + ******************************************************************************/ +int32_t L2_Scan_Proc(L2_INST *instPtr) +{ + if (L2_GetNodeType(instPtr) == L2_NODE_HEAD) + { + DBG_ERROR("Only member node needs to execute L2_Scan_Proc. \n"); + return FAILURE; + } + int32_t Status; + DBG_INFO("-------- ScanNet Process --------\n"); + + Status = Scan_AdvHandler(instPtr); + + return Status; +} + /*****************************************************************************/ /** diff --git a/src/networking/phy_layer/phy_antenna.c b/src/networking/phy_layer/phy_antenna.c index 1a2f45d..0aaf004 100644 --- a/src/networking/phy_layer/phy_antenna.c +++ b/src/networking/phy_layer/phy_antenna.c @@ -407,9 +407,13 @@ int32_t Phy_SetAntIndex(PHY_INST *instPtr, PHY_ANT_INDEX antIndex) return EPARAM; } DBG_INFO("Set new antenna index: %" PRId8 "\n", antIndex); - instPtr->antIndex.next = antIndex; Phy_SetTxAntIndex(instPtr, antIndex); Phy_SetRxAntIndex(instPtr, antIndex); + + instPtr->antIndex.last = instPtr->antIndex.curr; + instPtr->antIndex.curr = instPtr->antIndex.next; + instPtr->antIndex.next = antIndex; + return SUCCESS; } @@ -424,4 +428,4 @@ inline size_t Phy_GetNextAntIndex(PHY_INST *instPtr) /** @} PhysicalLayer_AntennaControl*/ -/** @} PhysicalLayer*/ \ No newline at end of file +/** @} PhysicalLayer*/ diff --git a/src/networking/phy_layer/phy_layer.h b/src/networking/phy_layer/phy_layer.h index 9922ecc..561599a 100644 --- a/src/networking/phy_layer/phy_layer.h +++ b/src/networking/phy_layer/phy_layer.h @@ -24,12 +24,12 @@ #include "common.h" #ifndef EMULATION #include "xbram.h" +#include "../config/app_config.h" #ifdef HSJL_GE // Zynq MPSoC (Zynq UltraScale+) #include "xsysmon.h" #else #include "xadcps.h" #endif -#include "../config/app_config.h" #endif #include "../../system_monitor.h" #include "../../time_ctrl/time_ctrl.h"