hj_datalink/src/systembus/systembus.c

/*****************************************************************************/
/**
 * @file systembus.c
 * @author FanHao ([email protected])
 * @brief System bus protocol implementation file.
 * @version 0.1
 * @date 2023-05-06
 *
 * @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
 * Lab of Communication and Network
 *
 ******************************************************************************/

/**
 * @addtogroup SystemBus
 * @{
 */

/***************************** Include Files *********************************/
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <stdbool.h>
#include "systembus_uart.h"
#include "systembus.h"
#include "util.h"
#include "common.h"
#include "error.h"
#include "circ_queue.h"
// TODO: better approach?
#include "../networking/link_layer/link_layer.h"
#include "../networking/phy_layer/phy_layer.h"

/**
 * @addtogroup SystemBus_Protocol_Routine
 * @{
 */

/************************** Constant Definitions *****************************/
/**
 * @name SystemBus_MSG_Constants
 * @{
 */
// Common Field
// -- Command word
#define WD_CMD 0x00

// -- Production type
#define WD_PRD 0x01
#define PRD_DL_K 0xDD
#define PRD_DL_C 0xEE
#define PRD_ALL 0xFF

// Self-Test
#define CMD_SELF_TEST 0xE1
// -- Request
// -- -- --
// -- Response
#define ST_RSP_LEN 23
// -- -- Self-Test Result
#define WD_ST_RET 0x02
#define ST_RET_OK 0xAA
#define ST_RET_ERROR 0x55
// -- -- Hardware info offset
#define SP_INFO_OFFSET 0x03
#define ST_INFO_LEN 20

// Data Echo Test Command
#define CMD_ECHO 0xE2

// Configuration
#define CMD_CONFIG 0xE3
// -- Request
// -- -- Enable control
#define WD_EN_CMD 0x02
#define EN_CMD_ENABLE 0xAA
#define EN_CMD_SILENT 0x55
// -- -- Node mode control
#define WD_ID_CMD 0x03
// -- -- Service Rate control
#define WD_RATE_CMD 0x04
#define RATE_CMD_128K 0x22
#define RATE_CMD_512K 0x33
#define RATE_CMD_2M 0x44
// -- -- Routing table select
#define WD_RTNE_CMD 0x05
// -- Response
// -- -- none response

// System info update
#define CMD_SYS_INFO 0x33
// -- Request
// -- -- 54 byte system info
#define SYS_INFO_OFFSET 0x02
// -- Response
// -- -- none response

// Data Send Command
#define CMD_SEND_DATA 0xE4
// -- Request
// -- -- destination ID
#define SEND_DATA_ID_OFFSET 0x02
#define SEND_DATA_ID_LEN 4
#define WD_DST_ID 0x06
// -- -- data length
#define SEND_DATA_LEN_OFFSET 0x07
#define SEND_DATA_LEN_LEN 2
// -- -- <= 1000 byte payload
#define SEND_DATA_OFFSET 0x09
// -- Response
// -- -- none response

// Data Receive Command
#define CMD_RECV_DATA 0xE5
// -- Request
// -- -- --
// -- Response
// -- -- source ID
#define RECV_DATA_ID_OFFSET 0x02
#define RECV_DATA_ID_LEN 4
#define WD_SRC_ID 0x06
// -- -- data length
#define RECV_DATA_LEN_OFFSET 0x07
#define RECV_DATA_LEN_LEN 2
// -- -- <= 1000 byte payload
#define RECV_DATA_OFFSET 0x09

// Remote Test
#define CMD_RM_TEST 0xE6
// -- Request
// -- -- --
// -- Response
#define RM_TEST_RSP_LEN 202
// -- -- 8 byte time stamp
#define RT_TIME_STAMP_OFFSET 0x02
#define RT_TIME_STAMP_LEN 8
// -- -- 16 byte system bus log
#define RT_SYSBUS_LOG_OFFSET 0x0A
#define RT_SYSBUS_LOG_LEN 16
// -- -- 88 byte link layer log
#define RT_LINK_LOG_OFFSET 0x1A
#define RT_LINK_LOG_LEN 100
// -- -- 56 byte physical layer log
#define RT_PHY_LOG_OFFSET 0x7E
#define RT_PHY_LOG_LEN 56
// -- -- 20 byte hardware log
#define RT_HARDWARE_ST_OFFSET 0xB6
#define RT_HARDWARE_ST_LEN 20

// My Production Type
#define MY_PRD_ID PRD_DL_C
/** @} SystemBus_MSG_Constants*/
#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
/************************** Data Type Definitions ****************************/
/**
 * @struct PINGPONG
 * @brief PingPing Control
 */
typedef struct
{
	uint8_t latest; ///< latest available index
	uint8_t rBusy;	///< current read busy index
	uint8_t wBusy;	///< current write busy index
} PINGPONG;

/**
 * @struct SYSMONITOR
 * @brief System Monitor struct holds all the system log info
 */
typedef struct
{
	SYSMON_CONFIG config; ///< system monitor initialization parameters
	CIRC_QUEUE *sendQueue[L2_NET_SCALE];    ///< send queue
	CIRC_QUEUE *recvQueue[L2_NET_SCALE];    ///< receive queue
	PINGPONG sysInfoCtrl;
	SYSINFO sysInfo[2];
	PINGPONG linkLogCtrl;
	LINKLOG linkLog[2];
	PINGPONG phyLogCtrl;
	PHYLOG phyLog[2];
	PINGPONG hwStateCtrl;
	HWSTATE hwState[2];
} SYSMONITOR;
/************************** Function Prototypes ******************************/
static uint8_t GetWritable(PINGPONG ctrllor);
static uint8_t GetReadable(PINGPONG ctrllor);
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_GenRemoteTestRsp(SYSBUS *instPtr, uint8_t *msg);
/**************************** Type Definitions *******************************/
/************************** Variable Definitions *****************************/
// TODO: better approach?
extern L2_INST *linkInstPtr;
extern L2_CONFIG linkConfig;
extern SYSBUS *sysbusDescPtr;
static SYSMONITOR *sysMonitorInst;

inline static 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 static 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);
}

inline bool SystemBus_IsReqPending(SYSBUS *instPtr)
{
	ASSERT(instPtr != NULL);
	return (!isCircQueueEmpty(instPtr->rxQueue));
}

inline bool SystemBus_IsRespPending(SYSBUS *instPtr)
{
	ASSERT(instPtr != NULL);
	return (!isCircQueueEmpty(instPtr->txQueue));
}

inline bool SystemBus_IsTxBusy(SYSBUS *instPtr)
{
	ASSERT(instPtr != NULL);
	return (!instPtr->isTxDone);
}

inline bool SystemBus_IsRxBusy(SYSBUS *instPtr)
{
	ASSERT(instPtr != NULL);
	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 = 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 (L2_isStarted(linkInstPtr))
	{
		sysbusRsp->data[WD_ST_RET] = (hwStatsPtr->isSpOk && hwStatsPtr->isAntOk) ? ST_RET_OK : ST_RET_ERROR;
	}
	else
	{
		sysbusRsp->data[WD_ST_RET] = (hwStatsPtr->isSpOk) ? ST_RET_OK : ST_RET_ERROR;
	}
	// apply status and omit 2 bytes of overhead.
	memcpy(sysbusRsp->data + SP_INFO_OFFSET, &(hwStatsPtr->spFpgaVolt), ST_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;

	// Node mode control
	if (msg[WD_ID_CMD] >= 0 && msg[WD_ID_CMD] < linkConfig.netScale)
	{
		reconfig |= (msg[WD_ID_CMD] != linkConfig.myId);
		linkConfig.myType = (msg[WD_ID_CMD] == L2_HEAD_ID) ? L2_NODE_HEAD : L2_NODE_MEMBER;
		linkConfig.myId = msg[WD_ID_CMD];
	}
	else
	{
		// Ignore current command
		DBG_ERROR("Unexpected config command 0x%02X:0x%02X",
				  WD_ID_CMD, msg[WD_ID_CMD]);
		return EPARAM;
	}

	// Service Rate control
	switch (msg[WD_RATE_CMD])
	{
	case RATE_CMD_128K:
	{
		reconfig |= (linkConfig.serviceRate != L2_SVC_128K);
		linkConfig.serviceRate = L2_SVC_128K;
		break;
	}
	case RATE_CMD_512K:
	{
		reconfig |= (linkConfig.serviceRate != L2_SVC_512K);
		linkConfig.serviceRate = L2_SVC_512K;
		break;
	}
	case RATE_CMD_2M:
	{
		reconfig |= (linkConfig.serviceRate != L2_SVC_2M);
		linkConfig.serviceRate = L2_SVC_2M;
		break;
	}
	default:
	{
		// Ignore current command
		DBG_ERROR("Unexpected config command 0x%02X:0x%02X",
				  WD_RATE_CMD, msg[WD_RATE_CMD]);
		return EPARAM;
	}
	}

	// Routing table selection
	if (msg[WD_RTNE_CMD] != instPtr->sysbusLog.lastConfig[3])
	{
		switch (msg[WD_RTNE_CMD])
		{
		case 1:
		{
			reconfig |= (linkConfig.initTable != normalRoutTable1);
			reconfig |= (linkConfig.normalTable != normalRoutTable1);
			linkConfig.initTable = normalRoutTable1;
			linkConfig.normalTable = normalRoutTable1;
			break;
		}
		case 2:
		{
			reconfig |= (linkConfig.initTable != normalRoutTable2);
			reconfig |= (linkConfig.normalTable != normalRoutTable2);
			linkConfig.initTable = normalRoutTable2;
			linkConfig.normalTable = normalRoutTable2;
			break;
		}
		case 3:
		{
			reconfig |= (linkConfig.initTable != normalRoutTable3);
			reconfig |= (linkConfig.normalTable != normalRoutTable3);
			linkConfig.initTable = normalRoutTable3;
			linkConfig.normalTable = normalRoutTable3;
			break;
		}
		default:
		{
			// Ignore current command
			DBG_ERROR("Unexpected config command 0x%02X:0x%02X",
					WD_RTNE_CMD, msg[WD_RTNE_CMD]);
			return EIGNORE;
		}
		}
	}

	// Enable control
	if (msg[WD_EN_CMD] == EN_CMD_ENABLE)
	{
		if (reconfig || !L2_isStarted(linkInstPtr))
		{
			Linklayer_Reconfig(linkInstPtr, &linkConfig);
		}
		Linklayer_Start(linkInstPtr);
	}
	else if (msg[WD_EN_CMD] == EN_CMD_SILENT)
	{
		Linklayer_Halt(linkInstPtr);
		// configure phy to generate remote test
		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_FRAME_ADV_24MS, NULL);
	}
	else
	{
		// Ignore current command
		DBG_ERROR("Unexpected config command 0x%02X:0x%02X",
				  WD_EN_CMD, msg[WD_EN_CMD]);
		return EIGNORE;
	}

	instPtr->sysbusLog.lastConfig[0] = msg[WD_EN_CMD];
	instPtr->sysbusLog.lastConfig[1] = msg[WD_ID_CMD];
	instPtr->sysbusLog.lastConfig[2] = msg[WD_RATE_CMD];
	instPtr->sysbusLog.lastConfig[3] = msg[WD_RTNE_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 ECHO TEST 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 receive call command response
 */
int32_t SystemBus_GenRecvRsp(SYSBUS *instPtr, SYSBUS_SVC_PLD *payload)
{
	ASSERT(instPtr != NULL);
	ASSERT(payload != NULL);

	SYSBUS_UART_FRAME *sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
	if (sysbusRsp == NULL)
	{
		DBG_ERROR("Failed to allocate memory for Receive Response!\n");
		return ENOMEM;
	}

	sysbusRsp->len = payload->dataLen + RECV_DATA_OFFSET;

	sysbusRsp->data[WD_CMD] = CMD_RECV_DATA;
	sysbusRsp->data[WD_PRD] = MY_PRD_ID;
	memcpy(sysbusRsp->data + RECV_DATA_ID_OFFSET, payload, sizeof(SYSBUS_SVC_PLD));

	if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false)
	{
		free(sysbusRsp);
		DBG_WARNING("SystemBus::TX Queue overflowed!\n");
		return EQUEUE;
	}
	instPtr->sysbusLog.recvCnt++;
	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_RM_TEST)
	{
		sysbusRsp->len = RM_TEST_RSP_LEN;
		memset(sysbusRsp->data, 0x00, RM_TEST_RSP_LEN);
		sysbusRsp->data[WD_CMD] = CMD_RM_TEST;
		sysbusRsp->data[WD_PRD] = MY_PRD_ID;

		memcpy(sysbusRsp->data + RT_TIME_STAMP_OFFSET, &time, RT_TIME_STAMP_LEN);
		SysMonitor_GetLinklayerLog(&(instPtr->linkLog));
		SysMonitor_GetPhysicalLayerLog(&(instPtr->phyLog));
		SysMonitor_GetHardwareStatus(&(instPtr->hwState));
		memcpy(sysbusRsp->data + RT_SYSBUS_LOG_OFFSET, &(instPtr->sysbusLog), RT_SYSBUS_LOG_LEN);
		memcpy(sysbusRsp->data + RT_LINK_LOG_OFFSET, &(instPtr->linkLog), RT_LINK_LOG_LEN);
		memcpy(sysbusRsp->data + RT_PHY_LOG_OFFSET, &(instPtr->phyLog), RT_PHY_LOG_LEN);
		memcpy(sysbusRsp->data + RT_HARDWARE_ST_OFFSET, &(instPtr->hwState.spFpgaVolt), RT_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   A handler function. This handler  evaluate the message from system
 * 			bus receive queue, and make appropriate response.
 *
 * @param   instPtr A pointer to System Bus instance.
 *
 * @return  None
 *
 ******************************************************************************/
int32_t SystemBus_HandleCmd(SYSBUS *instPtr)
{
	ASSERT(instPtr != NULL);
	int32_t Status;
	SYSBUS_UART_FRAME *sysbusReq;
	uint8_t *msg;
	if (CircQueuePop(instPtr->rxQueue, (void **)(&sysbusReq)) == false)
	{
		DBG_WARNING("SystemBus::Nothing to be handled.\n");
		return EIGNORE;
	}
	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 (msg[WD_CMD])
	{
	case CMD_SELF_TEST:
	{
		Status = SystemBus_GenSelfTestRsp(instPtr);
		break;
	}
	case CMD_ECHO:
	{
		Status = SystemBus_Echo(instPtr, sysbusReq);
		break;
	}
	case CMD_CONFIG:
	{
		Status = SystemBus_SystemConfig(instPtr, msg);
		break;
	}
	case CMD_SYS_INFO:
	{
		SysMonitor_UpdateSysinfo((SYSINFO *)(msg + SYS_INFO_OFFSET));
		Status = SUCCESS;
		break;
	}
	case CMD_SEND_DATA:
	{
		Status = SysMonitor_UpdateSendPayload((SYSBUS_SVC_PLD *)(msg + SEND_DATA_ID_OFFSET));
		if (Status == SUCCESS)
		{
			instPtr->sysbusLog.sendCnt++;
		}
		break;
	}
	case CMD_RM_TEST:
	{
		Status = SystemBus_GenRemoteTestRsp(instPtr, msg);
		break;
	}
	default:
	{
		DBG_ERROR("SystemBus::Unknown command code!\n");
		Status = FAILURE;
		break;
	}
	}
	free(sysbusReq);
	return Status;
}

/*****************************************************************************/
/**
 * @brief   This function send a repose frame from system bus tx queue.
 *
 * @param   instPtr A pointer to System Bus instance.
 *
 * @return  None
 *
 ******************************************************************************/
int32_t SystemBus_MakeResp(SYSBUS *instPtr)
{
	ASSERT(instPtr != NULL);
	int32_t Status;
	SYSBUS_UART_FRAME *sysbusRsp;
	if (CircQueuePop(instPtr->txQueue, (void **)(&sysbusRsp)) == false)
	{
		DBG_WARNING("SystemBus::Nothing to be sent.\n");
		return EIGNORE;
	}
	instPtr->isTxDone = false; // A transmission procedure is started.
	Status = SysbusUart_Send(instPtr->uartInst, sysbusRsp);
	free(sysbusRsp);
	return Status;
}

/** @} */

/*****************************************************************************/
/**
 * @brief   Initialize System Bus instance.
 *
 * @param   InstPtr A double pointer to System Bus instance.
 * @param   config  A pointer to System Bus configurations.
 *
 * @return  Error code.
 *
 ******************************************************************************/
int32_t SystemBus_Init(SYSBUS **instPtr, SYSBUS_CONFIG *config)
{
	ASSERT(instPtr != NULL);
	ASSERT(config != NULL);
	SYSBUS_UART_CONFIG uartConfig;
	int32_t Status;
	(*instPtr) = malloc(sizeof(SYSBUS));
	if ((*instPtr) == NULL)
	{
		return ENOMEM;
	}
	(*instPtr)->rxQueue = NULL;
	(*instPtr)->txQueue = NULL;
	(*instPtr)->uartInst = NULL;
	memcpy(&((*instPtr)->config), config, sizeof(SYSBUS_CONFIG));
	memset(&((*instPtr)->sysbusLog), 0x00, sizeof(SYSBUSLOG));
	memset(&((*instPtr)->linkLog), 0x00, sizeof(LINKLOG));
	memset(&((*instPtr)->phyLog), 0x00, sizeof(PHYLOG));
	memset(&((*instPtr)->hwState), 0x00, sizeof(HWSTATE));
	uartConfig.baseAddr = config->baseAddr;
	Status = SysbusUart_Init(&((*instPtr)->uartInst), &uartConfig);
	if (Status != SUCCESS)
	{
		SystemBus_Destroy(*instPtr);
		return ENOMEM;
	}
	if (CircQueueCreate(&((*instPtr)->txQueue), (*instPtr)->config.txQueueMax) == false)
	{
		SystemBus_Destroy(*instPtr);
		return ENOMEM;
	}
	if (CircQueueCreate(&((*instPtr)->rxQueue), (*instPtr)->config.rxQueueMax) == false)
	{
		SystemBus_Destroy(*instPtr);
		return ENOMEM;
	}
	(*instPtr)->isRxDone = false; // nothing received.
	(*instPtr)->isTxDone = true;  // nothing to be sent.
	return SUCCESS;
}


/*****************************************************************************/
/**
 * @brief   Destroy System Bus instance.
 *
 * @param   InstPtr A pointer to System Bus instance.
 *
 * @return  None.
 *
 ******************************************************************************/
void SystemBus_Destroy(SYSBUS *instPtr)
{
	if (instPtr == NULL)
	{
		return;
	}

	free(instPtr->uartInst);
	free(instPtr->txQueue);
	free(instPtr->rxQueue);
	free(instPtr);
}

/*****************************************************************************/
/**
 * @brief   This function handles the systembus tx irq events.
 *
 * @param   instPtr A pointer to System Bus instance.
 *
 * @return  None
 *
 ******************************************************************************/
int32_t SystemBus_TxIntrHandler(SYSBUS *instPtr)
{
	ASSERT(instPtr != NULL);
	// nothing special...
	instPtr->isTxDone = true; // A transmission procedure finished.
	return SUCCESS;
}

/*****************************************************************************/
/**
 * @brief   This function handles the systembus rx irq events.
 *
 * @param   instPtr A pointer to System Bus instance.
 *
 * @return  None
 *
 ******************************************************************************/
int32_t SystemBus_RxIntrHandler(SYSBUS *instPtr)
{
	ASSERT(instPtr != NULL);
	instPtr->isRxDone = true; // whether reception successes or not, receiving procedure ended.
	int32_t Status;
	SYSBUS_UART_FRAME *sysbusReq = malloc(sizeof(SYSBUS_UART_FRAME));
	if (sysbusReq == NULL)
	{
		return ENOMEM;
	}
	Status = SysbusUart_Recv(instPtr->uartInst, sysbusReq);
	if (Status != SUCCESS)
	{
		free(sysbusReq);
		return Status;
	}
	if (CircQueuePush(instPtr->rxQueue, (void *)sysbusReq) == false)
	{
		DBG_ERROR("SystemBus::RX Queue overflowed!\n");
		free(sysbusReq);
		return EQUEUE;
	}
	instPtr->isRxDone = false; // another reception is undergoing.
	return SUCCESS;
}

/**
 * @brief Initialize System monitor
 *
 * @return SUCCESS
 */
int32_t SysMonitor_Init(SYSMON_CONFIG *config)
{
	ASSERT(config != NULL);

	sysMonitorInst = calloc(1, sizeof(SYSMONITOR));
	if (sysMonitorInst == NULL)
	{
		return ENOMEM;
	}

	memcpy(&(sysMonitorInst->config), config, sizeof(SYSMON_CONFIG));
	memset(&(sysMonitorInst->sysInfoCtrl), BUFF_NONE, sizeof(PINGPONG));
	memset(&(sysMonitorInst->linkLogCtrl), BUFF_NONE, sizeof(PINGPONG));
	memset(&(sysMonitorInst->phyLogCtrl), BUFF_NONE, sizeof(PINGPONG));
	memset(&(sysMonitorInst->hwStateCtrl), BUFF_NONE, sizeof(PINGPONG));

	for (uint8_t ii = 0; ii < L2_NET_SCALE; ii++)
	{
		sysMonitorInst->sendQueue[ii] = NULL;
		sysMonitorInst->recvQueue[ii] = NULL;
		if (CircQueueCreate(&(sysMonitorInst->sendQueue[ii]),
				sysMonitorInst->config.sendQueueMax) == false)
		{
			SysMonitor_Destroy();
			return ENOMEM;
		}
		if (CircQueueCreate(&(sysMonitorInst->recvQueue[ii]),
				sysMonitorInst->config.recvQueueMax) == false)
		{
			SysMonitor_Destroy();
			return ENOMEM;
		}
	}

	return SUCCESS;
}

/**
 * @brief Destroy system monitor release all allocated memory
 */
void SysMonitor_Destroy(void)
{
	free(sysMonitorInst);
}

/**
 * @name SystemInfo_API
 * @{
 */

/**
 * @brief Update system info.
 *
 * @param info
 */
void SysMonitor_UpdateSysinfo(const SYSINFO *info)
{
	ASSERT(info != NULL);
	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;
}

/**
 * @brief Get system info.
 *
 * @param info
 */
void SysMonitor_GetSysinfo(SYSINFO *info)
{
	ASSERT(sysMonitorInst != NULL);
	ASSERT(info != 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;
}
/** @} SystemInfo_API*/

/**
 * @name ServicePayload_API
 * @{
 */

/**
 * @brief Update send payload by
 *
 * @param payload
 */
int32_t SysMonitor_UpdateSendPayload(SYSBUS_SVC_PLD *payload)
{
	ASSERT(sysMonitorInst != NULL);
	ASSERT(payload != NULL);

	// validity check
	if (payload->dataLen > SYSBUS_PLD_MAX)
	{
		DBG_ERROR("Payload length %d exceeds valid range.", payload->dataLen);
		return EPARAM;
	}
	if (payload->nodeId < L2_NODE_NONE || payload->nodeId > L2_NET_SCALE)
	{
		DBG_ERROR("Node ID %d exceeds valid range.", payload->nodeId);
		return EPARAM;
	}

	// push message into send queue
	SYSBUS_SVC_PLD *payloadCopy = malloc(sizeof(SYSBUS_SVC_PLD));
	if (payloadCopy == NULL)
	{
		DBG_ERROR("Allocate memory for send payload failed.\n");
		return ENOMEM;
	}
	memcpy(payloadCopy, payload, sizeof(SYSBUS_SVC_PLD));
	// valid id starts from 0x01
	if (CircQueuePush(sysMonitorInst->sendQueue[payload->nodeId-1], (void *)payloadCopy) == false)
	{
		DBG_WARNING("SystemMonitor::SEND Queue overflowed!\n");
		free(payloadCopy);
		return EQUEUE;
	}

	return SUCCESS;
}

/**
 * @brief Get the latest send payload.
 *
 * @param payload
 */
void SysMonitor_GetSendPayload(SYSBUS_SVC_PLD **payload, uint8_t dstId)
{
	ASSERT(sysMonitorInst != NULL);
	ASSERT(payload != NULL);

	// valid id starts from 0x01
	if (CircQueuePop(sysMonitorInst->sendQueue[dstId-1], (void **)payload) == false)
	{
		*payload = NULL;
		DBG_INFO("SystemMonitor::Empty sendQueue, nothing need sent.\n");
	}
}

/**
 * @brief Update receive payload.
 *
 * @param payload
 */
int32_t SysMonitor_GenRecvRsp(SYSBUS_SVC_PLD *payload)
{
	ASSERT(payload != NULL);
	int32_t Status;

	if (payload->dataId == DATA_ID_NONE || payload->dataLen > SYSBUS_PLD_MAX)
	{
		DBG_INFO("SystemMonitor::Drop invalid message.\n");
		return EIGNORE;
	}

	Status = SystemBus_GenRecvRsp(sysbusDescPtr, payload);

	return Status;
}
/** @} ServicePayload_API*/

/**
 * @name Linklayer_Log_API
 * @{
 */

/**
 * @brief Update link layer log
 *
 * @param log
 */
void SysMonitor_UpdateLinklayerLog(const LINKLOG *log)
{
	ASSERT(sysMonitorInst != NULL);
	ASSERT(log != NULL);
	uint8_t buffIndx;
	buffIndx = GetWritable(sysMonitorInst->linkLogCtrl);
	sysMonitorInst->linkLogCtrl.wBusy = buffIndx;
	memcpy(&(sysMonitorInst->linkLog[buffIndx]), log, sizeof(LINKLOG));
	sysMonitorInst->linkLogCtrl.latest = buffIndx;
	sysMonitorInst->linkLogCtrl.wBusy = BUFF_NONE;
}

/**
 * @brief Get the latest link layer log
 *
 * @param log
 */
void SysMonitor_GetLinklayerLog(LINKLOG *log)
{
	ASSERT(sysMonitorInst != NULL);
	ASSERT(log != NULL);
	uint8_t buffIndx, buffIndex_old;
	buffIndex_old = sysMonitorInst->linkLogCtrl.rBusy;
	if (buffIndex_old != BUFF_NONE)
		buffIndx = buffIndex_old; // Keep it busy
	else
		buffIndx = GetReadable(sysMonitorInst->linkLogCtrl);
	sysMonitorInst->linkLogCtrl.rBusy = buffIndx;
	memcpy(log, &(sysMonitorInst->linkLog[buffIndx]), sizeof(LINKLOG));
	sysMonitorInst->linkLogCtrl.rBusy = buffIndex_old;
}
/** @} Linklayer_Log_API*/

/**
 * @name PhysicalLayer_Log_API
 * @{
 */

/**
 * @brief Update physical layer log
 *
 * @param log
 */
void SysMonitor_UpdatePhysicalLayerLog(const PHYLOG *log)
{
	ASSERT(sysMonitorInst != NULL);
	ASSERT(log != NULL);
	uint8_t buffIndx;
	buffIndx = GetWritable(sysMonitorInst->phyLogCtrl);
	sysMonitorInst->phyLogCtrl.wBusy = buffIndx;
	memcpy(&(sysMonitorInst->phyLog[buffIndx]), log, sizeof(PHYLOG));
	sysMonitorInst->phyLogCtrl.latest = buffIndx;
	sysMonitorInst->phyLogCtrl.wBusy = BUFF_NONE;
}

/**
 * @brief Get the latest physical layer log
 *
 * @param log
 */
void SysMonitor_GetPhysicalLayerLog(PHYLOG *log)
{
	ASSERT(sysMonitorInst != NULL);
	ASSERT(log != NULL);
	uint8_t buffIndx, buffIndex_old;
	buffIndex_old = sysMonitorInst->phyLogCtrl.rBusy;
	if (buffIndex_old != BUFF_NONE)
		buffIndx = buffIndex_old; // Keep it busy
	else
		buffIndx = GetReadable(sysMonitorInst->phyLogCtrl);
	sysMonitorInst->phyLogCtrl.rBusy = buffIndx;
	memcpy(log, &(sysMonitorInst->phyLog[buffIndx]), sizeof(PHYLOG));
	sysMonitorInst->phyLogCtrl.rBusy = buffIndex_old;
}
/** @} */

/**
 * @name HardwareState_Log_API
 * @{
 */

/**
 * @brief Log hardware running status.
 *
 * @param state
 */
void SysMonitor_UpdateHardwareStatus(const HWSTATE *state)
{
	ASSERT(sysMonitorInst != NULL);
	ASSERT(state != 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;
}

/**
 * @brief Get the latest hardware running status.
 *
 * @param state
 */
void SysMonitor_GetHardwareStatus(HWSTATE *state)
{
	ASSERT(sysMonitorInst != NULL);
	ASSERT(state != 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;
}


/** @} SystemBus*/