From a1fc87dc97f9104f23d7354df9cc46585873085f Mon Sep 17 00:00:00 2001 From: Gatsby Van Date: Tue, 12 Mar 2024 20:41:59 +0800 Subject: [PATCH] enhance readability --- df_datalink_sp/src/systembus/systembus.c | 557 ++++++++++++----------- 1 file changed, 288 insertions(+), 269 deletions(-) diff --git a/df_datalink_sp/src/systembus/systembus.c b/df_datalink_sp/src/systembus/systembus.c index 6dff0ca..9ec9e03 100644 --- a/df_datalink_sp/src/systembus/systembus.c +++ b/df_datalink_sp/src/systembus/systembus.c @@ -195,6 +195,11 @@ #define MY_PRD_ID PRD_DL_K /************************** Function Prototypes ******************************/ +static int32_t SystemBus_GenSelfTestRsp(SYSBUS *instPtr); +static int32_t SystemBus_SystemConfig(SYSBUS *instPtr, uint8_t *msg); +static int32_t SystemBus_Echo(SYSBUS *instPtr, SYSBUS_UART_FRAME *frame); +static int32_t SystemBus_GenPollRsp(SYSBUS *instPtr, uint8_t *msg); +static int32_t SystemBus_GenRemoteTestRsp(SYSBUS *instPtr, uint8_t *msg); /**************************** Type Definitions *******************************/ /************************** Variable Definitions *****************************/ // TODO: better approach? @@ -226,6 +231,270 @@ inline bool SystemBus_IsRxBusy(SYSBUS *instPtr) return SysbusUart_IsRxBusy(instPtr->uartInst); } +/** + * @brief Generate System Bus SelfTest response. + */ +static int32_t SystemBus_GenSelfTestRsp(SYSBUS *instPtr) +{ + ASSERT(instPtr != NULL); + SYSBUS_UART_FRAME *sysbusRsp; + sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME)); + if (sysbusRsp == NULL) { + DBG_ERROR("Failed to allocate memory for SELF TEST Response!\n"); + return ENOMEM; + } + HWSTATE *hwStatsPtr = &(instPtr->hwState); + sysbusRsp->len = ST_RSP_LEN; + memset(sysbusRsp->data, 0x00, ST_RSP_LEN); + sysbusRsp->data[WD_CMD] = CMD_SELF_TEST; + sysbusRsp->data[WD_PRD] = MY_PRD_ID; + SysMonitor_GetHardwareStatus(hwStatsPtr); + if (linkInstPtr->isEnable == false) + { + sysbusRsp->data[WD_ST_RET] = (hwStatsPtr->isSpOk) ? ST_RET_OK : ST_RET_ERROR; + } + else + { + sysbusRsp->data[WD_ST_RET] = (hwStatsPtr->isSpOk && hwStatsPtr->isAntOk) ? ST_RET_OK : ST_RET_ERROR; + } + // apply status and omit 2 bytes of overhead. + memcpy(sysbusRsp->data + WD_SP_FPGA_VOLT, &(hwStatsPtr->spFpgaVolt), ST_RET_INFO_LEN); + + if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false) + { + free(sysbusRsp); + DBG_ERROR("SystemBus::TX Queue overflowed!\n"); + return EQUEUE; + } + return SUCCESS; +} + +/** + * @brief Apply system configuration + */ +static int32_t SystemBus_SystemConfig(SYSBUS *instPtr, uint8_t *msg) +{ + ASSERT(instPtr != NULL); + ASSERT(msg != NULL); + PHY_INST *phyRef = linkInstPtr->config.phyInstPtr; + TMC_INST *tmcRef = linkInstPtr->config.tmcInstPtr; + bool reconfig = false; + bool start = false; + + if (msg[WD_MODE_CMD] == MODE_CMD_HEAD) + { + // omit unnecessary reconfiguration + if (L2_GetNodeType(linkInstPtr) != L2_NODE_HEAD) + { + // head node config + linkConfig.myType = L2_NODE_HEAD; + linkConfig.myId = 1; + reconfig = true; + } + } + + else if (msg[WD_MODE_CMD] == MODE_CMD_MEMBER ) + { + // omit unnecessary reconfiguration + if (L2_GetNodeType(linkInstPtr) != L2_NODE_MEMBER) + { + // member node config + linkConfig.myType = L2_NODE_MEMBER; + linkConfig.myId = 2; + reconfig = true; + } + } + else + { + // Ignore current command + DBG_ERROR("Unexpected config command 0x%02X:0x%02X", + WD_MODE_CMD, msg[WD_MODE_CMD]); + return EPARAM; + } + + if(msg[WD_EN_CMD] == EN_CMD_ON_HIGH_POWER) + { + // high power config + Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_FULL); + start = true; + } + else if(msg[WD_EN_CMD] == EN_CMD_ON_LOW_POWER) + { + // low power config + Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_LOW); + start = true; + } + else if(msg[WD_EN_CMD] == EN_CMD_SILENT) + { + // silence config + // TODO: better approach? + start = false; + Linklayer_Halt(linkInstPtr); + Phy_AntConfigMode(phyRef, PHY_ANT_MODE_STANDBY); + TIME_WORD pullTime = {0, 0}; + pullTime.int_part = TimeCtrl_GetIntCnt(tmcRef) + 1; + if (Phy_SetTimeWord(phyRef, &pullTime, PHY_TIME_WORD_POLL) != SUCCESS) + { + DBG_ERROR("Physical set pull time failed.\n"); + } + Phy_SetupNextSlot(phyRef, PHY_TOR_RX, L1_10KBPS_10MS_FRAME, NULL); + } + if (reconfig == true) + { + Linklayer_Reconfig(linkInstPtr, &linkConfig); + } + if (start == true) + { + Linklayer_Start(linkInstPtr); + } + instPtr->sysbusLog.lastConfig[0] = msg[WD_MODE_CMD]; + instPtr->sysbusLog.lastConfig[1] = msg[WD_EN_CMD]; + return SUCCESS; +} + +/** + * @brief Echo System Bus frame + */ +static int32_t SystemBus_Echo(SYSBUS *instPtr, SYSBUS_UART_FRAME *frame) +{ + ASSERT(instPtr != NULL); + ASSERT(frame != NULL); + SYSBUS_UART_FRAME *sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME)); + if (sysbusRsp == NULL) { + DBG_ERROR("Failed to allocate memory for DATA TEST A Response!\n"); + return ENOMEM; + } + sysbusRsp->len = frame->len; + memcpy(sysbusRsp->data, frame->data, sysbusRsp->len); + if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false) + { + free(sysbusRsp); + DBG_ERROR("SystemBus::TX Queue overflowed!\n"); + return EQUEUE; + } + return SUCCESS; +} + +/** + * @brief Generate System Bus poll call command response + */ +static int32_t SystemBus_GenPollRsp(SYSBUS *instPtr, uint8_t *msg) +{ + ASSERT(instPtr != NULL); + ASSERT(msg != NULL); + size_t dataLen; + uint8_t *payload; + SYSBUS_UART_FRAME *sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME)); + if (sysbusRsp == NULL) { + DBG_ERROR("Failed to allocate memory for FLJ POLL Response!\n"); + return ENOMEM; + } + + if(msg[WD_CMD] == CMD_FLJ_POLL) + { + sysbusRsp->len = FLJ_POLL_RESP_LEN; + sysbusRsp->data[WD_CMD] = CMD_FLJ_POLL; + sysbusRsp->data[WD_PRD] = MY_PRD_ID; + dataLen = PAYLOAD_LEN_FLJ_RX; + payload = sysbusRsp->data + FLJ_DATA_OFFSET; + } + else if(msg[WD_CMD] == CMD_LJ_POLL) + { + sysbusRsp->len = LJ_POLL_RESP_LEN; + sysbusRsp->data[WD_CMD] = CMD_LJ_POLL; + sysbusRsp->data[WD_PRD] = MY_PRD_ID; + dataLen = PAYLOAD_LEN_LJ_RX; + payload = sysbusRsp->data + LJ_DATA_OFFSET; + } + else + { + DBG_ERROR("Unexpected command 0x%02X.\n", msg[WD_CMD]); + free(sysbusRsp); + return EUNEXP; + } + uint32_t dataId; + SysMonitor_GetRecvPayload(payload, &dataId, dataLen); // omit control info + if (dataId == DATA_ID_NONE) + { + free(sysbusRsp); + DBG_WARNING("SystemBus::no RX data!\n"); + return SUCCESS; + } + if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false) + { + free(sysbusRsp); + DBG_WARNING("SystemBus::TX Queue overflowed!\n"); + return EQUEUE; + } + instPtr->sysbusLog.pollCnt++; + return SUCCESS; +} + +/** + * @brief Generate System Bus remote test command response + */ +static int32_t SystemBus_GenRemoteTestRsp(SYSBUS *instPtr, uint8_t *msg) +{ + ASSERT(instPtr != NULL); + ASSERT(msg != NULL); + SYSBUS_UART_FRAME *sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME)); + if (sysbusRsp == NULL) { + DBG_ERROR("Failed to allocate memory for LJ REMOTE TEST Response!\n"); + return ENOMEM; + } + + TIME_WORD tmWord; + uint64_t time; + TimeCtrl_GetCurrentTime(linkInstPtr->config.tmcInstPtr, &tmWord); + time = (uint64_t)tmWord.int_part * TIME_FRAC_MAX + tmWord.frac_part; + + if(msg[WD_CMD] == CMD_FLJ_RM_TEST) + { + sysbusRsp->len = FLJ_RM_TEST_RESP_LEN; + memset(sysbusRsp->data, 0x00, FLJ_RM_TEST_RESP_LEN); + sysbusRsp->data[WD_CMD] = CMD_FLJ_RM_TEST; + sysbusRsp->data[WD_PRD] = MY_PRD_ID; + + memcpy(sysbusRsp->data + WD_FLJ_TIME_STAMP, &time, WD_FLJ_TIME_STAMP_LEN); + SysMonitor_GetLinklayerLog(&(instPtr->linkLog)); + SysMonitor_GetPhysicalLayerLog(&(instPtr->phyLog)); + SysMonitor_GetHardwareStatus(&(instPtr->hwState)); + memcpy(sysbusRsp->data + WD_FLJ_SYSBUS_LOG, &(instPtr->sysbusLog), WD_FLJ_SYSBUS_LOG_LEN); + memcpy(sysbusRsp->data + WD_FLJ_LINK_LOG, &(instPtr->linkLog), WD_FLJ_LINK_LOG_LEN); + memcpy(sysbusRsp->data + WD_FLJ_PHY_LOG, &(instPtr->phyLog), WD_FLJ_PHY_LOG_LEN); + memcpy(sysbusRsp->data + WD_FLJ_HARDWARE_ST, &(instPtr->hwState.spFpgaVolt), WD_FLJ_HARDWARE_ST_LEN); + } + else if (msg[WD_CMD] == CMD_LJ_RM_TEST) + { + sysbusRsp->len = LJ_RM_TEST_RESP_LEN; + sysbusRsp->data[WD_CMD] = CMD_LJ_RM_TEST; + sysbusRsp->data[WD_PRD] = MY_PRD_ID; + + memcpy(sysbusRsp->data + WD_LJ_TIME_STAMP, &time, WD_LJ_TIME_STAMP_LEN); + SysMonitor_GetLinklayerLog(&(instPtr->linkLog)); + SysMonitor_GetPhysicalLayerLog(&(instPtr->phyLog)); + SysMonitor_GetHardwareStatus(&(instPtr->hwState)); + memcpy(sysbusRsp->data + WD_LJ_SYSBUS_LOG, &(instPtr->sysbusLog), WD_LJ_SYSBUS_LOG_LEN); + memcpy(sysbusRsp->data + WD_LJ_LINK_LOG, &(instPtr->linkLog), WD_LJ_LINK_LOG_LEN); + memcpy(sysbusRsp->data + WD_LJ_PHY_LOG, &(instPtr->phyLog), WD_LJ_PHY_LOG_LEN); + memcpy(sysbusRsp->data + WD_LJ_HARDWARE_ST, &(instPtr->hwState.spFpgaVolt), WD_LJ_HARDWARE_ST_LEN); + } + else + { + DBG_ERROR("Unexpected command 0x%02X.\n", msg[WD_CMD]); + free(sysbusRsp); + return EUNEXP; + } + + if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false){ + free(sysbusRsp); + DBG_WARNING("SystemBus::TX Queue overflowed!\n"); + return EQUEUE; + } + instPtr->sysbusLog.remoteCnt++; + return SUCCESS; +} + /*****************************************************************************/ /** * @brief Initialize System Bus instance. @@ -298,350 +567,100 @@ int32_t SystemBus_HandleCmd(SYSBUS *instPtr) { ASSERT(instPtr != NULL); int32_t Status; - SYSBUS_UART_FRAME *sysbusReq, *sysbusRsp; - uint8_t *data; + SYSBUS_UART_FRAME *sysbusReq; + uint8_t *msg; if (CircQueuePop(instPtr->rxQueue, (void **)(&sysbusReq)) == false) { DBG_WARNING("SystemBus::Nothing to be handled.\n"); return EIGNORE; } - data = sysbusReq->data; - if (data[WD_PRD] != MY_PRD_ID && data[WD_PRD] != PRD_ALL) + msg = sysbusReq->data; + if (msg[WD_PRD] != MY_PRD_ID && msg[WD_PRD] != PRD_ALL) { DBG_WARNING("SystemBus::Production code mismatch!\n"); return EIGNORE; } - switch (data[WD_CMD]) + switch (msg[WD_CMD]) { case CMD_SELF_TEST: { - HWSTATE *hwStatsPtr = &(instPtr->hwState); - sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME)); - if (sysbusRsp == NULL) { - DBG_ERROR("Failed to allocate memory for SELF TEST Response!\n"); - Status = ENOMEM; - break; - } - sysbusRsp->len = ST_RSP_LEN; - memset(sysbusRsp->data, 0x00, ST_RSP_LEN); - sysbusRsp->data[WD_CMD] = CMD_SELF_TEST; - sysbusRsp->data[WD_PRD] = MY_PRD_ID; - SysMonitor_GetHardwareStatus(hwStatsPtr); - if (linkInstPtr->isEnable == false) - { - sysbusRsp->data[WD_ST_RET] = (hwStatsPtr->isSpOk) ? ST_RET_OK : ST_RET_ERROR; - } - else - { - sysbusRsp->data[WD_ST_RET] = (hwStatsPtr->isSpOk && hwStatsPtr->isAntOk) ? ST_RET_OK : ST_RET_ERROR; - } - // apply status and omit 2 bytes of overhead. - memcpy(sysbusRsp->data + WD_SP_FPGA_VOLT, &(hwStatsPtr->spFpgaVolt), ST_RET_INFO_LEN); - - if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false) - { - free(sysbusRsp); - DBG_ERROR("SystemBus::TX Queue overflowed!\n"); - Status = EQUEUE; - break; - } - Status = SUCCESS; + Status = SystemBus_GenSelfTestRsp(instPtr); break; } case CMD_DATA_TEST_A: - { - sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME)); - if (sysbusRsp == NULL) { - DBG_ERROR("Failed to allocate memory for DATA TEST A Response!\n"); - Status = ENOMEM; - break; - } - sysbusRsp->len = DATA_TEST_RSP_A_LEN; - memcpy(sysbusRsp->data, sysbusReq->data, DATA_TEST_RSP_A_LEN); - if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false) - { - free(sysbusRsp); - DBG_ERROR("SystemBus::TX Queue overflowed!\n"); - Status = EQUEUE; - break; - } - Status = SUCCESS; - break; - } case CMD_DATA_TEST_B: { - sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME)); - if (sysbusRsp == NULL) { - DBG_ERROR("Failed to allocate memory for DATA TEST B Response!\n"); - Status = ENOMEM; - break; - } - sysbusRsp->len = DATA_TEST_RSP_B_LEN; - memcpy(sysbusRsp->data, sysbusReq->data, DATA_TEST_RSP_B_LEN); - if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false) - { - free(sysbusRsp); - DBG_ERROR("SystemBus::TX Queue overflowed!\n"); - Status = EQUEUE; - break; - } - Status = SUCCESS; + Status = SystemBus_Echo(instPtr, sysbusReq); break; } case CMD_CONFIG: { - PHY_INST *phyRef = linkInstPtr->config.phyInstPtr; - TMC_INST *tmcRef = linkInstPtr->config.tmcInstPtr; - bool reconfig = false; - - if (data[WD_MODE_CMD] == MODE_CMD_HEAD) - { - // omit unnecessary reconfiguration - if (L2_GetNodeType(linkInstPtr) != L2_NODE_HEAD) - { - // head node config - linkConfig.myType = L2_NODE_HEAD; - linkConfig.myId = 1; - reconfig = true; - } - } - - else if (data[WD_MODE_CMD] == MODE_CMD_MEMBER ) - { - // omit unnecessary reconfiguration - if (L2_GetNodeType(linkInstPtr) != L2_NODE_MEMBER) - { - // member node config - linkConfig.myType = L2_NODE_MEMBER; - linkConfig.myId = 2; - reconfig = true; - } - } - else - { - // Ignore current command - DBG_ERROR("Unexpected config command 0x%02X:0x%02X", - WD_MODE_CMD, data[WD_MODE_CMD]); - Status = EPARAM; - break; - } - - if(data[WD_EN_CMD] == EN_CMD_ON_HIGH_POWER) - { - // high power config - Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_FULL); - } - else if(data[WD_EN_CMD] == EN_CMD_ON_LOW_POWER) - { - // low power config - Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_LOW); - } - else if(data[WD_EN_CMD] == EN_CMD_SILENT) - { - // silence config - // TODO: better approach? - Linklayer_Halt(linkInstPtr); - Phy_AntConfigMode(phyRef, PHY_ANT_MODE_STANDBY); - TIME_WORD pullTime = {0, 0}; - pullTime.int_part = TimeCtrl_GetIntCnt(tmcRef) + 1; - Status = Phy_SetTimeWord(phyRef, &pullTime, PHY_TIME_WORD_POLL); - if (Status != SUCCESS) - { - DBG_ERROR("Physical set pull time failed, with error code: %" PRId32 ".\n", Status); - } - Phy_SetupNextSlot(phyRef, PHY_TOR_RX, L1_10KBPS_10MS_FRAME, NULL); - break; - } - if (reconfig == true) - { - Linklayer_Reconfig(linkInstPtr, &linkConfig); - } - Linklayer_Start(linkInstPtr); - instPtr->sysbusLog.lastConfig[0] = data[WD_MODE_CMD]; - instPtr->sysbusLog.lastConfig[1] = data[WD_EN_CMD]; - Status = SUCCESS; + Status = SystemBus_SystemConfig(instPtr, msg); break; } case CMD_SYS_INFO: { - SysMonitor_UpdateSysinfo((SYSINFO *)(data + SYS_INFO_OFFSET)); + SysMonitor_UpdateSysinfo((SYSINFO *)(msg + SYS_INFO_OFFSET)); Status = SUCCESS; break; } - // TODO case CMD_FLJ_SEND: { - SysMonitor_UpdateSendPayload(data + FLJ_DATA_OFFSET, PAYLOAD_LEN_FLJ_TX); // omit control info + SysMonitor_UpdateSendPayload(msg + FLJ_DATA_OFFSET, PAYLOAD_LEN_FLJ_TX); // omit control info instPtr->sysbusLog.svcCnt++; Status = SUCCESS; break; } case CMD_LJ_SEND: { - SysMonitor_UpdateSendPayload(data + LJ_DATA_OFFSET, PAYLOAD_LEN_LJ_TX); // omit control info + SysMonitor_UpdateSendPayload(msg + LJ_DATA_OFFSET, PAYLOAD_LEN_LJ_TX); // omit control info instPtr->sysbusLog.svcCnt++; Status = SUCCESS; break; } case CMD_FLJ_POLL: { - if (data[WD_FLJ_POLL] != FLJ_POLL_CODE) + if (msg[WD_FLJ_POLL] != FLJ_POLL_CODE) { DBG_ERROR("SystemBus::Invalid roll call code!\n"); Status = EIGNORE; break; } - sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME)); - if (sysbusRsp == NULL) { - DBG_ERROR("Failed to allocate memory for FLJ POLL Response!\n"); - Status = ENOMEM; - break; - } - sysbusRsp->len = FLJ_POLL_RESP_LEN; - sysbusRsp->data[WD_CMD] = CMD_FLJ_POLL; - sysbusRsp->data[WD_PRD] = MY_PRD_ID; - uint32_t dataId; - SysMonitor_GetRecvPayload(sysbusRsp->data + FLJ_DATA_OFFSET, &dataId, PAYLOAD_LEN_FLJ_RX); // omit control info - if (dataId == DATA_ID_NONE) - { - free(sysbusRsp); - DBG_WARNING("SystemBus::no RX data!\n"); - Status = SUCCESS; - break; - } - if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false) - { - free(sysbusRsp); - DBG_WARNING("SystemBus::TX Queue overflowed!\n"); - Status = EQUEUE; - break; - } - instPtr->sysbusLog.pollCnt++; - Status = SUCCESS; + Status = SystemBus_GenPollRsp(instPtr, msg); break; } case CMD_LJ_POLL: { - if (data[WD_LJ_POLL] != LJ_POLL_CODE) + if (msg[WD_LJ_POLL] != LJ_POLL_CODE) { DBG_WARNING("SystemBus::Invalid roll call code!\n"); Status = EIGNORE; break; } - sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME)); - if (sysbusRsp == NULL) { - DBG_ERROR("Failed to allocate memory for LJ POLL Response!\n"); - Status = ENOMEM; - break; - } - sysbusRsp->len = LJ_POLL_RESP_LEN; - sysbusRsp->data[WD_CMD] = CMD_LJ_POLL; - sysbusRsp->data[WD_PRD] = MY_PRD_ID; - uint32_t dataId; - SysMonitor_GetRecvPayload(sysbusRsp->data + LJ_DATA_OFFSET, &dataId, PAYLOAD_LEN_LJ_RX); // omit control info - if (dataId == DATA_ID_NONE) - { - free(sysbusRsp); - DBG_WARNING("SystemBus::no RX data!\n"); - Status = SUCCESS; - break; - } - if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false) - { - free(sysbusRsp); - DBG_WARNING("SystemBus::TX Queue overflowed!\n"); - Status = EQUEUE; - break; - } - instPtr->sysbusLog.pollCnt++; - Status = SUCCESS; + Status = SystemBus_GenPollRsp(instPtr, msg); break; } case CMD_FLJ_RM_TEST: { - if (data[WD_FLJ_RM_TEST] != FLJ_RM_TEST_CODE) + if (msg[WD_FLJ_RM_TEST] != FLJ_RM_TEST_CODE) { DBG_WARNING("SystemBus::Invalid remote test code!\n"); Status = EIGNORE; break; } - sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME)); - if (sysbusRsp == NULL) { - DBG_ERROR("Failed to allocate memory for FLJ REMOTE TEST Response!\n"); - Status = ENOMEM; - break; - } - sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME)); - sysbusRsp->len = FLJ_RM_TEST_RESP_LEN; - memset(sysbusRsp->data, 0x00, FLJ_RM_TEST_RESP_LEN); - sysbusRsp->data[WD_CMD] = CMD_FLJ_RM_TEST; - sysbusRsp->data[WD_PRD] = MY_PRD_ID; - - TIME_WORD tmWord; - uint64_t time; - TimeCtrl_GetCurrentTime(linkInstPtr->config.tmcInstPtr, &tmWord); - time = (uint64_t)tmWord.int_part * TIME_FRAC_MAX + tmWord.frac_part; - memcpy(sysbusRsp->data + WD_FLJ_TIME_STAMP, &time, WD_FLJ_TIME_STAMP_LEN); - SysMonitor_GetLinklayerLog(&(instPtr->linkLog)); - SysMonitor_GetPhysicalLayerLog(&(instPtr->phyLog)); - SysMonitor_GetHardwareStatus(&(instPtr->hwState)); - memcpy(sysbusRsp->data + WD_FLJ_SYSBUS_LOG, &(instPtr->sysbusLog), WD_FLJ_SYSBUS_LOG_LEN); - memcpy(sysbusRsp->data + WD_FLJ_LINK_LOG, &(instPtr->linkLog), WD_FLJ_LINK_LOG_LEN); - memcpy(sysbusRsp->data + WD_FLJ_PHY_LOG, &(instPtr->phyLog), WD_FLJ_PHY_LOG_LEN); - memcpy(sysbusRsp->data + WD_FLJ_HARDWARE_ST, &(instPtr->hwState.spFpgaVolt), WD_FLJ_HARDWARE_ST_LEN); - - if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false){ - free(sysbusRsp); - DBG_WARNING("SystemBus::TX Queue overflowed!\n"); - Status = EQUEUE; - break; - } - instPtr->sysbusLog.remoteCnt++; - Status = SUCCESS; + Status = SystemBus_GenRemoteTestRsp(instPtr, msg); break; } case CMD_LJ_RM_TEST: { - if (data[WD_LJ_RM_TEST] != LJ_RM_TEST_CODE) + if (msg[WD_LJ_RM_TEST] != LJ_RM_TEST_CODE) { DBG_WARNING("SystemBus::Invalid remote test code!\n"); Status = EIGNORE; break; } - sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME)); - if (sysbusRsp == NULL) { - DBG_ERROR("Failed to allocate memory for LJ REMOTE TEST Response!\n"); - Status = ENOMEM; - break; - } - sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME)); - sysbusRsp->len = LJ_RM_TEST_RESP_LEN; - memset(sysbusRsp->data, 0x00, LJ_RM_TEST_RESP_LEN); - sysbusRsp->data[WD_CMD] = CMD_LJ_RM_TEST; - sysbusRsp->data[WD_PRD] = MY_PRD_ID; - - TIME_WORD tmWord; - uint64_t time; - TimeCtrl_GetCurrentTime(linkInstPtr->config.tmcInstPtr, &tmWord); - time = (uint64_t)tmWord.int_part * TIME_FRAC_MAX + tmWord.frac_part; - memcpy(sysbusRsp->data + WD_LJ_TIME_STAMP, &time, WD_LJ_TIME_STAMP_LEN); - SysMonitor_GetLinklayerLog(&(instPtr->linkLog)); - SysMonitor_GetPhysicalLayerLog(&(instPtr->phyLog)); - SysMonitor_GetHardwareStatus(&(instPtr->hwState)); - memcpy(sysbusRsp->data + WD_LJ_SYSBUS_LOG, &(instPtr->sysbusLog), WD_LJ_SYSBUS_LOG_LEN); - memcpy(sysbusRsp->data + WD_LJ_LINK_LOG, &(instPtr->linkLog), WD_LJ_LINK_LOG_LEN); - memcpy(sysbusRsp->data + WD_LJ_PHY_LOG, &(instPtr->phyLog), WD_LJ_PHY_LOG_LEN); - memcpy(sysbusRsp->data + WD_LJ_HARDWARE_ST, &(instPtr->hwState.spFpgaVolt), WD_LJ_HARDWARE_ST_LEN); - - if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false){ - free(sysbusRsp); - DBG_WARNING("SystemBus::TX Queue overflowed!\n"); - Status = EQUEUE; - break; - } - instPtr->sysbusLog.remoteCnt++; - Status = SUCCESS; + Status = SystemBus_GenRemoteTestRsp(instPtr, msg); break; } default: