#include #include #include #include "common.h" #include "system_monitor.h" #define BUFF_A 0x00 #define BUFF_B 0x01 #define BUFF_NONE 0xFF #define SYSINFO_PACK_LEN 54 // bytes #define PAYLOAD_PACK_LEN 46 // bytes #define SLOTLOG_PACK_LEN 88 // bytes #define PRTCLLOG_PACK_ELN 24 // bytes #define HWSTATE_PACK_ELN 12 // bytes typedef struct { uint8_t latest; uint8_t rBusy; uint8_t wBusy; } PINGPONG; typedef struct { PINGPONG recvPLCtrl; PAYLOAD recvPL[2]; PINGPONG sendPLCtrl; PAYLOAD sendPL[2]; PINGPONG sysInfoCtrl; SYSINFO sysInfo[2]; PINGPONG sendLogCtrl; SLOTLOG sendLog[2]; PINGPONG recvLogCtrl; SLOTLOG recvLog[2]; PINGPONG protocolLogCtrl; PRTCLLOG protocolLog[2]; PINGPONG hwStateCtrl; HWSTATE hwState[2]; } SYSMONITOR; static SYSMONITOR *sysMonitorInst; uint8_t GetWritable(PINGPONG ctrllor); uint8_t GetReadable(PINGPONG ctrllor); inline uint8_t GetWritable(PINGPONG ctrllor) { // Allow only one write. ASSERT(ctrllor.wBusy == BUFF_NONE); if (ctrllor.rBusy == BUFF_NONE) return ((ctrllor.latest == BUFF_B) ? BUFF_A : BUFF_B); else return ((ctrllor.rBusy == BUFF_B) ? BUFF_A : BUFF_B); } inline uint8_t GetReadable(PINGPONG ctrllor) { if (ctrllor.wBusy == BUFF_NONE || ctrllor.wBusy != ctrllor.latest) return ((ctrllor.latest == BUFF_NONE) ? BUFF_A : BUFF_B); else return ((ctrllor.wBusy == BUFF_B) ? BUFF_A : BUFF_B); } // System monitor initialise int32_t SysMonitor_Init(void) { sysMonitorInst = calloc(1, sizeof(SYSMONITOR)); if (sysMonitorInst == NULL) { return ENOMEM; } memset(&(sysMonitorInst->recvPLCtrl), BUFF_NONE, sizeof(PINGPONG)); memset(&(sysMonitorInst->sendPLCtrl), BUFF_NONE, sizeof(PINGPONG)); memset(&(sysMonitorInst->sysInfoCtrl), BUFF_NONE, sizeof(PINGPONG)); memset(&(sysMonitorInst->sendLogCtrl), BUFF_NONE, sizeof(PINGPONG)); memset(&(sysMonitorInst->recvLogCtrl), BUFF_NONE, sizeof(PINGPONG)); memset(&(sysMonitorInst->protocolLogCtrl), BUFF_NONE, sizeof(PINGPONG)); memset(&(sysMonitorInst->hwStateCtrl), BUFF_NONE, sizeof(PINGPONG)); sysMonitorInst->recvPL[0].dataId = DATA_ID_NONE; sysMonitorInst->recvPL[1].dataId = DATA_ID_NONE; sysMonitorInst->sendPL[0].dataId = DATA_ID_NONE; sysMonitorInst->sendPL[1].dataId = DATA_ID_NONE; return SUCCESS; } // System monitor initialise void SysMonitor_Destroy(void) { free(sysMonitorInst); } // @System Info void SysMonitor_UpdateSysinfo(const SYSINFO *info) { uint8_t buffIndx; buffIndx = GetWritable(sysMonitorInst->sysInfoCtrl); sysMonitorInst->sysInfoCtrl.wBusy = buffIndx; memcpy(&(sysMonitorInst->sysInfo[buffIndx]), info, sizeof(SYSINFO)); sysMonitorInst->sysInfoCtrl.latest = buffIndx; sysMonitorInst->sysInfoCtrl.wBusy = BUFF_NONE; } void SysMonitor_UpdateSysinfoRaw(const uint8_t *buff) { ASSERT(sysMonitorInst != NULL); uint8_t buffIndx; buffIndx = GetWritable(sysMonitorInst->sysInfoCtrl); sysMonitorInst->sysInfoCtrl.wBusy = buffIndx; // Decapsulate Systeminfo SYSINFO *infoEntry = &(sysMonitorInst->sysInfo[buffIndx]); memcpy(&(infoEntry->psBDS), buff, sizeof(infoEntry->psBDS)); buff += sizeof(infoEntry->psBDS); memcpy(&(infoEntry->pBDS), buff, sizeof(infoEntry->pBDS)); buff += sizeof(infoEntry->pBDS); memcpy(&(infoEntry->psGPS), buff, sizeof(infoEntry->psGPS)); buff += sizeof(infoEntry->psGPS); memcpy(&(infoEntry->pGPS), buff, sizeof(infoEntry->pGPS)); buff += sizeof(infoEntry->pGPS); memcpy(&(infoEntry->tmInd), buff, sizeof(infoEntry->tmInd)); buff += sizeof(infoEntry->tmInd); memcpy(&(infoEntry->tmFly), buff, sizeof(infoEntry->tmFly)); buff += sizeof(infoEntry->tmFly); memcpy(&(infoEntry->stFly), buff, sizeof(infoEntry->stFly)); buff += sizeof(infoEntry->stFly); memcpy(&(infoEntry->posX), buff, sizeof(infoEntry->posX)); buff += sizeof(infoEntry->posX); memcpy(&(infoEntry->posY), buff, sizeof(infoEntry->posY)); buff += sizeof(infoEntry->posY); memcpy(&(infoEntry->posZ), buff, sizeof(infoEntry->posZ)); buff += sizeof(infoEntry->posZ); memcpy(&(infoEntry->vX), buff, sizeof(infoEntry->vX)); buff += sizeof(infoEntry->vX); memcpy(&(infoEntry->vY), buff, sizeof(infoEntry->vY)); buff += sizeof(infoEntry->vY); memcpy(&(infoEntry->vZ), buff, sizeof(infoEntry->vZ)); buff += sizeof(infoEntry->vZ); memcpy(&(infoEntry->pitch), buff, sizeof(infoEntry->pitch)); buff += sizeof(infoEntry->pitch); memcpy(&(infoEntry->yaw), buff, sizeof(infoEntry->yaw)); buff += sizeof(infoEntry->yaw); memcpy(&(infoEntry->roll), buff, sizeof(infoEntry->roll)); sysMonitorInst->sysInfoCtrl.latest = buffIndx; sysMonitorInst->sysInfoCtrl.wBusy = BUFF_NONE; } void SysMonitor_GetSysinfo(SYSINFO *info) { ASSERT(sysMonitorInst != NULL); uint8_t buffIndx, buffIndex_old; buffIndex_old = sysMonitorInst->sysInfoCtrl.rBusy; if (buffIndex_old != BUFF_NONE) buffIndx = buffIndex_old; // Keep it busy else buffIndx = GetReadable(sysMonitorInst->sysInfoCtrl); sysMonitorInst->sysInfoCtrl.rBusy = buffIndx; memcpy(info, &(sysMonitorInst->sysInfo[buffIndx]), sizeof(SYSINFO)); sysMonitorInst->sysInfoCtrl.rBusy = buffIndex_old; } void SysMonitor_GetSysinfoRaw(uint8_t *buff) { uint8_t buffIndx, buffIndex_old; buffIndex_old = sysMonitorInst->sysInfoCtrl.rBusy; if (buffIndex_old != BUFF_NONE) buffIndx = buffIndex_old; // Keep it busy else buffIndx = GetReadable(sysMonitorInst->sysInfoCtrl); sysMonitorInst->sysInfoCtrl.rBusy = buffIndx; // Encapsulate Systeminfo SYSINFO *infoEntry = &(sysMonitorInst->sysInfo[buffIndx]); memcpy(buff, &(infoEntry->psBDS), sizeof(infoEntry->psBDS)); buff += sizeof(infoEntry->psBDS); memcpy(buff, &(infoEntry->pBDS), sizeof(infoEntry->pBDS)); buff += sizeof(infoEntry->pBDS); memcpy(buff, &(infoEntry->psGPS), sizeof(infoEntry->psGPS)); buff += sizeof(infoEntry->psGPS); memcpy(buff, &(infoEntry->pGPS), sizeof(infoEntry->pGPS)); buff += sizeof(infoEntry->pGPS); memcpy(buff, &(infoEntry->tmInd), sizeof(infoEntry->tmInd)); buff += sizeof(infoEntry->tmInd); memcpy(buff, &(infoEntry->tmFly), sizeof(infoEntry->tmFly)); buff += sizeof(infoEntry->tmFly); memcpy(buff, &(infoEntry->stFly), sizeof(infoEntry->stFly)); buff += sizeof(infoEntry->stFly); memcpy(buff, &(infoEntry->posX), sizeof(infoEntry->posX)); buff += sizeof(infoEntry->posX); memcpy(buff, &(infoEntry->posY), sizeof(infoEntry->posY)); buff += sizeof(infoEntry->posY); memcpy(buff, &(infoEntry->posZ), sizeof(infoEntry->posZ)); buff += sizeof(infoEntry->posZ); memcpy(buff, &(infoEntry->vX), sizeof(infoEntry->vX)); buff += sizeof(infoEntry->vX); memcpy(buff, &(infoEntry->vY), sizeof(infoEntry->vY)); buff += sizeof(infoEntry->vY); memcpy(buff, &(infoEntry->vZ), sizeof(infoEntry->vZ)); buff += sizeof(infoEntry->vZ); memcpy(buff, &(infoEntry->pitch), sizeof(infoEntry->pitch)); buff += sizeof(infoEntry->pitch); memcpy(buff, &(infoEntry->yaw), sizeof(infoEntry->yaw)); buff += sizeof(infoEntry->yaw); memcpy(buff, &(infoEntry->roll), sizeof(infoEntry->roll)); sysMonitorInst->sysInfoCtrl.rBusy = buffIndex_old; } // @Send Payload /** * Update send payload by raw data. */ void SysMonitor_UpdateSendPayloadRaw(const uint8_t *buff, size_t len) { ASSERT(sysMonitorInst != NULL); uint8_t buffIndx; buffIndx = GetWritable(sysMonitorInst->sendPLCtrl); sysMonitorInst->sendPLCtrl.wBusy = buffIndx; memcpy(sysMonitorInst->sendPL[buffIndx].data, buff, len); sysMonitorInst->sendPL[buffIndx].dataId++; sysMonitorInst->sendPLCtrl.latest = buffIndx; sysMonitorInst->sendPLCtrl.wBusy = BUFF_NONE; } /** * Get send payload as raw data. */ void SysMonitor_GetSendPayloadRaw(uint8_t *buff, uint32_t *id, size_t len) { ASSERT(sysMonitorInst != NULL); uint8_t buffIndx, buffIndex_old; buffIndex_old = sysMonitorInst->sendPLCtrl.rBusy; if (buffIndex_old != BUFF_NONE) buffIndx = buffIndex_old; // Keep it busy else buffIndx = GetReadable(sysMonitorInst->sendPLCtrl); sysMonitorInst->sendPLCtrl.rBusy = buffIndx; if (sysMonitorInst->sendPL[buffIndx].dataId != DATA_ID_NONE) { memcpy(buff, sysMonitorInst->sendPL[buffIndx].data, len); } *id = sysMonitorInst->sendPL[buffIndx].dataId; sysMonitorInst->sendPLCtrl.rBusy = buffIndex_old; } // @Receive Payload /** * Update receive payload by raw data. */ void SysMonitor_UpdateRecvPayloadRaw(const uint8_t *buff, size_t len) { ASSERT(sysMonitorInst != NULL); uint8_t buffIndx; buffIndx = GetWritable(sysMonitorInst->recvPLCtrl); sysMonitorInst->recvPLCtrl.wBusy = buffIndx; memcpy(sysMonitorInst->recvPL[buffIndx].data, buff, len); sysMonitorInst->recvPL[buffIndx].dataId++; sysMonitorInst->recvPLCtrl.latest = buffIndx; sysMonitorInst->recvPLCtrl.wBusy = BUFF_NONE; } /** * Get send payload as raw data. */ void SysMonitor_GetRecvPayloadRaw(uint8_t *buff, uint32_t *id, size_t len) { ASSERT(sysMonitorInst != NULL); uint8_t buffIndx, buffIndex_old; buffIndex_old = sysMonitorInst->recvPLCtrl.rBusy; if (buffIndex_old != BUFF_NONE) buffIndx = buffIndex_old; // Keep it busy else buffIndx = GetReadable(sysMonitorInst->recvPLCtrl); sysMonitorInst->recvPLCtrl.rBusy = buffIndx; if (sysMonitorInst->recvPL[buffIndx].dataId != DATA_ID_NONE) { memcpy(buff, sysMonitorInst->recvPL[buffIndx].data, len); } *id = sysMonitorInst->recvPL[buffIndx].dataId; sysMonitorInst->recvPLCtrl.rBusy = buffIndex_old; } // @Send Slot log /** * Log send slot information */ void SysMonitor_UpdateSendSlotLog(const SLOTLOG *log) { ASSERT(sysMonitorInst != NULL); uint8_t buffIndx; buffIndx = GetWritable(sysMonitorInst->sendLogCtrl); sysMonitorInst->sendLogCtrl.wBusy = buffIndx; memcpy(&(sysMonitorInst->sendLog[buffIndx]), log, sizeof(SLOTLOG)); sysMonitorInst->sendLogCtrl.latest = buffIndx; sysMonitorInst->sendLogCtrl.wBusy = BUFF_NONE; } /** * Get the latest send slot log */ void SysMonitor_GetSendSlotLog(SLOTLOG *log) { ASSERT(sysMonitorInst != NULL); uint8_t buffIndx, buffIndex_old; buffIndex_old = sysMonitorInst->sendLogCtrl.rBusy; if (buffIndex_old != BUFF_NONE) buffIndx = buffIndex_old; // Keep it busy else buffIndx = GetReadable(sysMonitorInst->sendLogCtrl); sysMonitorInst->sendLogCtrl.rBusy = buffIndx; memcpy(log, &(sysMonitorInst->sendLog[buffIndx]), sizeof(SLOTLOG)); sysMonitorInst->sendLogCtrl.rBusy = buffIndex_old; } /** * Get the latest send slot log as raw data */ void SysMonitor_GetSendSlotLogRaw(uint8_t *buff) { ASSERT(sysMonitorInst != NULL); uint8_t buffIndx, buffIndex_old; buffIndex_old = sysMonitorInst->sendLogCtrl.rBusy; if (buffIndex_old != BUFF_NONE) buffIndx = buffIndex_old; // Keep it busy else buffIndx = GetReadable(sysMonitorInst->sendLogCtrl); sysMonitorInst->sendLogCtrl.rBusy = buffIndx; // Encapsulate log SLOTLOG *logEntry = &(sysMonitorInst->sendLog[buffIndx]); memcpy(buff, &(logEntry->tuNo), sizeof(logEntry->tuNo)); buff += sizeof(logEntry->tuNo); memcpy(buff, &(logEntry->slotNo), sizeof(logEntry->slotNo)); buff += sizeof(logEntry->slotNo); memcpy(buff, &(logEntry->frameType), sizeof(logEntry->frameType)); buff += sizeof(logEntry->frameType); memcpy(buff, &(logEntry->payload), sizeof(logEntry->payload)); buff += sizeof(logEntry->payload); memcpy(buff, &(logEntry->tmStamp), sizeof(logEntry->tmStamp)); buff += sizeof(logEntry->tmStamp); memcpy(buff, &(logEntry->tmDevi), sizeof(logEntry->tmDevi)); buff += sizeof(logEntry->tmDevi); memcpy(buff, &(logEntry->freqDevi), sizeof(logEntry->freqDevi)); sysMonitorInst->sendLogCtrl.rBusy = buffIndex_old; } // @Receive Slot log /** * Log receive slot information */ void SysMonitor_UpdateRecvSlotLog(const SLOTLOG *log) { ASSERT(sysMonitorInst != NULL); uint8_t buffIndx; buffIndx = GetWritable(sysMonitorInst->recvLogCtrl); sysMonitorInst->recvLogCtrl.wBusy = buffIndx; memcpy(&(sysMonitorInst->recvLog[buffIndx]), log, sizeof(SLOTLOG)); sysMonitorInst->recvLogCtrl.latest = buffIndx; sysMonitorInst->recvLogCtrl.wBusy = BUFF_NONE; } /** * Get the latest receive slot information */ void SysMonitor_GetRecvSlotLog(SLOTLOG *log) { ASSERT(sysMonitorInst != NULL); uint8_t buffIndx, buffIndex_old; buffIndex_old = sysMonitorInst->recvLogCtrl.rBusy; if (buffIndex_old != BUFF_NONE) buffIndx = buffIndex_old; // Keep it busy else buffIndx = GetReadable(sysMonitorInst->recvLogCtrl); sysMonitorInst->recvLogCtrl.rBusy = buffIndx; memcpy(log, &(sysMonitorInst->recvLog[buffIndx]), sizeof(SLOTLOG)); sysMonitorInst->recvLogCtrl.rBusy = buffIndex_old; } /** * Get the latest receive slot information as raw data */ void SysMonitor_GetRecvSlotLogRaw(uint8_t *buff) { ASSERT(sysMonitorInst != NULL); uint8_t buffIndx, buffIndex_old; buffIndex_old = sysMonitorInst->recvLogCtrl.rBusy; if (buffIndex_old != BUFF_NONE) buffIndx = buffIndex_old; // Keep it busy else buffIndx = GetReadable(sysMonitorInst->recvLogCtrl); sysMonitorInst->recvLogCtrl.rBusy = buffIndx; // Encapsulate log SLOTLOG *logEntry = &(sysMonitorInst->recvLog[buffIndx]); memcpy(buff, &(logEntry->tuNo), sizeof(logEntry->tuNo)); buff += sizeof(logEntry->tuNo); memcpy(buff, &(logEntry->slotNo), sizeof(logEntry->slotNo)); buff += sizeof(logEntry->slotNo); memcpy(buff, &(logEntry->frameType), sizeof(logEntry->frameType)); buff += sizeof(logEntry->frameType); memcpy(buff, &(logEntry->payload), sizeof(logEntry->payload)); buff += sizeof(logEntry->payload); memcpy(buff, &(logEntry->tmStamp), sizeof(logEntry->tmStamp)); buff += sizeof(logEntry->tmStamp); memcpy(buff, &(logEntry->tmDevi), sizeof(logEntry->tmDevi)); buff += sizeof(logEntry->tmDevi); memcpy(buff, &(logEntry->freqDevi), sizeof(logEntry->freqDevi)); sysMonitorInst->recvLogCtrl.rBusy = buffIndex_old; } // @Protocol log /** * Log protocol running status. */ void SysMonitor_UpdateProtocolLog(const PRTCLLOG *log) { ASSERT(sysMonitorInst != NULL); uint8_t buffIndx; buffIndx = GetWritable(sysMonitorInst->protocolLogCtrl); sysMonitorInst->protocolLogCtrl.wBusy = buffIndx; memcpy(&(sysMonitorInst->protocolLog[buffIndx]), log, sizeof(PRTCLLOG)); sysMonitorInst->protocolLogCtrl.latest = buffIndx; sysMonitorInst->protocolLogCtrl.wBusy = BUFF_NONE; } /** * Get the protocol running log. */ void SysMonitor_GetProtocolLog(PRTCLLOG *log) { ASSERT(sysMonitorInst != NULL); uint8_t buffIndx, buffIndex_old; buffIndex_old = sysMonitorInst->protocolLogCtrl.rBusy; if (buffIndex_old != BUFF_NONE) buffIndx = buffIndex_old; // Keep it busy else buffIndx = GetReadable(sysMonitorInst->protocolLogCtrl); sysMonitorInst->protocolLogCtrl.rBusy = buffIndx; memcpy(log, &(sysMonitorInst->protocolLog[buffIndx]), sizeof(PRTCLLOG)); sysMonitorInst->protocolLogCtrl.rBusy = buffIndex_old; } /** * Get the protocol running log as raw data. */ void SysMonitor_GetProtocolLogRaw(uint8_t *buff) { ASSERT(sysMonitorInst != NULL); uint8_t buffIndx, buffIndex_old; buffIndex_old = sysMonitorInst->protocolLogCtrl.rBusy; if (buffIndex_old != BUFF_NONE) buffIndx = buffIndex_old; // Keep it busy else buffIndx = GetReadable(sysMonitorInst->protocolLogCtrl); sysMonitorInst->protocolLogCtrl.rBusy = buffIndx; // Encapsulate log PRTCLLOG *logEntry = &(sysMonitorInst->protocolLog[buffIndx]); memcpy(buff, &(logEntry->tmSys), sizeof(logEntry->tmSys)); buff += sizeof(logEntry->tmSys); memcpy(buff, &(logEntry->phySt), sizeof(logEntry->phySt)); buff += sizeof(logEntry->phySt); memcpy(buff, &(logEntry->tmLocal), sizeof(logEntry->tmLocal)); buff += sizeof(logEntry->tmLocal); memcpy(buff, &(logEntry->beamNo), sizeof(logEntry->beamNo)); buff += sizeof(logEntry->beamNo); memcpy(buff, &(logEntry->beamWidth), sizeof(logEntry->beamWidth)); buff += sizeof(logEntry->beamWidth); memcpy(buff, &(logEntry->stage), sizeof(logEntry->stage)); buff += sizeof(logEntry->stage); memcpy(buff, &(logEntry->recvCnt), sizeof(logEntry->recvCnt)); buff += sizeof(logEntry->recvCnt); sysMonitorInst->protocolLogCtrl.rBusy = buffIndex_old; } // @Hardware state /** * Log hardware running status. */ void SysMonitor_UpdateHardwareStatus(const HWSTATE *state) { ASSERT(sysMonitorInst != NULL); uint8_t buffIndx; buffIndx = GetWritable(sysMonitorInst->hwStateCtrl); sysMonitorInst->hwStateCtrl.wBusy = buffIndx; memcpy(&(sysMonitorInst->hwState[buffIndx]), state, sizeof(HWSTATE)); sysMonitorInst->hwStateCtrl.latest = buffIndx; sysMonitorInst->hwStateCtrl.wBusy = BUFF_NONE; } /** * Get the latest hardware running status. */ void SysMonitor_GetHardwareStatus(HWSTATE *state) { ASSERT(sysMonitorInst != NULL); uint8_t buffIndx, buffIndex_old; buffIndex_old = sysMonitorInst->hwStateCtrl.rBusy; if (buffIndex_old != BUFF_NONE) buffIndx = buffIndex_old; // Keep it busy else buffIndx = GetReadable(sysMonitorInst->hwStateCtrl); sysMonitorInst->hwStateCtrl.rBusy = buffIndx; memcpy(state, &(sysMonitorInst->hwState[buffIndx]), sizeof(HWSTATE)); sysMonitorInst->hwStateCtrl.rBusy = buffIndex_old; } // @System log /** * Get the latest system running log. */ void SysMonitor_GetSyslog(SYSLOG *log) { ASSERT(sysMonitorInst != NULL); SysMonitor_GetSendSlotLog(&(log->sendLog)); SysMonitor_GetRecvSlotLog(&(log->recvLog)); SysMonitor_GetProtocolLog(&(log->protocolLog)); SysMonitor_GetHardwareStatus(&(log->hwSate)); } /** * Get the latest system running log as raw data. */ void SysMonitor_GetSyslogRaw(uint8_t *buff) { ASSERT(sysMonitorInst != NULL); SysMonitor_GetSendSlotLogRaw(buff); buff += SLOTLOG_PACK_LEN; SysMonitor_GetRecvSlotLogRaw(buff); buff += SLOTLOG_PACK_LEN; SysMonitor_GetProtocolLogRaw(buff); }