From ebe3ebf71d81b2f6bda7bf0f97a2191c69b9f2fa Mon Sep 17 00:00:00 2001 From: Gatsby Van Date: Thu, 21 Dec 2023 14:48:28 +0800 Subject: [PATCH] miscellaneous update. --- .../src/networking/link_layer/framing.c | 16 +- .../src/networking/link_layer/link_layer.c | 600 +++++++++--------- .../src/networking/link_layer/link_layer.h | 66 +- df_datalink/src/networking/link_layer/scan.c | 126 ++-- .../src/networking/link_layer/service.c | 198 +++--- .../src/networking/link_layer/time_sync.c | 32 +- 6 files changed, 519 insertions(+), 519 deletions(-) diff --git a/df_datalink/src/networking/link_layer/framing.c b/df_datalink/src/networking/link_layer/framing.c index 6238e79..fe1a707 100644 --- a/df_datalink/src/networking/link_layer/framing.c +++ b/df_datalink/src/networking/link_layer/framing.c @@ -30,9 +30,9 @@ * @brief Generate crc of specified data. * * @param dataBuff pointer to data buffer. - * dataLen length of specified data. - * crcType type of crc - * crc a pointer to 32 bit crc container. + * @param dataLen length of specified data. + * @param crcType type of crc + * @param crc a pointer to 32 bit crc container. * * @return FAILURE crc type not support. * SUCCESS @@ -63,9 +63,9 @@ static int32_t L2_CrcGen(uint8_t* dataBuff, uint32_t dataLen, L2_CRC_TYPE crcTyp * @brief Append a crc of specified data, to a specified position. * * @param dataBuff pointer to data buffer. - * dataLen length of specified data. - * crcPos position of crc insert position - * crcType type of crc + * @param dataLen length of specified data. + * @param crcPos position of crc insert position + * @param crcType type of crc * * @return FAILURE crc type not support. * SUCCESS @@ -105,8 +105,8 @@ int32_t L2_CrcAppend(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CR * @brief Check data validation by crc. * * @param dataBuff pointer to data buffer. - * dataLen length of specified data. - * crcType type of crc + * @param dataLen length of specified data. + * @param crcType type of crc * * @return FAILURE crc type not support. * SUCCESS diff --git a/df_datalink/src/networking/link_layer/link_layer.c b/df_datalink/src/networking/link_layer/link_layer.c index ef77eaa..c9f30e5 100644 --- a/df_datalink/src/networking/link_layer/link_layer.c +++ b/df_datalink/src/networking/link_layer/link_layer.c @@ -35,120 +35,120 @@ /**************************** Type Definitions *******************************/ /************************** Function Prototypes ******************************/ static void RcdShift(L2_RCD* rcd, int32_t netxVal); -static void UpdateMainSate(L2_INST *l2InstPtr, L2_NODE_STATE newSate); -static void SlotNoUpdate(L2_INST *l2InstPtr, uint32_t newSlotNo); -static void FrameNoUpdate(L2_INST *l2InstPtr, uint32_t newFrameNo); -static void DataAvailUpdate(L2_INST *l2InstPtr, bool status); -static void EventPoolHandler(L2_INST *l2InstPtr); +static void UpdateMainSate(L2_INST *instPtr, L2_NODE_STATE newSate); +static void SlotNoUpdate(L2_INST *instPtr, uint32_t newSlotNo); +static void FrameNoUpdate(L2_INST *instPtr, uint32_t newFrameNo); +static void DataAvailUpdate(L2_INST *instPtr, bool status); +static void EventPoolHandler(L2_INST *instPtr); -static inline void UpdateMainSate(L2_INST *l2InstPtr, L2_NODE_STATE newSate) { - ASSERT(l2InstPtr != NULL); - DBG_INFO("MainStateUpdate: %d -> %d\n", l2InstPtr->mainState, newSate); - l2InstPtr->nextState = newSate; +static inline void UpdateMainSate(L2_INST *instPtr, L2_NODE_STATE newSate) { + ASSERT(instPtr != NULL); + DBG_INFO("MainStateUpdate: %d -> %d\n", instPtr->mainState, newSate); + instPtr->nextState = newSate; } -static inline void ForceUpdateMainSate(L2_INST *l2InstPtr, L2_NODE_STATE newSate) { - ASSERT(l2InstPtr != NULL); - DBG_INFO("MainState Force Update: %d -> %d\n", l2InstPtr->mainState, newSate); - l2InstPtr->mainState = newSate; - l2InstPtr->nextState = newSate; +static inline void ForceUpdateMainSate(L2_INST *instPtr, L2_NODE_STATE newSate) { + ASSERT(instPtr != NULL); + DBG_INFO("MainState Force Update: %d -> %d\n", instPtr->mainState, newSate); + instPtr->mainState = newSate; + instPtr->nextState = newSate; } /** * @brief Update frame number, based on new next slotNo */ // FIXME: frame number update should effect 'last' and 'curr' frame number record. -static inline void SlotNoUpdate(L2_INST *l2InstPtr, uint32_t newSlotNo) { - ASSERT(l2InstPtr != NULL); +static inline void SlotNoUpdate(L2_INST *instPtr, uint32_t newSlotNo) { + ASSERT(instPtr != NULL); DBG_INFO("Update slot number to %lu\n", newSlotNo); - l2InstPtr->slotNo.next = newSlotNo; + instPtr->slotNo.next = newSlotNo; } /** * @brief Update slot number, based on new next frameNo */ // TODO: slot number update should effect 'last' and 'curr' slot number record. -static inline void FrameNoUpdate(L2_INST *l2InstPtr, uint32_t newFrameNo) { - ASSERT(l2InstPtr != NULL); +static inline void FrameNoUpdate(L2_INST *instPtr, uint32_t newFrameNo) { + ASSERT(instPtr != NULL); DBG_INFO("Update frame number to %lu\n", newFrameNo); - l2InstPtr->frameNo.next = newFrameNo; + instPtr->frameNo.next = newFrameNo; } /** * @brief Update l2 frame buffer state * infer that if there is a new valid l2 frame up. */ -static inline void DataAvailUpdate(L2_INST *l2InstPtr, bool status) { - ASSERT(l2InstPtr != NULL); - l2InstPtr->dataAvail = status; +static inline void DataAvailUpdate(L2_INST *instPtr, bool status) { + ASSERT(instPtr != NULL); + instPtr->dataAvail = status; } -L2_NODE_STATE L2_GetMainState(L2_INST *l2InstPtr){ - ASSERT(l2InstPtr != NULL); - return l2InstPtr->mainState; +L2_NODE_STATE L2_GetMainState(L2_INST *instPtr){ + ASSERT(instPtr != NULL); + return instPtr->mainState; } /** * @brief Check if there is a valid l2 frame. */ -inline bool L2_isDataAvail(L2_INST *l2InstPtr) { - ASSERT(l2InstPtr != NULL); - return l2InstPtr->dataAvail; +inline bool L2_isDataAvail(L2_INST *instPtr) { + ASSERT(instPtr != NULL); + return instPtr->dataAvail; } /** * @brief Check if node is connected to the network */ -inline bool L2_isCxn2Net(L2_INST *l2InstPtr) { - ASSERT(l2InstPtr != NULL); - return l2InstPtr->cxn2Net; +inline bool L2_isCxn2Net(L2_INST *instPtr) { + ASSERT(instPtr != NULL); + return instPtr->cxn2Net; } /** * @brief Get last frame number. */ -inline uint32_t L2_GetLastFrameNo(L2_INST *l2InstPtr) { - ASSERT(l2InstPtr != NULL); - return l2InstPtr->frameNo.last; +inline uint32_t L2_GetLastFrameNo(L2_INST *instPtr) { + ASSERT(instPtr != NULL); + return instPtr->frameNo.last; } /** * @brief Get last slot number. */ -inline uint32_t L2_GetLastSlotNo(L2_INST *l2InstPtr) { - ASSERT(l2InstPtr != NULL); - return l2InstPtr->slotNo.last; +inline uint32_t L2_GetLastSlotNo(L2_INST *instPtr) { + ASSERT(instPtr != NULL); + return instPtr->slotNo.last; } /** * @brief Get current frame number. */ -inline uint32_t L2_GetCurrentFrameNo(L2_INST *l2InstPtr) { - ASSERT(l2InstPtr != NULL); - return l2InstPtr->frameNo.curr; +inline uint32_t L2_GetCurrentFrameNo(L2_INST *instPtr) { + ASSERT(instPtr != NULL); + return instPtr->frameNo.curr; } /** * @brief Get current slot number. */ -inline uint32_t L2_GetCurrentSlotNo(L2_INST *l2InstPtr) { - ASSERT(l2InstPtr != NULL); - return l2InstPtr->slotNo.curr; +inline uint32_t L2_GetCurrentSlotNo(L2_INST *instPtr) { + ASSERT(instPtr != NULL); + return instPtr->slotNo.curr; } /** * @brief Get next frame number. */ -inline uint32_t L2_GetNextFrameNo(L2_INST *l2InstPtr) { - ASSERT(l2InstPtr != NULL); - return l2InstPtr->frameNo.next; +inline uint32_t L2_GetNextFrameNo(L2_INST *instPtr) { + ASSERT(instPtr != NULL); + return instPtr->frameNo.next; } /** * @brief Get next slot number. */ -inline uint32_t L2_GetNextSlotNo(L2_INST *l2InstPtr) { - ASSERT(l2InstPtr != NULL); - return l2InstPtr->slotNo.next; +inline uint32_t L2_GetNextSlotNo(L2_INST *instPtr) { + ASSERT(instPtr != NULL); + return instPtr->slotNo.next; } /*****************************************************************************/ @@ -161,10 +161,10 @@ inline uint32_t L2_GetNextSlotNo(L2_INST *l2InstPtr) { * Pointer to schedule event. * ******************************************************************************/ -L2_SCHD_EVENT* L2_CreatNewSchdEvent(L2_INST *l2InstPtr, L2_EVENT_TYPE event){ - ASSERT(l2InstPtr != NULL); +L2_SCHD_EVENT* L2_CreatNewSchdEvent(L2_INST *instPtr, L2_EVENT_TYPE event){ + ASSERT(instPtr != NULL); L2_SCHD_EVENT* schdEventPtr = NULL; - if(l2InstPtr->schdEventCnt >= L2_SCHD_EVENT_MAX){ + if(instPtr->schdEventCnt >= L2_SCHD_EVENT_MAX){ DBG_ERROR("Schedule event number exceeds the max, no more schedule event can be created.\n"); return NULL; } @@ -177,13 +177,13 @@ L2_SCHD_EVENT* L2_CreatNewSchdEvent(L2_INST *l2InstPtr, L2_EVENT_TYPE event){ DBG_ERROR("Unknown event type: %d.\n", event); return NULL; } - schdEventPtr->eventId = l2InstPtr->schdEventCnt; + schdEventPtr->eventId = instPtr->schdEventCnt; schdEventPtr->l2Event.msg = NULL; schdEventPtr->isSchd = false; schdEventPtr->schdSlot = L2_SLOTNO_NONE; schdEventPtr->schdFrame = L2_FRAMENO_NONE; - l2InstPtr->schdEventPool[l2InstPtr->schdEventCnt] = schdEventPtr; - l2InstPtr->schdEventCnt++; + instPtr->schdEventPool[instPtr->schdEventCnt] = schdEventPtr; + instPtr->schdEventCnt++; return schdEventPtr; } @@ -199,20 +199,20 @@ L2_SCHD_EVENT* L2_CreatNewSchdEvent(L2_INST *l2InstPtr, L2_EVENT_TYPE event){ * error code * ******************************************************************************/ -int32_t L2_DeletSchdEvent(L2_INST *l2InstPtr, L2_SCHD_EVENT *schdEvent){ - ASSERT(l2InstPtr != NULL); +int32_t L2_DeletSchdEvent(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent){ + ASSERT(instPtr != NULL); ASSERT(schdEvent != NULL); free(schdEvent->l2Event.msg); - for(int32_t i = schdEvent->eventId; i < l2InstPtr->schdEventCnt; i++){ - if(i == l2InstPtr->schdEventCnt - 1){ - l2InstPtr->schdEventPool[i] = NULL; + for(int32_t i = schdEvent->eventId; i < instPtr->schdEventCnt; i++){ + if(i == instPtr->schdEventCnt - 1){ + instPtr->schdEventPool[i] = NULL; } else{ - l2InstPtr->schdEventPool[i] = l2InstPtr->schdEventPool[i+1]; + instPtr->schdEventPool[i] = instPtr->schdEventPool[i+1]; } } free(schdEvent); - l2InstPtr->schdEventCnt --; + instPtr->schdEventCnt --; return SUCCESS; } @@ -260,15 +260,15 @@ inline bool L2_isEventScheduled(L2_SCHD_EVENT* schdEvent){ * SUCCESS * ******************************************************************************/ -int32_t L2_ScheduleEventAt(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo){ +int32_t L2_ScheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo){ ASSERT(schdEvent != NULL); if(L2_isEventScheduled(schdEvent) == true){ DBG_ERROR("Event %d has already been scheduled.\n", schdEvent->eventId); return FAILURE; } - if(frameNo <= L2_GetCurrentFrameNo(l2InstPtr) && slotNo < L2_GetCurrentSlotNo(l2InstPtr)){ + if(frameNo <= L2_GetCurrentFrameNo(instPtr) && slotNo < L2_GetCurrentSlotNo(instPtr)){ DBG_ERROR("Cannot schedule event %d to the past(current: [%ld-%ld], schedule: [%lu-%lu]).\n", - schdEvent->eventId, L2_GetCurrentFrameNo(l2InstPtr), L2_GetCurrentSlotNo(l2InstPtr), frameNo, slotNo); + schdEvent->eventId, L2_GetCurrentFrameNo(instPtr), L2_GetCurrentSlotNo(instPtr), frameNo, slotNo); return FAILURE; } schdEvent->isSchd = true; @@ -288,15 +288,15 @@ int32_t L2_ScheduleEventAt(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32_ * SUCCESS * ******************************************************************************/ -int32_t L2_ScheduleEventAfter(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots){ +int32_t L2_ScheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots){ ASSERT(schdEvent != NULL); if(L2_isEventScheduled(schdEvent) == true){ DBG_ERROR("Event %d has already been scheduled.\n", schdEvent->eventId); return FAILURE; } - uint32_t frameNo = L2_GetCurrentFrameNo(l2InstPtr) + (numSlots/L2_SLOTS_PER_FRAME); - uint32_t slotNo = L2_GetCurrentSlotNo(l2InstPtr) + (numSlots%L2_SLOTS_PER_FRAME); - return L2_ScheduleEventAt(l2InstPtr, schdEvent, frameNo, slotNo); + uint32_t frameNo = L2_GetCurrentFrameNo(instPtr) + (numSlots/L2_SLOTS_PER_FRAME); + uint32_t slotNo = L2_GetCurrentSlotNo(instPtr) + (numSlots%L2_SLOTS_PER_FRAME); + return L2_ScheduleEventAt(instPtr, schdEvent, frameNo, slotNo); } /*****************************************************************************/ @@ -311,13 +311,13 @@ int32_t L2_ScheduleEventAfter(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint * SUCCESS * ******************************************************************************/ -int32_t L2_RecheduleEventAt(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo){ +int32_t L2_RecheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo){ ASSERT(schdEvent != NULL); if(schdEvent->isSchd == false){ DBG_WARNING("Event %d has not been scheduled yet.\n", schdEvent->eventId); } schdEvent->isSchd = false; - return L2_ScheduleEventAt(l2InstPtr, schdEvent, frameNo, slotNo); + return L2_ScheduleEventAt(instPtr, schdEvent, frameNo, slotNo); } /*****************************************************************************/ @@ -333,12 +333,12 @@ int32_t L2_RecheduleEventAt(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32 * @return NONE. * ******************************************************************************/ -int32_t L2_RecheduleEventAfter(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots){ +int32_t L2_RecheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots){ ASSERT(schdEvent != NULL); if(schdEvent->isSchd == false){ DBG_WARNING("Event %d has not been scheduled yet.\n", schdEvent->eventId); } - return L2_ScheduleEventAfter(l2InstPtr, schdEvent, numSlots); + return L2_ScheduleEventAfter(instPtr, schdEvent, numSlots); } /*****************************************************************************/ @@ -368,13 +368,13 @@ int32_t L2_CancelEvent(L2_SCHD_EVENT* schdEvent){ * @return NONE. * ******************************************************************************/ -static void EventPoolHandler(L2_INST *l2InstPtr){ - ASSERT(l2InstPtr != NULL); - for(int32_t i = 0; i < l2InstPtr->schdEventCnt; i++){ - if(l2InstPtr->schdEventPool[i]->isSchd == true){ - if(l2InstPtr->schdEventPool[i]->schdFrame == l2InstPtr->frameNo.curr - && l2InstPtr->schdEventPool[i]->schdSlot == l2InstPtr->slotNo.curr){ - AssertL2Event(l2InstPtr, &(l2InstPtr->schdEventPool[i]->l2Event)); +static void EventPoolHandler(L2_INST *instPtr){ + ASSERT(instPtr != NULL); + for(int32_t i = 0; i < instPtr->schdEventCnt; i++){ + if(instPtr->schdEventPool[i]->isSchd == true){ + if(instPtr->schdEventPool[i]->schdFrame == instPtr->frameNo.curr + && instPtr->schdEventPool[i]->schdSlot == instPtr->slotNo.curr){ + AssertL2Event(instPtr, &(instPtr->schdEventPool[i]->l2Event)); } } } @@ -405,12 +405,12 @@ static inline void RcdShift(L2_RCD* rcd, int32_t netxVal) { * ******************************************************************************/ -L2_FRAME_TYPE L2_GetLastRcvFrameType(L2_INST *l2InstPtr) { - if (l2InstPtr->dataAvail == false) { +L2_FRAME_TYPE L2_GetLastRcvFrameType(L2_INST *instPtr) { + if (instPtr->dataAvail == false) { return L2_NONE_FRAME; } else { - return l2InstPtr->frame.last; + return instPtr->frame.last; } } @@ -424,35 +424,35 @@ L2_FRAME_TYPE L2_GetLastRcvFrameType(L2_INST *l2InstPtr) { * @return NONE. * ******************************************************************************/ -void L2_FrameStateUpdate(L2_INST *l2InstPtr) { - ASSERT(l2InstPtr != NULL); - PHY_INST* phyRef = l2InstPtr->config.phyInstRef; +void L2_FrameStateUpdate(L2_INST *instPtr) { + ASSERT(instPtr != NULL); + PHY_INST* phyRef = instPtr->config.phyInstRef; bool isRecved, isValid; // Update Phisical layer state - Phy_UpdateState(l2InstPtr->config.phyInstRef); + Phy_UpdateState(instPtr->config.phyInstRef); // Frame types shift (last <- curr <- next) - RcdShift(&(l2InstPtr->frame), L2_NONE_FRAME); - RcdShift(&(l2InstPtr->torState), L2_TOR_SUSPEND); + RcdShift(&(instPtr->frame), L2_NONE_FRAME); + RcdShift(&(instPtr->torState), L2_TOR_SUSPEND); - if(Phy_IsFrameValid(l2InstPtr->config.phyInstRef)) { - if(Phy_RetrieveRecvData(phyRef, l2InstPtr->recvPktData) != SUCCESS) { + if(Phy_IsFrameValid(instPtr->config.phyInstRef)) { + if(Phy_RetrieveRecvData(phyRef, instPtr->recvPktData) != SUCCESS) { ASSERT(false); } } // Update frame state. - switch (l2InstPtr->frame.last) { + switch (instPtr->frame.last) { case L2_SCAN_FRAME : - isRecved = Phy_IsFrameDetected(l2InstPtr->config.phyInstRef); + isRecved = Phy_IsFrameDetected(instPtr->config.phyInstRef); isValid = isRecved; break; case L2_SVC_FRAME: - isRecved = Phy_IsFrameValid(l2InstPtr->config.phyInstRef); + isRecved = Phy_IsFrameValid(instPtr->config.phyInstRef); if (isRecved == true) { #ifndef DATALINK_ENABLE_BERTEST - isValid = L2_CrcCheck(l2InstPtr->recvPktData, SVC_PACK_LEN, L2_CRC16_1021); + isValid = L2_CrcCheck(instPtr->recvPktData, SVC_PACK_LEN, L2_CRC16_1021); #else isValid = true; #endif @@ -462,9 +462,9 @@ void L2_FrameStateUpdate(L2_INST *l2InstPtr) { } break; case L2_INFO_FRAME: - isRecved = Phy_IsFrameValid(l2InstPtr->config.phyInstRef); + isRecved = Phy_IsFrameValid(instPtr->config.phyInstRef); if (isRecved == true) { - isValid = L2_CrcCheck(l2InstPtr->recvPktData, RSP_PACK_LEN, L2_CRC16_1021); + isValid = L2_CrcCheck(instPtr->recvPktData, RSP_PACK_LEN, L2_CRC16_1021); } else { isValid = false; @@ -475,64 +475,64 @@ void L2_FrameStateUpdate(L2_INST *l2InstPtr) { isValid = false; break; } - l2InstPtr->recvCnt = (isRecved == true) ? l2InstPtr->recvCnt+1 : l2InstPtr->recvCnt; - l2InstPtr->recvFineCnt = (isValid == true) ? l2InstPtr->recvFineCnt+1 : l2InstPtr->recvFineCnt; - DataAvailUpdate(l2InstPtr, isValid); + instPtr->recvCnt = (isRecved == true) ? instPtr->recvCnt+1 : instPtr->recvCnt; + instPtr->recvFineCnt = (isValid == true) ? instPtr->recvFineCnt+1 : instPtr->recvFineCnt; + DataAvailUpdate(instPtr, isValid); if (isRecved == true) { - DBG_INFO("Frame received type: %ld, validation: %d.\n", l2InstPtr->frame.last, isValid); + DBG_INFO("Frame received type: %ld, validation: %d.\n", instPtr->frame.last, isValid); } // Logging status. // -- send slot status - if(l2InstPtr->torState.last == L2_TOR_RX) { - l2InstPtr->rxSlotLog.tuNo = l2InstPtr->frameNo.last; - l2InstPtr->rxSlotLog.slotNo = l2InstPtr->slotNo.last; - l2InstPtr->rxSlotLog.frameType = l2InstPtr->frame.last; - l2InstPtr->rxSlotLog.srcId = l2InstPtr->config.myId; - l2InstPtr->rxSlotLog.dstId = (l2InstPtr->config.myType == L2_HEAD_NODE) ? L2_MEMBER_NODE_ID : L2_HEAD_NODE_ID; + if(instPtr->torState.last == L2_TOR_RX) { + instPtr->rxSlotLog.tuNo = instPtr->frameNo.last; + instPtr->rxSlotLog.slotNo = instPtr->slotNo.last; + instPtr->rxSlotLog.frameType = instPtr->frame.last; + instPtr->rxSlotLog.srcId = instPtr->config.myId; + instPtr->rxSlotLog.dstId = (instPtr->config.myType == L2_HEAD_NODE) ? L2_MEMBER_NODE_ID : L2_HEAD_NODE_ID; // TODO: baseInfo[18]; - memset(l2InstPtr->rxSlotLog.payload, 0x00, sizeof(l2InstPtr->rxSlotLog.payload)); + memset(instPtr->rxSlotLog.payload, 0x00, sizeof(instPtr->rxSlotLog.payload)); if(isValid == true) { - memcpy(l2InstPtr->rxSlotLog.payload, - l2InstPtr->recvPktData, - Phy_GetFrameLen(L2_L2frame2L1frame(l2InstPtr->frame.last))); - l2InstPtr->rxSlotLog.tmStamp = PhyRead(l2InstPtr->config.phyInstRef, FSTIME_L); - l2InstPtr->rxSlotLog.tmStamp |= ((uint64_t)PhyRead(l2InstPtr->config.phyInstRef, FSTIME_H))<<32; + memcpy(instPtr->rxSlotLog.payload, + instPtr->recvPktData, + Phy_GetFrameLen(L2_L2frame2L1frame(instPtr->frame.last))); + instPtr->rxSlotLog.tmStamp = PhyRead(instPtr->config.phyInstRef, FSTIME_L); + instPtr->rxSlotLog.tmStamp |= ((uint64_t)PhyRead(instPtr->config.phyInstRef, FSTIME_H))<<32; // Log time deviation. int64_t devi; devi = 0; - if (L2_TimeSync_GetDeviatoion(l2InstPtr, &devi) == SUCCESS) { + if (L2_TimeSync_GetDeviatoion(instPtr, &devi) == SUCCESS) { if (devi <= INT32_MAX && devi >= INT32_MIN) { - l2InstPtr->txSlotLog.tmDevi = (int32_t) devi; + instPtr->txSlotLog.tmDevi = (int32_t) devi; } - l2InstPtr->txSlotLog.tmDevi = (int32_t) devi; + instPtr->txSlotLog.tmDevi = (int32_t) devi; } // Log frequency deviation - l2InstPtr->rxSlotLog.freqDevi = Phy_GetFreqEst(l2InstPtr->config.phyInstRef); + instPtr->rxSlotLog.freqDevi = Phy_GetFreqEst(instPtr->config.phyInstRef); } else { - l2InstPtr->rxSlotLog.tmStamp = 0; - l2InstPtr->rxSlotLog.tmDevi = 0; - l2InstPtr->rxSlotLog.freqDevi = 0; + instPtr->rxSlotLog.tmStamp = 0; + instPtr->rxSlotLog.tmDevi = 0; + instPtr->rxSlotLog.freqDevi = 0; } - SysMonitor_UpdateRecvSlotLog(&(l2InstPtr->rxSlotLog)); + SysMonitor_UpdateRecvSlotLog(&(instPtr->rxSlotLog)); } // -- send slot status - if(l2InstPtr->torState.curr == L2_TOR_TX) { - l2InstPtr->txSlotLog.tuNo = l2InstPtr->frameNo.curr; - l2InstPtr->txSlotLog.slotNo = l2InstPtr->slotNo.curr; - l2InstPtr->txSlotLog.frameType = l2InstPtr->frame.curr; - l2InstPtr->txSlotLog.srcId = l2InstPtr->config.myId; - l2InstPtr->txSlotLog.dstId = (l2InstPtr->config.myType == L2_HEAD_NODE) ? L2_MEMBER_NODE_ID : L2_HEAD_NODE_ID; + if(instPtr->torState.curr == L2_TOR_TX) { + instPtr->txSlotLog.tuNo = instPtr->frameNo.curr; + instPtr->txSlotLog.slotNo = instPtr->slotNo.curr; + instPtr->txSlotLog.frameType = instPtr->frame.curr; + instPtr->txSlotLog.srcId = instPtr->config.myId; + instPtr->txSlotLog.dstId = (instPtr->config.myType == L2_HEAD_NODE) ? L2_MEMBER_NODE_ID : L2_HEAD_NODE_ID; // TODO: baseInfo[18]; - memset(l2InstPtr->rxSlotLog.payload, 0x00, sizeof(l2InstPtr->txSlotLog.payload)); - memcpy(l2InstPtr->txSlotLog.payload, - l2InstPtr->recvPktData, - Phy_GetFrameLen(L2_L2frame2L1frame(l2InstPtr->frame.curr))); - l2InstPtr->txSlotLog.tmStamp = 0; - l2InstPtr->txSlotLog.tmDevi = 0; - l2InstPtr->txSlotLog.freqDevi = 0; - SysMonitor_UpdateSendSlotLog(&(l2InstPtr->txSlotLog)); + memset(instPtr->rxSlotLog.payload, 0x00, sizeof(instPtr->txSlotLog.payload)); + memcpy(instPtr->txSlotLog.payload, + instPtr->recvPktData, + Phy_GetFrameLen(L2_L2frame2L1frame(instPtr->frame.curr))); + instPtr->txSlotLog.tmStamp = 0; + instPtr->txSlotLog.tmDevi = 0; + instPtr->txSlotLog.freqDevi = 0; + SysMonitor_UpdateSendSlotLog(&(instPtr->txSlotLog)); } } @@ -540,7 +540,7 @@ void L2_FrameStateUpdate(L2_INST *l2InstPtr) { /** * @brief Steer antenna beam to specified node. * - * @param l2InstPtr a pointer to link layer descriptor instance + * @param instPtr a pointer to link layer descriptor instance * @param nodeId destination node * * @return Corresponding frame length @@ -548,26 +548,26 @@ void L2_FrameStateUpdate(L2_INST *l2InstPtr) { * ******************************************************************************/ // FIXME: To Complete. -int32_t L2_SteerBeam2Node(L2_INST *l2InstPtr, uint8_t nodeId){ - ASSERT(l2InstPtr != NULL); - ASSERT(l2InstPtr->config.phyInstRef != NULL); +int32_t L2_SteerBeam2Node(L2_INST *instPtr, uint8_t nodeId){ + ASSERT(instPtr != NULL); + ASSERT(instPtr->config.phyInstRef != NULL); int32_t Status; COORD myPos, dstPos, beamDir; - Status = GetNodePos(l2InstPtr->nodeInfoList, nodeId, &dstPos); + Status = GetNodePos(instPtr->nodeInfoList, nodeId, &dstPos); if(Status != SUCCESS) { DBG_ERROR("Failed to retrieve position info of node %u, remain last configuration.\n", nodeId); return EPARAM; } - myPos.x = l2InstPtr->sysInfo.posX; - myPos.y = l2InstPtr->sysInfo.posY; - myPos.z = l2InstPtr->sysInfo.posZ; + myPos.x = instPtr->sysInfo.posX; + myPos.y = instPtr->sysInfo.posY; + myPos.z = instPtr->sysInfo.posZ; // TODO Check data format EULERANGLES attitudeEuler, antAttitudeEuler; - attitudeEuler.alpha = DEG2RAD(l2InstPtr->sysInfo.pitch); - attitudeEuler.beta = DEG2RAD(l2InstPtr->sysInfo.yaw); - attitudeEuler.gamma = DEG2RAD(l2InstPtr->sysInfo.roll); + attitudeEuler.alpha = DEG2RAD(instPtr->sysInfo.pitch); + attitudeEuler.beta = DEG2RAD(instPtr->sysInfo.yaw); + attitudeEuler.gamma = DEG2RAD(instPtr->sysInfo.roll); // FIXME: confirm date source. antAttitudeEuler.alpha = DEG2RAD(0); @@ -595,7 +595,7 @@ int32_t L2_SteerBeam2Node(L2_INST *l2InstPtr, uint8_t nodeId){ double oAxis, rAxis; oAxis = RAD2DEG(atan2(beamDir.z,beamDir.y)); rAxis = RAD2DEG(acos(Coord_Dot(beamDir, COORD_X_AXIS))); - Status = Phy_SetBeamAngle(l2InstPtr->config.phyInstRef, oAxis, rAxis); + Status = Phy_SetBeamAngle(instPtr->config.phyInstRef, oAxis, rAxis); return Status; } @@ -603,7 +603,7 @@ int32_t L2_SteerBeam2Node(L2_INST *l2InstPtr, uint8_t nodeId){ /** * @brief Layer 2 setup next slot operation, update current operating frame. * - * @param l2InstPtr a pointer to link layer descriptor instance + * @param instPtr a pointer to link layer descriptor instance * @param txrxMode tx/rx state for next slot * @param frameType frame type of next slot. * @@ -611,9 +611,9 @@ int32_t L2_SteerBeam2Node(L2_INST *l2InstPtr, uint8_t nodeId){ * L2_FRAME_NONE_LEN if the frame type code cannot be recognized. * ******************************************************************************/ -int32_t L2_SetupNextSlot(L2_INST *l2InstPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE frameType) { - ASSERT(l2InstPtr != NULL); - ASSERT(l2InstPtr->config.phyInstRef != NULL); +int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE frameType) { + ASSERT(instPtr != NULL); + ASSERT(instPtr->config.phyInstRef != NULL); int32_t Status = SUCCESS; if (frameType < L2_SCAN_FRAME || frameType > L2_INFO_FRAME) { return FAILURE; @@ -647,11 +647,11 @@ int32_t L2_SetupNextSlot(L2_INST *l2InstPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE break; } DBG_INFO("# Setup Next Slot # [%lu-%lu], %s, %s.\n", - l2InstPtr->frameNo.next, - l2InstPtr->slotNo.next, + instPtr->frameNo.next, + instPtr->slotNo.next, torStr, frameStr); #endif - l2InstPtr->frame.next = frameType; + instPtr->frame.next = frameType; PHY_TOR_TYPE tor; switch(txrxMode) { case L2_TOR_SUSPEND: @@ -667,7 +667,7 @@ int32_t L2_SetupNextSlot(L2_INST *l2InstPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE DBG_ERROR("Invalid txrx mode :%d.\n", txrxMode); return EPARAM; } - Status = Phy_SetupNextSlot(l2InstPtr->config.phyInstRef, tor, L2_L2frame2L1frame(frameType), l2InstPtr->sendPktData); + Status = Phy_SetupNextSlot(instPtr->config.phyInstRef, tor, L2_L2frame2L1frame(frameType), instPtr->sendPktData); if(Status != SUCCESS){ DBG_ERROR("Physical setup failed, with error code: %ld\n", Status); } @@ -714,94 +714,94 @@ L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame) { * @brief Initialize link layer fsm instance, allocate memory to storage fsm * data. Apply the default values to fsm. * - * @param l2InstPtr + * @param instPtr * @param config * * @return SUCCESS * Other error code. * ******************************************************************************/ -int32_t Linklayer_Init(L2_INST **l2InstPtr, L2_CONFIG *config) { - ASSERT(l2InstPtr != NULL); +int32_t Linklayer_Init(L2_INST **instPtr, L2_CONFIG *config) { + ASSERT(instPtr != NULL); ASSERT(config != NULL); int32_t Status; - (*l2InstPtr) = (L2_INST*) malloc(sizeof(L2_INST)); - if ((*l2InstPtr) == NULL) { + (*instPtr) = (L2_INST*) malloc(sizeof(L2_INST)); + if ((*instPtr) == NULL) { DBG_ERROR("Allocate memory for L2_InstPtr failed.\n"); return ENOMEM; } - memcpy(&((*l2InstPtr)->config), config, sizeof(L2_CONFIG)); + memcpy(&((*instPtr)->config), config, sizeof(L2_CONFIG)); - (*l2InstPtr)->cxn2Net = (config->myType == L2_HEAD_NODE) ? true : false; - (*l2InstPtr)->dataAvail = false; - (*l2InstPtr)->frame.last = L2_NONE_FRAME; - (*l2InstPtr)->frame.curr = L2_NONE_FRAME; - (*l2InstPtr)->frame.next = L2_NONE_FRAME; - (*l2InstPtr)->torState.last = L2_TOR_SUSPEND; - (*l2InstPtr)->torState.curr = L2_TOR_SUSPEND; - (*l2InstPtr)->torState.next = L2_TOR_SUSPEND; - (*l2InstPtr)->rtneConfig.rtneTable = normalTable; - (*l2InstPtr)->rtneConfig.rtnePrd = ARRAY_SIZE(normalTable)*L2_SLOTS_PER_FRAME; - (*l2InstPtr)->rtneConfig.rtneStep = 0; - (*l2InstPtr)->rtneConfig.last = NULL; - (*l2InstPtr)->rtneConfig.curr = NULL; - (*l2InstPtr)->rtneConfig.next = NULL; - (*l2InstPtr)->mainState = INIT_STATE; - (*l2InstPtr)->nextState = INIT_STATE; - (*l2InstPtr)->frameNo.last = 0; - (*l2InstPtr)->frameNo.curr = 0; - (*l2InstPtr)->frameNo.next = 0; - (*l2InstPtr)->slotNo.last = 0; - (*l2InstPtr)->slotNo.curr = 0; - (*l2InstPtr)->slotNo.next = 0; - (*l2InstPtr)->schdEventCnt = 0; - (*l2InstPtr)->nodeInfoList = NULL; - (*l2InstPtr)->txQueue = NULL; + (*instPtr)->cxn2Net = (config->myType == L2_HEAD_NODE) ? true : false; + (*instPtr)->dataAvail = false; + (*instPtr)->frame.last = L2_NONE_FRAME; + (*instPtr)->frame.curr = L2_NONE_FRAME; + (*instPtr)->frame.next = L2_NONE_FRAME; + (*instPtr)->torState.last = L2_TOR_SUSPEND; + (*instPtr)->torState.curr = L2_TOR_SUSPEND; + (*instPtr)->torState.next = L2_TOR_SUSPEND; + (*instPtr)->rtneConfig.rtneTable = normalTable; + (*instPtr)->rtneConfig.rtnePrd = ARRAY_SIZE(normalTable)*L2_SLOTS_PER_FRAME; + (*instPtr)->rtneConfig.rtneStep = 0; + (*instPtr)->rtneConfig.last = NULL; + (*instPtr)->rtneConfig.curr = NULL; + (*instPtr)->rtneConfig.next = NULL; + (*instPtr)->mainState = INIT_STATE; + (*instPtr)->nextState = INIT_STATE; + (*instPtr)->frameNo.last = 0; + (*instPtr)->frameNo.curr = 0; + (*instPtr)->frameNo.next = 0; + (*instPtr)->slotNo.last = 0; + (*instPtr)->slotNo.curr = 0; + (*instPtr)->slotNo.next = 0; + (*instPtr)->schdEventCnt = 0; + (*instPtr)->nodeInfoList = NULL; + (*instPtr)->txQueue = NULL; - (*l2InstPtr)->recvCnt = 0; - (*l2InstPtr)->recvFineCnt = 0; - (*l2InstPtr)->sentCnt = 0; + (*instPtr)->recvCnt = 0; + (*instPtr)->recvFineCnt = 0; + (*instPtr)->sentCnt = 0; - memset((*l2InstPtr)->schdEventPool, 0x00, sizeof(L2_SCHD_EVENT *)*L2_SCHD_EVENT_MAX); + memset((*instPtr)->schdEventPool, 0x00, sizeof(L2_SCHD_EVENT *)*L2_SCHD_EVENT_MAX); - (*l2InstPtr)->sendPktData = malloc(PACK_LEN_MAX*sizeof(uint8_t)); - if((*l2InstPtr)->sendPktData == NULL) { + (*instPtr)->sendPktData = malloc(PACK_LEN_MAX*sizeof(uint8_t)); + if((*instPtr)->sendPktData == NULL) { DBG_ERROR("Failed to allocate memory for sendPktData.\n"); goto init_error; } - (*l2InstPtr)->recvPktData = malloc(PACK_LEN_MAX*sizeof(uint8_t)); - if((*l2InstPtr)->recvPktData == NULL) { + (*instPtr)->recvPktData = malloc(PACK_LEN_MAX*sizeof(uint8_t)); + if((*instPtr)->recvPktData == NULL) { DBG_ERROR("Failed to allocate memory for recvPktData.\n"); goto init_error; } - Status = NodeList_Init(&((*l2InstPtr)->nodeInfoList)); + Status = NodeList_Init(&((*instPtr)->nodeInfoList)); if(Status != SUCCESS) { goto init_error; } - Status = L2_Scan_Init(*l2InstPtr); + Status = L2_Scan_Init(*instPtr); if(Status != SUCCESS) { goto init_error; } - Status = L2_Service_Init(*l2InstPtr); + Status = L2_Service_Init(*instPtr); if(Status != SUCCESS) { goto init_error; } - if (CircQueueCreate(&((*l2InstPtr)->txQueue), TX_QUEUE_MAX) == false) { + if (CircQueueCreate(&((*instPtr)->txQueue), TX_QUEUE_MAX) == false) { Status = ENOMEM; goto init_error; } return SUCCESS; init_error: - free((*l2InstPtr)->recvPktData); - free((*l2InstPtr)->sendPktData); - NodeList_Destroy((*l2InstPtr)->nodeInfoList); - free(*l2InstPtr); + free((*instPtr)->recvPktData); + free((*instPtr)->sendPktData); + NodeList_Destroy((*instPtr)->nodeInfoList); + free(*instPtr); return Status; } @@ -809,28 +809,28 @@ init_error: /** * @brief Handle l2 event called by other model. * - * @param l2InstPtr + * @param instPtr * @param l2Event * * @return SUCCESS - Event has been successfully handled. * FAIL - Event cannot be recognized or error occurred while process. * ******************************************************************************/ -int32_t Linklayer_EventHandler(L2_INST *l2InstPtr, L2_EVENT* l2Event) { +int32_t Linklayer_EventHandler(L2_INST *instPtr, L2_EVENT* l2Event) { switch (l2Event->event) { case L2_EV_FRAME_NO_UPDATE: { DBG_INFO("Event L2_EV_FRAME_NO_UPDATE asserted....\n"); - FrameNoUpdate(l2InstPtr, *((uint32_t*) l2Event->msg)); + FrameNoUpdate(instPtr, *((uint32_t*) l2Event->msg)); break; } case L2_EV_SLOT_NO_UPDATE: { DBG_INFO("Event L2_EV_SLOT_NO_UPDATE asserted....\n"); - SlotNoUpdate(l2InstPtr, *((uint32_t*) l2Event->msg)); + SlotNoUpdate(instPtr, *((uint32_t*) l2Event->msg)); break; } case L2_EV_MAIN_STATE_UPDATE: { DBG_INFO("Event L2_EV_MAIN_STATE_UPDATE asserted....\n"); - UpdateMainSate(l2InstPtr, *((L2_NODE_STATE*) l2Event->msg)); + UpdateMainSate(instPtr, *((L2_NODE_STATE*) l2Event->msg)); break; } case L2_EV_SCAN_PROC_DONE: { @@ -839,12 +839,12 @@ int32_t Linklayer_EventHandler(L2_INST *l2InstPtr, L2_EVENT* l2Event) { } case L2_EV_NET_SETUP_DONE: { DBG_INFO("Event L2_EV_NET_SETUP_DONE asserted....\n"); - if(l2InstPtr->config.myType != L2_HEAD_NODE){ - l2InstPtr->cxn2Net = true; - UpdateMainSate(l2InstPtr, NORMAL_STATE); + if(instPtr->config.myType != L2_HEAD_NODE){ + instPtr->cxn2Net = true; + UpdateMainSate(instPtr, NORMAL_STATE); } else { - ForceUpdateMainSate(l2InstPtr, NORMAL_STATE); + ForceUpdateMainSate(instPtr, NORMAL_STATE); } break; @@ -855,56 +855,56 @@ int32_t Linklayer_EventHandler(L2_INST *l2InstPtr, L2_EVENT* l2Event) { int64_t tmAdj; tmAdj = *((int64_t *)l2Event->msg); DBG_INFO("Time adjustment is: %lld.\n", tmAdj); - L2_TimeSync_LocalTimerAdd(l2InstPtr, tmAdj); + L2_TimeSync_LocalTimerAdd(instPtr, tmAdj); #else DBG_INFO("Using auto slot alignment.\n"); - L2_TimeSync_SlotAlign(l2InstPtr); + L2_TimeSync_SlotAlign(instPtr); #endif break; } case L2_EV_LOST_CXN: { DBG_INFO("Event L2_EV_LOST_CXN asserted....\n"); - if(l2InstPtr->config.myType == L2_HEAD_NODE){ + if(instPtr->config.myType == L2_HEAD_NODE){ DBG_ERROR("Illegal event, only None_Head node can assert L2_EV_LOST_CXN.\n"); break; } - DBG_WARNING("Lost connection with head node [%d].\n", l2InstPtr->config.headId); - L2_Scan_Init(l2InstPtr); - L2_Service_Init(l2InstPtr); - SlotNoUpdate(l2InstPtr, 0); - FrameNoUpdate(l2InstPtr, 0); - l2InstPtr->cxn2Net = false; - UpdateMainSate(l2InstPtr, INIT_STATE); + DBG_WARNING("Lost connection with head node [%d].\n", instPtr->config.headId); + L2_Scan_Init(instPtr); + L2_Service_Init(instPtr); + SlotNoUpdate(instPtr, 0); + FrameNoUpdate(instPtr, 0); + instPtr->cxn2Net = false; + UpdateMainSate(instPtr, INIT_STATE); break; } case L2_EV_LOST_NODE: { DBG_INFO("Event L2_EV_LOST_NODE asserted....\n"); - L2_Scan_Init(l2InstPtr); - L2_Service_Init(l2InstPtr); - SlotNoUpdate(l2InstPtr, 0); - FrameNoUpdate(l2InstPtr, 0); - UpdateMainSate(l2InstPtr, INIT_STATE); + L2_Scan_Init(instPtr); + L2_Service_Init(instPtr); + SlotNoUpdate(instPtr, 0); + FrameNoUpdate(instPtr, 0); + UpdateMainSate(instPtr, INIT_STATE); break; } case L2_ROUTINE_FINISH: { DBG_INFO("Event L2_ROUTINE_FINISH asserted....\n"); - if(l2InstPtr->nextState == NORMAL_STATE){ - l2InstPtr->rtneConfig.rtneTable = normalTable; - l2InstPtr->rtneConfig.rtnePrd = ARRAY_SIZE(normalTable)*L2_SLOTS_PER_FRAME; - l2InstPtr->rtneConfig.rtneStep = 0; + if(instPtr->nextState == NORMAL_STATE){ + instPtr->rtneConfig.rtneTable = normalTable; + instPtr->rtneConfig.rtnePrd = ARRAY_SIZE(normalTable)*L2_SLOTS_PER_FRAME; + instPtr->rtneConfig.rtneStep = 0; } else{ - DBG_WARNING("Unsupported routine state [%d], Continue withe previous routine table.\n", l2InstPtr->nextState); - l2InstPtr->rtneConfig.rtneStep = 0; + DBG_WARNING("Unsupported routine state [%d], Continue withe previous routine table.\n", instPtr->nextState); + instPtr->rtneConfig.rtneStep = 0; } break; } case L2_ROUTINE_UPDATE: { DBG_INFO("Event L2_ROUTINE_UPDATE asserted....\n"); L2_RTNE_CONFIG* newRtneConfig = (L2_RTNE_CONFIG*) l2Event->msg; - l2InstPtr->rtneConfig.rtneTable = newRtneConfig->rtneTable; - l2InstPtr->rtneConfig.rtnePrd = newRtneConfig->rtnePrd; - l2InstPtr->rtneConfig.rtneStep = newRtneConfig->rtneStep; + instPtr->rtneConfig.rtneTable = newRtneConfig->rtneTable; + instPtr->rtneConfig.rtnePrd = newRtneConfig->rtnePrd; + instPtr->rtneConfig.rtneStep = newRtneConfig->rtneStep; break; } default: { @@ -919,32 +919,32 @@ int32_t Linklayer_EventHandler(L2_INST *l2InstPtr, L2_EVENT* l2Event) { /** * @brief Handle l2 packet correctly received. * - * @param l2InstPtr + * @param instPtr * * @return Status code. * ******************************************************************************/ -int32_t Linklayer_PktHandler(L2_INST *l2InstPtr) { +int32_t Linklayer_PktHandler(L2_INST *instPtr) { int32_t Status; L2_FRAME_TYPE frameType; - frameType = L2_GetLastRcvFrameType(l2InstPtr); + frameType = L2_GetLastRcvFrameType(instPtr); DBG_INFO("Link layer handling packet, type: %ld.\n", frameType); - if (l2InstPtr->dataAvail == false) { + if (instPtr->dataAvail == false) { return FAILURE; } switch (frameType) { case L2_SCAN_FRAME: - Status = L2_Scan_PktHandler(l2InstPtr, l2InstPtr->recvPktData); + Status = L2_Scan_PktHandler(instPtr, instPtr->recvPktData); break; case L2_SVC_FRAME: - Status = L2_Service_PktHandler(l2InstPtr, l2InstPtr->recvPktData); + Status = L2_Service_PktHandler(instPtr, instPtr->recvPktData); break; case L2_INFO_FRAME: - if(l2InstPtr->mainState == INIT_STATE) { - Status = L2_Scan_PktHandler(l2InstPtr, l2InstPtr->recvPktData); + if(instPtr->mainState == INIT_STATE) { + Status = L2_Scan_PktHandler(instPtr, instPtr->recvPktData); } else {// NORMAL_STATE - Status = L2_Service_PktHandler(l2InstPtr, l2InstPtr->recvPktData); + Status = L2_Service_PktHandler(instPtr, instPtr->recvPktData); } break; default: @@ -980,64 +980,64 @@ static bool IsNodeConnected(NODE_ENTRY *node) { /** * @brief Schedule next slot based on time allocate slice. * - * @param l2InstPtr. + * @param instPtr. * * @return SUCCESS * Other error code. * ******************************************************************************/ -static int32_t Linklayer_Routine (L2_INST *l2InstPtr) { - ASSERT(l2InstPtr != NULL); +static int32_t Linklayer_Routine (L2_INST *instPtr) { + ASSERT(instPtr != NULL); uint32_t usedSlots, tableIndx, entryIndx; L2_RTNE_TABLE_ENTRY rtneEntry; // Determine which routine entry is going to be performed. - tableIndx = (l2InstPtr->rtneConfig.rtneStep)/L2_SLOTS_PER_FRAME; - entryIndx = (l2InstPtr->rtneConfig.rtneStep)%L2_SLOTS_PER_FRAME; + tableIndx = (instPtr->rtneConfig.rtneStep)/L2_SLOTS_PER_FRAME; + entryIndx = (instPtr->rtneConfig.rtneStep)%L2_SLOTS_PER_FRAME; // Shift routine entry record. - l2InstPtr->rtneConfig.last = l2InstPtr->rtneConfig.curr; - l2InstPtr->rtneConfig.curr = l2InstPtr->rtneConfig.next; - l2InstPtr->rtneConfig.next = &(l2InstPtr->rtneConfig.rtneTable[tableIndx][entryIndx]); - rtneEntry = l2InstPtr->rtneConfig.rtneTable[tableIndx][entryIndx]; + instPtr->rtneConfig.last = instPtr->rtneConfig.curr; + instPtr->rtneConfig.curr = instPtr->rtneConfig.next; + instPtr->rtneConfig.next = &(instPtr->rtneConfig.rtneTable[tableIndx][entryIndx]); + rtneEntry = instPtr->rtneConfig.rtneTable[tableIndx][entryIndx]; switch (rtneEntry.slotType) { case L2_SLOT_IDLE: // Skip a 5ms slot. usedSlots = NUM_SLOTS_5MS; - Phy_SetupNextSlot(l2InstPtr->config.phyInstRef, L2_TOR_SUSPEND, L1_NONE_FRAME, NULL); + Phy_SetupNextSlot(instPtr->config.phyInstRef, L2_TOR_SUSPEND, L1_NONE_FRAME, NULL); break; case L2_SLOT_SCAN: - usedSlots = L2_Scan_Routine(l2InstPtr, &rtneEntry); + usedSlots = L2_Scan_Routine(instPtr, &rtneEntry); break; case L2_SLOT_SERVICE ... L2_SLOT_ADVERTISE: - usedSlots = L2_Service_Routine(l2InstPtr, &rtneEntry); + usedSlots = L2_Service_Routine(instPtr, &rtneEntry); break; default: DBG_ERROR("UNKNOWN slot type [%d].\n", rtneEntry.slotType); return NUM_SLOTS_NONE; } - DBG_INFO("## Used slots: %lu, step: %lu, period: %lu.\n", usedSlots, l2InstPtr->rtneConfig.rtneStep, l2InstPtr->rtneConfig.rtnePrd); + DBG_INFO("## Used slots: %lu, step: %lu, period: %lu.\n", usedSlots, instPtr->rtneConfig.rtneStep, instPtr->rtneConfig.rtnePrd); - l2InstPtr->rtneConfig.rtneStep += usedSlots; + instPtr->rtneConfig.rtneStep += usedSlots; // check if routine is finished. - if(l2InstPtr->rtneConfig.rtneStep >= l2InstPtr->rtneConfig.rtnePrd){ - L2_EVENT rtneFinishEvent = {L2_ROUTINE_FINISH, &(l2InstPtr->rtneConfig)}; - AssertL2Event(l2InstPtr, &rtneFinishEvent); + if(instPtr->rtneConfig.rtneStep >= instPtr->rtneConfig.rtnePrd){ + L2_EVENT rtneFinishEvent = {L2_ROUTINE_FINISH, &(instPtr->rtneConfig)}; + AssertL2Event(instPtr, &rtneFinishEvent); } // Update TTL - IterEditNode(l2InstPtr->nodeInfoList, &usedSlots, IntcNodeTTL); + IterEditNode(instPtr->nodeInfoList, &usedSlots, IntcNodeTTL); #ifdef DATALINK_ENABLE_AUTORECONNECT - IterDeleteNode(l2InstPtr->nodeInfoList, IsNodeConnected); - if (GetNodeNum(l2InstPtr->nodeInfoList) < l2InstPtr->config.netScale - 1) { + IterDeleteNode(instPtr->nodeInfoList, IsNodeConnected); + if (GetNodeNum(instPtr->nodeInfoList) < instPtr->config.netScale - 1) { // reset scan process L2_EVENT l2Event; // Assert Net-Setup Done. - l2Event.event = (l2InstPtr->config.myType == L2_HEAD_NODE) ? L2_EV_LOST_NODE : L2_EV_LOST_CXN; + l2Event.event = (instPtr->config.myType == L2_HEAD_NODE) ? L2_EV_LOST_NODE : L2_EV_LOST_CXN; l2Event.msg = NULL; - AssertL2Event(l2InstPtr, &l2Event); + AssertL2Event(instPtr, &l2Event); usedSlots = 0; // Stop scheduling } #endif @@ -1049,37 +1049,37 @@ static int32_t Linklayer_Routine (L2_INST *l2InstPtr) { /** * @brief Execute a link layer fsm process. * - * @param l2InstPtr. + * @param instPtr. * * @return SUCCESS * Other error code. * ******************************************************************************/ -int32_t Linklayer_Proc(L2_INST *l2InstPtr) { - ASSERT(l2InstPtr != NULL); - ASSERT(l2InstPtr->config.myType != L2_NONE_TYPE); +int32_t Linklayer_Proc(L2_INST *instPtr) { + ASSERT(instPtr != NULL); + ASSERT(instPtr->config.myType != L2_NONE_TYPE); uint32_t usedSlots = 0; DBG_PRINT("######## LinkLlayer Procedure ########\n"); // Sync System info - SysMonitor_GetSysinfo(&(l2InstPtr->sysInfo)); + SysMonitor_GetSysinfo(&(instPtr->sysInfo)); // Update frame status. - L2_FrameStateUpdate(l2InstPtr); + L2_FrameStateUpdate(instPtr); // Handle frame received in last slot. - if(L2_isDataAvail(l2InstPtr) == true){ - if(Linklayer_PktHandler(l2InstPtr) != SUCCESS) { - DBG_ERROR("Failed to handle last received frame: %ld.\n", l2InstPtr->frame.last); + if(L2_isDataAvail(instPtr) == true){ + if(Linklayer_PktHandler(instPtr) != SUCCESS) { + DBG_ERROR("Failed to handle last received frame: %ld.\n", instPtr->frame.last); } } // Run Link layer routine, setup next slot procedure. - if (l2InstPtr->mainState == INIT_STATE) { - usedSlots = L2_Scan_Proc(l2InstPtr); + if (instPtr->mainState == INIT_STATE) { + usedSlots = L2_Scan_Proc(instPtr); } else { - usedSlots = Linklayer_Routine(l2InstPtr); + usedSlots = Linklayer_Routine(instPtr); } // Check if previous runs has error occurred. @@ -1088,34 +1088,34 @@ int32_t Linklayer_Proc(L2_INST *l2InstPtr) { } // Check and execute scheduled event. - EventPoolHandler(l2InstPtr); + EventPoolHandler(instPtr); // Update Protocol log - l2InstPtr->prtcLog.tmSys = l2InstPtr->frameNo.last*L2_SLOTS_PER_FRAME + l2InstPtr->slotNo.last; - l2InstPtr->prtcLog.phySt = 0;// TODO: Update physical state. - l2InstPtr->prtcLog.tmLocal = PhyRead(l2InstPtr->config.phyInstRef, TM_LOCAL_L) >> 16; // 2^16 clk per LSB - l2InstPtr->prtcLog.tmLocal |= PhyRead(l2InstPtr->config.phyInstRef, TM_LOCAL_H); - l2InstPtr->prtcLog.beamNo = l2InstPtr->config.phyInstRef->beamNo.last; - l2InstPtr->prtcLog.beamNo = l2InstPtr->config.phyInstRef->beamWd.last; - l2InstPtr->prtcLog.stage = l2InstPtr->mainState; - l2InstPtr->prtcLog.recvCnt = l2InstPtr->recvCnt; - l2InstPtr->prtcLog.recvFineCnt = l2InstPtr->recvFineCnt; - l2InstPtr->prtcLog.nodeCnt = l2InstPtr->mainState; - l2InstPtr->prtcLog.myId = l2InstPtr->config.myId; - l2InstPtr->prtcLog.myId = (l2InstPtr->config.myType == L2_HEAD_NODE) ? L2_MEMBER_NODE_ID : L2_HEAD_NODE_ID; - SysMonitor_UpdateProtocolLog(&(l2InstPtr->prtcLog)); + instPtr->prtcLog.tmSys = instPtr->frameNo.last*L2_SLOTS_PER_FRAME + instPtr->slotNo.last; + instPtr->prtcLog.phySt = 0;// TODO: Update physical state. + instPtr->prtcLog.tmLocal = PhyRead(instPtr->config.phyInstRef, TM_LOCAL_L) >> 16; // 2^16 clk per LSB + instPtr->prtcLog.tmLocal |= PhyRead(instPtr->config.phyInstRef, TM_LOCAL_H); + instPtr->prtcLog.beamNo = instPtr->config.phyInstRef->beamNo.last; + instPtr->prtcLog.beamNo = instPtr->config.phyInstRef->beamWd.last; + instPtr->prtcLog.stage = instPtr->mainState; + instPtr->prtcLog.recvCnt = instPtr->recvCnt; + instPtr->prtcLog.recvFineCnt = instPtr->recvFineCnt; + instPtr->prtcLog.nodeCnt = instPtr->mainState; + instPtr->prtcLog.myId = instPtr->config.myId; + instPtr->prtcLog.myId = (instPtr->config.myType == L2_HEAD_NODE) ? L2_MEMBER_NODE_ID : L2_HEAD_NODE_ID; + SysMonitor_UpdateProtocolLog(&(instPtr->prtcLog)); // Update slot number. - RcdShift(&(l2InstPtr->frameNo), l2InstPtr->frameNo.next); - RcdShift(&(l2InstPtr->slotNo), l2InstPtr->slotNo.next); + RcdShift(&(instPtr->frameNo), instPtr->frameNo.next); + RcdShift(&(instPtr->slotNo), instPtr->slotNo.next); // Update slot number. - l2InstPtr->slotNo.next += usedSlots; - if (l2InstPtr->slotNo.next >= L2_SLOTS_PER_FRAME) { // a time frame ended - l2InstPtr->frameNo.next = (l2InstPtr->frameNo.next + 1) % L2_FRAME_NO_MAX; + instPtr->slotNo.next += usedSlots; + if (instPtr->slotNo.next >= L2_SLOTS_PER_FRAME) { // a time frame ended + instPtr->frameNo.next = (instPtr->frameNo.next + 1) % L2_FRAME_NO_MAX; } - l2InstPtr->slotNo.next %= L2_SLOTS_PER_FRAME; + instPtr->slotNo.next %= L2_SLOTS_PER_FRAME; // update link layer state. - l2InstPtr->mainState = l2InstPtr->nextState; + instPtr->mainState = instPtr->nextState; return SUCCESS; } diff --git a/df_datalink/src/networking/link_layer/link_layer.h b/df_datalink/src/networking/link_layer/link_layer.h index ae6f425..105359f 100644 --- a/df_datalink/src/networking/link_layer/link_layer.h +++ b/df_datalink/src/networking/link_layer/link_layer.h @@ -86,57 +86,57 @@ typedef struct { extern L2_RTNE_TABLE_ENTRY* normalTable[L2_RTNE_PERIOD]; /************************** Function Prototypes ******************************/ // Main -int32_t Linklayer_Init(L2_INST **l2InstPtr, L2_CONFIG *config); -int32_t Linklayer_Proc(L2_INST *l2InstPtr); -int32_t Linklayer_EventHandler(L2_INST *l2InstPtr, L2_EVENT* l2Event); -int32_t Linklayer_PktHandler(L2_INST *l2InstPtr); -int32_t L2_SteerBeam2Node(L2_INST *l2InstPtr, uint8_t nodeId); -int32_t L2_SetupNextSlot(L2_INST *l2InstPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE frameType); +int32_t Linklayer_Init(L2_INST **instPtr, L2_CONFIG *config); +int32_t Linklayer_Proc(L2_INST *instPtr); +int32_t Linklayer_EventHandler(L2_INST *instPtr, L2_EVENT* l2Event); +int32_t Linklayer_PktHandler(L2_INST *instPtr); +int32_t L2_SteerBeam2Node(L2_INST *instPtr, uint8_t nodeId); +int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE frameType); // Event -L2_SCHD_EVENT* L2_CreatNewSchdEvent(L2_INST *l2InstPtr, L2_EVENT_TYPE event); +L2_SCHD_EVENT* L2_CreatNewSchdEvent(L2_INST *instPtr, L2_EVENT_TYPE event); bool L2_isEventScheduled(L2_SCHD_EVENT* schdEvent); int32_t L2_CancelEvent(L2_SCHD_EVENT* schdEvent); int32_t L2_SchdEventAppendMsg(L2_SCHD_EVENT* schdEvent, void* msg); -int32_t L2_ScheduleEventAt(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo); -int32_t L2_ScheduleEventAfter(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots); -int32_t L2_RecheduleEventAt(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo); -int32_t L2_RecheduleEventAfter(L2_INST *l2InstPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots); +int32_t L2_ScheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo); +int32_t L2_ScheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots); +int32_t L2_RecheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo); +int32_t L2_RecheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots); // Framing -void L2_FrameStateUpdate(L2_INST *l2InstPtr); -L2_FRAME_TYPE L2_GetLastRcvFrameType(L2_INST *l2InstPtr); +void L2_FrameStateUpdate(L2_INST *instPtr); +L2_FRAME_TYPE L2_GetLastRcvFrameType(L2_INST *instPtr); L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame); int32_t L2_CrcAppend(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType); bool L2_CrcCheck(uint8_t* dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType); // Miscellaneous -L2_NODE_STATE L2_GetMainState(L2_INST *l2InstPtr); -bool L2_isDataAvail(L2_INST *l2InstPtr); -bool L2_isCxn2Net(L2_INST *l2InstPtr); +L2_NODE_STATE L2_GetMainState(L2_INST *instPtr); +bool L2_isDataAvail(L2_INST *instPtr); +bool L2_isCxn2Net(L2_INST *instPtr); int32_t L2_GetFrameLen(L2_FRAME_TYPE l2FrameType); -uint32_t L2_GetLastFrameNo(L2_INST *l2InstPtr); -uint32_t L2_GetLastSlotNo(L2_INST *l2InstPtr); -uint32_t L2_GetCurrentFrameNo(L2_INST *l2InstPtr); -uint32_t L2_GetCurrentSlotNo(L2_INST *l2InstPtr); -uint32_t L2_GetNextFrameNo(L2_INST *l2InstPtr); -uint32_t L2_GetNextSlotNo(L2_INST *l2InstPtr); +uint32_t L2_GetLastFrameNo(L2_INST *instPtr); +uint32_t L2_GetLastSlotNo(L2_INST *instPtr); +uint32_t L2_GetCurrentFrameNo(L2_INST *instPtr); +uint32_t L2_GetCurrentSlotNo(L2_INST *instPtr); +uint32_t L2_GetNextFrameNo(L2_INST *instPtr); +uint32_t L2_GetNextSlotNo(L2_INST *instPtr); // Scanning -int32_t L2_Scan_Init(L2_INST *l2InstPtr); -int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg); -int32_t L2_Scan_Proc(L2_INST *l2InstPtr); -int32_t L2_Scan_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry); +int32_t L2_Scan_Init(L2_INST *instPtr); +int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg); +int32_t L2_Scan_Proc(L2_INST *instPtr); +int32_t L2_Scan_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry); // Service -int32_t L2_Service_Init(L2_INST *l2InstPtr); -int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg); -int32_t L2_Service_Proc(L2_INST *l2InstPtr); -int32_t L2_Service_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry); +int32_t L2_Service_Init(L2_INST *instPtr); +int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg); +int32_t L2_Service_Proc(L2_INST *instPtr); +int32_t L2_Service_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry); // Time Sync -int32_t L2_TimeSync_LocalTimerAdd(L2_INST *l2InstPtr, int64_t adjustment); -int32_t L2_TimeSync_SlotAlign(L2_INST *l2InstPtr); -int32_t L2_TimeSync_GetDeviatoion(L2_INST *l2InstPtr, int64_t *devi); +int32_t L2_TimeSync_LocalTimerAdd(L2_INST *instPtr, int64_t adjustment); +int32_t L2_TimeSync_SlotAlign(L2_INST *instPtr); +int32_t L2_TimeSync_GetDeviatoion(L2_INST *instPtr, int64_t *devi); #endif /* __LINK_LAYER_H_ */ diff --git a/df_datalink/src/networking/link_layer/scan.c b/df_datalink/src/networking/link_layer/scan.c index abf5ce4..6d08f54 100644 --- a/df_datalink/src/networking/link_layer/scan.c +++ b/df_datalink/src/networking/link_layer/scan.c @@ -33,7 +33,7 @@ /************************** Variable Definitions *****************************/ /************************** Function Prototypes ******************************/ -static int32_t ScanMsg_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type); +static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type); /*****************************************************************************/ /** * @brief Packing Scan message to a bit array. @@ -45,35 +45,35 @@ static int32_t ScanMsg_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type); * SUCCESS * ******************************************************************************/ -static int32_t ScanMsg_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) { - ASSERT(l2InstPtr != NULL); +static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) { + ASSERT(instPtr != NULL); int32_t Status; uint8_t *buff; uint32_t packLen; switch (type) { case L2_INFO_FRAME:{ packLen = RSP_PACK_LEN; - SysMonitor_GetSysinfo(&(l2InstPtr->sysInfo)); + SysMonitor_GetSysinfo(&(instPtr->sysInfo)); buff = malloc(packLen); if (buff == NULL) { DBG_ERROR("Not enough space, failed to pack frame...\n"); return ENOMEM; } - buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(l2InstPtr); - buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(l2InstPtr); - memcpy(buff+L2_MSG_INFO_POSX, &(l2InstPtr->sysInfo.posX), sizeof(float)); - memcpy(buff+L2_MSG_INFO_POSY, &(l2InstPtr->sysInfo.posY), sizeof(float)); - memcpy(buff+L2_MSG_INFO_POSZ, &(l2InstPtr->sysInfo.posZ), sizeof(float)); - memcpy(buff+L2_MSG_INFO_SPEEDX, &(l2InstPtr->sysInfo.vX), sizeof(float)); - memcpy(buff+L2_MSG_INFO_SPEEDY, &(l2InstPtr->sysInfo.vY), sizeof(float)); - memcpy(buff+L2_MSG_INFO_SPEEDZ, &(l2InstPtr->sysInfo.vZ), sizeof(float)); + buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr); + buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr); + memcpy(buff+L2_MSG_INFO_POSX, &(instPtr->sysInfo.posX), sizeof(float)); + memcpy(buff+L2_MSG_INFO_POSY, &(instPtr->sysInfo.posY), sizeof(float)); + memcpy(buff+L2_MSG_INFO_POSZ, &(instPtr->sysInfo.posZ), sizeof(float)); + memcpy(buff+L2_MSG_INFO_SPEEDX, &(instPtr->sysInfo.vX), sizeof(float)); + memcpy(buff+L2_MSG_INFO_SPEEDY, &(instPtr->sysInfo.vY), sizeof(float)); + memcpy(buff+L2_MSG_INFO_SPEEDZ, &(instPtr->sysInfo.vZ), sizeof(float)); Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021); if (Status != SUCCESS) { DBG_ERROR("Fail to append CRC, error code: %ld.\n", Status); return Status; } - if(CircQueuePush(l2InstPtr->txQueue, buff) == false){ + if(CircQueuePush(instPtr->txQueue, buff) == false){ DBG_ERROR("Cannot push packet to queue..\n"); return EQUEUE; } @@ -92,15 +92,15 @@ static int32_t ScanMsg_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) { /** * @brief Initialize scan FSM. * - * @param l2InstPtr + * @param instPtr * * @return * ******************************************************************************/ -int32_t L2_Scan_Init(L2_INST *l2InstPtr) { - ASSERT(l2InstPtr != NULL); - l2InstPtr->scanFsm.sacanCnt = 0; - l2InstPtr->scanFsm.beamNo = 0; +int32_t L2_Scan_Init(L2_INST *instPtr) { + ASSERT(instPtr != NULL); + instPtr->scanFsm.sacanCnt = 0; + instPtr->scanFsm.beamNo = 0; return SUCCESS; } @@ -113,32 +113,32 @@ int32_t L2_Scan_Init(L2_INST *l2InstPtr) { * @return * ******************************************************************************/ -int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) { - ASSERT(l2InstPtr != NULL); +int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) { + ASSERT(instPtr != NULL); ASSERT(msg != NULL); int32_t Status; - switch (l2InstPtr->frame.last) { + switch (instPtr->frame.last) { case L2_SCAN_FRAME: { - if (l2InstPtr->config.myType == L2_HEAD_NODE) { + if (instPtr->config.myType == L2_HEAD_NODE) { DBG_ERROR("Unexpected call from head node, Only None-Head Node can handle L2_SVC_FRAME.\n"); Status = EUNEXP; break; } - Status = AppendNode(l2InstPtr->nodeInfoList, L2_HEAD_NODE_ID, L2_HEAD_NODE); + Status = AppendNode(instPtr->nodeInfoList, L2_HEAD_NODE_ID, L2_HEAD_NODE); if (Status != SUCCESS) { DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); } break; } case L2_INFO_FRAME: { - if (l2InstPtr->config.myType != L2_HEAD_NODE) { + if (instPtr->config.myType != L2_HEAD_NODE) { DBG_ERROR("Unexpected call from member node, Only Head Node can handle L2_SVC_FRAME.\n"); Status = EUNEXP; break; } COORD coordTemp; float tmpVal; - Status = AppendNode(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, L2_MEMBER_NODE); + Status = AppendNode(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, L2_MEMBER_NODE); if (Status != SUCCESS) { DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); break; @@ -149,7 +149,7 @@ int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) { coordTemp.y = tmpVal; memcpy(&tmpVal, msg+L2_MSG_INFO_POSZ, sizeof(float)); coordTemp.z = tmpVal; - Status = UpdateNodePos(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp); + Status = UpdateNodePos(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp); if (Status != SUCCESS) { DBG_ERROR("Failed to update node %d position, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); } @@ -159,16 +159,16 @@ int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) { coordTemp.y = tmpVal; memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDZ, sizeof(float)); coordTemp.z = tmpVal; - Status = UpdateNodeSpeed(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp); + Status = UpdateNodeSpeed(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp); if (Status != SUCCESS) { DBG_ERROR("Failed to update node %d speed, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); } // Update Time deviation int64_t devi, deviTrunc; - Status = L2_TimeSync_GetDeviatoion(l2InstPtr, &devi); + Status = L2_TimeSync_GetDeviatoion(instPtr, &devi); if (Status != SUCCESS) { DBG_ERROR("Failed to compute slot deviation.\n"); - UpdateNodeTimeDeviation(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, 0); + UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, 0); } else { if(devi > INT32_MAX || devi < INT32_MIN) { @@ -182,25 +182,25 @@ int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) { else { deviTrunc = (int32_t)devi; } - UpdateNodeTimeDeviation(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc); + UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc); } break; } default: - DBG_ERROR("Unsupported frame type: %ld.\n", l2InstPtr->frame.last); + DBG_ERROR("Unsupported frame type: %ld.\n", instPtr->frame.last); Status = EUNEXP; break; } // Net setup finished. - if(L2_GetMainState(l2InstPtr) == INIT_STATE && Status == SUCCESS){ + if(L2_GetMainState(instPtr) == INIT_STATE && Status == SUCCESS){ L2_EVENT l2Event; L2_RTNE_CONFIG newRtneConfig; // Assert Net-Setup Done. l2Event.event = L2_EV_NET_SETUP_DONE; l2Event.msg = NULL; - AssertL2Event(l2InstPtr, &l2Event); + AssertL2Event(instPtr, &l2Event); // Assert Routine Table Update... newRtneConfig.rtneTable = normalTable; @@ -208,7 +208,7 @@ int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) { newRtneConfig.rtneStep = 0; l2Event.event = L2_ROUTINE_UPDATE; l2Event.msg = &newRtneConfig; - AssertL2Event(l2InstPtr, &l2Event); + AssertL2Event(instPtr, &l2Event); } return Status; } @@ -217,24 +217,24 @@ int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) { /** * @brief Execute a member node scan net procedure. * - * @param l2InstPtr A pointer to scan net fsm. + * @param instPtr A pointer to scan net fsm. * * @return used slot (5ms slot). * ******************************************************************************/ -int32_t L2_Scan_Proc(L2_INST *l2InstPtr) { +int32_t L2_Scan_Proc(L2_INST *instPtr) { int32_t Status; - /* l2InstPtr->nextState == NORMAL_STATE indicates network set up procedure was finished. + /* instPtr->nextState == NORMAL_STATE indicates network set up procedure was finished. * Skip 5ms for nodes for node configuration update. */ DBG_INFO("-------- Scan Process --------\n"); - if (l2InstPtr->nextState == NORMAL_STATE) { + if (instPtr->nextState == NORMAL_STATE) { DBG_INFO("NETSET DONE, halt 20ms before normal routine.\n"); - if(l2InstPtr->config.myType != L2_HEAD_NODE) { - Phy_SetBeamAngle(l2InstPtr->config.phyInstRef, 0.0, 0.0); - Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_120); - Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE); - L2_SetupNextSlot(l2InstPtr, L2_TOR_SUSPEND, L2_SCAN_FRAME); + if(instPtr->config.myType != L2_HEAD_NODE) { + Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0); + Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_120); + Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE); + L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_SCAN_FRAME); } else { DBG_ERROR("Illegal call from Head Node.\n"); @@ -249,23 +249,23 @@ int32_t L2_Scan_Proc(L2_INST *l2InstPtr) { * Increase beam number after a Tx-Rx procedure pair, reset beam number when * it reaches the boundary. */ - if (l2InstPtr->config.myType == L2_HEAD_NODE) { + if (instPtr->config.myType == L2_HEAD_NODE) { // Apply a scan procedure - if (l2InstPtr->scanFsm.sacanCnt % 2 == 0){ + if (instPtr->scanFsm.sacanCnt % 2 == 0){ // Antenna configure - Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_8); - Phy_SetBeamNo(l2InstPtr->config.phyInstRef, l2InstPtr->scanFsm.beamNo); - Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE); - L2_SetupNextSlot(l2InstPtr, L2_TOR_TX, L2_SCAN_FRAME); + Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8); + Phy_SetBeamNo(instPtr->config.phyInstRef, instPtr->scanFsm.beamNo); + Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE); + L2_SetupNextSlot(instPtr, L2_TOR_TX, L2_SCAN_FRAME); } else { // Antenna configure - Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_8); - Phy_SetBeamNo(l2InstPtr->config.phyInstRef, l2InstPtr->scanFsm.beamNo); - Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE); - L2_SetupNextSlot(l2InstPtr, L2_TOR_RX, L2_INFO_FRAME); + Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8); + Phy_SetBeamNo(instPtr->config.phyInstRef, instPtr->scanFsm.beamNo); + Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE); + L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INFO_FRAME); // Update Scan beam number - l2InstPtr->scanFsm.beamNo = (l2InstPtr->scanFsm.beamNo + 1) % L1_NUM_ANT_NRRW_PATTERN; + instPtr->scanFsm.beamNo = (instPtr->scanFsm.beamNo + 1) % L1_NUM_ANT_NRRW_PATTERN; } } /* Member node listening the head node pilot signal with wide beam width. @@ -278,26 +278,26 @@ int32_t L2_Scan_Proc(L2_INST *l2InstPtr) { else { uint8_t *sendData; // Pack node information. - Status = ScanMsg_Packing(l2InstPtr, L2_INFO_FRAME); + Status = ScanMsg_Packing(instPtr, L2_INFO_FRAME); if (Status != SUCCESS) { DBG_INFO("Failed to pack response.\n"); } - if (CircQueuePop(l2InstPtr->txQueue, (void **)(&sendData)) == true) { - memcpy(l2InstPtr->sendPktData, sendData, RSP_PACK_LEN); + if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true) { + memcpy(instPtr->sendPktData, sendData, RSP_PACK_LEN); free(sendData); } else { DBG_INFO("Failed to pop tx Queue.\n"); } // Antenna configure - Phy_SetBeamAngle(l2InstPtr->config.phyInstRef, 0.0, 0.0); - Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_120); - Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_LISTEN); - L2_SetupNextSlot(l2InstPtr, L2_TOR_RX, L2_SCAN_FRAME); + Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0); + Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_120); + Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_LISTEN); + L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_SCAN_FRAME); } // Increase scan counter (for debug purpose). - l2InstPtr->scanFsm.sacanCnt ++; + instPtr->scanFsm.sacanCnt ++; return NUM_SLOTS_SKIP; } @@ -311,7 +311,7 @@ int32_t L2_Scan_Proc(L2_INST *l2InstPtr) { * @return used slot (5ms slot). * ******************************************************************************/ -int32_t L2_Scan_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry) { +int32_t L2_Scan_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry) { DBG_INFO("-------- Scan Routine --------\n"); ASSERT(false); // Not support. return 0; diff --git a/df_datalink/src/networking/link_layer/service.c b/df_datalink/src/networking/link_layer/service.c index 66f98af..8aacc2e 100644 --- a/df_datalink/src/networking/link_layer/service.c +++ b/df_datalink/src/networking/link_layer/service.c @@ -34,7 +34,7 @@ /**************************** Type Definitions *******************************/ /************************** Function Prototypes ******************************/ -static int32_t Service_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type); +static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type); /************************** Variable Definitions *****************************/ #ifdef DATALINK_ENABLE_BERTEST @@ -58,9 +58,9 @@ static uint8_t berTestData[SVC_PACK_LEN] = { * SUCCESS * ******************************************************************************/ -static int32_t Service_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) { +static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) { #ifndef DATALINK_ENABLE_BERTEST - ASSERT(l2InstPtr != NULL); + ASSERT(instPtr != NULL); int32_t Status; uint8_t *buff; uint32_t packLen; @@ -75,11 +75,11 @@ static int32_t Service_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) { DBG_ERROR("Not enough space, failed to pack frame...\n"); return ENOMEM; } - buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(l2InstPtr); - buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(l2InstPtr); + buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr); + buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr); // TODO: Check Pack TimeSync info, and better approch?. - nodeEntry = GetNode(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID); + nodeEntry = GetNode(instPtr->nodeInfoList, L2_MEMBER_NODE_ID); if(nodeEntry != NULL) { // check if deviation info outdated. if(nodeEntry->ttlCnt <= NUM_SERVICE_PRD_SLOTS) { @@ -103,23 +103,23 @@ static int32_t Service_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) { memcpy(buff+L2_MSG_SVC_TMSYNC, &timeDevi, L2_MSG_SVC_TMSYNC_LEN); // Pack Seq ID - buff[L2_MSG_SVC_SEQ_ID] = l2InstPtr->serviceFsm.txSeqNum; + buff[L2_MSG_SVC_SEQ_ID] = instPtr->serviceFsm.txSeqNum; // Pack payload memcpy(buff+L2_MSG_SVC_PAYLOAD, - l2InstPtr->serviceFsm.txLog + L2_MSG_SVC_PAYLOAD_LEN*l2InstPtr->serviceFsm.txSeqNum, + instPtr->serviceFsm.txLog + L2_MSG_SVC_PAYLOAD_LEN*instPtr->serviceFsm.txSeqNum, L2_MSG_SVC_PAYLOAD_LEN); - if (l2InstPtr->config.myType == L2_HEAD_NODE) { + if (instPtr->config.myType == L2_HEAD_NODE) { // if last packet reset counting - l2InstPtr->serviceFsm.txSeqNum = (l2InstPtr->serviceFsm.txSeqNum + instPtr->serviceFsm.txSeqNum = (instPtr->serviceFsm.txSeqNum >= (PAYLOAD_FLJ-1)/L2_MSG_SVC_PAYLOAD+1) - ? 0 : l2InstPtr->serviceFsm.txSeqNum + 1; + ? 0 : instPtr->serviceFsm.txSeqNum + 1; } else{ // if last packet reset counting - l2InstPtr->serviceFsm.txSeqNum = (l2InstPtr->serviceFsm.txSeqNum + instPtr->serviceFsm.txSeqNum = (instPtr->serviceFsm.txSeqNum >= (PAYLOAD_LJ-1)/L2_MSG_SVC_PAYLOAD+1) - ? 0 : l2InstPtr->serviceFsm.txSeqNum + 1; + ? 0 : instPtr->serviceFsm.txSeqNum + 1; } @@ -129,7 +129,7 @@ static int32_t Service_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) { return Status; } - if(CircQueuePush(l2InstPtr->txQueue, buff) == false){ + if(CircQueuePush(instPtr->txQueue, buff) == false){ DBG_ERROR("Cannot push packet to queue..\n"); return EQUEUE; } @@ -137,27 +137,27 @@ static int32_t Service_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) { } case L2_INFO_FRAME:{ packLen = RSP_PACK_LEN; - SysMonitor_GetSysinfo(&(l2InstPtr->sysInfo)); + SysMonitor_GetSysinfo(&(instPtr->sysInfo)); buff = malloc(packLen); if (buff == NULL) { DBG_ERROR("Not enough space, failed to pack frame...\n"); return ENOMEM; } - buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(l2InstPtr); - buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(l2InstPtr); - memcpy(buff+L2_MSG_INFO_POSX, &(l2InstPtr->sysInfo.posX), sizeof(float)); - memcpy(buff+L2_MSG_INFO_POSY, &(l2InstPtr->sysInfo.posY), sizeof(float)); - memcpy(buff+L2_MSG_INFO_POSZ, &(l2InstPtr->sysInfo.posZ), sizeof(float)); - memcpy(buff+L2_MSG_INFO_SPEEDX, &(l2InstPtr->sysInfo.vX), sizeof(float)); - memcpy(buff+L2_MSG_INFO_SPEEDY, &(l2InstPtr->sysInfo.vY), sizeof(float)); - memcpy(buff+L2_MSG_INFO_SPEEDZ, &(l2InstPtr->sysInfo.vZ), sizeof(float)); + buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr); + buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr); + memcpy(buff+L2_MSG_INFO_POSX, &(instPtr->sysInfo.posX), sizeof(float)); + memcpy(buff+L2_MSG_INFO_POSY, &(instPtr->sysInfo.posY), sizeof(float)); + memcpy(buff+L2_MSG_INFO_POSZ, &(instPtr->sysInfo.posZ), sizeof(float)); + memcpy(buff+L2_MSG_INFO_SPEEDX, &(instPtr->sysInfo.vX), sizeof(float)); + memcpy(buff+L2_MSG_INFO_SPEEDY, &(instPtr->sysInfo.vY), sizeof(float)); + memcpy(buff+L2_MSG_INFO_SPEEDZ, &(instPtr->sysInfo.vZ), sizeof(float)); Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021); if (Status != SUCCESS) { DBG_ERROR("Fail to append CRC...\n"); return Status; } - if(CircQueuePush(l2InstPtr->txQueue, buff) == false){ + if(CircQueuePush(instPtr->txQueue, buff) == false){ DBG_ERROR("Cannot push packet to queue..\n"); return EQUEUE; } @@ -181,22 +181,22 @@ static int32_t Service_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) { /** * @brief Initialize scan FSM. * - * @param l2InstPtr + * @param instPtr * * @return * ******************************************************************************/ -int32_t L2_Service_Init(L2_INST *l2InstPtr) { - ASSERT(l2InstPtr != NULL); - l2InstPtr->serviceFsm.txSeqNum = 0; - l2InstPtr->serviceFsm.rxSeqNum = 0; - memset((&l2InstPtr->serviceFsm.rxLog), 0x00, sizeof(PAYLOAD)); - memset((&l2InstPtr->serviceFsm.txLog), 0x00, sizeof(PAYLOAD)); - l2InstPtr->serviceFsm.pktRecv = 0; - l2InstPtr->serviceFsm.pktRecved = 0; +int32_t L2_Service_Init(L2_INST *instPtr) { + ASSERT(instPtr != NULL); + instPtr->serviceFsm.txSeqNum = 0; + instPtr->serviceFsm.rxSeqNum = 0; + memset((&instPtr->serviceFsm.rxLog), 0x00, sizeof(PAYLOAD)); + memset((&instPtr->serviceFsm.txLog), 0x00, sizeof(PAYLOAD)); + instPtr->serviceFsm.pktRecv = 0; + instPtr->serviceFsm.pktRecved = 0; #ifdef DATALINK_ENABLE_BERTEST - l2InstPtr->serviceFsm.bitTotal = 0; - l2InstPtr->serviceFsm.bitError = 0; + instPtr->serviceFsm.bitTotal = 0; + instPtr->serviceFsm.bitError = 0; #endif return SUCCESS; } @@ -211,13 +211,13 @@ int32_t L2_Service_Init(L2_INST *l2InstPtr) { * other error occurred. * ******************************************************************************/ -int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) { - ASSERT(l2InstPtr != NULL); +int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) { + ASSERT(instPtr != NULL); ASSERT(msg != NULL); int32_t Status; - switch (l2InstPtr->frame.last) { + switch (instPtr->frame.last) { case L2_SVC_FRAME: { - if (l2InstPtr->config.myType == L2_HEAD_NODE) { + if (instPtr->config.myType == L2_HEAD_NODE) { DBG_ERROR("Unexpected call, Only None-Head Node can handle L2_SVC_FRAME.\n"); Status = EUNEXP; break; @@ -231,7 +231,7 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) { #ifdef DATALINK_ENABLE_AUTOTIMESYNC l2Event.msg = NULL; - AssertL2Event(l2InstPtr, &l2Event); + AssertL2Event(instPtr, &l2Event); #else int64_t timeAdj; l2Event.msg = &timeAdj; @@ -241,57 +241,57 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) { timeAdj |= (timeAdj & 0x0000000000800000) ? 0xFFFFFFFFFF000000 : 0x0000000000000000; // FIXME: better approach? // Implement a time sync procedure, first time received a time deviation in a service period. - if (seqNum % NUM_SERVICE_PRD_SLOTS == 0 || l2InstPtr->serviceFsm.seqNum != seqNum) { - AssertL2Event(l2InstPtr, &l2Event); + if (seqNum % NUM_SERVICE_PRD_SLOTS == 0 || instPtr->serviceFsm.seqNum != seqNum) { + AssertL2Event(instPtr, &l2Event); } #endif #ifndef DATALINK_ENABLE_BERTEST - memcpy(l2InstPtr->serviceFsm.rxLog + L2_MSG_SVC_PAYLOAD_LEN*seqNum, msg + L2_MSG_SVC_PAYLOAD, L2_MSG_SVC_PAYLOAD_LEN); - if (l2InstPtr->config.myType == L2_HEAD_NODE) { + memcpy(instPtr->serviceFsm.rxLog + L2_MSG_SVC_PAYLOAD_LEN*seqNum, msg + L2_MSG_SVC_PAYLOAD, L2_MSG_SVC_PAYLOAD_LEN); + if (instPtr->config.myType == L2_HEAD_NODE) { // if last packet reset counting if(seqNum >= (PAYLOAD_FLJ-1)/L2_MSG_SVC_PAYLOAD+1) { - if(seqNum == l2InstPtr->serviceFsm.rxSeqNum) { - SysMonitor_UpdateRecvPayloadLJRaw(l2InstPtr->serviceFsm.rxLog); + if(seqNum == instPtr->serviceFsm.rxSeqNum) { + SysMonitor_UpdateRecvPayloadLJRaw(instPtr->serviceFsm.rxLog); } - l2InstPtr->serviceFsm.rxSeqNum = 0; + instPtr->serviceFsm.rxSeqNum = 0; } else { - if(l2InstPtr->serviceFsm.rxSeqNum == seqNum){ - l2InstPtr->serviceFsm.rxSeqNum++; + if(instPtr->serviceFsm.rxSeqNum == seqNum){ + instPtr->serviceFsm.rxSeqNum++; } } } else{ // if last packet reset counting if(seqNum >= (PAYLOAD_LJ-1)/L2_MSG_SVC_PAYLOAD+1) { - if(seqNum == l2InstPtr->serviceFsm.rxSeqNum) { - SysMonitor_UpdateRecvPayloadFLJRaw(l2InstPtr->serviceFsm.rxLog); + if(seqNum == instPtr->serviceFsm.rxSeqNum) { + SysMonitor_UpdateRecvPayloadFLJRaw(instPtr->serviceFsm.rxLog); } - l2InstPtr->serviceFsm.rxSeqNum = 0; + instPtr->serviceFsm.rxSeqNum = 0; } else { - if(l2InstPtr->serviceFsm.rxSeqNum == seqNum){ - l2InstPtr->serviceFsm.rxSeqNum++; + if(instPtr->serviceFsm.rxSeqNum == seqNum){ + instPtr->serviceFsm.rxSeqNum++; } } } #else for(int i = 0; i < SVC_PACK_LEN; i++) { - l2InstPtr->serviceFsm.bitError += count_ones(msg[i]^berTestData[i]); + instPtr->serviceFsm.bitError += count_ones(msg[i]^berTestData[i]); } - l2InstPtr->serviceFsm.bitTotal += SVC_PACK_LEN*8; + instPtr->serviceFsm.bitTotal += SVC_PACK_LEN*8; #endif - l2InstPtr->serviceFsm.pktRecved++; + instPtr->serviceFsm.pktRecved++; // Reset Node TTL - Status = RstNodeTTL(l2InstPtr->nodeInfoList, L2_HEAD_NODE_ID); + Status = RstNodeTTL(instPtr->nodeInfoList, L2_HEAD_NODE_ID); if (Status != SUCCESS) { DBG_ERROR("Failed to reset node %d TTL, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); } break; } case L2_INFO_FRAME: { - if (l2InstPtr->config.myType != L2_HEAD_NODE) { + if (instPtr->config.myType != L2_HEAD_NODE) { DBG_ERROR("Unexpected call, Only Head Node can handle L2_SVC_FRAME.\n"); Status = EUNEXP; break; @@ -305,7 +305,7 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) { coordTemp.y = tmpVal; memcpy(&tmpVal, msg+L2_MSG_INFO_POSZ, sizeof(float)); coordTemp.z = tmpVal; - Status = UpdateNodePos(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp); + Status = UpdateNodePos(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp); if (Status != SUCCESS) { DBG_ERROR("Failed to update node %d position, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); } @@ -315,7 +315,7 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) { coordTemp.y = tmpVal; memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDZ, sizeof(float)); coordTemp.z = tmpVal; - Status = UpdateNodeSpeed(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp); + Status = UpdateNodeSpeed(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp); if (Status != SUCCESS) { DBG_ERROR("Failed to update node %d speed, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); } @@ -323,7 +323,7 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) { // TODO: Check // Update Time deviation int64_t devi, deviTrunc; - Status = L2_TimeSync_GetDeviatoion(l2InstPtr, &devi); + Status = L2_TimeSync_GetDeviatoion(instPtr, &devi); if (Status != SUCCESS) { DBG_ERROR("Failed to compute slot deviation.\n"); deviTrunc = 0; @@ -340,28 +340,28 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) { else { deviTrunc = (int32_t)devi; } - UpdateNodeTimeDeviation(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc); + UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc); } - Status = UpdateNodeTimeDeviation(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc); + Status = UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc); if (Status != SUCCESS) { DBG_ERROR("Failed to update node %d time deviation, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); } #else for(int i = 0; i < SVC_PACK_LEN; i++) { - l2InstPtr->serviceFsm.bitError += count_ones(msg[i]^berTestData[i]); + instPtr->serviceFsm.bitError += count_ones(msg[i]^berTestData[i]); } - l2InstPtr->serviceFsm.bitTotal += SVC_PACK_LEN*8; + instPtr->serviceFsm.bitTotal += SVC_PACK_LEN*8; #endif - l2InstPtr->serviceFsm.pktRecved++; + instPtr->serviceFsm.pktRecved++; // Reset Node TTL - Status = RstNodeTTL(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID); + Status = RstNodeTTL(instPtr->nodeInfoList, L2_MEMBER_NODE_ID); if (Status != SUCCESS) { DBG_ERROR("Failed to reset node %d ttl, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); } break; } default: - DBG_ERROR("Unsupported frame type: %ld.\n", l2InstPtr->frame.last); + DBG_ERROR("Unsupported frame type: %ld.\n", instPtr->frame.last); break; } return Status; @@ -371,24 +371,24 @@ int32_t L2_Service_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) { /** * @brief Service FSM Routine. * - * @param l2InstPtr + * @param instPtr * @param schedEntry * * @return Slot has been used. * ******************************************************************************/ -int32_t L2_Service_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry) { - ASSERT(l2InstPtr != NULL); +int32_t L2_Service_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry) { + ASSERT(instPtr != NULL); DBG_INFO("-------- ServiceNet Routine --------\n"); uint8_t *sendData; int32_t usedSlots; L2_TOR_TYPE torState; - if(schedEntry->src == l2InstPtr->config.myId){ + if(schedEntry->src == instPtr->config.myId){ torState = L2_TOR_TX; } - else if(schedEntry->dst == l2InstPtr->config.myId){ + else if(schedEntry->dst == instPtr->config.myId){ torState = L2_TOR_RX; - l2InstPtr->serviceFsm.pktRecv++; + instPtr->serviceFsm.pktRecv++; } else{ torState = L2_TOR_SUSPEND; @@ -398,41 +398,41 @@ int32_t L2_Service_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry) case L2_SLOT_SERVICE: if(torState == L2_TOR_TX){ #ifndef DATALINK_ENABLE_BERTEST - if (l2InstPtr->serviceFsm.txSeqNum == 0) { - if(l2InstPtr->config.myType == L2_HEAD_NODE){ - SysMonitor_GetSendPayloadFLJRaw(l2InstPtr->serviceFsm.txLog); + if (instPtr->serviceFsm.txSeqNum == 0) { + if(instPtr->config.myType == L2_HEAD_NODE){ + SysMonitor_GetSendPayloadFLJRaw(instPtr->serviceFsm.txLog); } else { - SysMonitor_GetSendPayloadLJRaw(l2InstPtr->serviceFsm.txLog); + SysMonitor_GetSendPayloadLJRaw(instPtr->serviceFsm.txLog); } } // Packing service data - Service_Packing(l2InstPtr, L2_SVC_FRAME); - if (CircQueuePop(l2InstPtr->txQueue, (void **)(&sendData)) == true) { - memcpy(l2InstPtr->sendPktData, sendData, SVC_PACK_LEN); + Service_Packing(instPtr, L2_SVC_FRAME); + if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true) { + memcpy(instPtr->sendPktData, sendData, SVC_PACK_LEN); free(sendData); } else { DBG_ERROR("Failed to pop tx queue.\n"); } #else - memcpy(l2InstPtr->config.phyInstRef->sendFrameBuff, berTestData, SVC_PACK_LEN); + memcpy(instPtr->config.phyInstRef->sendFrameBuff, berTestData, SVC_PACK_LEN); #endif } // FIXME: Steer antenna to point to destination. - Phy_SetBeamAngle(l2InstPtr->config.phyInstRef, 0.0, 0.0); - Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_8); - Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE); - L2_SetupNextSlot(l2InstPtr, torState, L2_SVC_FRAME); + Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0); + Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8); + Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE); + L2_SetupNextSlot(instPtr, torState, L2_SVC_FRAME); usedSlots = NUM_SERVICE_SLOTS; break; #ifndef DATALINK_ENABLE_BERTEST case L2_SLOT_ADVERTISE: if(torState == L2_TOR_TX){ // Packing response data. - Service_Packing(l2InstPtr, L2_INFO_FRAME); - if (CircQueuePop(l2InstPtr->txQueue, (void **)(&sendData)) == true) { - memcpy(l2InstPtr->sendPktData, sendData, RSP_PACK_LEN); + Service_Packing(instPtr, L2_INFO_FRAME); + if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true) { + memcpy(instPtr->sendPktData, sendData, RSP_PACK_LEN); free(sendData); } else { @@ -440,10 +440,10 @@ int32_t L2_Service_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry) } } // FIXME: Steer antenna to point to destination. - Phy_SetBeamAngle(l2InstPtr->config.phyInstRef, 0.0, 0.0); - Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_120); - Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE); - L2_SetupNextSlot(l2InstPtr, torState, L2_INFO_FRAME); + Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0); + Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_120); + Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE); + L2_SetupNextSlot(instPtr, torState, L2_INFO_FRAME); usedSlots = NUM_RESPONSE_SLOTS; break; #endif @@ -454,11 +454,11 @@ int32_t L2_Service_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry) #ifdef DATALINK_ENABLE_BERTEST uint32_t pktLoss; double errorRate, lossRate; - pktLoss = l2InstPtr->serviceFsm.pktRecv - l2InstPtr->serviceFsm.pktRecved; - lossRate = (l2InstPtr->serviceFsm.pktRecv == 0) ? 0 : (double)pktLoss / (double)l2InstPtr->serviceFsm.pktRecv; - errorRate = (l2InstPtr->serviceFsm.bitTotal == 0) ? 0 : (double)l2InstPtr->serviceFsm.bitError / (double)l2InstPtr->serviceFsm.bitTotal; - DBG_PRINT("Packet Loss Rate -- Total: %lu, Loss: %lu.\n", l2InstPtr->serviceFsm.pktRecv, pktLoss); - DBG_PRINT("Bit Error Rate -- Total:%lu, Error:%lu.\n", l2InstPtr->serviceFsm.bitTotal, l2InstPtr->serviceFsm.bitError); + pktLoss = instPtr->serviceFsm.pktRecv - instPtr->serviceFsm.pktRecved; + lossRate = (instPtr->serviceFsm.pktRecv == 0) ? 0 : (double)pktLoss / (double)instPtr->serviceFsm.pktRecv; + errorRate = (instPtr->serviceFsm.bitTotal == 0) ? 0 : (double)instPtr->serviceFsm.bitError / (double)instPtr->serviceFsm.bitTotal; + DBG_PRINT("Packet Loss Rate -- Total: %lu, Loss: %lu.\n", instPtr->serviceFsm.pktRecv, pktLoss); + DBG_PRINT("Bit Error Rate -- Total:%lu, Error:%lu.\n", instPtr->serviceFsm.bitTotal, instPtr->serviceFsm.bitError); #endif return usedSlots; } diff --git a/df_datalink/src/networking/link_layer/time_sync.c b/df_datalink/src/networking/link_layer/time_sync.c index 2f75e65..cef9533 100644 --- a/df_datalink/src/networking/link_layer/time_sync.c +++ b/df_datalink/src/networking/link_layer/time_sync.c @@ -54,16 +54,16 @@ /** * @brief Apply local time adjustment by addition. * - * @param l2InstPtr. + * @param instPtr. * @param adjustment * * @return * ******************************************************************************/ -inline int32_t L2_TimeSync_LocalTimerAdd(L2_INST *l2InstPtr, int64_t adjustment) { +inline int32_t L2_TimeSync_LocalTimerAdd(L2_INST *instPtr, int64_t adjustment) { // read nextSendTime from physical layer - Phy_SetTimeWord(l2InstPtr->config.phyInstRef, adjustment, PHY_TIME_WORD_LOCAL_ADJ); - PhyWrite(l2InstPtr->config.phyInstRef, TM_ADJ_MODE, TM_ADJ_MODE_ADD); + Phy_SetTimeWord(instPtr->config.phyInstRef, adjustment, PHY_TIME_WORD_LOCAL_ADJ); + PhyWrite(instPtr->config.phyInstRef, TM_ADJ_MODE, TM_ADJ_MODE_ADD); return SUCCESS; } @@ -71,42 +71,42 @@ inline int32_t L2_TimeSync_LocalTimerAdd(L2_INST *l2InstPtr, int64_t adjustment) /** * @brief Align slot, base on physical layer information. * - * @param l2InstPtr. + * @param instPtr. * * @return * ******************************************************************************/ -int32_t L2_TimeSync_SlotAlign(L2_INST *l2InstPtr) { +int32_t L2_TimeSync_SlotAlign(L2_INST *instPtr) { int32_t Status; int64_t slotAdj; - Status = L2_TimeSync_GetDeviatoion(l2InstPtr, &slotAdj); + Status = L2_TimeSync_GetDeviatoion(instPtr, &slotAdj); if (Status != SUCCESS) { DBG_ERROR("Failed to compute slot deviation.\n"); return Status; } - return L2_TimeSync_LocalTimerAdd(l2InstPtr, slotAdj); + return L2_TimeSync_LocalTimerAdd(instPtr, slotAdj); } /*****************************************************************************/ /** * @brief Compute slot deviation of received frame. * - * @param l2InstPtr. + * @param instPtr. * @param devi * * @return * ******************************************************************************/ -int32_t L2_TimeSync_GetDeviatoion(L2_INST *l2InstPtr, int64_t *devi) { +int32_t L2_TimeSync_GetDeviatoion(L2_INST *instPtr, int64_t *devi) { // read nextSendTime from physical layer int64_t currentTarget, lastFrameSync, slotMeasure; - currentTarget = PhyRead(l2InstPtr->config.phyInstRef, TM_TARGET_L); - currentTarget |= ((int64_t)PhyRead(l2InstPtr->config.phyInstRef, TM_TARGET_H)) << 32; - lastFrameSync = PhyRead(l2InstPtr->config.phyInstRef, FSTIME_L); - lastFrameSync |= ((int64_t)PhyRead(l2InstPtr->config.phyInstRef, FSTIME_H)) << 32; + currentTarget = PhyRead(instPtr->config.phyInstRef, TM_TARGET_L); + currentTarget |= ((int64_t)PhyRead(instPtr->config.phyInstRef, TM_TARGET_H)) << 32; + lastFrameSync = PhyRead(instPtr->config.phyInstRef, FSTIME_L); + lastFrameSync |= ((int64_t)PhyRead(instPtr->config.phyInstRef, FSTIME_H)) << 32; slotMeasure = (currentTarget > lastFrameSync) ? (currentTarget - lastFrameSync) : (currentTarget - lastFrameSync + PHY_LOCAL_CNT_MAX); - switch (Phy_GetLastDetectedFrameType(l2InstPtr->config.phyInstRef)) { + switch (Phy_GetLastDetectedFrameType(instPtr->config.phyInstRef)) { case L1_2KBPS_20MS_FRAME : *devi = ((int64_t)PHY_SYNCT2FEND_2K_20MS) - slotMeasure; break; @@ -114,7 +114,7 @@ int32_t L2_TimeSync_GetDeviatoion(L2_INST *l2InstPtr, int64_t *devi) { *devi = ((int64_t)PHY_SYNCT2FEND_2K_100MS) - slotMeasure; break; default: - DBG_ERROR("Unsupported frame type: %d.\n", Phy_GetLastDetectedFrameType(l2InstPtr->config.phyInstRef)); + DBG_ERROR("Unsupported frame type: %d.\n", Phy_GetLastDetectedFrameType(instPtr->config.phyInstRef)); return EIGNORE; } return SUCCESS;