diff --git a/datalink814_ge/src/linklayer/rtt.c b/datalink814_ge/src/linklayer/rtt.c new file mode 100644 index 0000000..d053d1e --- /dev/null +++ b/datalink814_ge/src/linklayer/rtt.c @@ -0,0 +1,472 @@ +/*****************************************************************************/ +/** + * Description: RTT algorithm. + * + * Author: FanHao + * Date: 2023-12-10 20:43 + * LastEditors: FanHao + * LastEditTime: 2023-12-10 20:43 + * Revisions: Rev 0.0 + * History: + * [2023-12-10]: Initial version, split form time_sync.c + * + * Copyright(c) 2021 Beijing Institute of Technology. + * Lab of Communication and Networking + * + ******************************************************************************/ + +/***************************** Include Files *********************************/ +#include +#include +#include "linklayer814.h" +#include "rtt.h" +#include "../time_ctrl/time_ctrl.h" +#include "../phy_ctrl/phy_ctrl.h" +#include "../error.h" +#include "../common.h" + +/************************** Constant Definitions *****************************/ +#define LIGHT_SPPED_AIR 299702547.0 // m/s +#define LIGHT_SPPED_VAC 299792458.0 // m/s +#define RTT_DOPPLER_NONE UINT32_MAX + +const TIME_WORD RTT_TIME_NONE = +{ + .int_part = UINT32_MAX, + .frac_part = UINT32_MAX +}; +/***************** Macros (Inline Functions) Definitions *********************/ +/**************************** Type Definitions *******************************/ +/************************** Variable Definitions *****************************/ +/************************** Function Prototypes ******************************/ + +static void RTT_StageUpdate(L2_RTT_FSM *rttFsmPtr, L2_RTT_STGE newStage); +static void RTT_GetFrameStart(L2_INST *instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType); + +/*****************************************************************************/ +/** + * @brief Update corrent time-sync for RTT stage + * + * @param scanFsmPtr A pointer to scan net fsm. + * @param newSatge New state. + * + * @return NONE. + * + ******************************************************************************/ +static void RTT_StageUpdate(L2_RTT_FSM *rttFsmPtr, L2_RTT_STGE newStage) +{ + ASSERT(rttFsmPtr != NULL); + ASSERT(newStage >= RTT_NONE_STAGE && newStage <= RTT_RSP2); + DBG_INFO("RTT Stage Update: %d -> %d.\n", rttFsmPtr->rttStage, newStage); + rttFsmPtr->rttStage = newStage; +} + +/*****************************************************************************/ +/** + * @brief Reset RTT FSM + * + * @param rttFsmPtr a pointer to rtt FSM instance. + * + * @return NONE + * + ******************************************************************************/ +void RTT_Reset(L2_RTT_FSM *rttFsmPtr) +{ + ASSERT(rttFsmPtr != NULL); + memset(&(rttFsmPtr->rttRecord), 0x00, sizeof(RTT_RECORD)); + rttFsmPtr->rttStage = RTT_REQ1; + rttFsmPtr->isR1Valid = false; + rttFsmPtr->isR2Valid = false; + rttFsmPtr->isR3Valid = false; + rttFsmPtr->isR4Valid = false; + rttFsmPtr->timeStamp.int_part = 0; + rttFsmPtr->timeStamp.frac_part = 0; + rttFsmPtr->doppler = 0; +} + +/*****************************************************************************/ +/** + * @brief Destroy RTT FSM, free allocated memory. + * + * @param rttFsmPtr a pointer to rtt FSM instance. + * + * @return NONE + * + ******************************************************************************/ +void RTT_Destroy(L2_RTT_FSM *rttFsmPtr) +{ + free(rttFsmPtr); +} + +/*****************************************************************************/ +/** + * @brief Get the frame start time. If torType == L2_TOR_TX, get the next + * slot start time, if torType == L2_TOR_RX get last frame start + * time. + * + * @param instPtr a pointer to linklayer layer instance + * @param tmStamp a pointer to store frame start time + * @param torType tx/rx mode + * + * @return NONE + * + ******************************************************************************/ +void RTT_GetFrameStart(L2_INST *instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType) +{ + ASSERT(instPtr != NULL); + ASSERT(tmStamp != NULL); + // Get the next frame start time + if (torType == L2_TOR_TX) + { + tmStamp->int_part = L2_GetNextFrameNum(instPtr) * L2_SLOTS_PER_FRAME + L2_GetNextSlotNum(instPtr); + tmStamp->frac_part = 0; + } + // Get the last reiceive frame start time + else if (torType == L2_TOR_RX) + { + PHY_INST *phyRef = instPtr->config.phyInstPtr; + TIME_WORD timeStamp; + int64_t timeDiff; + Phy_GetLastReceivedFrameSyncTimeStamp(phyRef, &timeStamp); + timeDiff = (uint64_t)timeStamp.frac_part; + timeDiff += ((uint64_t)timeStamp.int_part) * TIME_FRAC_MAX; + Phy_GetLastReceivedSlotStartTimeStamp(phyRef, tmStamp); + timeDiff -= ((uint64_t)tmStamp->int_part) * TIME_FRAC_MAX; + timeDiff -= (uint64_t)tmStamp->frac_part; + timeDiff = (int64_t)Phy_GetTimeComp(L2_L2frame2L1frame(L2_GetLastFrameType(instPtr))) - timeDiff; + if (timeDiff < 0) + { + timeDiff += TIME_FRAC_MAX; + tmStamp->int_part--; + _nop_(); + } + tmStamp->frac_part = (TIME_FRAC)timeDiff; + } + else + { + *tmStamp = RTT_TIME_NONE; + DBG_ERROR("Unexpected tx/rx mode, setting dummy values.\n"); + } +} + +/*****************************************************************************/ +/** + * @brief Initialize RTT FSM, allocate memory for FSM instance. + * + * @param rttFsmPtr + * + * @return SUCCESS Initialization successful. + * EMAC_MEMORY_ALLOC_ERROR Memory allocate error. + * + ******************************************************************************/ +int32_t RTT_Init(L2_RTT_FSM **rttFsmPtr) +{ + (*rttFsmPtr) = (L2_RTT_FSM *)malloc(sizeof(L2_RTT_FSM)); + if (*rttFsmPtr == NULL) + { + free(*rttFsmPtr); + return EALLOC; + } + RTT_Reset(*rttFsmPtr); + return SUCCESS; +} + +/*****************************************************************************/ +/** + * @brief Update RTT FSM accordingly. + * + * @param instPtr a pointer to linklayer layer instance + * @param torType tx/rx mode type + * + * @return SUCCESS + * error code + * + ******************************************************************************/ +int32_t RTT_FsmUpdate(L2_INST *instPtr, L2_TOR_TYPE torType) +{ + ASSERT(instPtr != NULL); + L2_RTT_FSM *rttFsmPtr = L2_GetRttFsm(instPtr); + RTT_RECORD *rttRecord = &(rttFsmPtr->rttRecord); + PHY_INST *phyRef = instPtr->config.phyInstPtr; + + DBG_INFO("-------- RTT FSM Update --------\n"); + // Stage trans + // For Head Node: Tx->Rx->Tx->Rx + if (L2_GetNodeType(instPtr) == L2_NODE_HEAD) + { + switch (rttFsmPtr->rttStage) + { + case RTT_REQ1: + if (torType == L2_TOR_TX) + { + DBG_INFO("Prepare data for RTT REQ-1\n"); + // Update Tt1 + RTT_GetFrameStart(instPtr, &(rttRecord->Tt1), torType); + DBG_INFO("Tt1-timeL:%u.\n", rttRecord->Tt1.int_part); + DBG_INFO("Tt1-timeH:%u.\n", rttRecord->Tt1.frac_part); + + // Prepare data for sending... + rttFsmPtr->timeStamp = rttRecord->Tt1; + rttFsmPtr->doppler = RTT_DOPPLER_NONE; + RTT_StageUpdate(rttFsmPtr, RTT_RSP1); + } + else + { + DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage); + } + break; + case RTT_RSP1: + if (torType == L2_TOR_RX) + { + if (L2_isDataAvail(instPtr) == true) + { + DBG_INFO("RTT RSP-1 have been received.\n"); + DBG_INFO("Preparing data for RTT REQ-2.\n"); + rttFsmPtr->isR2Valid = true; + // Update doppler + rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef); + // Update Tr2 + RTT_GetFrameStart(instPtr, &(rttRecord->Tr2), torType); + // Calculate TroundA + TimeCtrl_CalculateTimeGap(&(rttRecord->Tt1), &(rttRecord->Tr2), &(rttRecord->TroundA)); + } + else + { + DBG_WARNING("RTT RSP-1 had not been received [Stage: %d].\n", rttFsmPtr->rttStage); + DBG_WARNING("Set Tr2 dopplerR2 and TroundA to dummy value.\n"); + rttFsmPtr->isR2Valid = false; + rttRecord->dopplerR2 = RTT_DOPPLER_NONE; + rttRecord->Tr2 = RTT_TIME_NONE; + rttRecord->TroundA = RTT_TIME_NONE; + } + DBG_INFO("dopplerR2:%d.\n", rttRecord->dopplerR2); + DBG_INFO("Tr2-timeL:%u.\n", rttRecord->Tr2.frac_part); + DBG_INFO("Tr2-timeH:%u.\n", rttRecord->Tr2.int_part); + DBG_INFO("TroundA-timeL:%u.\n", rttRecord->TroundA.frac_part); + DBG_INFO("TroundA-timeH:%u.\n", rttRecord->TroundA.int_part); + RTT_StageUpdate(rttFsmPtr, RTT_REQ2); + } + else + { + DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage); + } + break; + case RTT_REQ2: + if (torType == L2_TOR_TX) + { + DBG_INFO("Prepare data for RTT REQ-2\n"); + // Update Tt3 + RTT_GetFrameStart(instPtr, &(rttRecord->Tt3), torType); + + if (rttFsmPtr->isR2Valid) + { + // Calculate reply A + TimeCtrl_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr2), &(rttRecord->TreplyA)); + } + else + { + DBG_WARNING("RTT RSP-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage); + DBG_WARNING("Set TreplyA to dummy value.\n"); + rttRecord->TreplyA = RTT_TIME_NONE; + } + DBG_INFO("Tt3-timeL:%u.\n", rttRecord->Tt3.frac_part); + DBG_INFO("Tt3-timeH:%u.\n", rttRecord->Tt3.int_part); + DBG_INFO("TreplyA-timeL:%u.\n", rttRecord->TreplyA.frac_part); + DBG_INFO("TreplyA-timeH:%u.\n", rttRecord->TreplyA.int_part); + + // Prepare data for sending. + rttFsmPtr->timeStamp = rttRecord->TroundA; + rttFsmPtr->doppler = rttRecord->dopplerR2; + RTT_StageUpdate(rttFsmPtr, RTT_RSP2); + } + else + { + DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage); + } + break; + case RTT_RSP2: + if (torType == L2_TOR_RX) + { + if (L2_isDataAvail(instPtr) == true) + { + DBG_INFO("RTT RSP-2 have been received.\n"); + rttFsmPtr->isR4Valid = true; + // Update doppler + double doppler = Phy_GetCoarseFreqEst(phyRef); + rttRecord->dopplerR2 = (int32_t)doppler; + // Update Tr4 + RTT_GetFrameStart(instPtr, &(rttRecord->Tr4), torType); + // Calculate TroundA + TimeCtrl_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr4), &(rttRecord->TroundA)); + } + else + { + DBG_WARNING("RTT RSP-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage); + DBG_WARNING("Set Tr4 dopplerR4 and TroundA to dummy value.\n"); + rttFsmPtr->isR4Valid = false; + rttRecord->dopplerR4 = RTT_DOPPLER_NONE; + rttRecord->Tr4 = RTT_TIME_NONE; + rttRecord->TroundA = RTT_TIME_NONE; + } + DBG_INFO("dopplerR4:%d.\n", rttRecord->dopplerR4); + DBG_INFO("Tr4-timeL:%u.\n", rttRecord->Tr4.frac_part); + DBG_INFO("Tr4-timeH:%u.\n", rttRecord->Tr4.int_part); + DBG_INFO("TroundA-timeL:%u.\n", rttRecord->TroundA.frac_part); + DBG_INFO("TroundA-timeH:%u.\n", rttRecord->TroundA.int_part); + RTT_StageUpdate(rttFsmPtr, RTT_REQ1); + DBG_INFO("RTT procedure end...\n"); + } + else + { + DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage); + } + break; + default: + DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStage); + break; + } + } + // For Member Node: Rx->Tx->Rx->Tx + else + { + switch (rttFsmPtr->rttStage) + { + case RTT_REQ1: + if (torType == L2_TOR_RX) + { + if (L2_isDataAvail(instPtr) == true) + { + DBG_INFO("RTT REQ-1 have been received.\n"); + DBG_INFO("Preparing data for RTT RSP-1.\n"); + rttFsmPtr->isR2Valid = true; + // Update doppler + rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef); + // Update Tr1 + RTT_GetFrameStart(instPtr, &(rttRecord->Tr1), torType); + } + else + { + DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage); + DBG_WARNING("Set dopplerR1 and Tr1 to dummy value.\n"); + rttFsmPtr->isR1Valid = false; + rttRecord->dopplerR1 = RTT_DOPPLER_NONE; + rttRecord->Tr1 = RTT_TIME_NONE; + } + DBG_INFO("dopplerR1:%d.\n", rttRecord->dopplerR1); + DBG_INFO("Tr1-timeL:%u.\n", rttRecord->Tr1.frac_part); + DBG_INFO("Tr1-timeH:%u.\n", rttRecord->Tr1.int_part); + RTT_StageUpdate(rttFsmPtr, RTT_RSP1); + } + else + { + DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage); + } + break; + case RTT_RSP1: + if (torType == L2_TOR_TX) + { + DBG_INFO("Prepare data for RTT RSP-1\n"); + // Update Tt2 + RTT_GetFrameStart(instPtr, &(rttRecord->Tt2), torType); + if (rttFsmPtr->isR1Valid) + { + // Calculate reply B + TimeCtrl_CalculateTimeGap(&(rttRecord->Tr1), &(rttRecord->Tt2), &(rttRecord->TreplyB)); + } + else + { + rttRecord->TreplyB = RTT_TIME_NONE; + DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage); + DBG_WARNING("Set ReplayB to dummy value.\n"); + } + DBG_INFO("Tt2-timeL:%u.\n", rttRecord->Tt2.frac_part); + DBG_INFO("Tt2-timeH:%u.\n", rttRecord->Tt2.int_part); + DBG_INFO("TreplyB-timeL:%u.\n", rttRecord->TreplyB.frac_part); + DBG_INFO("TreplyB-timeH:%u.\n", rttRecord->TreplyB.int_part); + + // Prepare data for sending. + rttFsmPtr->timeStamp = rttRecord->TreplyB; + rttFsmPtr->doppler = rttRecord->dopplerR1; + RTT_StageUpdate(rttFsmPtr, RTT_REQ2); + } + else + { + DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage); + } + break; + case RTT_REQ2: + if (torType == L2_TOR_RX) + { + if (L2_isDataAvail(instPtr) == true) + { + DBG_INFO("RTT REQ-2 have been received.\n"); + DBG_INFO("Preparing data for RTT RSP-2.\n"); + rttFsmPtr->isR3Valid = true; + // Update doppler + rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef); + // Update Tr3 + RTT_GetFrameStart(instPtr, &(rttRecord->Tr3), torType); + // Calculate TroundB + TimeCtrl_CalculateTimeGap(&(rttRecord->Tt2), &(rttRecord->Tr3), &(rttRecord->TroundB)); + } + else + { + DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStage); + DBG_WARNING("Set dopplerR3 and ReplayB to dummy value.\n"); + rttFsmPtr->isR3Valid = false; + rttRecord->dopplerR3 = RTT_DOPPLER_NONE; + rttRecord->TroundB = RTT_TIME_NONE; + } + DBG_INFO("dopplerR3:%d.\n", rttRecord->dopplerR3); + DBG_INFO("Tr3-timeL:%u.\n", rttRecord->Tr3.frac_part); + DBG_INFO("Tr3-timeH:%u.\n", rttRecord->Tr3.int_part); + DBG_INFO("TroundB-timeL:%u.\n", rttRecord->TroundB.frac_part); + DBG_INFO("TroundB-timeH:%u.\n", rttRecord->TroundB.int_part); + RTT_StageUpdate(rttFsmPtr, RTT_RSP2); + } + else + { + DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage); + } + break; + case RTT_RSP2: + if (torType == L2_TOR_TX) + { + DBG_INFO("Prepare data for RTT RSP-2\n"); + // Update Tt4 + RTT_GetFrameStart(instPtr, &(rttRecord->Tt4), torType); + if (rttFsmPtr->isR3Valid) + { + // Calculate reply B + TimeCtrl_CalculateTimeGap(&(rttRecord->Tr3), &(rttRecord->Tt4), &(rttRecord->TreplyB)); + // TODO: implement a time adjustment. + } + else + { + DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStage); + DBG_WARNING("Set ReplayB to dummy value.\n"); + rttRecord->TreplyB = RTT_TIME_NONE; + } + DBG_INFO("Tt4-timeL:%u.\n", rttRecord->Tt4.frac_part); + DBG_INFO("Tt4-timeH:%u.\n", rttRecord->Tt4.int_part); + DBG_INFO("TreplyB-timeL:%u.\n", rttRecord->TreplyB.frac_part); + DBG_INFO("TreplyB-timeH:%u.\n", rttRecord->TreplyB.int_part); + + // Prepare data for sending. + rttFsmPtr->timeStamp = rttRecord->TreplyB; + rttFsmPtr->doppler = rttRecord->dopplerR3; + RTT_StageUpdate(rttFsmPtr, RTT_REQ1); + DBG_INFO("RTT procedure end...\n"); + } + else + { + DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage); + } + break; + default: + DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStage); + break; + } + } + return SUCCESS; +} diff --git a/datalink814_ge/src/linklayer/time_sync.c b/datalink814_ge/src/linklayer/time_sync.c index 09362f9..584012b 100644 --- a/datalink814_ge/src/linklayer/time_sync.c +++ b/datalink814_ge/src/linklayer/time_sync.c @@ -22,25 +22,16 @@ #include "linklayer814.h" #include "rtt.h" #include "../time_ctrl/time_ctrl.h" +#include "../phy_ctrl/phy_ctrl.h" #include "../error.h" #include "../common.h" /************************** Constant Definitions *****************************/ -#define LIGHT_SPPED_AIR 299702547.0 // m/s -#define LIGHT_SPPED_VAC 299792458.0 // m/s -#define RTT_DOPPLER_NONE UINT32_MAX - -const TIME_WORD RTT_TIME_NONE = { - .int_part = UINT32_MAX, - .frac_part = UINT32_MAX}; /***************** Macros (Inline Functions) Definitions *********************/ /**************************** Type Definitions *******************************/ /************************** Variable Definitions *****************************/ /************************** Function Prototypes ******************************/ -static void RTT_StageUpdate(L2_RTT_FSM *rttFsmPtr, L2_RTT_STGE newStage); -static void RTT_GetFrameStart(L2_INST *instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType); - /*****************************************************************************/ /** * @brief a slot alignment, base on physical layer information. @@ -86,432 +77,4 @@ int32_t L2_TimeAlign(L2_INST *instPtr, uint32_t frameNum, uint32_t slotNum, bool slotTimerAdjmet.frac_part = (uint32_t)(timeDiff % TIME_FRAC_MAX); TimeCtrl_TimeAdjust(instPtr->config.tmcInstPtr, &slotTimerAdjmet, TM_ADJ_ADD); return SUCCESS; -} - -/*****************************************************************************/ -/** - * @brief Update corrent time-sync for RTT stage - * - * @param scanFsmPtr A pointer to scan net fsm. - * @param newSatge New state. - * - * @return NONE. - * - ******************************************************************************/ -static void RTT_StageUpdate(L2_RTT_FSM *rttFsmPtr, L2_RTT_STGE newStage) -{ - ASSERT(rttFsmPtr != NULL); - ASSERT(newStage >= RTT_NONE_STAGE && newStage <= RTT_RSP2); - DBG_INFO("RTT Stage Update: %d -> %d.\n", rttFsmPtr->rttStage, newStage); - rttFsmPtr->rttStage = newStage; -} - -/*****************************************************************************/ -/** - * @brief Reset RTT FSM - * - * @param rttFsmPtr a pointer to rtt FSM instance. - * - * @return NONE - * - ******************************************************************************/ -void RTT_Reset(L2_RTT_FSM *rttFsmPtr) -{ - ASSERT(rttFsmPtr != NULL); - memset(&(rttFsmPtr->rttRecord), 0x00, sizeof(RTT_RECORD)); - rttFsmPtr->rttStage = RTT_REQ1; - rttFsmPtr->isR1Valid = false; - rttFsmPtr->isR2Valid = false; - rttFsmPtr->isR3Valid = false; - rttFsmPtr->isR4Valid = false; - rttFsmPtr->timeStamp.int_part = 0; - rttFsmPtr->timeStamp.frac_part = 0; - rttFsmPtr->doppler = 0; -} - -/*****************************************************************************/ -/** - * @brief Destroy RTT FSM, free allocated memory. - * - * @param rttFsmPtr a pointer to rtt FSM instance. - * - * @return NONE - * - ******************************************************************************/ -void RTT_Destroy(L2_RTT_FSM *rttFsmPtr) -{ - free(rttFsmPtr); -} - -/*****************************************************************************/ -/** - * @brief Get the frame start time. If torType == L2_TOR_TX, get the next - * slot start time, if torType == L2_TOR_RX get last frame start - * time. - * - * @param instPtr a pointer to linklayer layer instance - * @param tmStamp a pointer to store frame start time - * @param torType tx/rx mode - * - * @return NONE - * - ******************************************************************************/ -void RTT_GetFrameStart(L2_INST *instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType) -{ - ASSERT(instPtr != NULL); - ASSERT(tmStamp != NULL); - // Get the next frame start time - if (torType == L2_TOR_TX) - { - tmStamp->int_part = L2_GetNextFrameNum(instPtr) * L2_SLOTS_PER_FRAME + L2_GetNextSlotNum(instPtr); - tmStamp->frac_part = 0; - } - // Get the last reiceive frame start time - else if (torType == L2_TOR_RX) - { - PHY_INST *phyRef = instPtr->config.phyInstPtr; - TIME_WORD timeStamp; - int64_t timeDiff; - Phy_GetLastReceivedFrameSyncTimeStamp(phyRef, &timeStamp); - timeDiff = (uint64_t)timeStamp.frac_part; - timeDiff += ((uint64_t)timeStamp.int_part) * TIME_FRAC_MAX; - Phy_GetLastReceivedSlotStartTimeStamp(phyRef, tmStamp); - timeDiff -= ((uint64_t)tmStamp->int_part) * TIME_FRAC_MAX; - timeDiff -= (uint64_t)tmStamp->frac_part; - timeDiff = (int64_t)Phy_GetTimeComp(L2_L2frame2L1frame(L2_GetLastFrameType(instPtr))) - timeDiff; - if (timeDiff < 0) - { - timeDiff += TIME_FRAC_MAX; - tmStamp->int_part--; - _nop_(); - } - tmStamp->frac_part = (TIME_FRAC)timeDiff; - } - else - { - *tmStamp = RTT_TIME_NONE; - DBG_ERROR("Unexpected tx/rx mode, setting dummy values.\n"); - } -} - -/*****************************************************************************/ -/** - * @brief Initialize RTT FSM, allocate memory for FSM instance. - * - * @param rttFsmPtr - * - * @return SUCCESS Initialization successful. - * EMAC_MEMORY_ALLOC_ERROR Memory allocate error. - * - ******************************************************************************/ -int32_t RTT_Init(L2_RTT_FSM **rttFsmPtr) -{ - (*rttFsmPtr) = (L2_RTT_FSM *)malloc(sizeof(L2_RTT_FSM)); - if (*rttFsmPtr == NULL) - { - free(*rttFsmPtr); - return EALLOC; - } - RTT_Reset(*rttFsmPtr); - return SUCCESS; -} - -/*****************************************************************************/ -/** - * @brief Update RTT FSM accordingly. - * - * @param instPtr a pointer to linklayer layer instance - * @param torType tx/rx mode type - * - * @return SUCCESS - * error code - * - ******************************************************************************/ -int32_t RTT_FsmUpdate(L2_INST *instPtr, L2_TOR_TYPE torType) -{ - ASSERT(instPtr != NULL); - L2_RTT_FSM *rttFsmPtr = L2_GetRttFsm(instPtr); - RTT_RECORD *rttRecord = &(rttFsmPtr->rttRecord); - PHY_INST *phyRef = instPtr->config.phyInstPtr; - - DBG_INFO("-------- RTT FSM Update --------\n"); - // Stage trans - // For Head Node: Tx->Rx->Tx->Rx - if (L2_GetNodeType(instPtr) == L2_NODE_HEAD) - { - switch (rttFsmPtr->rttStage) - { - case RTT_REQ1: - if (torType == L2_TOR_TX) - { - DBG_INFO("Prepare data for RTT REQ-1\n"); - // Update Tt1 - RTT_GetFrameStart(instPtr, &(rttRecord->Tt1), torType); - DBG_INFO("Tt1-timeL:%u.\n", rttRecord->Tt1.int_part); - DBG_INFO("Tt1-timeH:%u.\n", rttRecord->Tt1.frac_part); - - // Prepare data for sending... - rttFsmPtr->timeStamp = rttRecord->Tt1; - rttFsmPtr->doppler = RTT_DOPPLER_NONE; - RTT_StageUpdate(rttFsmPtr, RTT_RSP1); - } - else - { - DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage); - } - break; - case RTT_RSP1: - if (torType == L2_TOR_RX) - { - if (L2_isDataAvail(instPtr) == true) - { - DBG_INFO("RTT RSP-1 have been received.\n"); - DBG_INFO("Preparing data for RTT REQ-2.\n"); - rttFsmPtr->isR2Valid = true; - // Update doppler - rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef); - // Update Tr2 - RTT_GetFrameStart(instPtr, &(rttRecord->Tr2), torType); - // Calculate TroundA - TimeCtrl_CalculateTimeGap(&(rttRecord->Tt1), &(rttRecord->Tr2), &(rttRecord->TroundA)); - } - else - { - DBG_WARNING("RTT RSP-1 had not been received [Stage: %d].\n", rttFsmPtr->rttStage); - DBG_WARNING("Set Tr2 dopplerR2 and TroundA to dummy value.\n"); - rttFsmPtr->isR2Valid = false; - rttRecord->dopplerR2 = RTT_DOPPLER_NONE; - rttRecord->Tr2 = RTT_TIME_NONE; - rttRecord->TroundA = RTT_TIME_NONE; - } - DBG_INFO("dopplerR2:%d.\n", rttRecord->dopplerR2); - DBG_INFO("Tr2-timeL:%u.\n", rttRecord->Tr2.frac_part); - DBG_INFO("Tr2-timeH:%u.\n", rttRecord->Tr2.int_part); - DBG_INFO("TroundA-timeL:%u.\n", rttRecord->TroundA.frac_part); - DBG_INFO("TroundA-timeH:%u.\n", rttRecord->TroundA.int_part); - RTT_StageUpdate(rttFsmPtr, RTT_REQ2); - } - else - { - DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage); - } - break; - case RTT_REQ2: - if (torType == L2_TOR_TX) - { - DBG_INFO("Prepare data for RTT REQ-2\n"); - // Update Tt3 - RTT_GetFrameStart(instPtr, &(rttRecord->Tt3), torType); - - if (rttFsmPtr->isR2Valid) - { - // Calculate reply A - TimeCtrl_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr2), &(rttRecord->TreplyA)); - } - else - { - DBG_WARNING("RTT RSP-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage); - DBG_WARNING("Set TreplyA to dummy value.\n"); - rttRecord->TreplyA = RTT_TIME_NONE; - } - DBG_INFO("Tt3-timeL:%u.\n", rttRecord->Tt3.frac_part); - DBG_INFO("Tt3-timeH:%u.\n", rttRecord->Tt3.int_part); - DBG_INFO("TreplyA-timeL:%u.\n", rttRecord->TreplyA.frac_part); - DBG_INFO("TreplyA-timeH:%u.\n", rttRecord->TreplyA.int_part); - - // Prepare data for sending. - rttFsmPtr->timeStamp = rttRecord->TroundA; - rttFsmPtr->doppler = rttRecord->dopplerR2; - RTT_StageUpdate(rttFsmPtr, RTT_RSP2); - } - else - { - DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage); - } - break; - case RTT_RSP2: - if (torType == L2_TOR_RX) - { - if (L2_isDataAvail(instPtr) == true) - { - DBG_INFO("RTT RSP-2 have been received.\n"); - rttFsmPtr->isR4Valid = true; - // Update doppler - double doppler = Phy_GetCoarseFreqEst(phyRef); - rttRecord->dopplerR2 = (int32_t)doppler; - // Update Tr4 - RTT_GetFrameStart(instPtr, &(rttRecord->Tr4), torType); - // Calculate TroundA - TimeCtrl_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr4), &(rttRecord->TroundA)); - } - else - { - DBG_WARNING("RTT RSP-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage); - DBG_WARNING("Set Tr4 dopplerR4 and TroundA to dummy value.\n"); - rttFsmPtr->isR4Valid = false; - rttRecord->dopplerR4 = RTT_DOPPLER_NONE; - rttRecord->Tr4 = RTT_TIME_NONE; - rttRecord->TroundA = RTT_TIME_NONE; - } - DBG_INFO("dopplerR4:%d.\n", rttRecord->dopplerR4); - DBG_INFO("Tr4-timeL:%u.\n", rttRecord->Tr4.frac_part); - DBG_INFO("Tr4-timeH:%u.\n", rttRecord->Tr4.int_part); - DBG_INFO("TroundA-timeL:%u.\n", rttRecord->TroundA.frac_part); - DBG_INFO("TroundA-timeH:%u.\n", rttRecord->TroundA.int_part); - RTT_StageUpdate(rttFsmPtr, RTT_REQ1); - DBG_INFO("RTT procedure end...\n"); - } - else - { - DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage); - } - break; - default: - DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStage); - break; - } - } - // For Member Node: Rx->Tx->Rx->Tx - else - { - switch (rttFsmPtr->rttStage) - { - case RTT_REQ1: - if (torType == L2_TOR_RX) - { - if (L2_isDataAvail(instPtr) == true) - { - DBG_INFO("RTT REQ-1 have been received.\n"); - DBG_INFO("Preparing data for RTT RSP-1.\n"); - rttFsmPtr->isR2Valid = true; - // Update doppler - rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef); - // Update Tr1 - RTT_GetFrameStart(instPtr, &(rttRecord->Tr1), torType); - } - else - { - DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage); - DBG_WARNING("Set dopplerR1 and Tr1 to dummy value.\n"); - rttFsmPtr->isR1Valid = false; - rttRecord->dopplerR1 = RTT_DOPPLER_NONE; - rttRecord->Tr1 = RTT_TIME_NONE; - } - DBG_INFO("dopplerR1:%d.\n", rttRecord->dopplerR1); - DBG_INFO("Tr1-timeL:%u.\n", rttRecord->Tr1.frac_part); - DBG_INFO("Tr1-timeH:%u.\n", rttRecord->Tr1.int_part); - RTT_StageUpdate(rttFsmPtr, RTT_RSP1); - } - else - { - DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage); - } - break; - case RTT_RSP1: - if (torType == L2_TOR_TX) - { - DBG_INFO("Prepare data for RTT RSP-1\n"); - // Update Tt2 - RTT_GetFrameStart(instPtr, &(rttRecord->Tt2), torType); - if (rttFsmPtr->isR1Valid) - { - // Calculate reply B - TimeCtrl_CalculateTimeGap(&(rttRecord->Tr1), &(rttRecord->Tt2), &(rttRecord->TreplyB)); - } - else - { - rttRecord->TreplyB = RTT_TIME_NONE; - DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage); - DBG_WARNING("Set ReplayB to dummy value.\n"); - } - DBG_INFO("Tt2-timeL:%u.\n", rttRecord->Tt2.frac_part); - DBG_INFO("Tt2-timeH:%u.\n", rttRecord->Tt2.int_part); - DBG_INFO("TreplyB-timeL:%u.\n", rttRecord->TreplyB.frac_part); - DBG_INFO("TreplyB-timeH:%u.\n", rttRecord->TreplyB.int_part); - - // Prepare data for sending. - rttFsmPtr->timeStamp = rttRecord->TreplyB; - rttFsmPtr->doppler = rttRecord->dopplerR1; - RTT_StageUpdate(rttFsmPtr, RTT_REQ2); - } - else - { - DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage); - } - break; - case RTT_REQ2: - if (torType == L2_TOR_RX) - { - if (L2_isDataAvail(instPtr) == true) - { - DBG_INFO("RTT REQ-2 have been received.\n"); - DBG_INFO("Preparing data for RTT RSP-2.\n"); - rttFsmPtr->isR3Valid = true; - // Update doppler - rttRecord->dopplerR2 = (int32_t)Phy_GetCoarseFreqEst(phyRef); - // Update Tr3 - RTT_GetFrameStart(instPtr, &(rttRecord->Tr3), torType); - // Calculate TroundB - TimeCtrl_CalculateTimeGap(&(rttRecord->Tt2), &(rttRecord->Tr3), &(rttRecord->TroundB)); - } - else - { - DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStage); - DBG_WARNING("Set dopplerR3 and ReplayB to dummy value.\n"); - rttFsmPtr->isR3Valid = false; - rttRecord->dopplerR3 = RTT_DOPPLER_NONE; - rttRecord->TroundB = RTT_TIME_NONE; - } - DBG_INFO("dopplerR3:%d.\n", rttRecord->dopplerR3); - DBG_INFO("Tr3-timeL:%u.\n", rttRecord->Tr3.frac_part); - DBG_INFO("Tr3-timeH:%u.\n", rttRecord->Tr3.int_part); - DBG_INFO("TroundB-timeL:%u.\n", rttRecord->TroundB.frac_part); - DBG_INFO("TroundB-timeH:%u.\n", rttRecord->TroundB.int_part); - RTT_StageUpdate(rttFsmPtr, RTT_RSP2); - } - else - { - DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage); - } - break; - case RTT_RSP2: - if (torType == L2_TOR_TX) - { - DBG_INFO("Prepare data for RTT RSP-2\n"); - // Update Tt4 - RTT_GetFrameStart(instPtr, &(rttRecord->Tt4), torType); - if (rttFsmPtr->isR3Valid) - { - // Calculate reply B - TimeCtrl_CalculateTimeGap(&(rttRecord->Tr3), &(rttRecord->Tt4), &(rttRecord->TreplyB)); - // TODO: implement a time adjustment. - } - else - { - DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStage); - DBG_WARNING("Set ReplayB to dummy value.\n"); - rttRecord->TreplyB = RTT_TIME_NONE; - } - DBG_INFO("Tt4-timeL:%u.\n", rttRecord->Tt4.frac_part); - DBG_INFO("Tt4-timeH:%u.\n", rttRecord->Tt4.int_part); - DBG_INFO("TreplyB-timeL:%u.\n", rttRecord->TreplyB.frac_part); - DBG_INFO("TreplyB-timeH:%u.\n", rttRecord->TreplyB.int_part); - - // Prepare data for sending. - rttFsmPtr->timeStamp = rttRecord->TreplyB; - rttFsmPtr->doppler = rttRecord->dopplerR3; - RTT_StageUpdate(rttFsmPtr, RTT_REQ1); - DBG_INFO("RTT procedure end...\n"); - } - else - { - DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage); - } - break; - default: - DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStage); - break; - } - } - return SUCCESS; -} +} \ No newline at end of file