diff --git a/df_datalink_sp/src/networking/link_layer/scan.c b/df_datalink_sp/src/networking/link_layer/scan.c index ef583dd..47cbd03 100644 --- a/df_datalink_sp/src/networking/link_layer/scan.c +++ b/df_datalink_sp/src/networking/link_layer/scan.c @@ -409,22 +409,28 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) // Reset node ttl node->ttlCnt = 0; - if (L2_isCxn2Net(instPtr) == false) + uint32_t netstartTime; + // Update net start time. + L2_FrameGetChunk(msg, L2_MSG_ADV_SRC_NETSTART_TIME, L2_MSG_ADV_SRC_NETSTART_TIME_LEN, &netstartTime); + L2_EVENT netSetupDoneEvent = { + .event = L2_EV_NET_ADV_UPDATE, + .msg = &netstartTime}; + AssertL2Event(instPtr, &netSetupDoneEvent); + + // Calculate time deviation + uint32_t frameNum, slotNum = 0; + L2_FrameGetChunk(msg, L2_MSG_FRAME_NUM, L2_MSG_FRAME_NUM_LEN, &frameNum); + L2_FrameGetChunk(msg, L2_MSG_SLOT_NUM, L2_MSG_SLOT_NUM_LEN, &slotNum); + Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev)); + if (Status != SUCCESS) { - uint32_t netstartTime, frameNum, slotNum = 0; - // Update net start time. - L2_FrameGetChunk(msg, L2_MSG_ADV_SRC_NETSTART_TIME, L2_MSG_ADV_SRC_NETSTART_TIME_LEN, &netstartTime); - L2_EVENT netSetupDoneEvent = { - .event = L2_EV_NET_ADV_UPDATE, - .msg = &netstartTime}; - AssertL2Event(instPtr, &netSetupDoneEvent); - - // Time sync. - L2_FrameGetChunk(msg, L2_MSG_FRAME_NUM, L2_MSG_FRAME_NUM_LEN, &frameNum); - L2_FrameGetChunk(msg, L2_MSG_SLOT_NUM, L2_MSG_SLOT_NUM_LEN, &slotNum); - // TODO: Check - L2_TimeAlign(instPtr, frameNum, slotNum, true); + DBG_ERROR("Failed to compute time deviation.\n") + break; } + + // Time sync. + L2_TimeAdjust(instPtr, &(instPtr->log.timeDev)); + // trigger flag AdvRecvStateUpdate(scanFsmPtr, true); Status = SUCCESS; @@ -465,6 +471,17 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) // Reset node ttl node->ttlCnt = 0; + // Calculate time deviation + uint32_t frameNum, slotNum = 0; + L2_FrameGetChunk(msg, L2_MSG_FRAME_NUM, L2_MSG_FRAME_NUM_LEN, &frameNum); + L2_FrameGetChunk(msg, L2_MSG_SLOT_NUM, L2_MSG_SLOT_NUM_LEN, &slotNum); + Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev)); + if (Status != SUCCESS) + { + DBG_ERROR("Failed to compute time deviation.\n") + break; + } + if (L2_GetNodeType(instPtr) != L2_NODE_HEAD) { // check if net setup finished. @@ -501,6 +518,8 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) l2Event.msg = &newState; AssertL2Event(instPtr, &l2Event); } + // Time sync. + L2_TimeAdjust(instPtr, &(instPtr->log.timeDev)); } Status = SUCCESS; break; diff --git a/df_datalink_sp/src/networking/link_layer/service.c b/df_datalink_sp/src/networking/link_layer/service.c index 86c41b4..3d93851 100644 --- a/df_datalink_sp/src/networking/link_layer/service.c +++ b/df_datalink_sp/src/networking/link_layer/service.c @@ -269,6 +269,17 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) // Reset node ttl node->ttlCnt = 0; + // Calculate time deviation + uint32_t frameNum, slotNum = 0; + L2_FrameGetChunk(msg, L2_MSG_FRAME_NUM, L2_MSG_FRAME_NUM_LEN, &frameNum); + L2_FrameGetChunk(msg, L2_MSG_SLOT_NUM, L2_MSG_SLOT_NUM_LEN, &slotNum); + Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev)); + if (Status != SUCCESS) + { + DBG_ERROR("Failed to compute time deviation.\n") + break; + } + uint32_t dataId; L2_FrameGetChunk(msg, L2_MSG_SVC_PAYLOAD_ID, L2_MSG_SVC_PAYLOAD_ID_LEN, &dataId); @@ -311,13 +322,21 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) instPtr->serviceFsm.bitError += errorBits; instPtr->serviceFsm.bitTotal += SVC_PACK_LEN * 8; + // Calculate time deviation + uint32_t frameNum, slotNum = 0; + L2_FrameGetChunk(msg, L2_MSG_FRAME_NUM, L2_MSG_FRAME_NUM_LEN, &frameNum); + L2_FrameGetChunk(msg, L2_MSG_SLOT_NUM, L2_MSG_SLOT_NUM_LEN, &slotNum); + Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev)); + if (Status != SUCCESS) + { + DBG_ERROR("Failed to compute time deviation.\n") + break; + } // Implement a auto time sync. if (L2_GetNodeType(instPtr) != L2_NODE_HEAD) { - uint32_t frameNum, slotNum; - L2_FrameGetChunk(msg, L2_MSG_FRAME_NUM, L2_MSG_FRAME_NUM_LEN, &frameNum); - L2_FrameGetChunk(msg, L2_MSG_SLOT_NUM, L2_MSG_SLOT_NUM_LEN, &slotNum); - L2_TimeAlign(instPtr, frameNum, slotNum, true); + // Time sync. + L2_TimeAdjust(instPtr, &(instPtr->log.timeDev)); } #endif }