0xha0/datalink_repo
2
2
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

formatted.

Browse Source
This commit is contained in:
Gatsby Van
2023-12-27 14:59:41 +08:00
parent fa5ff861d5
commit 677b8b4af5
27 changed files with 3211 additions and 2274 deletions
Download Patch File Download Diff File Expand all files Collapse all files
df_datalink/src/main.c
+110 -65
View File
@@ -47,23 +47,19 @@ void Systembus_RxIntrHandler(void *context);
/************************** Constant Definitions *****************************/ /************************** Constant Definitions *****************************/
struct callback_desc phy_callback_desc = { struct callback_desc phy_callback_desc = {
.callback = Phy_IntrHandler, .callback = Phy_IntrHandler,
.ctx = NULL .ctx = NULL};
};
struct callback_desc sysbus_rx_callback_desc = { struct callback_desc sysbus_rx_callback_desc = {
.callback = Systembus_RxIntrHandler, .callback = Systembus_RxIntrHandler,
.ctx = NULL .ctx = NULL};
};
struct callback_desc sysbus_tx_callback_desc = { struct callback_desc sysbus_tx_callback_desc = {
.callback = Systembus_RxIntrHandler, .callback = Systembus_RxIntrHandler,
.ctx = NULL .ctx = NULL};
};
struct callback_desc scutm_callback_desc = { struct callback_desc scutm_callback_desc = {
.callback = ScuTimer_IntrHandler, .callback = ScuTimer_IntrHandler,
.ctx = NULL .ctx = NULL};
};
static PHY_CONFIG phyConfig = { static PHY_CONFIG phyConfig = {
.devId = 0, .devId = 0,
@@ -71,8 +67,7 @@ static PHY_CONFIG phyConfig = {
.antBramId = PHY_ANTCONFIG_BRAM_DEVICE_ID, .antBramId = PHY_ANTCONFIG_BRAM_DEVICE_ID,
.txBramId = PHY_TXBUFF_BRAM_DEVICE_ID, .txBramId = PHY_TXBUFF_BRAM_DEVICE_ID,
.rxBramId = PHY_RXBUFF_BRAM_DEVICE_ID, .rxBramId = PHY_RXBUFF_BRAM_DEVICE_ID,
.hwXadcId = PHY_HW_XADC_DEVICE_ID .hwXadcId = PHY_HW_XADC_DEVICE_ID};
};
static L2_CONFIG linkConfig = { static L2_CONFIG linkConfig = {
.myType = L2_HEAD_NODE, .myType = L2_HEAD_NODE,
@@ -93,8 +88,7 @@ static L2_CONFIG linkConfig = {
static SYSBUS_CONFIG sysbusConfig = { static SYSBUS_CONFIG sysbusConfig = {
.baseAddr = SYSBUS_BASE_ADDR, .baseAddr = SYSBUS_BASE_ADDR,
.txQueueMax = 16, .txQueueMax = 16,
.rxQueueMax = 16 .rxQueueMax = 16};
};
/*********************** Driver Instance Definitions **************************/ /*********************** Driver Instance Definitions **************************/
static struct irq_ctrl_desc *intcDescPtr; // General interrupt controller descriptor static struct irq_ctrl_desc *intcDescPtr; // General interrupt controller descriptor
static struct timer_desc *logTmPtr; static struct timer_desc *logTmPtr;
@@ -119,12 +113,14 @@ static struct ad9361_reg_dev *ad9361_2_reg_inst;
* FAILURE if initialization failed. * FAILURE if initialization failed.
* *
*****************************************************************************/ *****************************************************************************/
int32_t SetupInterruptSystem(struct irq_ctrl_desc* descPtr) { int32_t SetupInterruptSystem(struct irq_ctrl_desc *descPtr)
{
int32_t Status; int32_t Status;
struct xil_irq_desc *xil_dev; struct xil_irq_desc *xil_dev;
Status = irq_ctrl_init(&descPtr, &intc_init_param); Status = irq_ctrl_init(&descPtr, &intc_init_param);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("Intc Init FAILED, ErrorCode: %ld.\n", Status); DBG_ERROR("Intc Init FAILED, ErrorCode: %ld.\n", Status);
return Status; return Status;
} }
@@ -137,57 +133,66 @@ int32_t SetupInterruptSystem(struct irq_ctrl_desc* descPtr) {
/* Initialize physical layer interrupt. */ /* Initialize physical layer interrupt. */
Status = irq_trigger_level_set(descPtr, PHY_INTR_ID, IRQ_EDGE_RISING); Status = irq_trigger_level_set(descPtr, PHY_INTR_ID, IRQ_EDGE_RISING);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("PHY_INTR trigger level set FAILED, ErrorCode: %ld.\n", Status); DBG_ERROR("PHY_INTR trigger level set FAILED, ErrorCode: %ld.\n", Status);
return Status; return Status;
} }
// Set highest interrupt priority // Set highest interrupt priority
Status = irq_set_priority(descPtr, PHY_INTR_ID, 0x00); Status = irq_set_priority(descPtr, PHY_INTR_ID, 0x00);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("PHY_INTR priority level set FAILED, ErrorCode: %ld.\n", Status); DBG_ERROR("PHY_INTR priority level set FAILED, ErrorCode: %ld.\n", Status);
return Status; return Status;
} }
// Connect the interrupt service routine isr1 to the interrupt controller // Connect the interrupt service routine isr1 to the interrupt controller
Status = irq_register_callback(descPtr, PHY_INTR_ID, &phy_callback_desc); Status = irq_register_callback(descPtr, PHY_INTR_ID, &phy_callback_desc);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("PHY_INTR register callback FAILED, ErrorCode: %ld.\n", Status); DBG_ERROR("PHY_INTR register callback FAILED, ErrorCode: %ld.\n", Status);
return Status; return Status;
} }
/* Initialize system bus interrupt. */ /* Initialize system bus interrupt. */
Status = irq_trigger_level_set(descPtr, SYSBUS_TX_INTR_ID, IRQ_EDGE_RISING); Status = irq_trigger_level_set(descPtr, SYSBUS_TX_INTR_ID, IRQ_EDGE_RISING);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("PHY_INTR trigger level set FAILED, ErrorCode: %ld.\n", Status); DBG_ERROR("PHY_INTR trigger level set FAILED, ErrorCode: %ld.\n", Status);
return Status; return Status;
} }
// Set interrupt priority // Set interrupt priority
Status = irq_set_priority(descPtr, SYSBUS_TX_INTR_ID, 0x08); Status = irq_set_priority(descPtr, SYSBUS_TX_INTR_ID, 0x08);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("PHY_INTR priority level set FAILED, ErrorCode: %ld.\n", Status); DBG_ERROR("PHY_INTR priority level set FAILED, ErrorCode: %ld.\n", Status);
return Status; return Status;
} }
// Connect the interrupt service routine isr1 to the interrupt controller // Connect the interrupt service routine isr1 to the interrupt controller
Status = irq_register_callback(descPtr, SYSBUS_TX_INTR_ID, &sysbus_tx_callback_desc); Status = irq_register_callback(descPtr, SYSBUS_TX_INTR_ID, &sysbus_tx_callback_desc);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("PHY_INTR register callback FAILED, ErrorCode: %ld.\n", Status); DBG_ERROR("PHY_INTR register callback FAILED, ErrorCode: %ld.\n", Status);
return Status; return Status;
} }
Status = irq_trigger_level_set(descPtr, SYSBUS_RX_INTR_ID, IRQ_EDGE_RISING); Status = irq_trigger_level_set(descPtr, SYSBUS_RX_INTR_ID, IRQ_EDGE_RISING);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("PHY_INTR trigger level set FAILED, ErrorCode: %ld.\n", Status); DBG_ERROR("PHY_INTR trigger level set FAILED, ErrorCode: %ld.\n", Status);
return Status; return Status;
} }
// Set interrupt priority // Set interrupt priority
Status = irq_set_priority(descPtr, SYSBUS_RX_INTR_ID, 0x10); Status = irq_set_priority(descPtr, SYSBUS_RX_INTR_ID, 0x10);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("PHY_INTR priority level set FAILED, ErrorCode: %ld.\n", Status); DBG_ERROR("PHY_INTR priority level set FAILED, ErrorCode: %ld.\n", Status);
return Status; return Status;
} }
// Connect the interrupt service routine isr1 to the interrupt controller // Connect the interrupt service routine isr1 to the interrupt controller
Status = irq_register_callback(descPtr, SYSBUS_RX_INTR_ID, &sysbus_rx_callback_desc); Status = irq_register_callback(descPtr, SYSBUS_RX_INTR_ID, &sysbus_rx_callback_desc);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("PHY_INTR register callback FAILED, ErrorCode: %ld.\n", Status); DBG_ERROR("PHY_INTR register callback FAILED, ErrorCode: %ld.\n", Status);
return Status; return Status;
} }
@@ -195,7 +200,8 @@ int32_t SetupInterruptSystem(struct irq_ctrl_desc* descPtr) {
/* Initialize Scu timer interrupt. */ /* Initialize Scu timer interrupt. */
// Connect the interrupt service routine isr1 to the interrupt controller // Connect the interrupt service routine isr1 to the interrupt controller
Status = irq_register_callback(descPtr, SCUTIMER_INTR_ID, &scutm_callback_desc); Status = irq_register_callback(descPtr, SCUTIMER_INTR_ID, &scutm_callback_desc);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("PHY_INTR register callback FAILED, ErrorCode: %ld.\n", Status); DBG_ERROR("PHY_INTR register callback FAILED, ErrorCode: %ld.\n", Status);
return Status; return Status;
} }
@@ -208,7 +214,8 @@ int32_t SetupInterruptSystem(struct irq_ctrl_desc* descPtr) {
// Enable Exceptions // Enable Exceptions
Status = irq_global_enable(descPtr); Status = irq_global_enable(descPtr);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("IRQ global enable FAILED, ErrorCode: %ld.\n", Status); DBG_ERROR("IRQ global enable FAILED, ErrorCode: %ld.\n", Status);
return Status; return Status;
} }
@@ -225,10 +232,12 @@ int32_t SetupInterruptSystem(struct irq_ctrl_desc* descPtr) {
* @return None. * @return None.
* *
*****************************************************************************/ *****************************************************************************/
void Phy_IntrHandler(void *context) { void Phy_IntrHandler(void *context)
{
int32_t Status; int32_t Status;
Status = Linklayer_Proc(linkInstPtr); Status = Linklayer_Proc(linkInstPtr);
if(Status != SUCCESS){ if (Status != SUCCESS)
{
DBG_ERROR("Link layer runs failed, with error code: %ld\n", Status); DBG_ERROR("Link layer runs failed, with error code: %ld\n", Status);
_nop_(); _nop_();
} }
@@ -243,10 +252,12 @@ void Phy_IntrHandler(void *context) {
* @return None. * @return None.
* *
******************************************************************************/ ******************************************************************************/
void Systembus_RxIntrHandler(void *context) { void Systembus_RxIntrHandler(void *context)
{
int32_t Status; int32_t Status;
Status = SystemBus_RxIntrHandler(sysbusDescPtr); Status = SystemBus_RxIntrHandler(sysbusDescPtr);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("SystemBus_RxIntrHandler exit with error code %ld,\n", Status); DBG_ERROR("SystemBus_RxIntrHandler exit with error code %ld,\n", Status);
} }
} }
@@ -260,10 +271,12 @@ void Systembus_RxIntrHandler(void *context) {
* @return None. * @return None.
* *
******************************************************************************/ ******************************************************************************/
void Systembus_TxIntrHandler(void *context) { void Systembus_TxIntrHandler(void *context)
{
int32_t Status; int32_t Status;
Status = SystemBus_TxIntrHandler(sysbusDescPtr); Status = SystemBus_TxIntrHandler(sysbusDescPtr);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("SystemBus_TxIntrHandler exit with error code %ld,\n", Status); DBG_ERROR("SystemBus_TxIntrHandler exit with error code %ld,\n", Status);
} }
} }
@@ -277,20 +290,22 @@ void Systembus_TxIntrHandler(void *context) {
* @return None. * @return None.
* *
******************************************************************************/ ******************************************************************************/
void ScuTimer_IntrHandler(void *context) { void ScuTimer_IntrHandler(void *context)
{
struct xil_timer_desc *xdesc; struct xil_timer_desc *xdesc;
xdesc = (struct xil_timer_desc *)(logTmPtr->extra); xdesc = (struct xil_timer_desc *)(logTmPtr->extra);
if (XScuTimer_IsExpired((XScuTimer*)xdesc->instance)) { if (XScuTimer_IsExpired((XScuTimer *)xdesc->instance))
{
// Update log // Update log
Phy_UpdateHwSate(phyInstPtr); Phy_UpdateHwSate(phyInstPtr);
XScuTimer_ClearInterruptStatus((XScuTimer *)xdesc->instance); XScuTimer_ClearInterruptStatus((XScuTimer *)xdesc->instance);
} }
else{ else
{
DBG_WARNING("UNEXPECTED CALL of ScuTimer_IntrHandler\n"); DBG_WARNING("UNEXPECTED CALL of ScuTimer_IntrHandler\n");
} }
} }
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Hardware_SwitchFreq * @brief Hardware_SwitchFreq
@@ -301,13 +316,16 @@ void ScuTimer_IntrHandler(void *context) {
* @return None. * @return None.
* *
******************************************************************************/ ******************************************************************************/
int Hardware_SwitchFreq(struct ad9361_reg_dev *dev, ad9361TxFreq freq){ int Hardware_SwitchFreq(struct ad9361_reg_dev *dev, ad9361TxFreq freq)
{
int32_t status = 0; int32_t status = 0;
uint8_t value = 0; uint8_t value = 0;
int32_t time_out_cnt = 0; int32_t time_out_cnt = 0;
switch(freq){ switch (freq)
case TX_FREQ_125: { {
case TX_FREQ_125:
{
//************************************************************ //************************************************************
// Setup Tx Frequency-Dependent Syntheisizer Registers // Setup Tx Frequency-Dependent Syntheisizer Registers
//************************************************************ //************************************************************
@@ -352,7 +370,8 @@ int Hardware_SwitchFreq(struct ad9361_reg_dev *dev, ad9361TxFreq freq){
break; break;
} }
case TX_FREQ_126: { case TX_FREQ_126:
{
//************************************************************ //************************************************************
// Setup Tx Frequency-Dependent Syntheisizer Registers // Setup Tx Frequency-Dependent Syntheisizer Registers
//************************************************************ //************************************************************
@@ -397,7 +416,8 @@ int Hardware_SwitchFreq(struct ad9361_reg_dev *dev, ad9361TxFreq freq){
break; break;
} }
case TX_FREQ_4125: { case TX_FREQ_4125:
{
//************************************************************ //************************************************************
// Setup Tx Frequency-Dependent Syntheisizer Registers // Setup Tx Frequency-Dependent Syntheisizer Registers
//************************************************************ //************************************************************
@@ -445,7 +465,8 @@ int Hardware_SwitchFreq(struct ad9361_reg_dev *dev, ad9361TxFreq freq){
// Wait for cal to complete (Done when 0x016[4]==0) // Wait for cal to complete (Done when 0x016[4]==0)
time_out_cnt = 0; time_out_cnt = 0;
do{ do
{
status |= ad9361_reg_read(dev, 0x0016, &value); status |= ad9361_reg_read(dev, 0x0016, &value);
mdelay(1); // Wait 1ms mdelay(1); // Wait 1ms
time_out_cnt++; time_out_cnt++;
@@ -464,7 +485,8 @@ int Hardware_SwitchFreq(struct ad9361_reg_dev *dev, ad9361TxFreq freq){
* @return None. * @return None.
* *
******************************************************************************/ ******************************************************************************/
int Hardware_Init(void) { int Hardware_Init(void)
{
int32_t Status; int32_t Status;
XGpioPs Gpio; XGpioPs Gpio;
XGpioPs_Config *ConfigPtr; XGpioPs_Config *ConfigPtr;
@@ -479,7 +501,8 @@ int Hardware_Init(void) {
/****************** HMC7044 Configuration *****************/ /****************** HMC7044 Configuration *****************/
/**********************************************************/ /**********************************************************/
Status = hmc7044_init(&hmc7044_inst, &hmc7044_default_init_param); Status = hmc7044_init(&hmc7044_inst, &hmc7044_default_init_param);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("hmc7044_init() error: %ld\n", Status); DBG_ERROR("hmc7044_init() error: %ld\n", Status);
return FAILURE; return FAILURE;
} }
@@ -587,12 +610,14 @@ int Hardware_Init(void) {
// read calibration status // read calibration status
int32_t time_out_cnt = 0; int32_t time_out_cnt = 0;
do{ do
{
mdelay(1); // Wait 1ms mdelay(1); // Wait 1ms
time_out_cnt++; time_out_cnt++;
} while (((XGpioPs_ReadPin(&Gpio, AD9361_1_CAL_DONE) == 0) && time_out_cnt <= AD9361_TIMEOUT_TH)); } while (((XGpioPs_ReadPin(&Gpio, AD9361_1_CAL_DONE) == 0) && time_out_cnt <= AD9361_TIMEOUT_TH));
if (time_out_cnt == AD9361_TIMEOUT_TH){ if (time_out_cnt == AD9361_TIMEOUT_TH)
{
return FAILURE; return FAILURE;
} }
@@ -603,12 +628,14 @@ int Hardware_Init(void) {
// read calibration status // read calibration status
time_out_cnt = 0; time_out_cnt = 0;
do{ do
{
mdelay(1); // Wait 1ms mdelay(1); // Wait 1ms
time_out_cnt++; time_out_cnt++;
} while (((XGpioPs_ReadPin(&Gpio, AD9361_2_CAL_DONE) == 0) && time_out_cnt <= AD9361_TIMEOUT_TH)); } while (((XGpioPs_ReadPin(&Gpio, AD9361_2_CAL_DONE) == 0) && time_out_cnt <= AD9361_TIMEOUT_TH));
if (time_out_cnt == AD9361_TIMEOUT_TH){ if (time_out_cnt == AD9361_TIMEOUT_TH)
{
return FAILURE; return FAILURE;
} }
@@ -638,35 +665,42 @@ int Hardware_Init(void) {
* @return * @return
* *
******************************************************************************/ ******************************************************************************/
int main(void) { int main(void)
{
int Status; int Status;
DBG_PRINT("Application Version: %2d.%2d.%4d.\n", _APP_VER_MAIN, _APP_VER_MINOR, _APP_VER_PATCH); DBG_PRINT("Application Version: %2d.%2d.%4d.\n", _APP_VER_MAIN, _APP_VER_MINOR, _APP_VER_PATCH);
/* System Monitor Initialize */ /* System Monitor Initialize */
Status = SysMonitor_Init(); Status = SysMonitor_Init();
if (Status == SUCCESS) { if (Status == SUCCESS)
{
DBG_PRINT("System monitor Init SUCCESS! \n"); DBG_PRINT("System monitor Init SUCCESS! \n");
} }
else { else
{
DBG_PRINT("System monitor Init FAILED! \n"); DBG_PRINT("System monitor Init FAILED! \n");
return Status; return Status;
} }
Status = Hardware_Init(); Status = Hardware_Init();
if (Status == SUCCESS) { if (Status == SUCCESS)
{
DBG_PRINT("Hardware Init SUCCESS! \n"); DBG_PRINT("Hardware Init SUCCESS! \n");
} }
else { else
{
DBG_PRINT("Hardware Init FAILED! \n"); DBG_PRINT("Hardware Init FAILED! \n");
return Status; return Status;
} }
/* Physical Layer Initialize */ /* Physical Layer Initialize */
Status = Phy_InstInit(&phyInstPtr, &phyConfig); Status = Phy_InstInit(&phyInstPtr, &phyConfig);
if (Status == SUCCESS) { if (Status == SUCCESS)
{
DBG_PRINT("Physical layer Init SUCCESS! \n"); DBG_PRINT("Physical layer Init SUCCESS! \n");
} }
else { else
{
DBG_PRINT("Physical layer FAILED! \n"); DBG_PRINT("Physical layer FAILED! \n");
return Status; return Status;
} }
@@ -674,34 +708,40 @@ int main(void) {
/* Link Layer Initialize */ /* Link Layer Initialize */
linkConfig.phyInstRef = phyInstPtr; linkConfig.phyInstRef = phyInstPtr;
Status = Linklayer_Init(&linkInstPtr, &linkConfig); Status = Linklayer_Init(&linkInstPtr, &linkConfig);
if (Status == SUCCESS) { if (Status == SUCCESS)
{
DBG_PRINT("LinkLayer Init SUCCESS! \n"); DBG_PRINT("LinkLayer Init SUCCESS! \n");
} }
else { else
{
DBG_PRINT("Linklayer Init FAILED! \n"); DBG_PRINT("Linklayer Init FAILED! \n");
return Status; return Status;
} }
/* System Bus Initialize */ /* System Bus Initialize */
Status = SystemBus_Init(&sysbusDescPtr, &sysbusConfig); Status = SystemBus_Init(&sysbusDescPtr, &sysbusConfig);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("Systembus init FAILED, with error code: %d\n", Status); DBG_ERROR("Systembus init FAILED, with error code: %d\n", Status);
return Status; return Status;
} }
/* System Monitor log timer Initialize */ /* System Monitor log timer Initialize */
Status = timer_init(&logTmPtr, &scutimer_init_param); Status = timer_init(&logTmPtr, &scutimer_init_param);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("SCU Timer init FAILED, with error code: %d\n", Status); DBG_ERROR("SCU Timer init FAILED, with error code: %d\n", Status);
return Status; return Status;
} }
/* interrupts Initialize */ /* interrupts Initialize */
Status = SetupInterruptSystem(intcDescPtr); Status = SetupInterruptSystem(intcDescPtr);
if (Status == SUCCESS) { if (Status == SUCCESS)
{
DBG_INFO("Interrupt system setup SUCCESS!\n"); DBG_INFO("Interrupt system setup SUCCESS!\n");
} }
else { else
{
DBG_ERROR("Interrupt system setup FAILED, ErrorCode: %d.\n", Status); DBG_ERROR("Interrupt system setup FAILED, ErrorCode: %d.\n", Status);
return Status; return Status;
} }
@@ -712,19 +752,24 @@ int main(void) {
Phy_Start(phyInstPtr); Phy_Start(phyInstPtr);
DBG_PRINT("Physical started...\n"); DBG_PRINT("Physical started...\n");
while (true) { while (true)
{
// System bus handler // System bus handler
if (SystemBus_IsReqPendding(sysbusDescPtr)) { if (SystemBus_IsReqPendding(sysbusDescPtr))
{
Status = SystemBus_HandleCmd(sysbusDescPtr); Status = SystemBus_HandleCmd(sysbusDescPtr);
if(Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("SystemBus_HandleCmd exit with ErrorCode: %d.\n", Status); DBG_ERROR("SystemBus_HandleCmd exit with ErrorCode: %d.\n", Status);
} }
} }
if (SystemBus_IsRsqPendding(sysbusDescPtr) && (!SystemBus_IsTxBusy(sysbusDescPtr))) { if (SystemBus_IsRsqPendding(sysbusDescPtr) && (!SystemBus_IsTxBusy(sysbusDescPtr)))
{
// FIXME: Required by 12 (CASC) // FIXME: Required by 12 (CASC)
udelay(200); udelay(200);
Status = SystemBus_MakeResp(sysbusDescPtr); Status = SystemBus_MakeResp(sysbusDescPtr);
if(Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("SystemBus_HandleCmd exit with ErrorCode: %d.\n", Status); DBG_ERROR("SystemBus_HandleCmd exit with ErrorCode: %d.\n", Status);
} }
} }
df_datalink/src/networking/link_layer/framing.c
+20 -11
View File
@@ -24,7 +24,6 @@
/************************** Constant Definitions *****************************/ /************************** Constant Definitions *****************************/
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Generate crc of specified data. * @brief Generate crc of specified data.
@@ -38,10 +37,12 @@
* SUCCESS * SUCCESS
* *
******************************************************************************/ ******************************************************************************/
static int32_t L2_CrcGen(uint8_t* dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t* crc){ static int32_t L2_CrcGen(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t *crc)
{
ASSERT(dataBuff != NULL); ASSERT(dataBuff != NULL);
int32_t Status; int32_t Status;
switch (crcType){ switch (crcType)
{
case L2_CRC16_1021: case L2_CRC16_1021:
Status = crc16_gen(dataBuff, dataLen, 0x1021, (uint16_t *)crc); Status = crc16_gen(dataBuff, dataLen, 0x1021, (uint16_t *)crc);
break; break;
@@ -71,17 +72,20 @@ static int32_t L2_CrcGen(uint8_t* dataBuff, uint32_t dataLen, L2_CRC_TYPE crcTyp
* SUCCESS * SUCCESS
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_CrcAppend(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType){ int32_t L2_CrcAppend(uint8_t *dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType)
{
ASSERT(dataBuff != NULL); ASSERT(dataBuff != NULL);
int32_t Status; int32_t Status;
uint32_t crcVal; uint32_t crcVal;
uint8_t *crcPtr; uint8_t *crcPtr;
crcPtr = dataBuff + crcPos; crcPtr = dataBuff + crcPos;
Status = L2_CrcGen(dataBuff, dataLen, crcType, &crcVal); Status = L2_CrcGen(dataBuff, dataLen, crcType, &crcVal);
if(Status != SUCCESS){ if (Status != SUCCESS)
{
return Status; return Status;
} }
switch (crcType){ switch (crcType)
{
case L2_CRC16_1021: case L2_CRC16_1021:
*(crcPtr + 1) = (uint8_t)(crcVal >> 8); *(crcPtr + 1) = (uint8_t)(crcVal >> 8);
*crcPtr = (uint8_t)(crcVal); *crcPtr = (uint8_t)(crcVal);
@@ -112,21 +116,26 @@ int32_t L2_CrcAppend(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CR
* SUCCESS * SUCCESS
* *
******************************************************************************/ ******************************************************************************/
bool L2_CrcCheck(uint8_t* dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType){ bool L2_CrcCheck(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType)
{
ASSERT(dataBuff != NULL); ASSERT(dataBuff != NULL);
int32_t Status; int32_t Status;
uint32_t crcVal; uint32_t crcVal;
crcVal = 0; crcVal = 0;
Status = L2_CrcGen(dataBuff, dataLen, crcType, &crcVal); Status = L2_CrcGen(dataBuff, dataLen, crcType, &crcVal);
if(Status == SUCCESS){ if (Status == SUCCESS)
if(crcVal == 0){ {
if (crcVal == 0)
{
return true; return true;
} }
else{ else
{
return false; return false;
} }
} }
else { else
{
return false; return false;
} }
} }
df_datalink/src/networking/link_layer/link_layer.c
+234 -121
View File
@@ -41,13 +41,15 @@ static void FrameNoUpdate(L2_INST *instPtr, uint32_t newFrameNo);
static void DataAvailUpdate(L2_INST *instPtr, bool status); static void DataAvailUpdate(L2_INST *instPtr, bool status);
static void EventPoolHandler(L2_INST *instPtr); static void EventPoolHandler(L2_INST *instPtr);
static inline void UpdateMainSate(L2_INST *instPtr, L2_NODE_STATE newSate) { static inline void UpdateMainSate(L2_INST *instPtr, L2_NODE_STATE newSate)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
DBG_INFO("MainStateUpdate: %d -> %d\n", instPtr->mainState, newSate); DBG_INFO("MainStateUpdate: %d -> %d\n", instPtr->mainState, newSate);
instPtr->nextState = newSate; instPtr->nextState = newSate;
} }
static inline void ForceUpdateMainSate(L2_INST *instPtr, L2_NODE_STATE newSate) { static inline void ForceUpdateMainSate(L2_INST *instPtr, L2_NODE_STATE newSate)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
DBG_INFO("MainState Force Update: %d -> %d\n", instPtr->mainState, newSate); DBG_INFO("MainState Force Update: %d -> %d\n", instPtr->mainState, newSate);
instPtr->mainState = newSate; instPtr->mainState = newSate;
@@ -58,7 +60,8 @@ static inline void ForceUpdateMainSate(L2_INST *instPtr, L2_NODE_STATE newSate)
* @brief Update frame number, based on new next slotNo * @brief Update frame number, based on new next slotNo
*/ */
// FIXME: frame number update should effect 'last' and 'curr' frame number record. // FIXME: frame number update should effect 'last' and 'curr' frame number record.
static inline void SlotNoUpdate(L2_INST *instPtr, uint32_t newSlotNo) { static inline void SlotNoUpdate(L2_INST *instPtr, uint32_t newSlotNo)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
DBG_INFO("Update slot number to %lu\n", newSlotNo); DBG_INFO("Update slot number to %lu\n", newSlotNo);
instPtr->slotNo.next = newSlotNo; instPtr->slotNo.next = newSlotNo;
@@ -68,7 +71,8 @@ static inline void SlotNoUpdate(L2_INST *instPtr, uint32_t newSlotNo) {
* @brief Update slot number, based on new next frameNo * @brief Update slot number, based on new next frameNo
*/ */
// TODO: slot number update should effect 'last' and 'curr' slot number record. // TODO: slot number update should effect 'last' and 'curr' slot number record.
static inline void FrameNoUpdate(L2_INST *instPtr, uint32_t newFrameNo) { static inline void FrameNoUpdate(L2_INST *instPtr, uint32_t newFrameNo)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
DBG_INFO("Update frame number to %lu\n", newFrameNo); DBG_INFO("Update frame number to %lu\n", newFrameNo);
instPtr->frameNo.next = newFrameNo; instPtr->frameNo.next = newFrameNo;
@@ -78,12 +82,14 @@ static inline void FrameNoUpdate(L2_INST *instPtr, uint32_t newFrameNo) {
* @brief Update l2 frame buffer state * @brief Update l2 frame buffer state
* infer that if there is a new valid l2 frame up. * infer that if there is a new valid l2 frame up.
*/ */
static inline void DataAvailUpdate(L2_INST *instPtr, bool status) { static inline void DataAvailUpdate(L2_INST *instPtr, bool status)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
instPtr->dataAvail = status; instPtr->dataAvail = status;
} }
L2_NODE_STATE L2_GetMainState(L2_INST *instPtr){ L2_NODE_STATE L2_GetMainState(L2_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
return instPtr->mainState; return instPtr->mainState;
} }
@@ -91,7 +97,8 @@ L2_NODE_STATE L2_GetMainState(L2_INST *instPtr){
/** /**
* @brief Check if there is a valid l2 frame. * @brief Check if there is a valid l2 frame.
*/ */
inline bool L2_isDataAvail(L2_INST *instPtr) { inline bool L2_isDataAvail(L2_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
return instPtr->dataAvail; return instPtr->dataAvail;
} }
@@ -99,7 +106,8 @@ inline bool L2_isDataAvail(L2_INST *instPtr) {
/** /**
* @brief Check if node is connected to the network * @brief Check if node is connected to the network
*/ */
inline bool L2_isCxn2Net(L2_INST *instPtr) { inline bool L2_isCxn2Net(L2_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
return instPtr->cxn2Net; return instPtr->cxn2Net;
} }
@@ -107,7 +115,8 @@ inline bool L2_isCxn2Net(L2_INST *instPtr) {
/** /**
* @brief Get last frame number. * @brief Get last frame number.
*/ */
inline uint32_t L2_GetLastFrameNo(L2_INST *instPtr) { inline uint32_t L2_GetLastFrameNo(L2_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
return instPtr->frameNo.last; return instPtr->frameNo.last;
} }
@@ -115,7 +124,8 @@ inline uint32_t L2_GetLastFrameNo(L2_INST *instPtr) {
/** /**
* @brief Get last slot number. * @brief Get last slot number.
*/ */
inline uint32_t L2_GetLastSlotNo(L2_INST *instPtr) { inline uint32_t L2_GetLastSlotNo(L2_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
return instPtr->slotNo.last; return instPtr->slotNo.last;
} }
@@ -123,7 +133,8 @@ inline uint32_t L2_GetLastSlotNo(L2_INST *instPtr) {
/** /**
* @brief Get current frame number. * @brief Get current frame number.
*/ */
inline uint32_t L2_GetCurrentFrameNo(L2_INST *instPtr) { inline uint32_t L2_GetCurrentFrameNo(L2_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
return instPtr->frameNo.curr; return instPtr->frameNo.curr;
} }
@@ -131,7 +142,8 @@ inline uint32_t L2_GetCurrentFrameNo(L2_INST *instPtr) {
/** /**
* @brief Get current slot number. * @brief Get current slot number.
*/ */
inline uint32_t L2_GetCurrentSlotNo(L2_INST *instPtr) { inline uint32_t L2_GetCurrentSlotNo(L2_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
return instPtr->slotNo.curr; return instPtr->slotNo.curr;
} }
@@ -139,14 +151,16 @@ inline uint32_t L2_GetCurrentSlotNo(L2_INST *instPtr) {
/** /**
* @brief Get next frame number. * @brief Get next frame number.
*/ */
inline uint32_t L2_GetNextFrameNo(L2_INST *instPtr) { inline uint32_t L2_GetNextFrameNo(L2_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
return instPtr->frameNo.next; return instPtr->frameNo.next;
} }
/** /**
* @brief Get next slot number. * @brief Get next slot number.
*/ */
inline uint32_t L2_GetNextSlotNo(L2_INST *instPtr) { inline uint32_t L2_GetNextSlotNo(L2_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
return instPtr->slotNo.next; return instPtr->slotNo.next;
} }
@@ -161,19 +175,23 @@ inline uint32_t L2_GetNextSlotNo(L2_INST *instPtr) {
* Pointer to schedule event. * Pointer to schedule event.
* *
******************************************************************************/ ******************************************************************************/
L2_SCHD_EVENT* L2_CreatNewSchdEvent(L2_INST *instPtr, L2_EVENT_TYPE event){ L2_SCHD_EVENT *L2_CreatNewSchdEvent(L2_INST *instPtr, L2_EVENT_TYPE event)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
L2_SCHD_EVENT *schdEventPtr = NULL; L2_SCHD_EVENT *schdEventPtr = NULL;
if(instPtr->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"); DBG_ERROR("Schedule event number exceeds the max, no more schedule event can be created.\n");
return NULL; return NULL;
} }
schdEventPtr = (L2_SCHD_EVENT *)malloc(sizeof(L2_SCHD_EVENT)); schdEventPtr = (L2_SCHD_EVENT *)malloc(sizeof(L2_SCHD_EVENT));
if(schdEventPtr == NULL){ if (schdEventPtr == NULL)
{
free(schdEventPtr); free(schdEventPtr);
return NULL; return NULL;
} }
if(event < L2_EV_SCAN_PROC_DONE || event > L2_ROUTINE_UPDATE){ if (event < L2_EV_SCAN_PROC_DONE || event > L2_ROUTINE_UPDATE)
{
DBG_ERROR("Unknown event type: %d.\n", event); DBG_ERROR("Unknown event type: %d.\n", event);
return NULL; return NULL;
} }
@@ -187,7 +205,6 @@ L2_SCHD_EVENT* L2_CreatNewSchdEvent(L2_INST *instPtr, L2_EVENT_TYPE event){
return schdEventPtr; return schdEventPtr;
} }
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Delete a schedule event, remove the event from event pool, * @brief Delete a schedule event, remove the event from event pool,
@@ -199,15 +216,19 @@ L2_SCHD_EVENT* L2_CreatNewSchdEvent(L2_INST *instPtr, L2_EVENT_TYPE event){
* error code * error code
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_DeletSchdEvent(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent){ int32_t L2_DeletSchdEvent(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
ASSERT(schdEvent != NULL); ASSERT(schdEvent != NULL);
free(schdEvent->l2Event.msg); free(schdEvent->l2Event.msg);
for(int32_t i = schdEvent->eventId; i < instPtr->schdEventCnt; i++){ for (int32_t i = schdEvent->eventId; i < instPtr->schdEventCnt; i++)
if(i == instPtr->schdEventCnt - 1){ {
if (i == instPtr->schdEventCnt - 1)
{
instPtr->schdEventPool[i] = NULL; instPtr->schdEventPool[i] = NULL;
} }
else{ else
{
instPtr->schdEventPool[i] = instPtr->schdEventPool[i + 1]; instPtr->schdEventPool[i] = instPtr->schdEventPool[i + 1];
} }
} }
@@ -227,7 +248,8 @@ int32_t L2_DeletSchdEvent(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent){
* error code. * error code.
* *
******************************************************************************/ ******************************************************************************/
inline int32_t L2_SchdEventAppendMsg(L2_SCHD_EVENT* schdEvent, void* msg){ inline int32_t L2_SchdEventAppendMsg(L2_SCHD_EVENT *schdEvent, void *msg)
{
ASSERT(schdEvent != NULL); ASSERT(schdEvent != NULL);
schdEvent->l2Event.msg = msg; schdEvent->l2Event.msg = msg;
return SUCCESS; return SUCCESS;
@@ -243,7 +265,8 @@ inline int32_t L2_SchdEventAppendMsg(L2_SCHD_EVENT* schdEvent, void* msg){
* false. * false.
* *
******************************************************************************/ ******************************************************************************/
inline bool L2_isEventScheduled(L2_SCHD_EVENT* schdEvent){ inline bool L2_isEventScheduled(L2_SCHD_EVENT *schdEvent)
{
ASSERT(schdEvent != NULL); ASSERT(schdEvent != NULL);
return schdEvent->isSchd; return schdEvent->isSchd;
} }
@@ -260,13 +283,16 @@ inline bool L2_isEventScheduled(L2_SCHD_EVENT* schdEvent){
* SUCCESS * SUCCESS
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_ScheduleEventAt(L2_INST *instPtr, 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); ASSERT(schdEvent != NULL);
if(L2_isEventScheduled(schdEvent) == true){ if (L2_isEventScheduled(schdEvent) == true)
{
DBG_ERROR("Event %d has already been scheduled.\n", schdEvent->eventId); DBG_ERROR("Event %d has already been scheduled.\n", schdEvent->eventId);
return FAILURE; return FAILURE;
} }
if(frameNo <= L2_GetCurrentFrameNo(instPtr) && slotNo < L2_GetCurrentSlotNo(instPtr)){ 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", DBG_ERROR("Cannot schedule event %d to the past(current: [%ld-%ld], schedule: [%lu-%lu]).\n",
schdEvent->eventId, L2_GetCurrentFrameNo(instPtr), L2_GetCurrentSlotNo(instPtr), frameNo, slotNo); schdEvent->eventId, L2_GetCurrentFrameNo(instPtr), L2_GetCurrentSlotNo(instPtr), frameNo, slotNo);
return FAILURE; return FAILURE;
@@ -288,9 +314,11 @@ int32_t L2_ScheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t
* SUCCESS * SUCCESS
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_ScheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots){ int32_t L2_ScheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t numSlots)
{
ASSERT(schdEvent != NULL); ASSERT(schdEvent != NULL);
if(L2_isEventScheduled(schdEvent) == true){ if (L2_isEventScheduled(schdEvent) == true)
{
DBG_ERROR("Event %d has already been scheduled.\n", schdEvent->eventId); DBG_ERROR("Event %d has already been scheduled.\n", schdEvent->eventId);
return FAILURE; return FAILURE;
} }
@@ -311,9 +339,11 @@ int32_t L2_ScheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32
* SUCCESS * SUCCESS
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_RecheduleEventAt(L2_INST *instPtr, 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); ASSERT(schdEvent != NULL);
if(schdEvent->isSchd == false){ if (schdEvent->isSchd == false)
{
DBG_WARNING("Event %d has not been scheduled yet.\n", schdEvent->eventId); DBG_WARNING("Event %d has not been scheduled yet.\n", schdEvent->eventId);
} }
schdEvent->isSchd = false; schdEvent->isSchd = false;
@@ -333,9 +363,11 @@ int32_t L2_RecheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t
* @return NONE. * @return NONE.
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_RecheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots){ int32_t L2_RecheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t numSlots)
{
ASSERT(schdEvent != NULL); ASSERT(schdEvent != NULL);
if(schdEvent->isSchd == false){ if (schdEvent->isSchd == false)
{
DBG_WARNING("Event %d has not been scheduled yet.\n", schdEvent->eventId); DBG_WARNING("Event %d has not been scheduled yet.\n", schdEvent->eventId);
} }
return L2_ScheduleEventAfter(instPtr, schdEvent, numSlots); return L2_ScheduleEventAfter(instPtr, schdEvent, numSlots);
@@ -350,9 +382,11 @@ int32_t L2_RecheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint3
* @return SUCCESS. * @return SUCCESS.
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_CancelEvent(L2_SCHD_EVENT* schdEvent){ int32_t L2_CancelEvent(L2_SCHD_EVENT *schdEvent)
{
ASSERT(schdEvent != NULL); ASSERT(schdEvent != NULL);
if(schdEvent->isSchd == false){ if (schdEvent->isSchd == false)
{
DBG_WARNING("Event %d has not been scheduled yet.\n", schdEvent->eventId); DBG_WARNING("Event %d has not been scheduled yet.\n", schdEvent->eventId);
} }
schdEvent->isSchd = false; schdEvent->isSchd = false;
@@ -368,12 +402,15 @@ int32_t L2_CancelEvent(L2_SCHD_EVENT* schdEvent){
* @return NONE. * @return NONE.
* *
******************************************************************************/ ******************************************************************************/
static void EventPoolHandler(L2_INST *instPtr){ static void EventPoolHandler(L2_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
for(int32_t i = 0; i < instPtr->schdEventCnt; i++){ for (int32_t i = 0; i < instPtr->schdEventCnt; i++)
if(instPtr->schdEventPool[i]->isSchd == true){ {
if(instPtr->schdEventPool[i]->schdFrame == instPtr->frameNo.curr if (instPtr->schdEventPool[i]->isSchd == true)
&& instPtr->schdEventPool[i]->schdSlot == instPtr->slotNo.curr){ {
if (instPtr->schdEventPool[i]->schdFrame == instPtr->frameNo.curr && instPtr->schdEventPool[i]->schdSlot == instPtr->slotNo.curr)
{
AssertL2Event(instPtr, &(instPtr->schdEventPool[i]->l2Event)); AssertL2Event(instPtr, &(instPtr->schdEventPool[i]->l2Event));
} }
} }
@@ -388,7 +425,8 @@ static void EventPoolHandler(L2_INST *instPtr){
* @return NONE. * @return NONE.
* *
******************************************************************************/ ******************************************************************************/
static inline void RcdShift(L2_RCD* rcd, int32_t netxVal) { static inline void RcdShift(L2_RCD *rcd, int32_t netxVal)
{
ASSERT(rcd != NULL); ASSERT(rcd != NULL);
rcd->last = rcd->curr; rcd->last = rcd->curr;
rcd->curr = rcd->next; rcd->curr = rcd->next;
@@ -405,11 +443,14 @@ static inline void RcdShift(L2_RCD* rcd, int32_t netxVal) {
* *
******************************************************************************/ ******************************************************************************/
L2_FRAME_TYPE L2_GetLastRcvFrameType(L2_INST *instPtr) { L2_FRAME_TYPE L2_GetLastRcvFrameType(L2_INST *instPtr)
if (instPtr->dataAvail == false) { {
if (instPtr->dataAvail == false)
{
return L2_NONE_FRAME; return L2_NONE_FRAME;
} }
else { else
{
return instPtr->frame.last; return instPtr->frame.last;
} }
} }
@@ -424,7 +465,8 @@ L2_FRAME_TYPE L2_GetLastRcvFrameType(L2_INST *instPtr) {
* @return NONE. * @return NONE.
* *
******************************************************************************/ ******************************************************************************/
void L2_FrameStateUpdate(L2_INST *instPtr) { void L2_FrameStateUpdate(L2_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
PHY_INST *phyRef = instPtr->config.phyInstRef; PHY_INST *phyRef = instPtr->config.phyInstRef;
bool isRecved, isValid; bool isRecved, isValid;
@@ -436,37 +478,44 @@ void L2_FrameStateUpdate(L2_INST *instPtr) {
RcdShift(&(instPtr->frame), L2_NONE_FRAME); RcdShift(&(instPtr->frame), L2_NONE_FRAME);
RcdShift(&(instPtr->torState), L2_TOR_SUSPEND); RcdShift(&(instPtr->torState), L2_TOR_SUSPEND);
if(Phy_IsFrameValid(instPtr->config.phyInstRef)) { if (Phy_IsFrameValid(instPtr->config.phyInstRef))
if(Phy_RetrieveRecvData(phyRef, instPtr->recvPktData) != SUCCESS) { {
if (Phy_RetrieveRecvData(phyRef, instPtr->recvPktData) != SUCCESS)
{
ASSERT(false); ASSERT(false);
} }
} }
// Update frame state. // Update frame state.
switch (instPtr->frame.last) { switch (instPtr->frame.last)
{
case L2_SCAN_FRAME: case L2_SCAN_FRAME:
isRecved = Phy_IsFrameDetected(instPtr->config.phyInstRef); isRecved = Phy_IsFrameDetected(instPtr->config.phyInstRef);
isValid = isRecved; isValid = isRecved;
break; break;
case L2_SVC_FRAME: case L2_SVC_FRAME:
isRecved = Phy_IsFrameValid(instPtr->config.phyInstRef); isRecved = Phy_IsFrameValid(instPtr->config.phyInstRef);
if (isRecved == true) { if (isRecved == true)
{
#ifndef DATALINK_ENABLE_BERTEST #ifndef DATALINK_ENABLE_BERTEST
isValid = L2_CrcCheck(instPtr->recvPktData, SVC_PACK_LEN, L2_CRC16_1021); isValid = L2_CrcCheck(instPtr->recvPktData, SVC_PACK_LEN, L2_CRC16_1021);
#else #else
isValid = true; isValid = true;
#endif #endif
} }
else { else
{
isValid = false; isValid = false;
} }
break; break;
case L2_INFO_FRAME: case L2_INFO_FRAME:
isRecved = Phy_IsFrameValid(instPtr->config.phyInstRef); isRecved = Phy_IsFrameValid(instPtr->config.phyInstRef);
if (isRecved == true) { if (isRecved == true)
{
isValid = L2_CrcCheck(instPtr->recvPktData, RSP_PACK_LEN, L2_CRC16_1021); isValid = L2_CrcCheck(instPtr->recvPktData, RSP_PACK_LEN, L2_CRC16_1021);
} }
else { else
{
isValid = false; isValid = false;
} }
break; break;
@@ -478,12 +527,14 @@ void L2_FrameStateUpdate(L2_INST *instPtr) {
instPtr->recvCnt = (isRecved == true) ? instPtr->recvCnt + 1 : instPtr->recvCnt; instPtr->recvCnt = (isRecved == true) ? instPtr->recvCnt + 1 : instPtr->recvCnt;
instPtr->recvFineCnt = (isValid == true) ? instPtr->recvFineCnt + 1 : instPtr->recvFineCnt; instPtr->recvFineCnt = (isValid == true) ? instPtr->recvFineCnt + 1 : instPtr->recvFineCnt;
DataAvailUpdate(instPtr, isValid); DataAvailUpdate(instPtr, isValid);
if (isRecved == true) { if (isRecved == true)
{
DBG_INFO("Frame received type: %ld, validation: %d.\n", instPtr->frame.last, isValid); DBG_INFO("Frame received type: %ld, validation: %d.\n", instPtr->frame.last, isValid);
} }
// Logging status. // Logging status.
// -- send slot status // -- send slot status
if(instPtr->torState.last == L2_TOR_RX) { if (instPtr->torState.last == L2_TOR_RX)
{
instPtr->rxSlotLog.tuNo = instPtr->frameNo.last; instPtr->rxSlotLog.tuNo = instPtr->frameNo.last;
instPtr->rxSlotLog.slotNo = instPtr->slotNo.last; instPtr->rxSlotLog.slotNo = instPtr->slotNo.last;
instPtr->rxSlotLog.frameType = instPtr->frame.last; instPtr->rxSlotLog.frameType = instPtr->frame.last;
@@ -491,7 +542,8 @@ void L2_FrameStateUpdate(L2_INST *instPtr) {
instPtr->rxSlotLog.dstId = (instPtr->config.myType == L2_HEAD_NODE) ? L2_MEMBER_NODE_ID : L2_HEAD_NODE_ID; instPtr->rxSlotLog.dstId = (instPtr->config.myType == L2_HEAD_NODE) ? L2_MEMBER_NODE_ID : L2_HEAD_NODE_ID;
// TODO: baseInfo[18]; // TODO: baseInfo[18];
memset(instPtr->rxSlotLog.payload, 0x00, sizeof(instPtr->rxSlotLog.payload)); memset(instPtr->rxSlotLog.payload, 0x00, sizeof(instPtr->rxSlotLog.payload));
if(isValid == true) { if (isValid == true)
{
memcpy(instPtr->rxSlotLog.payload, memcpy(instPtr->rxSlotLog.payload,
instPtr->recvPktData, instPtr->recvPktData,
Phy_GetFrameLen(L2_L2frame2L1frame(instPtr->frame.last))); Phy_GetFrameLen(L2_L2frame2L1frame(instPtr->frame.last)));
@@ -501,8 +553,10 @@ void L2_FrameStateUpdate(L2_INST *instPtr) {
// Log time deviation. // Log time deviation.
int64_t devi; int64_t devi;
devi = 0; devi = 0;
if (L2_TimeSync_GetDeviatoion(instPtr, &devi) == SUCCESS) { if (L2_TimeSync_GetDeviatoion(instPtr, &devi) == SUCCESS)
if (devi <= INT32_MAX && devi >= INT32_MIN) { {
if (devi <= INT32_MAX && devi >= INT32_MIN)
{
instPtr->txSlotLog.tmDevi = (int32_t)devi; instPtr->txSlotLog.tmDevi = (int32_t)devi;
} }
instPtr->txSlotLog.tmDevi = (int32_t)devi; instPtr->txSlotLog.tmDevi = (int32_t)devi;
@@ -510,7 +564,8 @@ void L2_FrameStateUpdate(L2_INST *instPtr) {
// Log frequency deviation // Log frequency deviation
instPtr->rxSlotLog.freqDevi = Phy_GetFreqEst(instPtr->config.phyInstRef); instPtr->rxSlotLog.freqDevi = Phy_GetFreqEst(instPtr->config.phyInstRef);
} }
else { else
{
instPtr->rxSlotLog.tmStamp = 0; instPtr->rxSlotLog.tmStamp = 0;
instPtr->rxSlotLog.tmDevi = 0; instPtr->rxSlotLog.tmDevi = 0;
instPtr->rxSlotLog.freqDevi = 0; instPtr->rxSlotLog.freqDevi = 0;
@@ -518,7 +573,8 @@ void L2_FrameStateUpdate(L2_INST *instPtr) {
SysMonitor_UpdateRecvSlotLog(&(instPtr->rxSlotLog)); SysMonitor_UpdateRecvSlotLog(&(instPtr->rxSlotLog));
} }
// -- send slot status // -- send slot status
if(instPtr->torState.curr == L2_TOR_TX) { if (instPtr->torState.curr == L2_TOR_TX)
{
instPtr->txSlotLog.tuNo = instPtr->frameNo.curr; instPtr->txSlotLog.tuNo = instPtr->frameNo.curr;
instPtr->txSlotLog.slotNo = instPtr->slotNo.curr; instPtr->txSlotLog.slotNo = instPtr->slotNo.curr;
instPtr->txSlotLog.frameType = instPtr->frame.curr; instPtr->txSlotLog.frameType = instPtr->frame.curr;
@@ -548,14 +604,16 @@ void L2_FrameStateUpdate(L2_INST *instPtr) {
* *
******************************************************************************/ ******************************************************************************/
// FIXME: To Complete. // FIXME: To Complete.
int32_t L2_SteerBeam2Node(L2_INST *instPtr, uint8_t nodeId){ int32_t L2_SteerBeam2Node(L2_INST *instPtr, uint8_t nodeId)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
ASSERT(instPtr->config.phyInstRef != NULL); ASSERT(instPtr->config.phyInstRef != NULL);
int32_t Status; int32_t Status;
COORD myPos, dstPos, beamDir; COORD myPos, dstPos, beamDir;
Status = GetNodePos(instPtr->nodeInfoList, nodeId, &dstPos); Status = GetNodePos(instPtr->nodeInfoList, nodeId, &dstPos);
if(Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("Failed to retrieve position info of node %u, remain last configuration.\n", nodeId); DBG_ERROR("Failed to retrieve position info of node %u, remain last configuration.\n", nodeId);
return EPARAM; return EPARAM;
} }
@@ -611,17 +669,20 @@ int32_t L2_SteerBeam2Node(L2_INST *instPtr, uint8_t nodeId){
* L2_FRAME_NONE_LEN if the frame type code cannot be recognized. * L2_FRAME_NONE_LEN if the frame type code cannot be recognized.
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE frameType) { int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE frameType)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
ASSERT(instPtr->config.phyInstRef != NULL); ASSERT(instPtr->config.phyInstRef != NULL);
int32_t Status = SUCCESS; int32_t Status = SUCCESS;
if (frameType < L2_SCAN_FRAME || frameType > L2_INFO_FRAME) { if (frameType < L2_SCAN_FRAME || frameType > L2_INFO_FRAME)
{
return FAILURE; return FAILURE;
} }
#if _DEBUG_LEVEL == 3 #if _DEBUG_LEVEL == 3
char *torStr = NULL; char *torStr = NULL;
char *frameStr = NULL; char *frameStr = NULL;
switch(txrxMode){ switch (txrxMode)
{
case L2_TOR_SUSPEND: case L2_TOR_SUSPEND:
torStr = "SUSPEND"; torStr = "SUSPEND";
break; break;
@@ -632,7 +693,8 @@ int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE f
torStr = "RX"; torStr = "RX";
break; break;
} }
switch(frameType){ switch (frameType)
{
case L2_SCAN_FRAME: case L2_SCAN_FRAME:
frameStr = "Scan"; frameStr = "Scan";
break; break;
@@ -653,7 +715,8 @@ int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE f
#endif #endif
instPtr->frame.next = frameType; instPtr->frame.next = frameType;
PHY_TOR_TYPE tor; PHY_TOR_TYPE tor;
switch(txrxMode) { switch (txrxMode)
{
case L2_TOR_SUSPEND: case L2_TOR_SUSPEND:
tor = PHY_TOR_SUSPEND; tor = PHY_TOR_SUSPEND;
break; break;
@@ -668,7 +731,8 @@ int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE f
return EPARAM; return EPARAM;
} }
Status = Phy_SetupNextSlot(instPtr->config.phyInstRef, tor, L2_L2frame2L1frame(frameType), instPtr->sendPktData); Status = Phy_SetupNextSlot(instPtr->config.phyInstRef, tor, L2_L2frame2L1frame(frameType), instPtr->sendPktData);
if(Status != SUCCESS){ if (Status != SUCCESS)
{
DBG_ERROR("Physical setup failed, with error code: %ld\n", Status); DBG_ERROR("Physical setup failed, with error code: %ld\n", Status);
} }
return Status; return Status;
@@ -684,7 +748,8 @@ int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE f
* L2_FRAME_NONE_LEN if the frame type code cannot be recognized. * L2_FRAME_NONE_LEN if the frame type code cannot be recognized.
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_GetFrameLen(L2_FRAME_TYPE l2FrameType) { int32_t L2_GetFrameLen(L2_FRAME_TYPE l2FrameType)
{
return Phy_GetFrameLen(L2_L2frame2L1frame(l2FrameType)); return Phy_GetFrameLen(L2_L2frame2L1frame(l2FrameType));
} }
@@ -697,8 +762,10 @@ int32_t L2_GetFrameLen(L2_FRAME_TYPE l2FrameType) {
* @return L1 frame type. * @return L1 frame type.
* *
******************************************************************************/ ******************************************************************************/
L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame) { L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame)
switch (l2Frame) { {
switch (l2Frame)
{
case L2_SCAN_FRAME: case L2_SCAN_FRAME:
return L1_2KBPS_20MS_FRAME; return L1_2KBPS_20MS_FRAME;
case L2_SVC_FRAME: case L2_SVC_FRAME:
@@ -721,18 +788,19 @@ L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame) {
* Other error code. * Other error code.
* *
******************************************************************************/ ******************************************************************************/
int32_t Linklayer_Init(L2_INST **instPtr, L2_CONFIG *config) { int32_t Linklayer_Init(L2_INST **instPtr, L2_CONFIG *config)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
ASSERT(config != NULL); ASSERT(config != NULL);
int32_t Status; int32_t Status;
(*instPtr) = (L2_INST *)malloc(sizeof(L2_INST)); (*instPtr) = (L2_INST *)malloc(sizeof(L2_INST));
if ((*instPtr) == NULL) { if ((*instPtr) == NULL)
{
DBG_ERROR("Allocate memory for L2_InstPtr failed.\n"); DBG_ERROR("Allocate memory for L2_InstPtr failed.\n");
return ENOMEM; return ENOMEM;
} }
memcpy(&((*instPtr)->config), config, sizeof(L2_CONFIG)); memcpy(&((*instPtr)->config), config, sizeof(L2_CONFIG));
(*instPtr)->cxn2Net = (config->myType == L2_HEAD_NODE) ? true : false; (*instPtr)->cxn2Net = (config->myType == L2_HEAD_NODE) ? true : false;
(*instPtr)->dataAvail = false; (*instPtr)->dataAvail = false;
(*instPtr)->frame.last = L2_NONE_FRAME; (*instPtr)->frame.last = L2_NONE_FRAME;
@@ -766,33 +834,39 @@ int32_t Linklayer_Init(L2_INST **instPtr, L2_CONFIG *config) {
memset((*instPtr)->schdEventPool, 0x00, sizeof(L2_SCHD_EVENT *) * L2_SCHD_EVENT_MAX); memset((*instPtr)->schdEventPool, 0x00, sizeof(L2_SCHD_EVENT *) * L2_SCHD_EVENT_MAX);
(*instPtr)->sendPktData = malloc(PACK_LEN_MAX * sizeof(uint8_t)); (*instPtr)->sendPktData = malloc(PACK_LEN_MAX * sizeof(uint8_t));
if((*instPtr)->sendPktData == NULL) { if ((*instPtr)->sendPktData == NULL)
{
DBG_ERROR("Failed to allocate memory for sendPktData.\n"); DBG_ERROR("Failed to allocate memory for sendPktData.\n");
goto init_error; goto init_error;
} }
(*instPtr)->recvPktData = malloc(PACK_LEN_MAX * sizeof(uint8_t)); (*instPtr)->recvPktData = malloc(PACK_LEN_MAX * sizeof(uint8_t));
if((*instPtr)->recvPktData == NULL) { if ((*instPtr)->recvPktData == NULL)
{
DBG_ERROR("Failed to allocate memory for recvPktData.\n"); DBG_ERROR("Failed to allocate memory for recvPktData.\n");
goto init_error; goto init_error;
} }
Status = NodeList_Init(&((*instPtr)->nodeInfoList)); Status = NodeList_Init(&((*instPtr)->nodeInfoList));
if(Status != SUCCESS) { if (Status != SUCCESS)
{
goto init_error; goto init_error;
} }
Status = L2_Scan_Init(*instPtr); Status = L2_Scan_Init(*instPtr);
if(Status != SUCCESS) { if (Status != SUCCESS)
{
goto init_error; goto init_error;
} }
Status = L2_Service_Init(*instPtr); Status = L2_Service_Init(*instPtr);
if(Status != SUCCESS) { if (Status != SUCCESS)
{
goto init_error; goto init_error;
} }
if (CircQueueCreate(&((*instPtr)->txQueue), TX_QUEUE_MAX) == false) { if (CircQueueCreate(&((*instPtr)->txQueue), TX_QUEUE_MAX) == false)
{
Status = ENOMEM; Status = ENOMEM;
goto init_error; goto init_error;
} }
@@ -816,40 +890,50 @@ init_error:
* FAIL - Event cannot be recognized or error occurred while process. * FAIL - Event cannot be recognized or error occurred while process.
* *
******************************************************************************/ ******************************************************************************/
int32_t Linklayer_EventHandler(L2_INST *instPtr, L2_EVENT* l2Event) { int32_t Linklayer_EventHandler(L2_INST *instPtr, L2_EVENT *l2Event)
switch (l2Event->event) { {
case L2_EV_FRAME_NO_UPDATE: { switch (l2Event->event)
{
case L2_EV_FRAME_NO_UPDATE:
{
DBG_INFO("Event L2_EV_FRAME_NO_UPDATE asserted....\n"); DBG_INFO("Event L2_EV_FRAME_NO_UPDATE asserted....\n");
FrameNoUpdate(instPtr, *((uint32_t *)l2Event->msg)); FrameNoUpdate(instPtr, *((uint32_t *)l2Event->msg));
break; break;
} }
case L2_EV_SLOT_NO_UPDATE: { case L2_EV_SLOT_NO_UPDATE:
{
DBG_INFO("Event L2_EV_SLOT_NO_UPDATE asserted....\n"); DBG_INFO("Event L2_EV_SLOT_NO_UPDATE asserted....\n");
SlotNoUpdate(instPtr, *((uint32_t *)l2Event->msg)); SlotNoUpdate(instPtr, *((uint32_t *)l2Event->msg));
break; break;
} }
case L2_EV_MAIN_STATE_UPDATE: { case L2_EV_MAIN_STATE_UPDATE:
{
DBG_INFO("Event L2_EV_MAIN_STATE_UPDATE asserted....\n"); DBG_INFO("Event L2_EV_MAIN_STATE_UPDATE asserted....\n");
UpdateMainSate(instPtr, *((L2_NODE_STATE *)l2Event->msg)); UpdateMainSate(instPtr, *((L2_NODE_STATE *)l2Event->msg));
break; break;
} }
case L2_EV_SCAN_PROC_DONE: { case L2_EV_SCAN_PROC_DONE:
{
DBG_INFO("Event L2_EV_SCAN_PROC_DONE asserted....\n"); DBG_INFO("Event L2_EV_SCAN_PROC_DONE asserted....\n");
break; break;
} }
case L2_EV_NET_SETUP_DONE: { case L2_EV_NET_SETUP_DONE:
{
DBG_INFO("Event L2_EV_NET_SETUP_DONE asserted....\n"); DBG_INFO("Event L2_EV_NET_SETUP_DONE asserted....\n");
if(instPtr->config.myType != L2_HEAD_NODE){ if (instPtr->config.myType != L2_HEAD_NODE)
{
instPtr->cxn2Net = true; instPtr->cxn2Net = true;
UpdateMainSate(instPtr, NORMAL_STATE); UpdateMainSate(instPtr, NORMAL_STATE);
} }
else { else
{
ForceUpdateMainSate(instPtr, NORMAL_STATE); ForceUpdateMainSate(instPtr, NORMAL_STATE);
} }
break; break;
} }
case L2_EV_TIME_SYNC: { case L2_EV_TIME_SYNC:
{
DBG_INFO("Event L2_EV_TIME_SYNC asserted....\n"); DBG_INFO("Event L2_EV_TIME_SYNC asserted....\n");
#ifndef DATALINK_ENABLE_AUTOTIMESYNC #ifndef DATALINK_ENABLE_AUTOTIMESYNC
int64_t tmAdj; int64_t tmAdj;
@@ -862,9 +946,11 @@ int32_t Linklayer_EventHandler(L2_INST *instPtr, L2_EVENT* l2Event) {
#endif #endif
break; break;
} }
case L2_EV_LOST_CXN: { case L2_EV_LOST_CXN:
{
DBG_INFO("Event L2_EV_LOST_CXN asserted....\n"); DBG_INFO("Event L2_EV_LOST_CXN asserted....\n");
if(instPtr->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"); DBG_ERROR("Illegal event, only None_Head node can assert L2_EV_LOST_CXN.\n");
break; break;
} }
@@ -877,7 +963,8 @@ int32_t Linklayer_EventHandler(L2_INST *instPtr, L2_EVENT* l2Event) {
UpdateMainSate(instPtr, INIT_STATE); UpdateMainSate(instPtr, INIT_STATE);
break; break;
} }
case L2_EV_LOST_NODE: { case L2_EV_LOST_NODE:
{
DBG_INFO("Event L2_EV_LOST_NODE asserted....\n"); DBG_INFO("Event L2_EV_LOST_NODE asserted....\n");
L2_Scan_Init(instPtr); L2_Scan_Init(instPtr);
L2_Service_Init(instPtr); L2_Service_Init(instPtr);
@@ -886,20 +973,24 @@ int32_t Linklayer_EventHandler(L2_INST *instPtr, L2_EVENT* l2Event) {
UpdateMainSate(instPtr, INIT_STATE); UpdateMainSate(instPtr, INIT_STATE);
break; break;
} }
case L2_ROUTINE_FINISH: { case L2_ROUTINE_FINISH:
{
DBG_INFO("Event L2_ROUTINE_FINISH asserted....\n"); DBG_INFO("Event L2_ROUTINE_FINISH asserted....\n");
if(instPtr->nextState == NORMAL_STATE){ if (instPtr->nextState == NORMAL_STATE)
{
instPtr->rtneConfig.rtneTable = normalTable; instPtr->rtneConfig.rtneTable = normalTable;
instPtr->rtneConfig.rtnePrd = ARRAY_SIZE(normalTable) * L2_SLOTS_PER_FRAME; instPtr->rtneConfig.rtnePrd = ARRAY_SIZE(normalTable) * L2_SLOTS_PER_FRAME;
instPtr->rtneConfig.rtneStep = 0; instPtr->rtneConfig.rtneStep = 0;
} }
else{ else
{
DBG_WARNING("Unsupported routine state [%d], Continue withe previous routine table.\n", instPtr->nextState); DBG_WARNING("Unsupported routine state [%d], Continue withe previous routine table.\n", instPtr->nextState);
instPtr->rtneConfig.rtneStep = 0; instPtr->rtneConfig.rtneStep = 0;
} }
break; break;
} }
case L2_ROUTINE_UPDATE: { case L2_ROUTINE_UPDATE:
{
DBG_INFO("Event L2_ROUTINE_UPDATE asserted....\n"); DBG_INFO("Event L2_ROUTINE_UPDATE asserted....\n");
L2_RTNE_CONFIG *newRtneConfig = (L2_RTNE_CONFIG *)l2Event->msg; L2_RTNE_CONFIG *newRtneConfig = (L2_RTNE_CONFIG *)l2Event->msg;
instPtr->rtneConfig.rtneTable = newRtneConfig->rtneTable; instPtr->rtneConfig.rtneTable = newRtneConfig->rtneTable;
@@ -907,7 +998,8 @@ int32_t Linklayer_EventHandler(L2_INST *instPtr, L2_EVENT* l2Event) {
instPtr->rtneConfig.rtneStep = newRtneConfig->rtneStep; instPtr->rtneConfig.rtneStep = newRtneConfig->rtneStep;
break; break;
} }
default: { default:
{
DBG_WARNING("UNKNOWN Event %d asserted.\n", l2Event->event); DBG_WARNING("UNKNOWN Event %d asserted.\n", l2Event->event);
break; break;
} }
@@ -924,15 +1016,18 @@ int32_t Linklayer_EventHandler(L2_INST *instPtr, L2_EVENT* l2Event) {
* @return Status code. * @return Status code.
* *
******************************************************************************/ ******************************************************************************/
int32_t Linklayer_PktHandler(L2_INST *instPtr) { int32_t Linklayer_PktHandler(L2_INST *instPtr)
{
int32_t Status; int32_t Status;
L2_FRAME_TYPE frameType; L2_FRAME_TYPE frameType;
frameType = L2_GetLastRcvFrameType(instPtr); frameType = L2_GetLastRcvFrameType(instPtr);
DBG_INFO("Link layer handling packet, type: %ld.\n", frameType); DBG_INFO("Link layer handling packet, type: %ld.\n", frameType);
if (instPtr->dataAvail == false) { if (instPtr->dataAvail == false)
{
return FAILURE; return FAILURE;
} }
switch (frameType) { switch (frameType)
{
case L2_SCAN_FRAME: case L2_SCAN_FRAME:
Status = L2_Scan_PktHandler(instPtr, instPtr->recvPktData); Status = L2_Scan_PktHandler(instPtr, instPtr->recvPktData);
break; break;
@@ -940,10 +1035,12 @@ int32_t Linklayer_PktHandler(L2_INST *instPtr) {
Status = L2_Service_PktHandler(instPtr, instPtr->recvPktData); Status = L2_Service_PktHandler(instPtr, instPtr->recvPktData);
break; break;
case L2_INFO_FRAME: case L2_INFO_FRAME:
if(instPtr->mainState == INIT_STATE) { if (instPtr->mainState == INIT_STATE)
{
Status = L2_Scan_PktHandler(instPtr, instPtr->recvPktData); Status = L2_Scan_PktHandler(instPtr, instPtr->recvPktData);
} }
else {// NORMAL_STATE else
{ // NORMAL_STATE
Status = L2_Service_PktHandler(instPtr, instPtr->recvPktData); Status = L2_Service_PktHandler(instPtr, instPtr->recvPktData);
} }
break; break;
@@ -955,23 +1052,29 @@ int32_t Linklayer_PktHandler(L2_INST *instPtr) {
return Status; return Status;
} }
static int32_t IntcNodeTTL(NODE_ENTRY *node, const void *param) { static int32_t IntcNodeTTL(NODE_ENTRY *node, const void *param)
if (node == NULL) { {
if (node == NULL)
{
return FAILURE; return FAILURE;
} }
node->ttlCnt += *((uint32_t *)param); node->ttlCnt += *((uint32_t *)param);
return SUCCESS; return SUCCESS;
} }
static bool IsNodeConnected(NODE_ENTRY *node) { static bool IsNodeConnected(NODE_ENTRY *node)
if (node == NULL) { {
if (node == NULL)
{
return false; return false;
} }
DBG_INFO("Node: %u, TTL: %lu.\n", node->nodeInfo.nodeId, node->ttlCnt); DBG_INFO("Node: %u, TTL: %lu.\n", node->nodeInfo.nodeId, node->ttlCnt);
if (node->ttlCnt < LINK_BROCK_THRESHOLD){ if (node->ttlCnt < LINK_BROCK_THRESHOLD)
{
return true; return true;
} }
else{ else
{
DBG_WARNING("Lost Connection to NODE %u.\n", node->nodeInfo.nodeId); DBG_WARNING("Lost Connection to NODE %u.\n", node->nodeInfo.nodeId);
return false; return false;
} }
@@ -986,7 +1089,8 @@ static bool IsNodeConnected(NODE_ENTRY *node) {
* Other error code. * Other error code.
* *
******************************************************************************/ ******************************************************************************/
static int32_t Linklayer_Routine (L2_INST *instPtr) { static int32_t Linklayer_Routine(L2_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
uint32_t usedSlots, tableIndx, entryIndx; uint32_t usedSlots, tableIndx, entryIndx;
L2_RTNE_TABLE_ENTRY rtneEntry; L2_RTNE_TABLE_ENTRY rtneEntry;
@@ -1001,7 +1105,8 @@ static int32_t Linklayer_Routine (L2_INST *instPtr) {
instPtr->rtneConfig.next = &(instPtr->rtneConfig.rtneTable[tableIndx][entryIndx]); instPtr->rtneConfig.next = &(instPtr->rtneConfig.rtneTable[tableIndx][entryIndx]);
rtneEntry = instPtr->rtneConfig.rtneTable[tableIndx][entryIndx]; rtneEntry = instPtr->rtneConfig.rtneTable[tableIndx][entryIndx];
switch (rtneEntry.slotType) { switch (rtneEntry.slotType)
{
case L2_SLOT_IDLE: case L2_SLOT_IDLE:
// Skip a 5ms slot. // Skip a 5ms slot.
usedSlots = NUM_SLOTS_5MS; usedSlots = NUM_SLOTS_5MS;
@@ -1021,7 +1126,8 @@ static int32_t Linklayer_Routine (L2_INST *instPtr) {
instPtr->rtneConfig.rtneStep += usedSlots; instPtr->rtneConfig.rtneStep += usedSlots;
// check if routine is finished. // check if routine is finished.
if(instPtr->rtneConfig.rtneStep >= instPtr->rtneConfig.rtnePrd){ if (instPtr->rtneConfig.rtneStep >= instPtr->rtneConfig.rtnePrd)
{
L2_EVENT rtneFinishEvent = {L2_ROUTINE_FINISH, &(instPtr->rtneConfig)}; L2_EVENT rtneFinishEvent = {L2_ROUTINE_FINISH, &(instPtr->rtneConfig)};
AssertL2Event(instPtr, &rtneFinishEvent); AssertL2Event(instPtr, &rtneFinishEvent);
} }
@@ -1031,7 +1137,8 @@ static int32_t Linklayer_Routine (L2_INST *instPtr) {
#ifdef DATALINK_ENABLE_AUTORECONNECT #ifdef DATALINK_ENABLE_AUTORECONNECT
IterDeleteNode(instPtr->nodeInfoList, IsNodeConnected); IterDeleteNode(instPtr->nodeInfoList, IsNodeConnected);
if (GetNodeNum(instPtr->nodeInfoList) < instPtr->config.netScale - 1) { if (GetNodeNum(instPtr->nodeInfoList) < instPtr->config.netScale - 1)
{
// reset scan process // reset scan process
L2_EVENT l2Event; L2_EVENT l2Event;
// Assert Net-Setup Done. // Assert Net-Setup Done.
@@ -1055,7 +1162,8 @@ static int32_t Linklayer_Routine (L2_INST *instPtr) {
* Other error code. * Other error code.
* *
******************************************************************************/ ******************************************************************************/
int32_t Linklayer_Proc(L2_INST *instPtr) { int32_t Linklayer_Proc(L2_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
ASSERT(instPtr->config.myType != L2_NONE_TYPE); ASSERT(instPtr->config.myType != L2_NONE_TYPE);
uint32_t usedSlots = 0; uint32_t usedSlots = 0;
@@ -1067,23 +1175,27 @@ int32_t Linklayer_Proc(L2_INST *instPtr) {
L2_FrameStateUpdate(instPtr); L2_FrameStateUpdate(instPtr);
// Handle frame received in last slot. // Handle frame received in last slot.
if(L2_isDataAvail(instPtr) == true){ if (L2_isDataAvail(instPtr) == true)
if(Linklayer_PktHandler(instPtr) != SUCCESS) { {
if (Linklayer_PktHandler(instPtr) != SUCCESS)
{
DBG_ERROR("Failed to handle last received frame: %ld.\n", instPtr->frame.last); DBG_ERROR("Failed to handle last received frame: %ld.\n", instPtr->frame.last);
} }
} }
// Run Link layer routine, setup next slot procedure. // Run Link layer routine, setup next slot procedure.
if (instPtr->mainState == INIT_STATE) { if (instPtr->mainState == INIT_STATE)
{
usedSlots = L2_Scan_Proc(instPtr); usedSlots = L2_Scan_Proc(instPtr);
} }
else { else
{
usedSlots = Linklayer_Routine(instPtr); usedSlots = Linklayer_Routine(instPtr);
} }
// Check if previous runs has error occurred. // Check if previous runs has error occurred.
if (usedSlots == NUM_SLOTS_NONE) { if (usedSlots == NUM_SLOTS_NONE)
{
return FAILURE; return FAILURE;
} }
@@ -1111,7 +1223,8 @@ int32_t Linklayer_Proc(L2_INST *instPtr) {
// Update slot number. // Update slot number.
instPtr->slotNo.next += usedSlots; instPtr->slotNo.next += usedSlots;
if (instPtr->slotNo.next >= L2_SLOTS_PER_FRAME) { // a time frame ended if (instPtr->slotNo.next >= L2_SLOTS_PER_FRAME)
{ // a time frame ended
instPtr->frameNo.next = (instPtr->frameNo.next + 1) % L2_FRAME_NO_MAX; instPtr->frameNo.next = (instPtr->frameNo.next + 1) % L2_FRAME_NO_MAX;
} }
instPtr->slotNo.next %= L2_SLOTS_PER_FRAME; instPtr->slotNo.next %= L2_SLOTS_PER_FRAME;
df_datalink/src/networking/link_layer/link_layer.h
+7 -4
View File
@@ -29,10 +29,11 @@
#define L2_HEAD_NODE_ID 1 #define L2_HEAD_NODE_ID 1
#define L2_MEMBER_NODE_ID 2 #define L2_MEMBER_NODE_ID 2
/***************** Macros (Inline Functions) Definitions *********************/ /***************** Macros (Inline Functions) Definitions *********************/
//#define AssertL2Event(inst, envent) Linklayer_EventHandler(inst, envent) // #define AssertL2Event(inst, event) Linklayer_EventHandler(inst, event)
/**************************** Type Definitions *******************************/ /**************************** Type Definitions *******************************/
typedef struct{ typedef struct
{
L2_RTNE_TABLE_ENTRY **rtneTable; L2_RTNE_TABLE_ENTRY **rtneTable;
uint32_t rtnePrd; uint32_t rtnePrd;
uint32_t rtneStep; uint32_t rtneStep;
@@ -41,7 +42,8 @@ typedef struct{
L2_RTNE_TABLE_ENTRY *next; L2_RTNE_TABLE_ENTRY *next;
} L2_RTNE_CONFIG; } L2_RTNE_CONFIG;
typedef struct { typedef struct
{
L2_NODE_TYPE myType; L2_NODE_TYPE myType;
// bool isInterHead; // bool isInterHead;
// uint8_t clustId; // uint8_t clustId;
@@ -52,7 +54,8 @@ typedef struct {
PHY_INST *phyInstRef; // pyhsical layer reference PHY_INST *phyInstRef; // pyhsical layer reference
} L2_CONFIG; } L2_CONFIG;
typedef struct { typedef struct
{
L2_CONFIG config; L2_CONFIG config;
bool dataAvail; // if there is a valid received frame data in rx buffer bool dataAvail; // if there is a valid received frame data in rx buffer
bool cxn2Net; // if the node is connected to the network bool cxn2Net; // if the node is connected to the network
df_datalink/src/networking/link_layer/link_layer_types.h
+28 -15
View File
@@ -54,13 +54,15 @@
#define AssertL2Event(inst, envent) Linklayer_EventHandler(inst, envent) #define AssertL2Event(inst, envent) Linklayer_EventHandler(inst, envent)
/**************************** Type Definitions *******************************/ /**************************** Type Definitions *******************************/
typedef struct { typedef struct
{
int32_t last; int32_t last;
int32_t curr; int32_t curr;
int32_t next; int32_t next;
} L2_RCD; } L2_RCD;
typedef enum { typedef enum
{
L2_TOR_SUSPEND = 0, L2_TOR_SUSPEND = 0,
L2_TOR_TX = 1, L2_TOR_TX = 1,
L2_TOR_RX = 2 L2_TOR_RX = 2
@@ -68,7 +70,8 @@ typedef enum {
/** @name Prior information type definition /** @name Prior information type definition
*/ */
typedef enum { typedef enum
{
L2_NONE_TYPE = 0, L2_NONE_TYPE = 0,
L2_HEAD_NODE = 1, L2_HEAD_NODE = 1,
L2_MEMBER_NODE = 2, L2_MEMBER_NODE = 2,
@@ -77,17 +80,18 @@ typedef enum {
/** @name Node state type definition /** @name Node state type definition
*/ */
typedef enum { typedef enum
{
NONE_STATE = 0, NONE_STATE = 0,
INIT_STATE = 1, INIT_STATE = 1,
NORMAL_STATE = 2 NORMAL_STATE = 2
} L2_NODE_STATE; } L2_NODE_STATE;
/** @name Node information type definition /** @name Node information type definition
* The following types define the detail node info which needed by the link layer. * The following types define the detail node info which needed by the link layer.
*/ */
typedef struct { typedef struct
{
uint8_t nodeId; uint8_t nodeId;
uint8_t nodeType; uint8_t nodeType;
uint8_t beamNo; uint8_t beamNo;
@@ -99,7 +103,8 @@ typedef struct {
/** @name L2 event enum /** @name L2 event enum
* A enum of L2 event types. * A enum of L2 event types.
*/ */
typedef enum { typedef enum
{
L2_EV_NONE = 0, L2_EV_NONE = 0,
L2_EV_SCAN_PROC_DONE = 1, L2_EV_SCAN_PROC_DONE = 1,
L2_EV_NET_SETUP_DONE = 2, L2_EV_NET_SETUP_DONE = 2,
@@ -116,7 +121,8 @@ typedef enum {
/** @name L2 event /** @name L2 event
* A definition of L2 event instance. * A definition of L2 event instance.
*/ */
typedef struct { typedef struct
{
L2_EVENT_TYPE event; L2_EVENT_TYPE event;
void *msg; void *msg;
} L2_EVENT; } L2_EVENT;
@@ -124,7 +130,8 @@ typedef struct {
/** @name L2 event /** @name L2 event
* A definition of L2 event instance. * A definition of L2 event instance.
*/ */
typedef struct { typedef struct
{
uint8_t eventId; uint8_t eventId;
bool isSchd; bool isSchd;
L2_EVENT l2Event; L2_EVENT l2Event;
@@ -135,7 +142,8 @@ typedef struct {
/** @name Slot types /** @name Slot types
* A definition of different slots. * A definition of different slots.
*/ */
typedef enum { typedef enum
{
L2_SLOT_NONE = 0, L2_SLOT_NONE = 0,
L2_SLOT_IDLE = 1, L2_SLOT_IDLE = 1,
L2_SLOT_SCAN = 2, L2_SLOT_SCAN = 2,
@@ -146,7 +154,8 @@ typedef enum {
/** @name Frame types /** @name Frame types
* A definition of different frame types. * A definition of different frame types.
*/ */
typedef enum { typedef enum
{
L2_NONE_FRAME = 0, L2_NONE_FRAME = 0,
L2_SCAN_FRAME = 1, L2_SCAN_FRAME = 1,
L2_SVC_FRAME = 2, L2_SVC_FRAME = 2,
@@ -156,7 +165,8 @@ typedef enum {
/** @name Frame types /** @name Frame types
* A definition of different frame types. * A definition of different frame types.
*/ */
typedef enum { typedef enum
{
L2_CRC_NONE = 0, L2_CRC_NONE = 0,
L2_CRC16_1021 = 1, L2_CRC16_1021 = 1,
L2_CRC16_8005 = 2, L2_CRC16_8005 = 2,
@@ -166,7 +176,8 @@ typedef enum {
/** @name Schedule subtable entry /** @name Schedule subtable entry
* A definition of Schedule subtable entry. * A definition of Schedule subtable entry.
*/ */
typedef struct { typedef struct
{
L2_SLOT_TYPE slotType : 8; L2_SLOT_TYPE slotType : 8;
uint8_t slotSeq; uint8_t slotSeq;
uint8_t src; uint8_t src;
@@ -175,14 +186,16 @@ typedef struct {
/** @name Scan procedure FSM /** @name Scan procedure FSM
*/ */
typedef struct { typedef struct
{
uint8_t sacanCnt; uint8_t sacanCnt;
uint8_t beamNo; uint8_t beamNo;
} SCAN_FSM; } SCAN_FSM;
/** @name Service procedure FSM /** @name Service procedure FSM
*/ */
typedef struct { typedef struct
{
uint32_t txSeqNum; uint32_t txSeqNum;
uint32_t rxSeqNum; uint32_t rxSeqNum;
uint8_t rxLog[256]; uint8_t rxLog[256];
df_datalink/src/networking/link_layer/node_list.c
+106 -56
View File
@@ -23,7 +23,8 @@
/************************** Constant Definitions *****************************/ /************************** Constant Definitions *****************************/
/***************** Macros (Inline Functions) Definitions *********************/ /***************** Macros (Inline Functions) Definitions *********************/
inline int32_t GetNodeNum(NODE_LIST *list) { inline int32_t GetNodeNum(NODE_LIST *list)
{
ASSERT(list != NULL); ASSERT(list != NULL);
return list->nodeNum; return list->nodeNum;
} }
@@ -43,14 +44,17 @@ inline int32_t GetNodeNum(NODE_LIST *list) {
* ENOMEM - not enough memory. * ENOMEM - not enough memory.
* *
******************************************************************************/ ******************************************************************************/
int32_t NodeList_Init(NODE_LIST **list) { int32_t NodeList_Init(NODE_LIST **list)
{
NODE_ENTRY *entry; NODE_ENTRY *entry;
(*list) = malloc(sizeof(NODE_LIST)); (*list) = malloc(sizeof(NODE_LIST));
if (*list == NULL) { if (*list == NULL)
{
return ENOMEM; return ENOMEM;
} }
entry = malloc(sizeof(NODE_ENTRY)); entry = malloc(sizeof(NODE_ENTRY));
if(entry == NULL) { if (entry == NULL)
{
free(*list); free(*list);
return ENOMEM; return ENOMEM;
} }
@@ -62,11 +66,13 @@ int32_t NodeList_Init(NODE_LIST **list) {
return SUCCESS; return SUCCESS;
} }
void NodeList_Clear(NODE_LIST *list){ void NodeList_Clear(NODE_LIST *list)
{
ASSERT(list != NULL); ASSERT(list != NULL);
NODE_ENTRY *nodeEntry, *deletEntry; NODE_ENTRY *nodeEntry, *deletEntry;
nodeEntry = list->head->nextNode; nodeEntry = list->head->nextNode;
while (nodeEntry != list->head) { while (nodeEntry != list->head)
{
deletEntry = nodeEntry; deletEntry = nodeEntry;
nodeEntry = nodeEntry->nextNode; nodeEntry = nodeEntry->nextNode;
deletEntry->preNode->nextNode = deletEntry->nextNode; deletEntry->preNode->nextNode = deletEntry->nextNode;
@@ -77,8 +83,10 @@ void NodeList_Clear(NODE_LIST *list){
} }
} }
void NodeList_Destroy(NODE_LIST *list){ void NodeList_Destroy(NODE_LIST *list)
if(list == NULL){ {
if (list == NULL)
{
return; return;
} }
NodeList_Clear(list); NodeList_Clear(list);
@@ -97,21 +105,27 @@ void NodeList_Destroy(NODE_LIST *list){
* NULL if node dose not exist. * NULL if node dose not exist.
* *
******************************************************************************/ ******************************************************************************/
NODE_ENTRY* GetNode(NODE_LIST *list, uint32_t nodeId) { NODE_ENTRY *GetNode(NODE_LIST *list, uint32_t nodeId)
{
ASSERT(list != NULL); ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = list->head->nextNode; NODE_ENTRY *nodeEntry = list->head->nextNode;
while (nodeEntry != list->head) { while (nodeEntry != list->head)
if (nodeEntry->nodeInfo.nodeId == nodeId) { {
if (nodeEntry->nodeInfo.nodeId == nodeId)
{
break; break;
} }
else { else
{
nodeEntry = nodeEntry->nextNode; nodeEntry = nodeEntry->nextNode;
} }
} }
if (nodeEntry == list->head) { if (nodeEntry == list->head)
{
return NULL; return NULL;
} }
else { else
{
return nodeEntry; return nodeEntry;
} }
} }
@@ -127,7 +141,8 @@ NODE_ENTRY* GetNode(NODE_LIST *list, uint32_t nodeId) {
* false node not in the list * false node not in the list
* *
******************************************************************************/ ******************************************************************************/
inline bool IsNodeInList(NODE_LIST *list, uint8_t nodeId) { inline bool IsNodeInList(NODE_LIST *list, uint8_t nodeId)
{
ASSERT(list != NULL); ASSERT(list != NULL);
return (GetNode(list, nodeId) != NULL); return (GetNode(list, nodeId) != NULL);
} }
@@ -147,13 +162,15 @@ inline bool IsNodeInList(NODE_LIST *list, uint8_t nodeId) {
int32_t AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType) int32_t AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType)
{ {
ASSERT(list != NULL); ASSERT(list != NULL);
if(IsNodeInList(list, nodeId)){ if (IsNodeInList(list, nodeId))
{
DBG_WARNING("Node [%d] already exist.\n", nodeId); DBG_WARNING("Node [%d] already exist.\n", nodeId);
return FAILURE; return FAILURE;
} }
DBG_INFO("Link layer appending node [%d] to node list.\n", nodeId); DBG_INFO("Link layer appending node [%d] to node list.\n", nodeId);
NODE_ENTRY *nodeEntry = (NODE_ENTRY *)malloc(sizeof(NODE_ENTRY)); NODE_ENTRY *nodeEntry = (NODE_ENTRY *)malloc(sizeof(NODE_ENTRY));
if (nodeEntry == NULL) { if (nodeEntry == NULL)
{
return ENOMEM; return ENOMEM;
} }
nodeEntry->nodeInfo.nodeId = nodeId; nodeEntry->nodeInfo.nodeId = nodeId;
@@ -186,10 +203,12 @@ int32_t AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType)
* Other Node not exist. * Other Node not exist.
* *
******************************************************************************/ ******************************************************************************/
int32_t DeletNode(NODE_LIST *list, uint8_t nodeId) { int32_t DeletNode(NODE_LIST *list, uint8_t nodeId)
{
ASSERT(list != NULL); ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId); NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if(nodeEntry == NULL){ if (nodeEntry == NULL)
{
DBG_WARNING("Node [%d] in not in node list, skip deleting process.\n", nodeId); DBG_WARNING("Node [%d] in not in node list, skip deleting process.\n", nodeId);
return SUCCESS; return SUCCESS;
} }
@@ -201,7 +220,6 @@ int32_t DeletNode(NODE_LIST *list, uint8_t nodeId) {
return SUCCESS; return SUCCESS;
} }
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Get the distance between me and the node. * @brief Get the distance between me and the node.
@@ -215,13 +233,16 @@ int32_t DeletNode(NODE_LIST *list, uint8_t nodeId) {
* *
******************************************************************************/ ******************************************************************************/
int32_t GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double* dst) { int32_t GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double *dst)
{
ASSERT(list != NULL); ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId); NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) { if (nodeEntry == NULL)
{
return FAILURE; return FAILURE;
} }
else { else
{
*dst = Coord_Distance(myCoord, nodeEntry->nodeInfo.pos); *dst = Coord_Distance(myCoord, nodeEntry->nodeInfo.pos);
return SUCCESS; return SUCCESS;
} }
@@ -239,19 +260,21 @@ int32_t GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double* dst)
* FAILURE node not exist. * FAILURE node not exist.
* *
******************************************************************************/ ******************************************************************************/
int32_t UpdateNodePos(NODE_LIST *list, uint8_t nodeId, const COORD *newPos) { int32_t UpdateNodePos(NODE_LIST *list, uint8_t nodeId, const COORD *newPos)
{
ASSERT(list != NULL); ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId); NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) { if (nodeEntry == NULL)
{
return FAILURE; return FAILURE;
} }
else { else
{
memcpy(&(nodeEntry->nodeInfo.pos), newPos, sizeof(COORD)); memcpy(&(nodeEntry->nodeInfo.pos), newPos, sizeof(COORD));
} }
return SUCCESS; return SUCCESS;
} }
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Get node positions information. * @brief Get node positions information.
@@ -264,13 +287,16 @@ int32_t UpdateNodePos(NODE_LIST *list, uint8_t nodeId, const COORD *newPos) {
* FAILURE node not exist. * FAILURE node not exist.
* *
******************************************************************************/ ******************************************************************************/
int32_t GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *pos) { int32_t GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *pos)
{
ASSERT(list != NULL); ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId); NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) { if (nodeEntry == NULL)
{
return FAILURE; return FAILURE;
} }
else { else
{
memcpy(pos, &(nodeEntry->nodeInfo.pos), sizeof(COORD)); memcpy(pos, &(nodeEntry->nodeInfo.pos), sizeof(COORD));
} }
return SUCCESS; return SUCCESS;
@@ -288,19 +314,21 @@ int32_t GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *pos) {
* FAILURE node not exist. * FAILURE node not exist.
* *
******************************************************************************/ ******************************************************************************/
int32_t UpdateNodeSpeed(NODE_LIST *list, uint8_t nodeId, const COORD *newSpeed) { int32_t UpdateNodeSpeed(NODE_LIST *list, uint8_t nodeId, const COORD *newSpeed)
{
ASSERT(list != NULL); ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId); NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) { if (nodeEntry == NULL)
{
return FAILURE; return FAILURE;
} }
else { else
{
memcpy(&(nodeEntry->nodeInfo.speed), newSpeed, sizeof(COORD)); memcpy(&(nodeEntry->nodeInfo.speed), newSpeed, sizeof(COORD));
} }
return SUCCESS; return SUCCESS;
} }
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Get node speed information. * @brief Get node speed information.
@@ -313,13 +341,16 @@ int32_t UpdateNodeSpeed(NODE_LIST *list, uint8_t nodeId, const COORD *newSpeed)
* FAILURE node not exist. * FAILURE node not exist.
* *
******************************************************************************/ ******************************************************************************/
int32_t GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed) { int32_t GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed)
{
ASSERT(list != NULL); ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId); NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) { if (nodeEntry == NULL)
{
return FAILURE; return FAILURE;
} }
else { else
{
memcpy(speed, &(nodeEntry->nodeInfo.speed), sizeof(COORD)); memcpy(speed, &(nodeEntry->nodeInfo.speed), sizeof(COORD));
} }
return SUCCESS; return SUCCESS;
@@ -336,13 +367,16 @@ int32_t GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed) {
* FAILURE Node not exist. * FAILURE Node not exist.
* *
******************************************************************************/ ******************************************************************************/
int32_t UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi) { int32_t UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi)
{
ASSERT(list != NULL); ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId); NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) { if (nodeEntry == NULL)
{
return FAILURE; return FAILURE;
} }
else { else
{
nodeEntry->timeDevi = devi; nodeEntry->timeDevi = devi;
} }
return SUCCESS; return SUCCESS;
@@ -359,13 +393,16 @@ int32_t UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi) {
* @return NONE * @return NONE
* *
******************************************************************************/ ******************************************************************************/
int32_t IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc) { int32_t IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc)
{
ASSERT(list != NULL); ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId); NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) { if (nodeEntry == NULL)
{
return FAILURE; return FAILURE;
} }
else { else
{
nodeEntry->ttlCnt += inc; nodeEntry->ttlCnt += inc;
} }
return SUCCESS; return SUCCESS;
@@ -382,13 +419,16 @@ int32_t IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc) {
* FAILURE Node not exist. * FAILURE Node not exist.
* *
******************************************************************************/ ******************************************************************************/
int32_t RstNodeTTL(NODE_LIST *list, uint8_t nodeId) { int32_t RstNodeTTL(NODE_LIST *list, uint8_t nodeId)
{
ASSERT(list != NULL); ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId); NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) { if (nodeEntry == NULL)
{
return FAILURE; return FAILURE;
} }
else { else
{
nodeEntry->ttlCnt = 0; nodeEntry->ttlCnt = 0;
} }
return SUCCESS; return SUCCESS;
@@ -405,13 +445,16 @@ int32_t RstNodeTTL(NODE_LIST *list, uint8_t nodeId) {
* @return NONE * @return NONE
* *
******************************************************************************/ ******************************************************************************/
int32_t GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl) { int32_t GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl)
{
ASSERT(list != NULL); ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId); NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) { if (nodeEntry == NULL)
{
return FAILURE; return FAILURE;
} }
else { else
{
*ttl = nodeEntry->ttlCnt; *ttl = nodeEntry->ttlCnt;
} }
return SUCCESS; return SUCCESS;
@@ -430,13 +473,16 @@ int32_t GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl) {
* Other Error occurred. * Other Error occurred.
* *
******************************************************************************/ ******************************************************************************/
int32_t IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)(NODE_ENTRY *, const void *)) { int32_t IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)(NODE_ENTRY *, const void *))
{
ASSERT(list != NULL); ASSERT(list != NULL);
int32_t Status = SUCCESS; int32_t Status = SUCCESS;
NODE_ENTRY *nodeEntry = list->head->nextNode; NODE_ENTRY *nodeEntry = list->head->nextNode;
while (nodeEntry != list->head) { while (nodeEntry != list->head)
{
Status = operation(nodeEntry, param); Status = operation(nodeEntry, param);
if(Status != SUCCESS){ if (Status != SUCCESS)
{
break; break;
} }
nodeEntry = nodeEntry->nextNode; nodeEntry = nodeEntry->nextNode;
@@ -455,12 +501,15 @@ int32_t IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)(NO
* @return SUCCESS * @return SUCCESS
* *
******************************************************************************/ ******************************************************************************/
int32_t IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *)) { int32_t IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *))
{
ASSERT(list != NULL); ASSERT(list != NULL);
NODE_ENTRY *nodeEntry, *deletEntry; NODE_ENTRY *nodeEntry, *deletEntry;
nodeEntry = list->head->nextNode; nodeEntry = list->head->nextNode;
while (nodeEntry != list->head) { while (nodeEntry != list->head)
if (condition(nodeEntry) == false) { {
if (condition(nodeEntry) == false)
{
deletEntry = nodeEntry; deletEntry = nodeEntry;
nodeEntry = nodeEntry->nextNode; nodeEntry = nodeEntry->nextNode;
deletEntry->preNode->nextNode = deletEntry->nextNode; deletEntry->preNode->nextNode = deletEntry->nextNode;
@@ -469,7 +518,8 @@ int32_t IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *)) {
free(deletEntry); free(deletEntry);
list->nodeNum--; list->nodeNum--;
} }
else { else
{
nodeEntry = nodeEntry->nextNode; nodeEntry = nodeEntry->nextNode;
} }
} }
df_datalink/src/networking/link_layer/node_list.h
+4 -2
View File
@@ -26,7 +26,8 @@
/************************** Constant Definitions *****************************/ /************************** Constant Definitions *****************************/
/***************** Macros (Inline Functions) Definitions *********************/ /***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/ /**************************** Type Definitions *******************************/
typedef struct NODE_LIST_ENTRY { typedef struct NODE_LIST_ENTRY
{
L2_NODE_INFO nodeInfo; L2_NODE_INFO nodeInfo;
uint32_t ttlCnt; uint32_t ttlCnt;
int32_t timeDevi; int32_t timeDevi;
@@ -34,7 +35,8 @@ typedef struct NODE_LIST_ENTRY {
struct NODE_LIST_ENTRY *preNode; struct NODE_LIST_ENTRY *preNode;
} NODE_ENTRY; } NODE_ENTRY;
typedef struct { typedef struct
{
uint32_t nodeNum; uint32_t nodeNum;
NODE_ENTRY *head; NODE_ENTRY *head;
} NODE_LIST; } NODE_LIST;
df_datalink/src/networking/link_layer/routine_table.c
+802 -410
View File
File diff suppressed because it is too large Load Diff
df_datalink/src/networking/link_layer/scan.c
+72 -36
View File
@@ -45,17 +45,21 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type);
* SUCCESS * SUCCESS
* *
******************************************************************************/ ******************************************************************************/
static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) { static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
int32_t Status; int32_t Status;
uint8_t *buff; uint8_t *buff;
uint32_t packLen; uint32_t packLen;
switch (type) { switch (type)
case L2_INFO_FRAME:{ {
case L2_INFO_FRAME:
{
packLen = RSP_PACK_LEN; packLen = RSP_PACK_LEN;
SysMonitor_GetSysinfo(&(instPtr->sysInfo)); SysMonitor_GetSysinfo(&(instPtr->sysInfo));
buff = malloc(packLen); buff = malloc(packLen);
if (buff == NULL) { if (buff == NULL)
{
DBG_ERROR("Not enough space, failed to pack frame...\n"); DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM; return ENOMEM;
} }
@@ -69,17 +73,20 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
memcpy(buff + L2_MSG_INFO_SPEEDZ, &(instPtr->sysInfo.vZ), 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); Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("Fail to append CRC, error code: %ld.\n", Status); DBG_ERROR("Fail to append CRC, error code: %ld.\n", Status);
return Status; return Status;
} }
if(CircQueuePush(instPtr->txQueue, buff) == false){ if (CircQueuePush(instPtr->txQueue, buff) == false)
{
DBG_ERROR("Cannot push packet to queue..\n"); DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE; return EQUEUE;
} }
break; break;
} }
default :{ default:
{
DBG_ERROR("Unexpected frame type: %d.\n", type); DBG_ERROR("Unexpected frame type: %d.\n", type);
return EPARAM; return EPARAM;
break; break;
@@ -97,7 +104,8 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
* @return * @return
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_Scan_Init(L2_INST *instPtr) { int32_t L2_Scan_Init(L2_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
instPtr->scanFsm.sacanCnt = 0; instPtr->scanFsm.sacanCnt = 0;
instPtr->scanFsm.beamNo = 0; instPtr->scanFsm.beamNo = 0;
@@ -113,25 +121,32 @@ int32_t L2_Scan_Init(L2_INST *instPtr) {
* @return * @return
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) { int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
ASSERT(msg != NULL); ASSERT(msg != NULL);
int32_t Status; int32_t Status;
switch (instPtr->frame.last) { switch (instPtr->frame.last)
case L2_SCAN_FRAME: { {
if (instPtr->config.myType == L2_HEAD_NODE) { case L2_SCAN_FRAME:
{
if (instPtr->config.myType == L2_HEAD_NODE)
{
DBG_ERROR("Unexpected call from head node, Only None-Head Node can handle L2_SVC_FRAME.\n"); DBG_ERROR("Unexpected call from head node, Only None-Head Node can handle L2_SVC_FRAME.\n");
Status = EUNEXP; Status = EUNEXP;
break; break;
} }
Status = AppendNode(instPtr->nodeInfoList, L2_HEAD_NODE_ID, L2_HEAD_NODE); Status = AppendNode(instPtr->nodeInfoList, L2_HEAD_NODE_ID, L2_HEAD_NODE);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
} }
break; break;
} }
case L2_INFO_FRAME: { case L2_INFO_FRAME:
if (instPtr->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"); DBG_ERROR("Unexpected call from member node, Only Head Node can handle L2_SVC_FRAME.\n");
Status = EUNEXP; Status = EUNEXP;
break; break;
@@ -139,7 +154,8 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) {
COORD coordTemp; COORD coordTemp;
float tmpVal; float tmpVal;
Status = AppendNode(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, L2_MEMBER_NODE); Status = AppendNode(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, L2_MEMBER_NODE);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
break; break;
} }
@@ -150,7 +166,8 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) {
memcpy(&tmpVal, msg + L2_MSG_INFO_POSZ, sizeof(float)); memcpy(&tmpVal, msg + L2_MSG_INFO_POSZ, sizeof(float));
coordTemp.z = tmpVal; coordTemp.z = tmpVal;
Status = UpdateNodePos(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp); Status = UpdateNodePos(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("Failed to update node %d position, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); DBG_ERROR("Failed to update node %d position, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
} }
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDX, sizeof(float)); memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDX, sizeof(float));
@@ -160,26 +177,32 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) {
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDZ, sizeof(float)); memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDZ, sizeof(float));
coordTemp.z = tmpVal; coordTemp.z = tmpVal;
Status = UpdateNodeSpeed(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp); Status = UpdateNodeSpeed(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("Failed to update node %d speed, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); DBG_ERROR("Failed to update node %d speed, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
} }
// Update Time deviation // Update Time deviation
int64_t devi, deviTrunc; int64_t devi, deviTrunc;
Status = L2_TimeSync_GetDeviatoion(instPtr, &devi); Status = L2_TimeSync_GetDeviatoion(instPtr, &devi);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("Failed to compute slot deviation.\n"); DBG_ERROR("Failed to compute slot deviation.\n");
UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, 0); UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, 0);
} }
else { else
if(devi > INT32_MAX || devi < INT32_MIN) { {
if (devi > INT32_MAX || devi < INT32_MIN)
{
DBG_ERROR("Illegal deviation value: %lld.\n", devi); DBG_ERROR("Illegal deviation value: %lld.\n", devi);
deviTrunc = 0; deviTrunc = 0;
} }
else if(devi > L2_MSG_SVC_TMSYNC_MAX || devi < L2_MSG_SVC_TMSYNC_MIN) { else if (devi > L2_MSG_SVC_TMSYNC_MAX || devi < L2_MSG_SVC_TMSYNC_MIN)
{
DBG_WARNING("Deviation value: %lld is out of adjustable range.\n", devi); DBG_WARNING("Deviation value: %lld is out of adjustable range.\n", devi);
deviTrunc = (int32_t)devi; deviTrunc = (int32_t)devi;
} }
else { else
{
deviTrunc = (int32_t)devi; deviTrunc = (int32_t)devi;
} }
UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc); UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc);
@@ -193,7 +216,8 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) {
} }
// Net setup finished. // Net setup finished.
if(L2_GetMainState(instPtr) == INIT_STATE && Status == SUCCESS){ if (L2_GetMainState(instPtr) == INIT_STATE && Status == SUCCESS)
{
L2_EVENT l2Event; L2_EVENT l2Event;
L2_RTNE_CONFIG newRtneConfig; L2_RTNE_CONFIG newRtneConfig;
@@ -222,21 +246,25 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) {
* @return used slot (5ms slot). * @return used slot (5ms slot).
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_Scan_Proc(L2_INST *instPtr) { int32_t L2_Scan_Proc(L2_INST *instPtr)
{
int32_t Status; int32_t Status;
/* instPtr->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. * Skip 5ms for nodes for node configuration update.
*/ */
DBG_INFO("-------- Scan Process --------\n"); DBG_INFO("-------- Scan Process --------\n");
if (instPtr->nextState == NORMAL_STATE) { if (instPtr->nextState == NORMAL_STATE)
{
DBG_INFO("NETSET DONE, halt 20ms before normal routine.\n"); DBG_INFO("NETSET DONE, halt 20ms before normal routine.\n");
if(instPtr->config.myType != L2_HEAD_NODE) { if (instPtr->config.myType != L2_HEAD_NODE)
{
Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0); Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_120); Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_120);
Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE); Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_SCAN_FRAME); L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_SCAN_FRAME);
} }
else { else
{
DBG_ERROR("Illegal call from Head Node.\n"); DBG_ERROR("Illegal call from Head Node.\n");
} }
return NUM_SLOTS_SKIP; return NUM_SLOTS_SKIP;
@@ -249,16 +277,19 @@ int32_t L2_Scan_Proc(L2_INST *instPtr) {
* Increase beam number after a Tx-Rx procedure pair, reset beam number when * Increase beam number after a Tx-Rx procedure pair, reset beam number when
* it reaches the boundary. * it reaches the boundary.
*/ */
if (instPtr->config.myType == L2_HEAD_NODE) { if (instPtr->config.myType == L2_HEAD_NODE)
{
// Apply a scan procedure // Apply a scan procedure
if (instPtr->scanFsm.sacanCnt % 2 == 0){ if (instPtr->scanFsm.sacanCnt % 2 == 0)
{
// Antenna configure // Antenna configure
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8); Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetBeamNo(instPtr->config.phyInstRef, instPtr->scanFsm.beamNo); Phy_SetBeamNo(instPtr->config.phyInstRef, instPtr->scanFsm.beamNo);
Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE); Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(instPtr, L2_TOR_TX, L2_SCAN_FRAME); L2_SetupNextSlot(instPtr, L2_TOR_TX, L2_SCAN_FRAME);
} }
else { else
{
// Antenna configure // Antenna configure
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8); Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetBeamNo(instPtr->config.phyInstRef, instPtr->scanFsm.beamNo); Phy_SetBeamNo(instPtr->config.phyInstRef, instPtr->scanFsm.beamNo);
@@ -275,18 +306,22 @@ int32_t L2_Scan_Proc(L2_INST *instPtr) {
* So that, a extra datagram with the up-to-date node information is attached * So that, a extra datagram with the up-to-date node information is attached
* to the listening configuration. * to the listening configuration.
*/ */
else { else
{
uint8_t *sendData; uint8_t *sendData;
// Pack node information. // Pack node information.
Status = ScanMsg_Packing(instPtr, L2_INFO_FRAME); Status = ScanMsg_Packing(instPtr, L2_INFO_FRAME);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_INFO("Failed to pack response.\n"); DBG_INFO("Failed to pack response.\n");
} }
if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true) { if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true)
{
memcpy(instPtr->sendPktData, sendData, RSP_PACK_LEN); memcpy(instPtr->sendPktData, sendData, RSP_PACK_LEN);
free(sendData); free(sendData);
} }
else { else
{
DBG_INFO("Failed to pop tx Queue.\n"); DBG_INFO("Failed to pop tx Queue.\n");
} }
// Antenna configure // Antenna configure
@@ -311,7 +346,8 @@ int32_t L2_Scan_Proc(L2_INST *instPtr) {
* @return used slot (5ms slot). * @return used slot (5ms slot).
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_Scan_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry) { int32_t L2_Scan_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry)
{
DBG_INFO("-------- Scan Routine --------\n"); DBG_INFO("-------- Scan Routine --------\n");
ASSERT(false); // Not support. ASSERT(false); // Not support.
return 0; return 0;
df_datalink/src/networking/link_layer/service.c
+133 -73
View File
@@ -42,8 +42,7 @@ static uint8_t berTestData[SVC_PACK_LEN] = {
0x27, 0x4E, 0xFB, 0x39, 0x11, 0xE7, 0x12, 0xDA, 0x27, 0x4E, 0xFB, 0x39, 0x11, 0xE7, 0x12, 0xDA,
0xDF, 0xD2, 0x27, 0x8F, 0xC9, 0x42, 0xCD, 0x0C, 0xDF, 0xD2, 0x27, 0x8F, 0xC9, 0x42, 0xCD, 0x0C,
0x76, 0x1C, 0x81, 0x87, 0x69, 0xC9, 0x60, 0xFD, 0x76, 0x1C, 0x81, 0x87, 0x69, 0xC9, 0x60, 0xFD,
0xC4, 0x27, 0xA5, 0x09, 0xE5, 0x71 0xC4, 0x27, 0xA5, 0x09, 0xE5, 0x71};
};
#endif #endif
/*****************************************************************************/ /*****************************************************************************/
@@ -58,20 +57,24 @@ static uint8_t berTestData[SVC_PACK_LEN] = {
* SUCCESS * SUCCESS
* *
******************************************************************************/ ******************************************************************************/
static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) { static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type)
{
#ifndef DATALINK_ENABLE_BERTEST #ifndef DATALINK_ENABLE_BERTEST
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
int32_t Status; int32_t Status;
uint8_t *buff; uint8_t *buff;
uint32_t packLen; uint32_t packLen;
switch (type) { switch (type)
case L2_SVC_FRAME:{ {
case L2_SVC_FRAME:
{
const NODE_ENTRY *nodeEntry; const NODE_ENTRY *nodeEntry;
int32_t timeDevi; int32_t timeDevi;
packLen = SVC_PACK_LEN; packLen = SVC_PACK_LEN;
buff = malloc(packLen); buff = malloc(packLen);
if (buff == NULL) { if (buff == NULL)
{
DBG_ERROR("Not enough space, failed to pack frame...\n"); DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM; return ENOMEM;
} }
@@ -80,22 +83,28 @@ static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
// TODO: Check Pack TimeSync info, and better approch?. // TODO: Check Pack TimeSync info, and better approch?.
nodeEntry = GetNode(instPtr->nodeInfoList, L2_MEMBER_NODE_ID); nodeEntry = GetNode(instPtr->nodeInfoList, L2_MEMBER_NODE_ID);
if(nodeEntry != NULL) { if (nodeEntry != NULL)
{
// check if deviation info outdated. // check if deviation info outdated.
if(nodeEntry->ttlCnt <= NUM_SERVICE_PRD_SLOTS) { if (nodeEntry->ttlCnt <= NUM_SERVICE_PRD_SLOTS)
{
// Member node should adjust in the opposite way. // Member node should adjust in the opposite way.
timeDevi = -nodeEntry->timeDevi; timeDevi = -nodeEntry->timeDevi;
} }
else { else
{
DBG_WARNING("Time deviation is outdated, omitted it.\n"); DBG_WARNING("Time deviation is outdated, omitted it.\n");
} }
} }
else { else
{
DBG_ERROR("Fail to get time deviation...\n"); DBG_ERROR("Fail to get time deviation...\n");
timeDevi = 0; timeDevi = 0;
} }
if(timeDevi > L2_MSG_SVC_TMSYNC_MAX || timeDevi < L2_MSG_SVC_TMSYNC_MIN) { if (timeDevi > L2_MSG_SVC_TMSYNC_MAX || timeDevi < L2_MSG_SVC_TMSYNC_MIN)
do { {
do
{
timeDevi /= 2; timeDevi /= 2;
} while (timeDevi > L2_MSG_SVC_TMSYNC_MAX || timeDevi < L2_MSG_SVC_TMSYNC_MIN); } while (timeDevi > L2_MSG_SVC_TMSYNC_MAX || timeDevi < L2_MSG_SVC_TMSYNC_MIN);
DBG_WARNING("Deviation is out of adjustable range, truncate it to %d.\n", timeDevi); DBG_WARNING("Deviation is out of adjustable range, truncate it to %d.\n", timeDevi);
@@ -109,37 +118,42 @@ static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
memcpy(buff + L2_MSG_SVC_PAYLOAD, memcpy(buff + L2_MSG_SVC_PAYLOAD,
instPtr->serviceFsm.txLog + L2_MSG_SVC_PAYLOAD_LEN * instPtr->serviceFsm.txSeqNum, instPtr->serviceFsm.txLog + L2_MSG_SVC_PAYLOAD_LEN * instPtr->serviceFsm.txSeqNum,
L2_MSG_SVC_PAYLOAD_LEN); L2_MSG_SVC_PAYLOAD_LEN);
if (instPtr->config.myType == L2_HEAD_NODE) { if (instPtr->config.myType == L2_HEAD_NODE)
{
// if last packet reset counting // if last packet reset counting
instPtr->serviceFsm.txSeqNum = (instPtr->serviceFsm.txSeqNum instPtr->serviceFsm.txSeqNum = (instPtr->serviceFsm.txSeqNum >= (PAYLOAD_FLJ - 1) / L2_MSG_SVC_PAYLOAD + 1)
>= (PAYLOAD_FLJ-1)/L2_MSG_SVC_PAYLOAD+1) ? 0
? 0 : instPtr->serviceFsm.txSeqNum + 1; : instPtr->serviceFsm.txSeqNum + 1;
} }
else{ else
{
// if last packet reset counting // if last packet reset counting
instPtr->serviceFsm.txSeqNum = (instPtr->serviceFsm.txSeqNum instPtr->serviceFsm.txSeqNum = (instPtr->serviceFsm.txSeqNum >= (PAYLOAD_LJ - 1) / L2_MSG_SVC_PAYLOAD + 1)
>= (PAYLOAD_LJ-1)/L2_MSG_SVC_PAYLOAD+1) ? 0
? 0 : instPtr->serviceFsm.txSeqNum + 1; : instPtr->serviceFsm.txSeqNum + 1;
} }
Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021); Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("Fail to append CRC...\n"); DBG_ERROR("Fail to append CRC...\n");
return Status; return Status;
} }
if(CircQueuePush(instPtr->txQueue, buff) == false){ if (CircQueuePush(instPtr->txQueue, buff) == false)
{
DBG_ERROR("Cannot push packet to queue..\n"); DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE; return EQUEUE;
} }
break; break;
} }
case L2_INFO_FRAME:{ case L2_INFO_FRAME:
{
packLen = RSP_PACK_LEN; packLen = RSP_PACK_LEN;
SysMonitor_GetSysinfo(&(instPtr->sysInfo)); SysMonitor_GetSysinfo(&(instPtr->sysInfo));
buff = malloc(packLen); buff = malloc(packLen);
if (buff == NULL) { if (buff == NULL)
{
DBG_ERROR("Not enough space, failed to pack frame...\n"); DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM; return ENOMEM;
} }
@@ -153,17 +167,20 @@ static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
memcpy(buff + L2_MSG_INFO_SPEEDZ, &(instPtr->sysInfo.vZ), 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); Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("Fail to append CRC...\n"); DBG_ERROR("Fail to append CRC...\n");
return Status; return Status;
} }
if(CircQueuePush(instPtr->txQueue, buff) == false){ if (CircQueuePush(instPtr->txQueue, buff) == false)
{
DBG_ERROR("Cannot push packet to queue..\n"); DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE; return EQUEUE;
} }
break; break;
} }
default :{ default:
{
DBG_ERROR("Unexpected frame type: %d.\n", type); DBG_ERROR("Unexpected frame type: %d.\n", type);
return EPARAM; return EPARAM;
break; break;
@@ -176,7 +193,6 @@ static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
#endif #endif
} }
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Initialize scan FSM. * @brief Initialize scan FSM.
@@ -186,7 +202,8 @@ static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
* @return * @return
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_Service_Init(L2_INST *instPtr) { int32_t L2_Service_Init(L2_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
instPtr->serviceFsm.txSeqNum = 0; instPtr->serviceFsm.txSeqNum = 0;
instPtr->serviceFsm.rxSeqNum = 0; instPtr->serviceFsm.rxSeqNum = 0;
@@ -211,13 +228,17 @@ int32_t L2_Service_Init(L2_INST *instPtr) {
* other error occurred. * other error occurred.
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) { int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
ASSERT(msg != NULL); ASSERT(msg != NULL);
int32_t Status; int32_t Status;
switch (instPtr->frame.last) { switch (instPtr->frame.last)
case L2_SVC_FRAME: { {
if (instPtr->config.myType == L2_HEAD_NODE) { case L2_SVC_FRAME:
{
if (instPtr->config.myType == L2_HEAD_NODE)
{
DBG_ERROR("Unexpected call, Only None-Head Node can handle L2_SVC_FRAME.\n"); DBG_ERROR("Unexpected call, Only None-Head Node can handle L2_SVC_FRAME.\n");
Status = EUNEXP; Status = EUNEXP;
break; break;
@@ -241,43 +262,55 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) {
timeAdj |= (timeAdj & 0x0000000000800000) ? 0xFFFFFFFFFF000000 : 0x0000000000000000; timeAdj |= (timeAdj & 0x0000000000800000) ? 0xFFFFFFFFFF000000 : 0x0000000000000000;
// FIXME: better approach? // FIXME: better approach?
// Implement a time sync procedure, first time received a time deviation in a service period. // Implement a time sync procedure, first time received a time deviation in a service period.
if (seqNum % NUM_SERVICE_PRD_SLOTS == 0 || instPtr->serviceFsm.seqNum != seqNum) { if (seqNum % NUM_SERVICE_PRD_SLOTS == 0 || instPtr->serviceFsm.seqNum != seqNum)
{
AssertL2Event(instPtr, &l2Event); AssertL2Event(instPtr, &l2Event);
} }
#endif #endif
#ifndef DATALINK_ENABLE_BERTEST #ifndef DATALINK_ENABLE_BERTEST
memcpy(instPtr->serviceFsm.rxLog + L2_MSG_SVC_PAYLOAD_LEN * seqNum, msg + L2_MSG_SVC_PAYLOAD, L2_MSG_SVC_PAYLOAD_LEN); 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 (instPtr->config.myType == L2_HEAD_NODE)
{
// if last packet reset counting // if last packet reset counting
if(seqNum >= (PAYLOAD_FLJ-1)/L2_MSG_SVC_PAYLOAD+1) { if (seqNum >= (PAYLOAD_FLJ - 1) / L2_MSG_SVC_PAYLOAD + 1)
if(seqNum == instPtr->serviceFsm.rxSeqNum) { {
if (seqNum == instPtr->serviceFsm.rxSeqNum)
{
SysMonitor_UpdateRecvPayloadLJRaw(instPtr->serviceFsm.rxLog); SysMonitor_UpdateRecvPayloadLJRaw(instPtr->serviceFsm.rxLog);
} }
instPtr->serviceFsm.rxSeqNum = 0; instPtr->serviceFsm.rxSeqNum = 0;
} }
else { else
if(instPtr->serviceFsm.rxSeqNum == seqNum){ {
if (instPtr->serviceFsm.rxSeqNum == seqNum)
{
instPtr->serviceFsm.rxSeqNum++; instPtr->serviceFsm.rxSeqNum++;
} }
} }
} }
else{ else
{
// if last packet reset counting // if last packet reset counting
if(seqNum >= (PAYLOAD_LJ-1)/L2_MSG_SVC_PAYLOAD+1) { if (seqNum >= (PAYLOAD_LJ - 1) / L2_MSG_SVC_PAYLOAD + 1)
if(seqNum == instPtr->serviceFsm.rxSeqNum) { {
if (seqNum == instPtr->serviceFsm.rxSeqNum)
{
SysMonitor_UpdateRecvPayloadFLJRaw(instPtr->serviceFsm.rxLog); SysMonitor_UpdateRecvPayloadFLJRaw(instPtr->serviceFsm.rxLog);
} }
instPtr->serviceFsm.rxSeqNum = 0; instPtr->serviceFsm.rxSeqNum = 0;
} }
else { else
if(instPtr->serviceFsm.rxSeqNum == seqNum){ {
if (instPtr->serviceFsm.rxSeqNum == seqNum)
{
instPtr->serviceFsm.rxSeqNum++; instPtr->serviceFsm.rxSeqNum++;
} }
} }
} }
#else #else
for(int i = 0; i < SVC_PACK_LEN; i++) { for (int i = 0; i < SVC_PACK_LEN; i++)
{
instPtr->serviceFsm.bitError += count_ones(msg[i] ^ berTestData[i]); instPtr->serviceFsm.bitError += count_ones(msg[i] ^ berTestData[i]);
} }
instPtr->serviceFsm.bitTotal += SVC_PACK_LEN * 8; instPtr->serviceFsm.bitTotal += SVC_PACK_LEN * 8;
@@ -285,13 +318,16 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) {
instPtr->serviceFsm.pktRecved++; instPtr->serviceFsm.pktRecved++;
// Reset Node TTL // Reset Node TTL
Status = RstNodeTTL(instPtr->nodeInfoList, L2_HEAD_NODE_ID); Status = RstNodeTTL(instPtr->nodeInfoList, L2_HEAD_NODE_ID);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("Failed to reset node %d TTL, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); DBG_ERROR("Failed to reset node %d TTL, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
} }
break; break;
} }
case L2_INFO_FRAME: { case L2_INFO_FRAME:
if (instPtr->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"); DBG_ERROR("Unexpected call, Only Head Node can handle L2_SVC_FRAME.\n");
Status = EUNEXP; Status = EUNEXP;
break; break;
@@ -306,7 +342,8 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) {
memcpy(&tmpVal, msg + L2_MSG_INFO_POSZ, sizeof(float)); memcpy(&tmpVal, msg + L2_MSG_INFO_POSZ, sizeof(float));
coordTemp.z = tmpVal; coordTemp.z = tmpVal;
Status = UpdateNodePos(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp); Status = UpdateNodePos(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("Failed to update node %d position, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); DBG_ERROR("Failed to update node %d position, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
} }
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDX, sizeof(float)); memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDX, sizeof(float));
@@ -316,7 +353,8 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) {
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDZ, sizeof(float)); memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDZ, sizeof(float));
coordTemp.z = tmpVal; coordTemp.z = tmpVal;
Status = UpdateNodeSpeed(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp); Status = UpdateNodeSpeed(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("Failed to update node %d speed, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); DBG_ERROR("Failed to update node %d speed, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
} }
@@ -324,30 +362,37 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) {
// Update Time deviation // Update Time deviation
int64_t devi, deviTrunc; int64_t devi, deviTrunc;
Status = L2_TimeSync_GetDeviatoion(instPtr, &devi); Status = L2_TimeSync_GetDeviatoion(instPtr, &devi);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("Failed to compute slot deviation.\n"); DBG_ERROR("Failed to compute slot deviation.\n");
deviTrunc = 0; deviTrunc = 0;
} }
else { else
if(devi > INT32_MAX || devi < INT32_MIN) { {
if (devi > INT32_MAX || devi < INT32_MIN)
{
DBG_WARNING("Illegal deviation value: %lld, omit it.\n", devi); DBG_WARNING("Illegal deviation value: %lld, omit it.\n", devi);
deviTrunc = 0; deviTrunc = 0;
} }
else if(devi > L2_MSG_SVC_TMSYNC_MAX || devi < L2_MSG_SVC_TMSYNC_MIN) { else if (devi > L2_MSG_SVC_TMSYNC_MAX || devi < L2_MSG_SVC_TMSYNC_MIN)
{
DBG_WARNING("Deviation value: %lld is out of adjustable range.\n", devi); DBG_WARNING("Deviation value: %lld is out of adjustable range.\n", devi);
deviTrunc = (int32_t)devi; deviTrunc = (int32_t)devi;
} }
else { else
{
deviTrunc = (int32_t)devi; deviTrunc = (int32_t)devi;
} }
UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc); UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc);
} }
Status = UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc); Status = UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("Failed to update node %d time deviation, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); DBG_ERROR("Failed to update node %d time deviation, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
} }
#else #else
for(int i = 0; i < SVC_PACK_LEN; i++) { for (int i = 0; i < SVC_PACK_LEN; i++)
{
instPtr->serviceFsm.bitError += count_ones(msg[i] ^ berTestData[i]); instPtr->serviceFsm.bitError += count_ones(msg[i] ^ berTestData[i]);
} }
instPtr->serviceFsm.bitTotal += SVC_PACK_LEN * 8; instPtr->serviceFsm.bitTotal += SVC_PACK_LEN * 8;
@@ -355,7 +400,8 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) {
instPtr->serviceFsm.pktRecved++; instPtr->serviceFsm.pktRecved++;
// Reset Node TTL // Reset Node TTL
Status = RstNodeTTL(instPtr->nodeInfoList, L2_MEMBER_NODE_ID); Status = RstNodeTTL(instPtr->nodeInfoList, L2_MEMBER_NODE_ID);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("Failed to reset node %d ttl, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status); DBG_ERROR("Failed to reset node %d ttl, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
} }
break; break;
@@ -377,42 +423,53 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) {
* @return Slot has been used. * @return Slot has been used.
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_Service_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry) { int32_t L2_Service_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
DBG_INFO("-------- ServiceNet Routine --------\n"); DBG_INFO("-------- ServiceNet Routine --------\n");
uint8_t *sendData; uint8_t *sendData;
int32_t usedSlots; int32_t usedSlots;
L2_TOR_TYPE torState; L2_TOR_TYPE torState;
if(schedEntry->src == instPtr->config.myId){ if (schedEntry->src == instPtr->config.myId)
{
torState = L2_TOR_TX; torState = L2_TOR_TX;
} }
else if(schedEntry->dst == instPtr->config.myId){ else if (schedEntry->dst == instPtr->config.myId)
{
torState = L2_TOR_RX; torState = L2_TOR_RX;
instPtr->serviceFsm.pktRecv++; instPtr->serviceFsm.pktRecv++;
} }
else{ else
{
torState = L2_TOR_SUSPEND; torState = L2_TOR_SUSPEND;
} }
switch(schedEntry->slotType){ switch (schedEntry->slotType)
{
case L2_SLOT_SERVICE: case L2_SLOT_SERVICE:
if(torState == L2_TOR_TX){ if (torState == L2_TOR_TX)
{
#ifndef DATALINK_ENABLE_BERTEST #ifndef DATALINK_ENABLE_BERTEST
if (instPtr->serviceFsm.txSeqNum == 0) { if (instPtr->serviceFsm.txSeqNum == 0)
if(instPtr->config.myType == L2_HEAD_NODE){ {
if (instPtr->config.myType == L2_HEAD_NODE)
{
SysMonitor_GetSendPayloadFLJRaw(instPtr->serviceFsm.txLog); SysMonitor_GetSendPayloadFLJRaw(instPtr->serviceFsm.txLog);
} }
else { else
{
SysMonitor_GetSendPayloadLJRaw(instPtr->serviceFsm.txLog); SysMonitor_GetSendPayloadLJRaw(instPtr->serviceFsm.txLog);
} }
} }
// Packing service data // Packing service data
Service_Packing(instPtr, L2_SVC_FRAME); Service_Packing(instPtr, L2_SVC_FRAME);
if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true) { if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true)
{
memcpy(instPtr->sendPktData, sendData, SVC_PACK_LEN); memcpy(instPtr->sendPktData, sendData, SVC_PACK_LEN);
free(sendData); free(sendData);
} }
else { else
{
DBG_ERROR("Failed to pop tx queue.\n"); DBG_ERROR("Failed to pop tx queue.\n");
} }
#else #else
@@ -428,14 +485,17 @@ int32_t L2_Service_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry) {
break; break;
#ifndef DATALINK_ENABLE_BERTEST #ifndef DATALINK_ENABLE_BERTEST
case L2_SLOT_ADVERTISE: case L2_SLOT_ADVERTISE:
if(torState == L2_TOR_TX){ if (torState == L2_TOR_TX)
{
// Packing response data. // Packing response data.
Service_Packing(instPtr, L2_INFO_FRAME); Service_Packing(instPtr, L2_INFO_FRAME);
if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true) { if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true)
{
memcpy(instPtr->sendPktData, sendData, RSP_PACK_LEN); memcpy(instPtr->sendPktData, sendData, RSP_PACK_LEN);
free(sendData); free(sendData);
} }
else { else
{
DBG_ERROR("Failed to pop tx queue.\n"); DBG_ERROR("Failed to pop tx queue.\n");
} }
} }
df_datalink/src/networking/link_layer/time_sync.c
+10 -7
View File
@@ -60,7 +60,8 @@
* @return * @return
* *
******************************************************************************/ ******************************************************************************/
inline int32_t L2_TimeSync_LocalTimerAdd(L2_INST *instPtr, int64_t adjustment) { inline int32_t L2_TimeSync_LocalTimerAdd(L2_INST *instPtr, int64_t adjustment)
{
// read nextSendTime from physical layer // read nextSendTime from physical layer
Phy_SetTimeWord(instPtr->config.phyInstRef, adjustment, PHY_TIME_WORD_LOCAL_ADJ); Phy_SetTimeWord(instPtr->config.phyInstRef, adjustment, PHY_TIME_WORD_LOCAL_ADJ);
PhyWrite(instPtr->config.phyInstRef, TM_ADJ_MODE, TM_ADJ_MODE_ADD); PhyWrite(instPtr->config.phyInstRef, TM_ADJ_MODE, TM_ADJ_MODE_ADD);
@@ -76,11 +77,13 @@ inline int32_t L2_TimeSync_LocalTimerAdd(L2_INST *instPtr, int64_t adjustment) {
* @return * @return
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_TimeSync_SlotAlign(L2_INST *instPtr) { int32_t L2_TimeSync_SlotAlign(L2_INST *instPtr)
{
int32_t Status; int32_t Status;
int64_t slotAdj; int64_t slotAdj;
Status = L2_TimeSync_GetDeviatoion(instPtr, &slotAdj); Status = L2_TimeSync_GetDeviatoion(instPtr, &slotAdj);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
DBG_ERROR("Failed to compute slot deviation.\n"); DBG_ERROR("Failed to compute slot deviation.\n");
return Status; return Status;
} }
@@ -97,7 +100,8 @@ int32_t L2_TimeSync_SlotAlign(L2_INST *instPtr) {
* @return * @return
* *
******************************************************************************/ ******************************************************************************/
int32_t L2_TimeSync_GetDeviatoion(L2_INST *instPtr, int64_t *devi) { int32_t L2_TimeSync_GetDeviatoion(L2_INST *instPtr, int64_t *devi)
{
// read nextSendTime from physical layer // read nextSendTime from physical layer
int64_t currentTarget, lastFrameSync, slotMeasure; int64_t currentTarget, lastFrameSync, slotMeasure;
currentTarget = PhyRead(instPtr->config.phyInstRef, TM_TARGET_L); currentTarget = PhyRead(instPtr->config.phyInstRef, TM_TARGET_L);
@@ -106,7 +110,8 @@ int32_t L2_TimeSync_GetDeviatoion(L2_INST *instPtr, int64_t *devi) {
lastFrameSync |= ((int64_t)PhyRead(instPtr->config.phyInstRef, FSTIME_H)) << 32; lastFrameSync |= ((int64_t)PhyRead(instPtr->config.phyInstRef, FSTIME_H)) << 32;
slotMeasure = (currentTarget > lastFrameSync) ? (currentTarget - lastFrameSync) slotMeasure = (currentTarget > lastFrameSync) ? (currentTarget - lastFrameSync)
: (currentTarget - lastFrameSync + PHY_LOCAL_CNT_MAX); : (currentTarget - lastFrameSync + PHY_LOCAL_CNT_MAX);
switch (Phy_GetLastDetectedFrameType(instPtr->config.phyInstRef)) { switch (Phy_GetLastDetectedFrameType(instPtr->config.phyInstRef))
{
case L1_2KBPS_20MS_FRAME: case L1_2KBPS_20MS_FRAME:
*devi = ((int64_t)PHY_SYNCT2FEND_2K_20MS) - slotMeasure; *devi = ((int64_t)PHY_SYNCT2FEND_2K_20MS) - slotMeasure;
break; break;
@@ -119,5 +124,3 @@ int32_t L2_TimeSync_GetDeviatoion(L2_INST *instPtr, int64_t *devi) {
} }
return SUCCESS; return SUCCESS;
} }
df_datalink/src/networking/phy_layer/phy_antenna.c
+14 -7
View File
@@ -230,11 +230,14 @@ int32_t Phy_AntConfigInit(PHY_ANT_COINFO *antConfigPtr)
int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo) int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo)
{ {
if(instPtr->beamWd.next == PHY_ANT_BEAM_8){ if (instPtr->beamWd.next == PHY_ANT_BEAM_8)
if (beamNo >= L1_NUM_ANT_NRRW_PATTERN) { {
if (beamNo >= L1_NUM_ANT_NRRW_PATTERN)
{
return FAILURE; return FAILURE;
} }
else { else
{
instPtr->beamNo.next = beamNo; instPtr->beamNo.next = beamNo;
instPtr->antConfigPtr.oaxis[1] = (uint8_t)(antNrrwPattern[beamNo].oaxis); instPtr->antConfigPtr.oaxis[1] = (uint8_t)(antNrrwPattern[beamNo].oaxis);
instPtr->antConfigPtr.oaxis[0] = (uint8_t)((antNrrwPattern[beamNo].oaxis) >> 8); instPtr->antConfigPtr.oaxis[0] = (uint8_t)((antNrrwPattern[beamNo].oaxis) >> 8);
@@ -243,11 +246,14 @@ int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo)
return SUCCESS; return SUCCESS;
} }
} }
else if(instPtr->beamWd.next == PHY_ANT_BEAM_25){ else if (instPtr->beamWd.next == PHY_ANT_BEAM_25)
if (beamNo >= L1_NUM_ANT_SUBNRRW_PATTERN) { {
if (beamNo >= L1_NUM_ANT_SUBNRRW_PATTERN)
{
return FAILURE; return FAILURE;
} }
else { else
{
instPtr->beamNo.next = beamNo; instPtr->beamNo.next = beamNo;
instPtr->antConfigPtr.oaxis[1] = (uint8_t)(antSubnrrwPattern[beamNo].oaxis); instPtr->antConfigPtr.oaxis[1] = (uint8_t)(antSubnrrwPattern[beamNo].oaxis);
instPtr->antConfigPtr.oaxis[0] = (uint8_t)((antSubnrrwPattern[beamNo].oaxis) >> 8); instPtr->antConfigPtr.oaxis[0] = (uint8_t)((antSubnrrwPattern[beamNo].oaxis) >> 8);
@@ -256,7 +262,8 @@ int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo)
return SUCCESS; return SUCCESS;
} }
} }
else{ else
{
// Beam number assign not support. // Beam number assign not support.
// Assign default angle (0,0) // Assign default angle (0,0)
instPtr->beamNo.next = 0; instPtr->beamNo.next = 0;
df_datalink/src/networking/phy_layer/phy_layer.c
+137 -82
View File
@@ -79,14 +79,17 @@ static int32_t SetSendData(PHY_INST* instPtr, L1_FRAME_TYPE frameType, uint8_t*
* FAILURE last frame type cannot be recognized. * FAILURE last frame type cannot be recognized.
* *
******************************************************************************/ ******************************************************************************/
int32_t Phy_RetrieveRecvData(PHY_INST* instPtr, uint8_t* buff) { int32_t Phy_RetrieveRecvData(PHY_INST *instPtr, uint8_t *buff)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
uint32_t frameLen, recvBramAddr; uint32_t frameLen, recvBramAddr;
frameLen = Phy_GetFrameLen(instPtr->frameType.last); frameLen = Phy_GetFrameLen(instPtr->frameType.last);
if (frameLen == L1_FRAME_NONE_LEN) { if (frameLen == L1_FRAME_NONE_LEN)
{
return FAILURE; return FAILURE;
} }
else { else
{
// Ping-pong flow. // Ping-pong flow.
recvBramAddr = instPtr->rxDataPtr + ((PhyRead(instPtr, RECV_DATA_SATE) & RECV_DATA_OFFSET_MASK) >> RECV_DATA_OFFSET_SHIFT); recvBramAddr = instPtr->rxDataPtr + ((PhyRead(instPtr, RECV_DATA_SATE) & RECV_DATA_OFFSET_MASK) >> RECV_DATA_OFFSET_SHIFT);
memcpy(buff, (uint8_t *)recvBramAddr, frameLen); memcpy(buff, (uint8_t *)recvBramAddr, frameLen);
@@ -94,7 +97,6 @@ int32_t Phy_RetrieveRecvData(PHY_INST* instPtr, uint8_t* buff) {
return SUCCESS; return SUCCESS;
} }
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Setup data will be sent in next slot. * @brief Setup data will be sent in next slot.
@@ -105,15 +107,18 @@ int32_t Phy_RetrieveRecvData(PHY_INST* instPtr, uint8_t* buff) {
* SUCCESS Setting data successful. * SUCCESS Setting data successful.
* *
******************************************************************************/ ******************************************************************************/
static int32_t SetSendData(PHY_INST* instPtr, L1_FRAME_TYPE frameType, uint8_t* buff) { static int32_t SetSendData(PHY_INST *instPtr, L1_FRAME_TYPE frameType, uint8_t *buff)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
uint32_t frameLen, sendCmd; uint32_t frameLen, sendCmd;
frameLen = Phy_GetFrameLen(frameType); frameLen = Phy_GetFrameLen(frameType);
if (frameLen == L1_FRAME_NONE_LEN) { if (frameLen == L1_FRAME_NONE_LEN)
{
return FAILURE; return FAILURE;
} }
else { else
{
// Implement a ping-pong flow, offset always is 0 or half of buffer depth. // Implement a ping-pong flow, offset always is 0 or half of buffer depth.
instPtr->sendOffset = (instPtr->sendOffset == PAYLOAD_MAX_LEN) ? 0 : PAYLOAD_MAX_LEN; instPtr->sendOffset = (instPtr->sendOffset == PAYLOAD_MAX_LEN) ? 0 : PAYLOAD_MAX_LEN;
memcpy((uint8_t *)(instPtr->txDataPtr + instPtr->sendOffset), buff, frameLen); memcpy((uint8_t *)(instPtr->txDataPtr + instPtr->sendOffset), buff, frameLen);
@@ -125,7 +130,6 @@ static int32_t SetSendData(PHY_INST* instPtr, L1_FRAME_TYPE frameType, uint8_t*
return SUCCESS; return SUCCESS;
} }
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Check whether a frame was detected. (Do not check if the frame was * @brief Check whether a frame was detected. (Do not check if the frame was
@@ -137,12 +141,15 @@ static int32_t SetSendData(PHY_INST* instPtr, L1_FRAME_TYPE frameType, uint8_t*
* false * false
* *
******************************************************************************/ ******************************************************************************/
inline bool Phy_IsFrameDetected(PHY_INST* instPtr) { inline bool Phy_IsFrameDetected(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
if(instPtr->TOR.last == PHY_TOR_RX && instPtr->isAcq && instPtr->isFrameSync) { if (instPtr->TOR.last == PHY_TOR_RX && instPtr->isAcq && instPtr->isFrameSync)
{
return true; return true;
} }
else { else
{
return false; return false;
} }
} }
@@ -158,13 +165,15 @@ inline bool Phy_IsFrameDetected(PHY_INST* instPtr) {
* false * false
* *
******************************************************************************/ ******************************************************************************/
inline bool Phy_IsFrameReceived(PHY_INST* instPtr) { inline bool Phy_IsFrameReceived(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
if(instPtr->TOR.last == PHY_TOR_RX && instPtr->isAcq if (instPtr->TOR.last == PHY_TOR_RX && instPtr->isAcq && instPtr->isFrameSync && instPtr->isDecode)
&& instPtr->isFrameSync && instPtr->isDecode) { {
return true; return true;
} }
else { else
{
return false; return false;
} }
} }
@@ -180,13 +189,15 @@ inline bool Phy_IsFrameReceived(PHY_INST* instPtr) {
* false * false
* *
******************************************************************************/ ******************************************************************************/
inline bool Phy_IsFrameValid(PHY_INST* instPtr) { inline bool Phy_IsFrameValid(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
if (instPtr->TOR.last == PHY_TOR_RX && instPtr->isAcq && instPtr->isFrameSync if (instPtr->TOR.last == PHY_TOR_RX && instPtr->isAcq && instPtr->isFrameSync && instPtr->isDecode && instPtr->isValid)
&& instPtr->isDecode && instPtr->isValid) { {
return true; return true;
} }
else { else
{
return false; return false;
} }
} }
@@ -200,12 +211,15 @@ inline bool Phy_IsFrameValid(PHY_INST* instPtr) {
* @return Layer2 frame type. Refer "frame_def.h" * @return Layer2 frame type. Refer "frame_def.h"
* *
******************************************************************************/ ******************************************************************************/
inline L1_FRAME_TYPE Phy_GetLastDetectedFrameType(PHY_INST* instPtr) { inline L1_FRAME_TYPE Phy_GetLastDetectedFrameType(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
if (Phy_IsFrameDetected(instPtr) == false) { if (Phy_IsFrameDetected(instPtr) == false)
{
return L1_NONE_FRAME; return L1_NONE_FRAME;
} }
else { else
{
return instPtr->frameType.last; return instPtr->frameType.last;
} }
} }
@@ -219,17 +233,19 @@ inline L1_FRAME_TYPE Phy_GetLastDetectedFrameType(PHY_INST* instPtr) {
* @return Layer2 frame type. Refer "frame_def.h" * @return Layer2 frame type. Refer "frame_def.h"
* *
******************************************************************************/ ******************************************************************************/
inline L1_FRAME_TYPE Phy_GetLastReceivedFrameType(PHY_INST* instPtr) { inline L1_FRAME_TYPE Phy_GetLastReceivedFrameType(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
if (Phy_IsFrameReceived(instPtr) == false) { if (Phy_IsFrameReceived(instPtr) == false)
{
return L1_NONE_FRAME; return L1_NONE_FRAME;
} }
else { else
{
return instPtr->frameType.last; return instPtr->frameType.last;
} }
} }
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Get frame type of the frame was sent in last slot. * @brief Get frame type of the frame was sent in last slot.
@@ -239,12 +255,15 @@ inline L1_FRAME_TYPE Phy_GetLastReceivedFrameType(PHY_INST* instPtr) {
* @return Layer2 frame type. Refer "frame_def.h" * @return Layer2 frame type. Refer "frame_def.h"
* *
******************************************************************************/ ******************************************************************************/
inline L1_FRAME_TYPE Phy_GetLastSentFrameType(PHY_INST* instPtr) { inline L1_FRAME_TYPE Phy_GetLastSentFrameType(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
if (instPtr->TOR.last != PHY_TOR_TX) { if (instPtr->TOR.last != PHY_TOR_TX)
{
return L1_NONE_FRAME; return L1_NONE_FRAME;
} }
else { else
{
return instPtr->frameType.last; return instPtr->frameType.last;
} }
} }
@@ -258,7 +277,8 @@ inline L1_FRAME_TYPE Phy_GetLastSentFrameType(PHY_INST* instPtr) {
* @return NONE. * @return NONE.
* *
******************************************************************************/ ******************************************************************************/
static void Phy_RcdShift(PHY_RCD* rcd, int32_t netxVal){ static void Phy_RcdShift(PHY_RCD *rcd, int32_t netxVal)
{
ASSERT(rcd != NULL); ASSERT(rcd != NULL);
rcd->last = rcd->curr; rcd->last = rcd->curr;
rcd->curr = rcd->next; rcd->curr = rcd->next;
@@ -275,8 +295,10 @@ static void Phy_RcdShift(PHY_RCD* rcd, int32_t netxVal){
* L1_FRAME_NONE_LEN if the frame type code cannot be recognized. * L1_FRAME_NONE_LEN if the frame type code cannot be recognized.
* *
******************************************************************************/ ******************************************************************************/
uint32_t Phy_GetFrameLen(L1_FRAME_TYPE frameType) { uint32_t Phy_GetFrameLen(L1_FRAME_TYPE frameType)
switch (frameType) { {
switch (frameType)
{
case L1_2KBPS_20MS_FRAME: case L1_2KBPS_20MS_FRAME:
// FIXME: for DUIFEI net setup // FIXME: for DUIFEI net setup
return L1_FRAME_2KBPS_100MS_LEN; return L1_FRAME_2KBPS_100MS_LEN;
@@ -300,8 +322,6 @@ uint32_t Phy_GetFrameLen(L1_FRAME_TYPE frameType) {
} }
} }
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Update Physical Layer reception status. * @brief Update Physical Layer reception status.
@@ -311,7 +331,8 @@ uint32_t Phy_GetFrameLen(L1_FRAME_TYPE frameType) {
* @return * @return
* *
******************************************************************************/ ******************************************************************************/
void Phy_UpdateState(PHY_INST* instPtr) { void Phy_UpdateState(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
Phy_RcdShift(&(instPtr->TOR), PHY_TOR_SUSPEND); Phy_RcdShift(&(instPtr->TOR), PHY_TOR_SUSPEND);
@@ -325,17 +346,18 @@ void Phy_UpdateState(PHY_INST* instPtr) {
instPtr->isFrameSync = ((txrxSate & TXRX_STATE_FSYNC_MASK) == TXRX_STATE_FSYNC_SUCCESS) ? true : false; instPtr->isFrameSync = ((txrxSate & TXRX_STATE_FSYNC_MASK) == TXRX_STATE_FSYNC_SUCCESS) ? true : false;
instPtr->isDecode = ((txrxSate & TXRX_STATE_DECODE_MASK) == TXRX_STATE_DECODE_SUCCESS) ? true : false; instPtr->isDecode = ((txrxSate & TXRX_STATE_DECODE_MASK) == TXRX_STATE_DECODE_SUCCESS) ? true : false;
if (Phy_IsFrameReceived(instPtr)) { if (Phy_IsFrameReceived(instPtr))
{
DBG_INFO("Physical layer finished reception procedure, retrieving received data...\n"); DBG_INFO("Physical layer finished reception procedure, retrieving received data...\n");
instPtr->isValid = true; instPtr->isValid = true;
} }
else { else
{
instPtr->isValid = false; instPtr->isValid = false;
} }
return; return;
} }
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Initialize Physical Layer instance, allocate memory for buffers, * @brief Initialize Physical Layer instance, allocate memory for buffers,
@@ -347,12 +369,14 @@ void Phy_UpdateState(PHY_INST* instPtr) {
* @return * @return
* *
******************************************************************************/ ******************************************************************************/
int32_t Phy_InstInit(PHY_INST** instPtr, PHY_CONFIG *phyConfig) { int32_t Phy_InstInit(PHY_INST **instPtr, PHY_CONFIG *phyConfig)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
ASSERT(phyConfig != NULL); ASSERT(phyConfig != NULL);
int32_t Status = SUCCESS; int32_t Status = SUCCESS;
*instPtr = (PHY_INST *)malloc(sizeof(PHY_INST)); *instPtr = (PHY_INST *)malloc(sizeof(PHY_INST));
if ((*instPtr) == NULL) { if ((*instPtr) == NULL)
{
return ENOMEM; return ENOMEM;
} }
@@ -374,7 +398,8 @@ int32_t Phy_InstInit(PHY_INST** instPtr, PHY_CONFIG *phyConfig) {
memset(&((*instPtr)->hwSate), 0x00, sizeof(HWSTATE)); memset(&((*instPtr)->hwSate), 0x00, sizeof(HWSTATE));
Status = Phy_AntConfigInit(&((*instPtr)->antConfigPtr)); Status = Phy_AntConfigInit(&((*instPtr)->antConfigPtr));
if (Status != SUCCESS) { if (Status != SUCCESS)
{
goto init_error; goto init_error;
} }
@@ -383,57 +408,65 @@ int32_t Phy_InstInit(PHY_INST** instPtr, PHY_CONFIG *phyConfig) {
XBram txDataBram; XBram txDataBram;
XBram rxDataBram; XBram rxDataBram;
bramConfigPtr = XBram_LookupConfig(phyConfig->antBramId); bramConfigPtr = XBram_LookupConfig(phyConfig->antBramId);
if (bramConfigPtr == NULL) { if (bramConfigPtr == NULL)
{
Status = EDCONF; Status = EDCONF;
goto init_error; goto init_error;
} }
Status = XBram_CfgInitialize(&antConfigBram, bramConfigPtr, Status = XBram_CfgInitialize(&antConfigBram, bramConfigPtr,
bramConfigPtr->CtrlBaseAddress); bramConfigPtr->CtrlBaseAddress);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
goto init_error; goto init_error;
} }
/* /*
* Execute the BRAM driver selftest. * Execute the BRAM driver selftest.
*/ */
Status = XBram_SelfTest(&antConfigBram, 0); Status = XBram_SelfTest(&antConfigBram, 0);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
goto init_error; goto init_error;
} }
bramConfigPtr = XBram_LookupConfig(phyConfig->txBramId); bramConfigPtr = XBram_LookupConfig(phyConfig->txBramId);
if (bramConfigPtr == NULL) { if (bramConfigPtr == NULL)
{
Status = EDCONF; Status = EDCONF;
goto init_error; goto init_error;
} }
Status = XBram_CfgInitialize(&txDataBram, bramConfigPtr, Status = XBram_CfgInitialize(&txDataBram, bramConfigPtr,
bramConfigPtr->CtrlBaseAddress); bramConfigPtr->CtrlBaseAddress);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
goto init_error; goto init_error;
} }
/* /*
* Execute the BRAM driver selftest. * Execute the BRAM driver selftest.
*/ */
Status = XBram_SelfTest(&txDataBram, 0); Status = XBram_SelfTest(&txDataBram, 0);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
goto init_error; goto init_error;
} }
bramConfigPtr = XBram_LookupConfig(phyConfig->rxBramId); bramConfigPtr = XBram_LookupConfig(phyConfig->rxBramId);
if (bramConfigPtr == NULL) { if (bramConfigPtr == NULL)
{
Status = EDCONF; Status = EDCONF;
goto init_error; goto init_error;
} }
Status = XBram_CfgInitialize(&rxDataBram, bramConfigPtr, Status = XBram_CfgInitialize(&rxDataBram, bramConfigPtr,
bramConfigPtr->CtrlBaseAddress); bramConfigPtr->CtrlBaseAddress);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
goto init_error; goto init_error;
} }
/* /*
* Execute the BRAM driver selftest. * Execute the BRAM driver selftest.
*/ */
Status = XBram_SelfTest(&rxDataBram, 0); Status = XBram_SelfTest(&rxDataBram, 0);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
goto init_error; goto init_error;
} }
@@ -445,7 +478,8 @@ int32_t Phy_InstInit(PHY_INST** instPtr, PHY_CONFIG *phyConfig) {
XAdcPs_Config *xadcConfigPtr; XAdcPs_Config *xadcConfigPtr;
xadcConfigPtr = XAdcPs_LookupConfig(phyConfig->hwXadcId); xadcConfigPtr = XAdcPs_LookupConfig(phyConfig->hwXadcId);
if (xadcConfigPtr == NULL) { if (xadcConfigPtr == NULL)
{
Status = EDCONF; Status = EDCONF;
goto init_error; goto init_error;
} }
@@ -453,7 +487,8 @@ int32_t Phy_InstInit(PHY_INST** instPtr, PHY_CONFIG *phyConfig) {
/* Self Test the XADC/ADC device */ /* Self Test the XADC/ADC device */
Status = XAdcPs_SelfTest(&((*instPtr)->hwXadc)); Status = XAdcPs_SelfTest(&((*instPtr)->hwXadc));
if (Status != SUCCESS) { if (Status != SUCCESS)
{
goto init_error; goto init_error;
} }
@@ -475,10 +510,12 @@ init_error:
* @return NONE * @return NONE
* *
******************************************************************************/ ******************************************************************************/
void Phy_Start(PHY_INST* instPtr) { void Phy_Start(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
uint32_t phyMainCtrl = PhyRead(instPtr, PHY_MAIN_CTRL); uint32_t phyMainCtrl = PhyRead(instPtr, PHY_MAIN_CTRL);
if((phyMainCtrl & PHY_MAIN_CTRL_EN_MASK) == PHY_MAIN_CTRL_EN_EN) { if ((phyMainCtrl & PHY_MAIN_CTRL_EN_MASK) == PHY_MAIN_CTRL_EN_EN)
{
DBG_WARNING("Physical layer was already enabled.\n"); DBG_WARNING("Physical layer was already enabled.\n");
} }
instPtr->isPhyEn = true; instPtr->isPhyEn = true;
@@ -495,17 +532,18 @@ void Phy_Start(PHY_INST* instPtr) {
* @return NONE * @return NONE
* *
******************************************************************************/ ******************************************************************************/
void Phy_Halt(PHY_INST* instPtr) { void Phy_Halt(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
uint32_t phyMainCtrl = PhyRead(instPtr, PHY_MAIN_CTRL); uint32_t phyMainCtrl = PhyRead(instPtr, PHY_MAIN_CTRL);
if((phyMainCtrl & PHY_MAIN_CTRL_EN_MASK) == PHY_MAIN_CTRL_EN_DSBL) { if ((phyMainCtrl & PHY_MAIN_CTRL_EN_MASK) == PHY_MAIN_CTRL_EN_DSBL)
{
DBG_WARNING("Physical layer was allready halted.\n"); DBG_WARNING("Physical layer was allready halted.\n");
} }
instPtr->isPhyEn = false; instPtr->isPhyEn = false;
PhyWrite(instPtr, PHY_MAIN_CTRL, (phyMainCtrl & (~PHY_MAIN_CTRL_EN_MASK))); PhyWrite(instPtr, PHY_MAIN_CTRL, (phyMainCtrl & (~PHY_MAIN_CTRL_EN_MASK)));
} }
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Update Physical Layer mode. * @brief Update Physical Layer mode.
@@ -516,10 +554,12 @@ void Phy_Halt(PHY_INST* instPtr) {
* @return * @return
* *
******************************************************************************/ ******************************************************************************/
int32_t Phy_SetMode(PHY_INST* instPtr, PHY_MODE modeCod) { int32_t Phy_SetMode(PHY_INST *instPtr, PHY_MODE modeCod)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
uint32_t phyMainCtrl = PhyRead(instPtr, PHY_MAIN_CTRL) & ~PHY_MAIN_CTRL_MODE_MASK; uint32_t phyMainCtrl = PhyRead(instPtr, PHY_MAIN_CTRL) & ~PHY_MAIN_CTRL_MODE_MASK;
switch (modeCod) { switch (modeCod)
{
case PHY_MODE_NONE: case PHY_MODE_NONE:
phyMainCtrl |= PHY_MAIN_CTRL_MODE_NONE; phyMainCtrl |= PHY_MAIN_CTRL_MODE_NONE;
break; break;
@@ -552,7 +592,8 @@ int32_t Phy_SetMode(PHY_INST* instPtr, PHY_MODE modeCod) {
* *
******************************************************************************/ ******************************************************************************/
int32_t Phy_SetupNextSlot(PHY_INST *instPtr, PHY_TOR_TYPE txrxMode, int32_t Phy_SetupNextSlot(PHY_INST *instPtr, PHY_TOR_TYPE txrxMode,
L1_FRAME_TYPE frameType, uint8_t* buff) { L1_FRAME_TYPE frameType, uint8_t *buff)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
int32_t Status = SUCCESS; int32_t Status = SUCCESS;
uint32_t phyModeCtrl, antConfigBramAddr, hopNum; uint32_t phyModeCtrl, antConfigBramAddr, hopNum;
@@ -564,7 +605,8 @@ int32_t Phy_SetupNextSlot(PHY_INST* instPtr, PHY_TOR_TYPE txrxMode,
instPtr->TOR.next = txrxMode; instPtr->TOR.next = txrxMode;
instPtr->frameType.next = frameType; instPtr->frameType.next = frameType;
switch (txrxMode) { switch (txrxMode)
{
case PHY_TOR_SUSPEND: case PHY_TOR_SUSPEND:
phyModeCtrl = PHY_MODE_CTRL_TOR_SUSPEND; phyModeCtrl = PHY_MODE_CTRL_TOR_SUSPEND;
break; break;
@@ -577,11 +619,14 @@ int32_t Phy_SetupNextSlot(PHY_INST* instPtr, PHY_TOR_TYPE txrxMode,
default: default:
return EPARAM; return EPARAM;
} }
if (instPtr->isPresetHop == false) { if (instPtr->isPresetHop == false)
{
hopPattern = hopPattern_Config; hopPattern = hopPattern_Config;
} }
else { else
switch (frameType) { {
switch (frameType)
{
case L1_2KBPS_20MS_FRAME: case L1_2KBPS_20MS_FRAME:
phyModeCtrl |= PHY_MODE_CTRL_FRAME_2KBPS_20MS; phyModeCtrl |= PHY_MODE_CTRL_FRAME_2KBPS_20MS;
hopNum = L1_FRAME_2KBPS_20MS_HOPS; hopNum = L1_FRAME_2KBPS_20MS_HOPS;
@@ -634,7 +679,8 @@ int32_t Phy_SetupNextSlot(PHY_INST* instPtr, PHY_TOR_TYPE txrxMode,
// TODO: Set antenna stand by, when suspend. // TODO: Set antenna stand by, when suspend.
const uint8_t *dataSrcPtr = (uint8_t *)(&(instPtr->antConfigPtr)); const uint8_t *dataSrcPtr = (uint8_t *)(&(instPtr->antConfigPtr));
for (int32_t i = 0; i < hopNum; i++) { for (int32_t i = 0; i < hopNum; i++)
{
instPtr->antConfigPtr.antFreq[1] = (uint8_t)(hopPattern[i] & 0x00FF); instPtr->antConfigPtr.antFreq[1] = (uint8_t)(hopPattern[i] & 0x00FF);
instPtr->antConfigPtr.antFreq[0] = (uint8_t)((hopPattern[i] >> 8) & 0x00FF); instPtr->antConfigPtr.antFreq[0] = (uint8_t)((hopPattern[i] >> 8) & 0x00FF);
instPtr->antConfigPtr.rfFreq[1] = (uint8_t)(hopPattern[i] & 0x00FF); instPtr->antConfigPtr.rfFreq[1] = (uint8_t)(hopPattern[i] & 0x00FF);
@@ -642,15 +688,15 @@ int32_t Phy_SetupNextSlot(PHY_INST* instPtr, PHY_TOR_TYPE txrxMode,
memcpy((uint8_t *)(antConfigBramAddr + i * ANT_COINFO_LENGTH), memcpy((uint8_t *)(antConfigBramAddr + i * ANT_COINFO_LENGTH),
dataSrcPtr, sizeof(PHY_ANT_COINFO)); dataSrcPtr, sizeof(PHY_ANT_COINFO));
} }
uint32_t antCtrl = (instPtr->antConfigOffset & ANT_CONFIG_CMD_OFFSET_MASK) uint32_t antCtrl = (instPtr->antConfigOffset & ANT_CONFIG_CMD_OFFSET_MASK) | ((hopNum << ANT_CONFIG_CMD_NUM_SHIFT) & ANT_CONFIG_CMD_NUM_MASK) | ((ANT_COINFO_LENGTH << ANT_CONFIG_CMD_LEN_SHIFT) & ANT_CONFIG_CMD_LEN_MASK);
| ((hopNum << ANT_CONFIG_CMD_NUM_SHIFT) & ANT_CONFIG_CMD_NUM_MASK)
| ((ANT_COINFO_LENGTH << ANT_CONFIG_CMD_LEN_SHIFT) & ANT_CONFIG_CMD_LEN_MASK);
PhyWrite(instPtr, ANT_CONFIG_CMD, antCtrl); PhyWrite(instPtr, ANT_CONFIG_CMD, antCtrl);
// FIXME: For DUIFEI network setup. Should only set send data in send slots. // FIXME: For DUIFEI network setup. Should only set send data in send slots.
if(txrxMode == PHY_TOR_TX) { if (txrxMode == PHY_TOR_TX)
{
Status = SetSendData(instPtr, frameType, buff); Status = SetSendData(instPtr, frameType, buff);
if(Status != SUCCESS) { if (Status != SUCCESS)
{
return Status; return Status;
} }
} }
@@ -671,8 +717,10 @@ int32_t Phy_SetupNextSlot(PHY_INST* instPtr, PHY_TOR_TYPE txrxMode,
* FAILURE frame type cannot be recognized * FAILURE frame type cannot be recognized
* *
******************************************************************************/ ******************************************************************************/
uint32_t Phy_GetHopLength(L1_FRAME_TYPE frameType){ uint32_t Phy_GetHopLength(L1_FRAME_TYPE frameType)
switch (frameType) { {
switch (frameType)
{
case L1_2KBPS_20MS_FRAME: case L1_2KBPS_20MS_FRAME:
return L1_FRAME_2KBPS_20MS_HOPS; return L1_FRAME_2KBPS_20MS_HOPS;
case L1_2KBPS_100MS_FRAME: case L1_2KBPS_100MS_FRAME:
@@ -703,8 +751,10 @@ uint32_t Phy_GetHopLength(L1_FRAME_TYPE frameType){
* @return 32bit unsigned integer (clock periods) * @return 32bit unsigned integer (clock periods)
* *
******************************************************************************/ ******************************************************************************/
uint32_t Phy_GetTimeComp(L1_FRAME_TYPE frameType){ uint32_t Phy_GetTimeComp(L1_FRAME_TYPE frameType)
switch (frameType) { {
switch (frameType)
{
case L1_2KBPS_20MS_FRAME: case L1_2KBPS_20MS_FRAME:
return L1_FRAME_2KBPS_20MS_COMP; return L1_FRAME_2KBPS_20MS_COMP;
case L1_2KBPS_100MS_FRAME: case L1_2KBPS_100MS_FRAME:
@@ -735,7 +785,8 @@ uint32_t Phy_GetTimeComp(L1_FRAME_TYPE frameType){
* @return 32bit signed integer (Hz) * @return 32bit signed integer (Hz)
* *
******************************************************************************/ ******************************************************************************/
int32_t Phy_GetFreqEst(PHY_INST *instPtr){ int32_t Phy_GetFreqEst(PHY_INST *instPtr)
{
uint32_t temp; uint32_t temp;
int32_t acqCh, fsCh; int32_t acqCh, fsCh;
double freqEst; double freqEst;
@@ -759,9 +810,11 @@ int32_t Phy_GetFreqEst(PHY_INST *instPtr){
* FAILURE pattern length exceeds maximum length. * FAILURE pattern length exceeds maximum length.
* *
******************************************************************************/ ******************************************************************************/
int32_t Phy_SetHopPattern(void* patternBuff, uint32_t patternLen){ int32_t Phy_SetHopPattern(void *patternBuff, uint32_t patternLen)
{
ASSERT(patternBuff != NULL); ASSERT(patternBuff != NULL);
if(patternLen > L1_FRAME_MAX_HOPS){ if (patternLen > L1_FRAME_MAX_HOPS)
{
return FAILURE; return FAILURE;
} }
memcpy(hopPattern_Config, patternBuff, patternLen * sizeof(uint16_t)); memcpy(hopPattern_Config, patternBuff, patternLen * sizeof(uint16_t));
@@ -780,9 +833,11 @@ int32_t Phy_SetHopPattern(void* patternBuff, uint32_t patternLen){
* FAILURE pattern length exceeds maximum length. * FAILURE pattern length exceeds maximum length.
* *
******************************************************************************/ ******************************************************************************/
int32_t Phy_SetTimeWord(PHY_INST* instPtr, uint64_t tmWord, PHY_TIME_WORD_TYPE tmType){ int32_t Phy_SetTimeWord(PHY_INST *instPtr, uint64_t tmWord, PHY_TIME_WORD_TYPE tmType)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
switch(tmType){ switch (tmType)
{
case PHY_TIME_WORD_POLL: case PHY_TIME_WORD_POLL:
PhyWrite(instPtr, POLLTIME_L, (uint32_t)tmWord); PhyWrite(instPtr, POLLTIME_L, (uint32_t)tmWord);
PhyWrite(instPtr, POLLTIME_H, (uint32_t)(tmWord >> 32)); PhyWrite(instPtr, POLLTIME_H, (uint32_t)(tmWord >> 32));
@@ -800,7 +855,6 @@ int32_t Phy_SetTimeWord(PHY_INST* instPtr, uint64_t tmWord, PHY_TIME_WORD_TYPE t
return SUCCESS; return SUCCESS;
} }
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief Setup a specific timer word register in physical controller. * @brief Setup a specific timer word register in physical controller.
@@ -812,7 +866,8 @@ int32_t Phy_SetTimeWord(PHY_INST* instPtr, uint64_t tmWord, PHY_TIME_WORD_TYPE t
* FAILURE pattern length exceeds maximum length. * FAILURE pattern length exceeds maximum length.
* *
******************************************************************************/ ******************************************************************************/
void Phy_UpdateHwSate(PHY_INST *instPtr){ void Phy_UpdateHwSate(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
uint32_t rawData; uint32_t rawData;
df_datalink/src/networking/phy_layer/phy_layer.h
+20 -12
View File
@@ -47,7 +47,8 @@
#define ANT_COINFO_LENGTH 32 // bytes #define ANT_COINFO_LENGTH 32 // bytes
/***************** Macros (Inline Functions) Definitions *********************/ /***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/ /**************************** Type Definitions *******************************/
typedef enum { typedef enum
{
L1_NONE_FRAME = 0, L1_NONE_FRAME = 0,
L1_2KBPS_20MS_FRAME = 1, L1_2KBPS_20MS_FRAME = 1,
L1_2KBPS_100MS_FRAME = 2, L1_2KBPS_100MS_FRAME = 2,
@@ -59,7 +60,8 @@ typedef enum {
L1_1M2BPS_5MS_FRAME = 8 L1_1M2BPS_5MS_FRAME = 8
} L1_FRAME_TYPE; } L1_FRAME_TYPE;
typedef struct { typedef struct
{
/*** Ant Control ***/ /*** Ant Control ***/
/* Antenna control frame head 0xAA */ /* Antenna control frame head 0xAA */
/* default 0, reserved for further use */ /* default 0, reserved for further use */
@@ -113,8 +115,8 @@ typedef struct {
uint8_t rfReserved1; uint8_t rfReserved1;
} PHY_ANT_COINFO; } PHY_ANT_COINFO;
typedef struct
typedef struct { {
/* off-axis angle range is 0~60, /* off-axis angle range is 0~60,
* 0.1 degree each LSB */ * 0.1 degree each LSB */
uint16_t oaxis; uint16_t oaxis;
@@ -123,15 +125,16 @@ typedef struct {
uint16_t raxis; uint16_t raxis;
} PHY_ANT_ANGLE; } PHY_ANT_ANGLE;
typedef enum
typedef enum { {
PHY_TIME_WORD_NONE = 0, PHY_TIME_WORD_NONE = 0,
PHY_TIME_WORD_POLL = 1, PHY_TIME_WORD_POLL = 1,
PHY_TIME_WORD_LOCAL_ADJ = 2, PHY_TIME_WORD_LOCAL_ADJ = 2,
PHY_TIME_WORD_STEP = 3, PHY_TIME_WORD_STEP = 3,
} PHY_TIME_WORD_TYPE; } PHY_TIME_WORD_TYPE;
typedef enum { typedef enum
{
PHY_ANT_BEAM_NONE = 0, PHY_ANT_BEAM_NONE = 0,
PHY_ANT_BEAM_120 = 1, PHY_ANT_BEAM_120 = 1,
PHY_ANT_BEAM_60 = 2, PHY_ANT_BEAM_60 = 2,
@@ -139,20 +142,23 @@ typedef enum {
PHY_ANT_BEAM_8 = 4 PHY_ANT_BEAM_8 = 4
} PHY_ANT_BEAM_WIDTH; } PHY_ANT_BEAM_WIDTH;
typedef enum { typedef enum
{
PHY_TOR_SUSPEND = 0, PHY_TOR_SUSPEND = 0,
PHY_TOR_TX = 1, PHY_TOR_TX = 1,
PHY_TOR_RX = 2 PHY_TOR_RX = 2
} PHY_TOR_TYPE; } PHY_TOR_TYPE;
typedef enum { typedef enum
{
PHY_MODE_NONE = 0, PHY_MODE_NONE = 0,
PHY_MODE_LISTEN = 1, PHY_MODE_LISTEN = 1,
PHY_MODE_SCHEDULE = 2, PHY_MODE_SCHEDULE = 2,
PHY_MODE_PULL = 3 PHY_MODE_PULL = 3
} PHY_MODE; } PHY_MODE;
typedef struct { typedef struct
{
uint32_t devId; uint32_t devId;
uint32_t txBramId; uint32_t txBramId;
uint32_t rxBramId; uint32_t rxBramId;
@@ -161,13 +167,15 @@ typedef struct {
uint32_t baseAddr; uint32_t baseAddr;
} PHY_CONFIG; } PHY_CONFIG;
typedef struct { typedef struct
{
int32_t last; int32_t last;
int32_t curr; int32_t curr;
int32_t next; int32_t next;
} PHY_RCD; } PHY_RCD;
typedef struct { typedef struct
{
PHY_CONFIG phyCofig; // Device address configs PHY_CONFIG phyCofig; // Device address configs
bool isPhyEn; // is physical instance enable bool isPhyEn; // is physical instance enable
bool isAcq; // is last reception acquisition success bool isAcq; // is last reception acquisition success
df_datalink/src/networking/phy_layer/phy_reg.h
-4
View File
@@ -203,7 +203,6 @@
#define RECV_DATA_OFFSET_WIDTH 16 #define RECV_DATA_OFFSET_WIDTH 16
#define RECV_DATA_OFFSET_MASK 0xFFFFF0000U #define RECV_DATA_OFFSET_MASK 0xFFFFF0000U
/** /**
* Register: FSTIME_L * Register: FSTIME_L
*/ */
@@ -265,7 +264,6 @@
*/ */
#define INTR_TYPE 0x00000124U #define INTR_TYPE 0x00000124U
/** /**
* Register: ANT_STATUS_0 * Register: ANT_STATUS_0
*/ */
@@ -283,7 +281,6 @@
#define ANT_STATUS_ANT_VOLT_WIDTH 8 #define ANT_STATUS_ANT_VOLT_WIDTH 8
#define ANT_STATUS_ANT_VOLT_MASK 0xFF000000U #define ANT_STATUS_ANT_VOLT_MASK 0xFF000000U
/** /**
* Register: ANT_FRAME_CNT * Register: ANT_FRAME_CNT
*/ */
@@ -326,7 +323,6 @@
#define RF_MGC_CH4_WIDTH 4 #define RF_MGC_CH4_WIDTH 4
#define RF_MGC_CH4_MASK 0x0F000000U #define RF_MGC_CH4_MASK 0x0F000000U
/** /**
* Register: RF_FRAME_CNT * Register: RF_FRAME_CNT
*/ */
df_datalink/src/system_monitor.c
+64 -32
View File
@@ -14,13 +14,15 @@
#define PRTCLLOG_PACK_ELN 24 // bytes #define PRTCLLOG_PACK_ELN 24 // bytes
#define HWSTATE_PACK_ELN 12 // bytes #define HWSTATE_PACK_ELN 12 // bytes
typedef struct{ typedef struct
{
int8_t latest; int8_t latest;
int8_t rBusy; int8_t rBusy;
int8_t wBusy; int8_t wBusy;
} PINGPONG; } PINGPONG;
typedef struct{ typedef struct
{
PINGPONG recvPLCtrl; PINGPONG recvPLCtrl;
PAYLOAD recvPL[2]; PAYLOAD recvPL[2];
PINGPONG sendPLCtrl; PINGPONG sendPLCtrl;
@@ -42,7 +44,8 @@ static SYSMONITOR *sysMonitorInst;
uint8_t GetWritable(PINGPONG ctrllor); uint8_t GetWritable(PINGPONG ctrllor);
uint8_t GetReadable(PINGPONG ctrllor); uint8_t GetReadable(PINGPONG ctrllor);
inline uint8_t GetWritable(PINGPONG ctrllor) { inline uint8_t GetWritable(PINGPONG ctrllor)
{
ASSERT(ctrllor.wBusy != BUFF_NONE); ASSERT(ctrllor.wBusy != BUFF_NONE);
if (ctrllor.rBusy == BUFF_NONE) if (ctrllor.rBusy == BUFF_NONE)
return ((ctrllor.latest == BUFF_B) ? BUFF_A : BUFF_B); return ((ctrllor.latest == BUFF_B) ? BUFF_A : BUFF_B);
@@ -50,7 +53,8 @@ inline uint8_t GetWritable(PINGPONG ctrllor) {
return ((ctrllor.rBusy == BUFF_B) ? BUFF_A : BUFF_B); return ((ctrllor.rBusy == BUFF_B) ? BUFF_A : BUFF_B);
} }
inline uint8_t GetReadable(PINGPONG ctrllor) { inline uint8_t GetReadable(PINGPONG ctrllor)
{
if (ctrllor.wBusy == BUFF_NONE || ctrllor.wBusy != ctrllor.latest) if (ctrllor.wBusy == BUFF_NONE || ctrllor.wBusy != ctrllor.latest)
return ctrllor.latest; return ctrllor.latest;
else else
@@ -58,9 +62,11 @@ inline uint8_t GetReadable(PINGPONG ctrllor) {
} }
// System monitor initialise // System monitor initialise
int32_t SysMonitor_Init(void){ int32_t SysMonitor_Init(void)
{
sysMonitorInst = calloc(1, sizeof(SYSMONITOR)); sysMonitorInst = calloc(1, sizeof(SYSMONITOR));
if (sysMonitorInst == NULL) { if (sysMonitorInst == NULL)
{
return ENOMEM; return ENOMEM;
} }
memset(&(sysMonitorInst->recvPLCtrl), BUFF_NONE, sizeof(PINGPONG)); memset(&(sysMonitorInst->recvPLCtrl), BUFF_NONE, sizeof(PINGPONG));
@@ -74,7 +80,8 @@ int32_t SysMonitor_Init(void){
} }
// @System Info // @System Info
void SysMonitor_UpdateSysinfo(SYSINFO *info) { void SysMonitor_UpdateSysinfo(SYSINFO *info)
{
uint8_t buffIndx; uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->sysInfoCtrl); buffIndx = GetWritable(sysMonitorInst->sysInfoCtrl);
sysMonitorInst->sysInfoCtrl.wBusy = buffIndx; sysMonitorInst->sysInfoCtrl.wBusy = buffIndx;
@@ -83,7 +90,8 @@ void SysMonitor_UpdateSysinfo(SYSINFO *info) {
sysMonitorInst->sysInfoCtrl.wBusy = BUFF_NONE; sysMonitorInst->sysInfoCtrl.wBusy = BUFF_NONE;
} }
void SysMonitor_UpdateSysinfoRaw(uint8_t *buff){ void SysMonitor_UpdateSysinfoRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx; uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->sysInfoCtrl); buffIndx = GetWritable(sysMonitorInst->sysInfoCtrl);
@@ -125,7 +133,8 @@ void SysMonitor_UpdateSysinfoRaw(uint8_t *buff){
sysMonitorInst->sysInfoCtrl.wBusy = BUFF_NONE; sysMonitorInst->sysInfoCtrl.wBusy = BUFF_NONE;
} }
void SysMonitor_GetSysinfo(SYSINFO *info){ void SysMonitor_GetSysinfo(SYSINFO *info)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old; uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->sysInfoCtrl.rBusy; buffIndex_old = sysMonitorInst->sysInfoCtrl.rBusy;
@@ -138,7 +147,8 @@ void SysMonitor_GetSysinfo(SYSINFO *info){
sysMonitorInst->sysInfoCtrl.rBusy = buffIndex_old; sysMonitorInst->sysInfoCtrl.rBusy = buffIndex_old;
} }
void SysMonitor_GetSysinfoRaw(uint8_t *buff) { void SysMonitor_GetSysinfoRaw(uint8_t *buff)
{
uint8_t buffIndx, buffIndex_old; uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->sysInfoCtrl.rBusy; buffIndex_old = sysMonitorInst->sysInfoCtrl.rBusy;
if (buffIndex_old != BUFF_NONE) if (buffIndex_old != BUFF_NONE)
@@ -186,7 +196,8 @@ void SysMonitor_GetSysinfoRaw(uint8_t *buff) {
/** /**
* Update send FLJ payload by raw data. * Update send FLJ payload by raw data.
*/ */
void SysMonitor_UpdateSendPayloadFLJRaw(uint8_t *buff){ void SysMonitor_UpdateSendPayloadFLJRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx; uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->sendPLCtrl); buffIndx = GetWritable(sysMonitorInst->sendPLCtrl);
@@ -199,7 +210,8 @@ void SysMonitor_UpdateSendPayloadFLJRaw(uint8_t *buff){
/** /**
* Get send FLJ payload as raw data. * Get send FLJ payload as raw data.
*/ */
void SysMonitor_GetSendPayloadFLJRaw(uint8_t *buff) { void SysMonitor_GetSendPayloadFLJRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old; uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->sendPLCtrl.rBusy; buffIndex_old = sysMonitorInst->sendPLCtrl.rBusy;
@@ -215,7 +227,8 @@ void SysMonitor_GetSendPayloadFLJRaw(uint8_t *buff) {
/** /**
* Update LJ send payload by raw data. * Update LJ send payload by raw data.
*/ */
void SysMonitor_UpdateSendPayloadLJRaw(uint8_t *buff){ void SysMonitor_UpdateSendPayloadLJRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx; uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->sendPLCtrl); buffIndx = GetWritable(sysMonitorInst->sendPLCtrl);
@@ -228,7 +241,8 @@ void SysMonitor_UpdateSendPayloadLJRaw(uint8_t *buff){
/** /**
* Get send LJ payload as raw data. * Get send LJ payload as raw data.
*/ */
void SysMonitor_GetSendPayloadLJRaw(uint8_t *buff) { void SysMonitor_GetSendPayloadLJRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old; uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->sendPLCtrl.rBusy; buffIndex_old = sysMonitorInst->sendPLCtrl.rBusy;
@@ -245,7 +259,8 @@ void SysMonitor_GetSendPayloadLJRaw(uint8_t *buff) {
/** /**
* Update receive FLJ payload by raw data. * Update receive FLJ payload by raw data.
*/ */
void SysMonitor_UpdateRecvPayloadFLJRaw(uint8_t *buff){ void SysMonitor_UpdateRecvPayloadFLJRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx; uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->recvPLCtrl); buffIndx = GetWritable(sysMonitorInst->recvPLCtrl);
@@ -258,7 +273,8 @@ void SysMonitor_UpdateRecvPayloadFLJRaw(uint8_t *buff){
/** /**
* Get send FLJ payload as raw data. * Get send FLJ payload as raw data.
*/ */
void SysMonitor_GetRecvPayloadFLJRaw(uint8_t *buff){ void SysMonitor_GetRecvPayloadFLJRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old; uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->recvPLCtrl.rBusy; buffIndex_old = sysMonitorInst->recvPLCtrl.rBusy;
@@ -274,7 +290,8 @@ void SysMonitor_GetRecvPayloadFLJRaw(uint8_t *buff){
/** /**
* Update receive LJ payload by raw data. * Update receive LJ payload by raw data.
*/ */
void SysMonitor_UpdateRecvPayloadLJRaw(uint8_t *buff){ void SysMonitor_UpdateRecvPayloadLJRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx; uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->recvPLCtrl); buffIndx = GetWritable(sysMonitorInst->recvPLCtrl);
@@ -287,7 +304,8 @@ void SysMonitor_UpdateRecvPayloadLJRaw(uint8_t *buff){
/** /**
* Get send LJ payload as raw data. * Get send LJ payload as raw data.
*/ */
void SysMonitor_GetRecvPayloadLJRaw(uint8_t *buff){ void SysMonitor_GetRecvPayloadLJRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old; uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->recvPLCtrl.rBusy; buffIndex_old = sysMonitorInst->recvPLCtrl.rBusy;
@@ -304,7 +322,8 @@ void SysMonitor_GetRecvPayloadLJRaw(uint8_t *buff){
/** /**
* Log send slot information * Log send slot information
*/ */
void SysMonitor_UpdateSendSlotLog(SLOTLOG *log){ void SysMonitor_UpdateSendSlotLog(SLOTLOG *log)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx; uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->sendLogCtrl); buffIndx = GetWritable(sysMonitorInst->sendLogCtrl);
@@ -317,7 +336,8 @@ void SysMonitor_UpdateSendSlotLog(SLOTLOG *log){
/** /**
* Get the latest send slot log * Get the latest send slot log
*/ */
void SysMonitor_GetSendSlotLog(SLOTLOG *log){ void SysMonitor_GetSendSlotLog(SLOTLOG *log)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old; uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->sendLogCtrl.rBusy; buffIndex_old = sysMonitorInst->sendLogCtrl.rBusy;
@@ -333,7 +353,8 @@ void SysMonitor_GetSendSlotLog(SLOTLOG *log){
/** /**
* Get the latest send slot log as raw data * Get the latest send slot log as raw data
*/ */
void SysMonitor_GetSendSlotLogRaw(uint8_t *buff){ void SysMonitor_GetSendSlotLogRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old; uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->sendLogCtrl.rBusy; buffIndex_old = sysMonitorInst->sendLogCtrl.rBusy;
@@ -371,7 +392,8 @@ void SysMonitor_GetSendSlotLogRaw(uint8_t *buff){
/** /**
* Log receive slot information * Log receive slot information
*/ */
void SysMonitor_UpdateRecvSlotLog(SLOTLOG *log){ void SysMonitor_UpdateRecvSlotLog(SLOTLOG *log)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx; uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->recvLogCtrl); buffIndx = GetWritable(sysMonitorInst->recvLogCtrl);
@@ -384,7 +406,8 @@ void SysMonitor_UpdateRecvSlotLog(SLOTLOG *log){
/** /**
* Get the latest receive slot information * Get the latest receive slot information
*/ */
void SysMonitor_GetRecvSlotLog(SLOTLOG *log){ void SysMonitor_GetRecvSlotLog(SLOTLOG *log)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old; uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->recvLogCtrl.rBusy; buffIndex_old = sysMonitorInst->recvLogCtrl.rBusy;
@@ -400,7 +423,8 @@ void SysMonitor_GetRecvSlotLog(SLOTLOG *log){
/** /**
* Get the latest receive slot information as raw data * Get the latest receive slot information as raw data
*/ */
void SysMonitor_GetRecvSlotLogRaw(uint8_t *buff) { void SysMonitor_GetRecvSlotLogRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old; uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->recvLogCtrl.rBusy; buffIndex_old = sysMonitorInst->recvLogCtrl.rBusy;
@@ -438,7 +462,8 @@ void SysMonitor_GetRecvSlotLogRaw(uint8_t *buff) {
/** /**
* Log protocol running status. * Log protocol running status.
*/ */
void SysMonitor_UpdateProtocolLog(PRTCLLOG *log){ void SysMonitor_UpdateProtocolLog(PRTCLLOG *log)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx; uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->protocolLogCtrl); buffIndx = GetWritable(sysMonitorInst->protocolLogCtrl);
@@ -451,7 +476,8 @@ void SysMonitor_UpdateProtocolLog(PRTCLLOG *log){
/** /**
* Get the protocol running log. * Get the protocol running log.
*/ */
void SysMonitor_GetProtocolLog(PRTCLLOG *log){ void SysMonitor_GetProtocolLog(PRTCLLOG *log)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old; uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->protocolLogCtrl.rBusy; buffIndex_old = sysMonitorInst->protocolLogCtrl.rBusy;
@@ -467,7 +493,8 @@ void SysMonitor_GetProtocolLog(PRTCLLOG *log){
/** /**
* Get the protocol running log as raw data. * Get the protocol running log as raw data.
*/ */
void SysMonitor_GetProtocolLogRaw(uint8_t *buff){ void SysMonitor_GetProtocolLogRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old; uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->protocolLogCtrl.rBusy; buffIndex_old = sysMonitorInst->protocolLogCtrl.rBusy;
@@ -507,7 +534,8 @@ void SysMonitor_GetProtocolLogRaw(uint8_t *buff){
/** /**
* Log hardware running status. * Log hardware running status.
*/ */
void SysMonitor_UpdateHardwareStatus(HWSTATE *state){ void SysMonitor_UpdateHardwareStatus(HWSTATE *state)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx; uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->hwStateCtrl); buffIndx = GetWritable(sysMonitorInst->hwStateCtrl);
@@ -520,7 +548,8 @@ void SysMonitor_UpdateHardwareStatus(HWSTATE *state){
/** /**
* Get the latest hardware running status. * Get the latest hardware running status.
*/ */
void SysMonitor_GetHardwareStatus(HWSTATE *state){ void SysMonitor_GetHardwareStatus(HWSTATE *state)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old; uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->hwStateCtrl.rBusy; buffIndex_old = sysMonitorInst->hwStateCtrl.rBusy;
@@ -536,7 +565,8 @@ void SysMonitor_GetHardwareStatus(HWSTATE *state){
/** /**
* Get the latest hardware running status as raw data. * Get the latest hardware running status as raw data.
*/ */
void SysMonitor_GetHardwareStatusRaw(uint8_t *buff){ void SysMonitor_GetHardwareStatusRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old; uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->hwStateCtrl.rBusy; buffIndex_old = sysMonitorInst->hwStateCtrl.rBusy;
@@ -570,7 +600,8 @@ void SysMonitor_GetHardwareStatusRaw(uint8_t *buff){
/** /**
* Get the latest system running log. * Get the latest system running log.
*/ */
void SysMonitor_GetSyslog(SYSLOG *log){ void SysMonitor_GetSyslog(SYSLOG *log)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
SysMonitor_GetSendSlotLog(&(log->sendLog)); SysMonitor_GetSendSlotLog(&(log->sendLog));
SysMonitor_GetRecvSlotLog(&(log->recvLog)); SysMonitor_GetRecvSlotLog(&(log->recvLog));
@@ -581,7 +612,8 @@ void SysMonitor_GetSyslog(SYSLOG *log){
/** /**
* Get the latest system running log as raw data. * Get the latest system running log as raw data.
*/ */
void SysMonitor_GetSyslogRaw(uint8_t *buff){ void SysMonitor_GetSyslogRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL); ASSERT(sysMonitorInst != NULL);
SysMonitor_GetSendSlotLogRaw(buff); SysMonitor_GetSendSlotLogRaw(buff);
buff += SLOTLOG_PACK_LEN; buff += SLOTLOG_PACK_LEN;
df_datalink/src/system_monitor.h
+14 -9
View File
@@ -1,19 +1,19 @@
#ifndef __SYSTEM_MONITOR_H_ #ifndef __SYSTEM_MONITOR_H_
#define __SYSTEM_MONITOR_H_ #define __SYSTEM_MONITOR_H_
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
#define PAYLOAD_FLJ 78 #define PAYLOAD_FLJ 78
#define PAYLOAD_LJ 156 #define PAYLOAD_LJ 156
typedef struct{ typedef struct
{
// FIXME: To Complete // FIXME: To Complete
} SYSCONFIG; } SYSCONFIG;
typedef struct{ typedef struct
{
// -- unit: ms, 0~604799999 // -- unit: ms, 0~604799999
uint32_t psBDS; uint32_t psBDS;
// -- unit: week, 0~0~8191 // -- unit: week, 0~0~8191
@@ -51,12 +51,14 @@ typedef struct{
float roll; float roll;
} SYSINFO; } SYSINFO;
typedef struct{ typedef struct
{
uint8_t data_lj[PAYLOAD_LJ]; uint8_t data_lj[PAYLOAD_LJ];
uint8_t data_flj[PAYLOAD_FLJ]; uint8_t data_flj[PAYLOAD_FLJ];
} PAYLOAD; } PAYLOAD;
typedef struct{ typedef struct
{
// Network log // Network log
uint32_t tuNo; uint32_t tuNo;
uint8_t slotNo; uint8_t slotNo;
@@ -72,7 +74,8 @@ typedef struct{
uint32_t freqDevi; uint32_t freqDevi;
} SLOTLOG; } SLOTLOG;
typedef struct{ typedef struct
{
// Protocol log // Protocol log
// -- System time // -- System time
uint32_t tmSys; uint32_t tmSys;
@@ -91,7 +94,8 @@ typedef struct{
uint8_t dstId; uint8_t dstId;
} PRTCLLOG; } PRTCLLOG;
typedef struct{ typedef struct
{
// Hardware log // Hardware log
// -- Signal processor status // -- Signal processor status
uint16_t spVolt; uint16_t spVolt;
@@ -105,7 +109,8 @@ typedef struct{
uint8_t antSt; uint8_t antSt;
} HWSTATE; } HWSTATE;
typedef struct{ typedef struct
{
// Send slot log // Send slot log
SLOTLOG sendLog; SLOTLOG sendLog;
df_datalink/src/systembus/systembus.c
+78 -43
View File
@@ -153,22 +153,25 @@
// My Production Type // My Production Type
#define MY_PRD_ID PRD_DL_K #define MY_PRD_ID PRD_DL_K
inline bool SystemBus_IsReqPendding(SYSBUS *instPtr)
inline bool SystemBus_IsReqPendding(SYSBUS *instPtr){ {
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
return (!isCircQueueEmpty(instPtr->rxQueue)); return (!isCircQueueEmpty(instPtr->rxQueue));
} }
inline bool SystemBus_IsRsqPendding(SYSBUS *instPtr){ inline bool SystemBus_IsRsqPendding(SYSBUS *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
return (!isCircQueueEmpty(instPtr->txQueue)); return (!isCircQueueEmpty(instPtr->txQueue));
} }
inline bool SystemBus_IsTxBusy(SYSBUS *instPtr) { inline bool SystemBus_IsTxBusy(SYSBUS *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
return (!instPtr->isTxDone); return (!instPtr->isTxDone);
} }
inline bool SystemBus_IsRxBusy(SYSBUS *instPtr) { inline bool SystemBus_IsRxBusy(SYSBUS *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
return SysbusUart_IsRxBusy(instPtr->uartInst); return SysbusUart_IsRxBusy(instPtr->uartInst);
} }
@@ -183,13 +186,15 @@ inline bool SystemBus_IsRxBusy(SYSBUS *instPtr) {
* @return None. * @return None.
* *
******************************************************************************/ ******************************************************************************/
int32_t SystemBus_Init(SYSBUS **instPtr, SYSBUS_CONFIG *config) { int32_t SystemBus_Init(SYSBUS **instPtr, SYSBUS_CONFIG *config)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
ASSERT(config != NULL); ASSERT(config != NULL);
SYSBUS_UART_CONFIG uartConfig; SYSBUS_UART_CONFIG uartConfig;
int32_t Status; int32_t Status;
(*instPtr) = malloc(sizeof(SYSBUS)); (*instPtr) = malloc(sizeof(SYSBUS));
if ((*instPtr) == NULL) { if ((*instPtr) == NULL)
{
return FAILURE; return FAILURE;
} }
(*instPtr)->rxQueue = NULL; (*instPtr)->rxQueue = NULL;
@@ -198,15 +203,18 @@ int32_t SystemBus_Init(SYSBUS **instPtr, SYSBUS_CONFIG *config) {
memcpy(&((*instPtr)->config), config, sizeof(SYSBUS_CONFIG)); memcpy(&((*instPtr)->config), config, sizeof(SYSBUS_CONFIG));
uartConfig.baseAddr = config->baseAddr; uartConfig.baseAddr = config->baseAddr;
Status = SysbusUart_Init(&((*instPtr)->uartInst), &uartConfig); Status = SysbusUart_Init(&((*instPtr)->uartInst), &uartConfig);
if (Status != SUCCESS) { if (Status != SUCCESS)
{
Status = ENOMEM; Status = ENOMEM;
goto sysbus_init_error; goto sysbus_init_error;
} }
if(CircQueueCreate(&((*instPtr)->txQueue), (*instPtr)->config.txQueueMax) == false){ if (CircQueueCreate(&((*instPtr)->txQueue), (*instPtr)->config.txQueueMax) == false)
{
Status = ENOMEM; Status = ENOMEM;
goto sysbus_init_error; goto sysbus_init_error;
} }
if(CircQueueCreate(&((*instPtr)->rxQueue), (*instPtr)->config.rxQueueMax) == false){ if (CircQueueCreate(&((*instPtr)->rxQueue), (*instPtr)->config.rxQueueMax) == false)
{
Status = ENOMEM; Status = ENOMEM;
goto sysbus_init_error; goto sysbus_init_error;
} }
@@ -232,22 +240,27 @@ sysbus_init_error:
* @return None * @return None
* *
******************************************************************************/ ******************************************************************************/
int32_t SystemBus_HandleCmd(SYSBUS* instPtr) { int32_t SystemBus_HandleCmd(SYSBUS *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
int32_t Status; int32_t Status;
SYSBUS_UART_FRAME *sysbusReq, *sysbusRsp; SYSBUS_UART_FRAME *sysbusReq, *sysbusRsp;
uint8_t *data; uint8_t *data;
if (CircQueuePop(instPtr->rxQueue, (void**)(&sysbusReq)) == false) { if (CircQueuePop(instPtr->rxQueue, (void **)(&sysbusReq)) == false)
{
DBG_WARNING("SystemBus::Nothing to be handled.\n"); DBG_WARNING("SystemBus::Nothing to be handled.\n");
return EIGNORE; return EIGNORE;
} }
data = sysbusReq->data; data = sysbusReq->data;
if(data[WD_PRD] != MY_PRD_ID && data[WD_PRD] != PRD_ALL) { if (data[WD_PRD] != MY_PRD_ID && data[WD_PRD] != PRD_ALL)
{
DBG_WARNING("SystemBus::Production code mismatch!\n"); DBG_WARNING("SystemBus::Production code mismatch!\n");
return EIGNORE; return EIGNORE;
} }
switch(data[WD_CMD]){ switch (data[WD_CMD])
case CMD_SELF_TEST : { {
case CMD_SELF_TEST:
{
sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME)); sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
sysbusRsp->len = ST_RSP_LEN; sysbusRsp->len = ST_RSP_LEN;
memset(sysbusRsp->data, 0x00, ST_RSP_LEN); memset(sysbusRsp->data, 0x00, ST_RSP_LEN);
@@ -256,7 +269,8 @@ int32_t SystemBus_HandleCmd(SYSBUS* instPtr) {
// FIXME: run self-test and insert result // FIXME: run self-test and insert result
sysbusRsp->data[WD_ST_RET] = ST_RET_OK; sysbusRsp->data[WD_ST_RET] = ST_RET_OK;
if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false){ if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false)
{
DBG_WARNING("SystemBus::TX Queue overflowed!\n"); DBG_WARNING("SystemBus::TX Queue overflowed!\n");
Status = EQUEUE; Status = EQUEUE;
break; break;
@@ -264,11 +278,13 @@ int32_t SystemBus_HandleCmd(SYSBUS* instPtr) {
Status = SUCCESS; Status = SUCCESS;
break; break;
} }
case CMD_DATA_TEST_A : { case CMD_DATA_TEST_A:
{
sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME)); sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
sysbusRsp->len = DATA_TEST_RSP_A_LEN; sysbusRsp->len = DATA_TEST_RSP_A_LEN;
memcpy(sysbusRsp->data, sysbusReq->data, DATA_TEST_RSP_A_LEN); memcpy(sysbusRsp->data, sysbusReq->data, DATA_TEST_RSP_A_LEN);
if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false){ if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false)
{
DBG_WARNING("SystemBus::TX Queue overflowed!\n"); DBG_WARNING("SystemBus::TX Queue overflowed!\n");
Status = EQUEUE; Status = EQUEUE;
break; break;
@@ -276,11 +292,13 @@ int32_t SystemBus_HandleCmd(SYSBUS* instPtr) {
Status = SUCCESS; Status = SUCCESS;
break; break;
} }
case CMD_DATA_TEST_B : { case CMD_DATA_TEST_B:
{
sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME)); sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
sysbusRsp->len = DATA_TEST_RSP_B_LEN; sysbusRsp->len = DATA_TEST_RSP_B_LEN;
memcpy(sysbusRsp->data, sysbusReq->data, DATA_TEST_RSP_B_LEN); memcpy(sysbusRsp->data, sysbusReq->data, DATA_TEST_RSP_B_LEN);
if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false){ if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false)
{
DBG_WARNING("SystemBus::TX Queue overflowed!\n"); DBG_WARNING("SystemBus::TX Queue overflowed!\n");
Status = EQUEUE; Status = EQUEUE;
break; break;
@@ -288,7 +306,8 @@ int32_t SystemBus_HandleCmd(SYSBUS* instPtr) {
Status = SUCCESS; Status = SUCCESS;
break; break;
} }
case CMD_CONFIG : { case CMD_CONFIG:
{
// FIXME: apply configurations // FIXME: apply configurations
// Power + Mode // Power + Mode
// -- No response // -- No response
@@ -296,22 +315,27 @@ int32_t SystemBus_HandleCmd(SYSBUS* instPtr) {
Status = SUCCESS; Status = SUCCESS;
break; break;
} }
case CMD_SYS_INFO : { case CMD_SYS_INFO:
{
SysMonitor_UpdateSysinfoRaw(data + 2); SysMonitor_UpdateSysinfoRaw(data + 2);
Status = SUCCESS; Status = SUCCESS;
break; break;
} }
// TODO // TODO
case CMD_FLJ_SEND : { case CMD_FLJ_SEND:
{
SysMonitor_UpdateSendPayloadFLJRaw(data + 2); // ommit 2 bytes control info SysMonitor_UpdateSendPayloadFLJRaw(data + 2); // ommit 2 bytes control info
break; break;
} }
case CMD_LJ_SEND : { case CMD_LJ_SEND:
{
SysMonitor_UpdateSendPayloadLJRaw(data + 2); // ommit 2 bytes control info SysMonitor_UpdateSendPayloadLJRaw(data + 2); // ommit 2 bytes control info
break; break;
} }
case CMD_FLJ_POLL : { case CMD_FLJ_POLL:
if (data[WD_FLJ_POLL] != FLJ_POLL_CODE) { {
if (data[WD_FLJ_POLL] != FLJ_POLL_CODE)
{
DBG_WARNING("SystemBus::Invalid roll call code!\n"); DBG_WARNING("SystemBus::Invalid roll call code!\n");
Status = EIGNORE; Status = EIGNORE;
break; break;
@@ -321,7 +345,8 @@ int32_t SystemBus_HandleCmd(SYSBUS* instPtr) {
sysbusRsp->data[WD_CMD] = WD_FLJ_POLL; sysbusRsp->data[WD_CMD] = WD_FLJ_POLL;
sysbusRsp->data[WD_PRD] = MY_PRD_ID; sysbusRsp->data[WD_PRD] = MY_PRD_ID;
SysMonitor_GetRecvPayloadFLJRaw(sysbusRsp->data + 2); // ommit 2 bytes control info SysMonitor_GetRecvPayloadFLJRaw(sysbusRsp->data + 2); // ommit 2 bytes control info
if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false){ if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false)
{
DBG_WARNING("SystemBus::TX Queue overflowed!\n"); DBG_WARNING("SystemBus::TX Queue overflowed!\n");
Status = EQUEUE; Status = EQUEUE;
break; break;
@@ -341,8 +366,10 @@ int32_t SystemBus_HandleCmd(SYSBUS* instPtr) {
Status = SUCCESS; Status = SUCCESS;
break; break;
} }
case CMD_LJ_POLL : { case CMD_LJ_POLL:
if (data[WD_LJ_POLL] != LJ_POLL_CODE) { {
if (data[WD_LJ_POLL] != LJ_POLL_CODE)
{
DBG_WARNING("SystemBus::Invalid roll call code!\n"); DBG_WARNING("SystemBus::Invalid roll call code!\n");
Status = EIGNORE; Status = EIGNORE;
break; break;
@@ -352,7 +379,8 @@ int32_t SystemBus_HandleCmd(SYSBUS* instPtr) {
sysbusRsp->data[WD_CMD] = WD_LJ_POLL; sysbusRsp->data[WD_CMD] = WD_LJ_POLL;
sysbusRsp->data[WD_PRD] = MY_PRD_ID; sysbusRsp->data[WD_PRD] = MY_PRD_ID;
SysMonitor_GetRecvPayloadLJRaw(sysbusRsp->data + 2); // ommit 2 bytes control info SysMonitor_GetRecvPayloadLJRaw(sysbusRsp->data + 2); // ommit 2 bytes control info
if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false){ if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false)
{
DBG_WARNING("SystemBus::TX Queue overflowed!\n"); DBG_WARNING("SystemBus::TX Queue overflowed!\n");
Status = EQUEUE; Status = EQUEUE;
break; break;
@@ -372,7 +400,8 @@ int32_t SystemBus_HandleCmd(SYSBUS* instPtr) {
Status = SUCCESS; Status = SUCCESS;
break; break;
} }
default:{ default:
{
DBG_ERROR("SystemBus::Unknown command code!\n"); DBG_ERROR("SystemBus::Unknown command code!\n");
break; break;
} }
@@ -381,19 +410,18 @@ int32_t SystemBus_HandleCmd(SYSBUS* instPtr) {
return Status; return Status;
} }
int32_t SystemBus_FLJ_PeriodUpdate(SYSBUS *instPtr)
int32_t SystemBus_FLJ_PeriodUpdate(SYSBUS *instPtr){ {
// TODO // TODO
return SUCCESS; return SUCCESS;
} }
int32_t SystemBus_LJ_PeriodUpdate(SYSBUS *instPtr)
int32_t SystemBus_LJ_PeriodUpdate(SYSBUS *instPtr){ {
// TODO // TODO
return SUCCESS; return SUCCESS;
} }
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief This function send a repose frame from system bus tx queue. * @brief This function send a repose frame from system bus tx queue.
@@ -403,11 +431,13 @@ int32_t SystemBus_LJ_PeriodUpdate(SYSBUS *instPtr){
* @return None * @return None
* *
******************************************************************************/ ******************************************************************************/
int32_t SystemBus_MakeResp(SYSBUS *instPtr){ int32_t SystemBus_MakeResp(SYSBUS *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
int32_t Status; int32_t Status;
SYSBUS_UART_FRAME *sysbusRsp; SYSBUS_UART_FRAME *sysbusRsp;
if (CircQueuePop(instPtr->txQueue, (void**)(&sysbusRsp)) == false) { if (CircQueuePop(instPtr->txQueue, (void **)(&sysbusRsp)) == false)
{
DBG_WARNING("SystemBus::Nothing to be sent.\n"); DBG_WARNING("SystemBus::Nothing to be sent.\n");
return EIGNORE; return EIGNORE;
} }
@@ -426,7 +456,8 @@ int32_t SystemBus_MakeResp(SYSBUS *instPtr){
* @return None * @return None
* *
******************************************************************************/ ******************************************************************************/
int32_t SystemBus_TxIntrHandler(SYSBUS *instPtr){ int32_t SystemBus_TxIntrHandler(SYSBUS *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
// nothing special... // nothing special...
instPtr->isTxDone = true; // A transmission procedure finished. instPtr->isTxDone = true; // A transmission procedure finished.
@@ -442,20 +473,24 @@ int32_t SystemBus_TxIntrHandler(SYSBUS *instPtr){
* @return None * @return None
* *
******************************************************************************/ ******************************************************************************/
int32_t SystemBus_RxIntrHandler(SYSBUS *instPtr){ int32_t SystemBus_RxIntrHandler(SYSBUS *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
int32_t Status; int32_t Status;
SYSBUS_UART_FRAME *sysbusReq; SYSBUS_UART_FRAME *sysbusReq;
instPtr->isRxDone = true; // whether reception successes or not, receiving procedure ended. instPtr->isRxDone = true; // whether reception successes or not, receiving procedure ended.
sysbusReq = malloc(sizeof(SYSBUS_UART_FRAME)); sysbusReq = malloc(sizeof(SYSBUS_UART_FRAME));
if (sysbusReq == NULL) { if (sysbusReq == NULL)
{
return ENOMEM; return ENOMEM;
} }
Status = SysbusUart_Recv(instPtr->uartInst, sysbusReq); Status = SysbusUart_Recv(instPtr->uartInst, sysbusReq);
if(Status != SUCCESS) { if (Status != SUCCESS)
{
goto recv_error; goto recv_error;
} }
if (CircQueuePush(instPtr->rxQueue, (void *)sysbusReq) == false){ if (CircQueuePush(instPtr->rxQueue, (void *)sysbusReq) == false)
{
DBG_WARNING("SystemBus::RX Queue overflowed!\n"); DBG_WARNING("SystemBus::RX Queue overflowed!\n");
Status = EQUEUE; Status = EQUEUE;
goto recv_error; goto recv_error;
df_datalink/src/systembus/systembus.h
+4 -4
View File
@@ -22,17 +22,18 @@
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
/************************** Constant Definitions *****************************/ /************************** Constant Definitions *****************************/
/************************** Data Type Definitions ****************************/ /************************** Data Type Definitions ****************************/
// Definition of SystemBus instance. // Definition of SystemBus instance.
typedef struct { typedef struct
{
uintptr_t baseAddr; uintptr_t baseAddr;
uint32_t txQueueMax; uint32_t txQueueMax;
uint32_t rxQueueMax; uint32_t rxQueueMax;
} SYSBUS_CONFIG; } SYSBUS_CONFIG;
typedef struct { typedef struct
{
SYSBUS_CONFIG config; SYSBUS_CONFIG config;
SYSBUS_UART *uartInst; SYSBUS_UART *uartInst;
CIRC_QUEUE *txQueue; CIRC_QUEUE *txQueue;
@@ -61,5 +62,4 @@ bool SystemBus_IsRsqPendding(SYSBUS *instPtr);
bool SystemBus_IsTxBusy(SYSBUS *instPtr); bool SystemBus_IsTxBusy(SYSBUS *instPtr);
bool SystemBus_IsRxBusy(SYSBUS *instPtr); bool SystemBus_IsRxBusy(SYSBUS *instPtr);
#endif //__SYSTEMBUS_H_ #endif //__SYSTEMBUS_H_
df_datalink/src/systembus/systembus_uart.c
+40 -20
View File
@@ -59,11 +59,13 @@
* @return None. * @return None.
* *
******************************************************************************/ ******************************************************************************/
int SysbusUart_Init(SYSBUS_UART **instPtr, SYSBUS_UART_CONFIG *config) { int SysbusUart_Init(SYSBUS_UART **instPtr, SYSBUS_UART_CONFIG *config)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
ASSERT(config != NULL); ASSERT(config != NULL);
(*instPtr) = malloc(sizeof(SYSBUS_UART)); (*instPtr) = malloc(sizeof(SYSBUS_UART));
if ((*instPtr) == NULL){ if ((*instPtr) == NULL)
{
return FAILURE; return FAILURE;
} }
(*instPtr)->config.baseAddr = config->baseAddr; (*instPtr)->config.baseAddr = config->baseAddr;
@@ -81,25 +83,30 @@ int SysbusUart_Init(SYSBUS_UART **instPtr, SYSBUS_UART_CONFIG *config) {
* @return * @return
* *
******************************************************************************/ ******************************************************************************/
int SysbusUart_Send(SYSBUS_UART* instPtr, SYSBUS_UART_FRAME *frame) { int SysbusUart_Send(SYSBUS_UART *instPtr, SYSBUS_UART_FRAME *frame)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
ASSERT(frame != NULL); ASSERT(frame != NULL);
uint8_t dLen, checkSum; uint8_t dLen, checkSum;
uint8_t sendLen; uint8_t sendLen;
uint8_t *sendBuff; uint8_t *sendBuff;
if (frame->len > UART_TXBUFF_MAX || frame->len == 0) { if (frame->len > UART_TXBUFF_MAX || frame->len == 0)
{
return FAILURE; return FAILURE;
} }
sendBuff = malloc(sizeof(uint8_t) * UART_TXBUFF_MAX); sendBuff = malloc(sizeof(uint8_t) * UART_TXBUFF_MAX);
if (sendBuff == NULL) { if (sendBuff == NULL)
{
return FAILURE; return FAILURE;
} }
sendBuff[0] = FRAME_HEAD_LOW; sendBuff[0] = FRAME_HEAD_LOW;
sendBuff[1] = FRAME_HEAD_HIGH; sendBuff[1] = FRAME_HEAD_HIGH;
dLen = 0; dLen = 0;
checkSum = 0; checkSum = 0;
for (int i = 0; i < frame->len - 1; i++) { for (int i = 0; i < frame->len - 1; i++)
if (frame->data[i] == FRAME_TAIL_LOW && frame->data[i+1] == FRAME_TAIL_HIGH) { {
if (frame->data[i] == FRAME_TAIL_LOW && frame->data[i + 1] == FRAME_TAIL_HIGH)
{
sendBuff[FRAME_HEAD_LEN + dLen] = 0x00; sendBuff[FRAME_HEAD_LEN + dLen] = 0x00;
dLen++; dLen++;
} }
@@ -121,7 +128,8 @@ int SysbusUart_Send(SYSBUS_UART* instPtr, SYSBUS_UART_FRAME *frame) {
sendBuff[FRAME_HEAD_LEN + dLen + 3] = FRAME_TAIL_LOW; sendBuff[FRAME_HEAD_LEN + dLen + 3] = FRAME_TAIL_LOW;
sendBuff[FRAME_HEAD_LEN + dLen + 4] = FRAME_TAIL_HIGH; sendBuff[FRAME_HEAD_LEN + dLen + 4] = FRAME_TAIL_HIGH;
sendLen = dLen + FRAME_OVERHEAD_LEN; sendLen = dLen + FRAME_OVERHEAD_LEN;
if (sendLen > UART_TXBUFF_MAX) { if (sendLen > UART_TXBUFF_MAX)
{
free(sendBuff); free(sendBuff);
return FAILURE; return FAILURE;
} }
@@ -142,20 +150,23 @@ int SysbusUart_Send(SYSBUS_UART* instPtr, SYSBUS_UART_FRAME *frame) {
* @return * @return
* *
******************************************************************************/ ******************************************************************************/
int SysbusUart_Recv(SYSBUS_UART* instPtr, SYSBUS_UART_FRAME *frame) { int SysbusUart_Recv(SYSBUS_UART *instPtr, SYSBUS_UART_FRAME *frame)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
ASSERT(frame != NULL); ASSERT(frame != NULL);
uint8_t dLen, dWLen, plLen, checkSum, checkSumCal; uint8_t dLen, dWLen, plLen, checkSum, checkSumCal;
uint8_t *plBuff, *dBuff; uint8_t *plBuff, *dBuff;
dLen = SYSTEMBUS_UART_mReadReg(instPtr->config.baseAddr, UART_RECV_CNT); dLen = SYSTEMBUS_UART_mReadReg(instPtr->config.baseAddr, UART_RECV_CNT);
if (dLen == 0 || dLen > UART_TXBUFF_MAX) { if (dLen == 0 || dLen > UART_TXBUFF_MAX)
{
return EPARAM; return EPARAM;
} }
dWLen = (dLen + FRAME_HEAD_LEN + FRAME_INFO_LEN - 1) / 4 + 1; dWLen = (dLen + FRAME_HEAD_LEN + FRAME_INFO_LEN - 1) / 4 + 1;
dBuff = malloc(sizeof(uint32_t) * dWLen); dBuff = malloc(sizeof(uint32_t) * dWLen);
if (dBuff == NULL){ if (dBuff == NULL)
{
return ENOMEM; return ENOMEM;
} }
memcpy(dBuff, (void *)(instPtr->config.baseAddr + UART_RXBUFF_LOW), sizeof(uint32_t) * dWLen); memcpy(dBuff, (void *)(instPtr->config.baseAddr + UART_RXBUFF_LOW), sizeof(uint32_t) * dWLen);
@@ -163,8 +174,10 @@ int SysbusUart_Recv(SYSBUS_UART* instPtr, SYSBUS_UART_FRAME *frame) {
checkSumCal = 0; checkSumCal = 0;
checkSum = plBuff[dLen + FRAME_DUMMY_LEN]; checkSum = plBuff[dLen + FRAME_DUMMY_LEN];
plLen = 0; plLen = 0;
for(int i = 0; i < dLen; i++){ for (int i = 0; i < dLen; i++)
if (plBuff[i+1] == FRAME_TAIL_LOW && plBuff[i+2] == FRAME_TAIL_HIGH) { {
if (plBuff[i + 1] == FRAME_TAIL_LOW && plBuff[i + 2] == FRAME_TAIL_HIGH)
{
continue; continue;
} }
checkSumCal += plBuff[i]; checkSumCal += plBuff[i];
@@ -174,7 +187,8 @@ int SysbusUart_Recv(SYSBUS_UART* instPtr, SYSBUS_UART_FRAME *frame) {
free(dBuff); free(dBuff);
frame->len = plLen; frame->len = plLen;
frame->checkSum = checkSumCal; frame->checkSum = checkSumCal;
if (checkSumCal != checkSum){ if (checkSumCal != checkSum)
{
frame->isValid = false; frame->isValid = false;
return FAILURE; return FAILURE;
} }
@@ -192,15 +206,18 @@ int SysbusUart_Recv(SYSBUS_UART* instPtr, SYSBUS_UART_FRAME *frame) {
* false - SYSBUS_UART rx channel is idle. * false - SYSBUS_UART rx channel is idle.
* *
******************************************************************************/ ******************************************************************************/
bool SysbusUart_IsRxBusy(SYSBUS_UART* instPtr) { bool SysbusUart_IsRxBusy(SYSBUS_UART *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
uint32_t Status; uint32_t Status;
Status = SYSTEMBUS_UART_mReadReg(instPtr->config.baseAddr, UART_STATUS); Status = SYSTEMBUS_UART_mReadReg(instPtr->config.baseAddr, UART_STATUS);
if (Status & UART_STATUS_RX_BUSY) { if (Status & UART_STATUS_RX_BUSY)
{
return true; return true;
} }
else { else
{
return false; return false;
} }
} }
@@ -215,14 +232,17 @@ bool SysbusUart_IsRxBusy(SYSBUS_UART* instPtr) {
* false - SYSBUS_UART tx channel is idle. * false - SYSBUS_UART tx channel is idle.
* *
******************************************************************************/ ******************************************************************************/
bool SysbusUart_IsTxBusy(SYSBUS_UART* instPtr) { bool SysbusUart_IsTxBusy(SYSBUS_UART *instPtr)
{
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
uint32_t Status; uint32_t Status;
Status = SYSTEMBUS_UART_mReadReg(instPtr->config.baseAddr, UART_STATUS); Status = SYSTEMBUS_UART_mReadReg(instPtr->config.baseAddr, UART_STATUS);
if (Status & UART_STATUS_TX_BUSY) { if (Status & UART_STATUS_TX_BUSY)
{
return true; return true;
} }
else { else
{
return false; return false;
} }
} }
df_datalink/src/systembus/systembus_uart.h
+6 -3
View File
@@ -61,15 +61,18 @@
IO_In32((baseAddr) + (offset)) IO_In32((baseAddr) + (offset))
/************************** Data Type Definitions ****************************/ /************************** Data Type Definitions ****************************/
typedef struct { typedef struct
{
uintptr_t baseAddr; uintptr_t baseAddr;
} SYSBUS_UART_CONFIG; } SYSBUS_UART_CONFIG;
typedef struct { typedef struct
{
SYSBUS_UART_CONFIG config; SYSBUS_UART_CONFIG config;
} SYSBUS_UART; } SYSBUS_UART;
typedef struct { typedef struct
{
uint8_t len; uint8_t len;
uint8_t checkSum; uint8_t checkSum;
uint8_t isValid; uint8_t isValid;
df_datalink/src/util/circ_queue.c
+20 -11
View File
@@ -35,21 +35,23 @@
#include <stdlib.h> #include <stdlib.h>
#include <string.h> #include <string.h>
inline bool isCircQueueFull(CIRC_QUEUE *queue){ inline bool isCircQueueFull(CIRC_QUEUE *queue)
{
ASSERT(queue != NULL); ASSERT(queue != NULL);
return ((queue)->nelts == (queue)->bounds); return ((queue)->nelts == (queue)->bounds);
} }
inline bool isCircQueueEmpty(CIRC_QUEUE *queue){ inline bool isCircQueueEmpty(CIRC_QUEUE *queue)
{
ASSERT(queue != NULL); ASSERT(queue != NULL);
return ((queue)->nelts == 0); return ((queue)->nelts == 0);
} }
void CircQueueFree(CIRC_QUEUE *queue) void CircQueueFree(CIRC_QUEUE *queue)
{ {
ASSERT(queue != NULL); ASSERT(queue != NULL);
if (queue->data) free(queue->data); if (queue->data)
free(queue->data);
free(queue); free(queue);
} }
@@ -61,7 +63,8 @@ bool CircQueueCreate(CIRC_QUEUE **q, uint32_t queueCapacity)
ASSERT(q != NULL); ASSERT(q != NULL);
CIRC_QUEUE *queue; CIRC_QUEUE *queue;
queue = malloc(sizeof(CIRC_QUEUE)); queue = malloc(sizeof(CIRC_QUEUE));
if (!queue) { if (!queue)
{
return false; return false;
} }
*q = queue; *q = queue;
@@ -69,7 +72,8 @@ bool CircQueueCreate(CIRC_QUEUE **q, uint32_t queueCapacity)
/* Set all the data in the queue to NULL */ /* Set all the data in the queue to NULL */
queue->data = malloc(queueCapacity * sizeof(void *)); queue->data = malloc(queueCapacity * sizeof(void *));
if(queue->data == NULL){ if (queue->data == NULL)
{
goto error; goto error;
} }
queue->bounds = queueCapacity; queue->bounds = queueCapacity;
@@ -89,13 +93,15 @@ bool CircQueuePush(CIRC_QUEUE *queue, void *data)
{ {
ASSERT(queue != NULL); ASSERT(queue != NULL);
if (isCircQueueFull(queue)) { if (isCircQueueFull(queue))
{
return false; return false;
} }
queue->data[queue->in] = data; queue->data[queue->in] = data;
queue->in++; queue->in++;
if (queue->in >= queue->bounds) { if (queue->in >= queue->bounds)
{
queue->in -= queue->bounds; queue->in -= queue->bounds;
} }
queue->nelts++; queue->nelts++;
@@ -106,7 +112,8 @@ bool CircQueuePush(CIRC_QUEUE *queue, void *data)
/** /**
* Return current number of element in queue. * Return current number of element in queue.
*/ */
inline uint32_t CircQueueSize(CIRC_QUEUE *queue) { inline uint32_t CircQueueSize(CIRC_QUEUE *queue)
{
return queue->nelts; return queue->nelts;
} }
@@ -117,7 +124,8 @@ bool CircQueuePop(CIRC_QUEUE *queue, void **data)
{ {
ASSERT(queue != NULL); ASSERT(queue != NULL);
if (isCircQueueEmpty(queue)) { if (isCircQueueEmpty(queue))
{
return false; return false;
} }
@@ -125,7 +133,8 @@ bool CircQueuePop(CIRC_QUEUE *queue, void **data)
queue->nelts--; queue->nelts--;
queue->out++; queue->out++;
if (queue->out >= queue->bounds) { if (queue->out >= queue->bounds)
{
queue->out -= queue->bounds; queue->out -= queue->bounds;
} }
return true; return true;
df_datalink/src/util/common.c
+4 -2
View File
@@ -38,7 +38,9 @@
* @return NONE. * @return NONE.
* *
******************************************************************************/ ******************************************************************************/
void AppAssert(const char * file, int line){ void AppAssert(const char *file, int line)
{
DBG_PRINT("in %s #%4d [assert] .\n", file, line); DBG_PRINT("in %s #%4d [assert] .\n", file, line);
while(1); while (1)
;
} }
df_datalink/src/util/crc.c
+42 -32
View File
@@ -54,8 +54,7 @@ static const uint16_t crc16_1021_lsb_lut[256] = {
0xE70E, 0xF687, 0xC41C, 0xD595, 0xA12A, 0xB0A3, 0x8238, 0x93B1, 0xE70E, 0xF687, 0xC41C, 0xD595, 0xA12A, 0xB0A3, 0x8238, 0x93B1,
0x6B46, 0x7ACF, 0x4854, 0x59DD, 0x2D62, 0x3CEB, 0x0E70, 0x1FF9, 0x6B46, 0x7ACF, 0x4854, 0x59DD, 0x2D62, 0x3CEB, 0x0E70, 0x1FF9,
0xF78F, 0xE606, 0xD49D, 0xC514, 0xB1AB, 0xA022, 0x92B9, 0x8330, 0xF78F, 0xE606, 0xD49D, 0xC514, 0xB1AB, 0xA022, 0x92B9, 0x8330,
0x7BC7, 0x6A4E, 0x58D5, 0x495C, 0x3DE3, 0x2C6A, 0x1EF1, 0x0F78 0x7BC7, 0x6A4E, 0x58D5, 0x495C, 0x3DE3, 0x2C6A, 0x1EF1, 0x0F78};
};
// CRC-16-MAXIM G(X)=X16+X15+X2+1 // CRC-16-MAXIM G(X)=X16+X15+X2+1
const uint16_t crc16_8005_lsb_lut[256] = { const uint16_t crc16_8005_lsb_lut[256] = {
@@ -90,8 +89,7 @@ const uint16_t crc16_8005_lsb_lut[256] = {
0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40, 0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40,
0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41, 0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41,
0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641, 0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641,
0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040 0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040};
};
// CRC-8-MAXIM G(X)=X8+X5+X4+1 // CRC-8-MAXIM G(X)=X8+X5+X4+1
const uint8_t crc8_31_lsb_lut[256] = { const uint8_t crc8_31_lsb_lut[256] = {
@@ -126,8 +124,7 @@ const uint8_t crc8_31_lsb_lut[256] = {
0xE9, 0xB7, 0x55, 0x0B, 0x88, 0xD6, 0x34, 0x6A, 0xE9, 0xB7, 0x55, 0x0B, 0x88, 0xD6, 0x34, 0x6A,
0x2B, 0x75, 0x97, 0xC9, 0x4A, 0x14, 0xF6, 0xA8, 0x2B, 0x75, 0x97, 0xC9, 0x4A, 0x14, 0xF6, 0xA8,
0x74, 0x2A, 0xC8, 0x96, 0x15, 0x4B, 0xA9, 0xF7, 0x74, 0x2A, 0xC8, 0x96, 0x15, 0x4B, 0xA9, 0xF7,
0xB6, 0xE8, 0x0A, 0x54, 0xD7, 0x89, 0x6B, 0x35 0xB6, 0xE8, 0x0A, 0x54, 0xD7, 0x89, 0x6B, 0x35};
};
#else #else
// CRC-16-CCITT G(X)=X16+X12+X5+1 // CRC-16-CCITT G(X)=X16+X12+X5+1
static const uint16_t crc16_1021_msb_lut[256] = { static const uint16_t crc16_1021_msb_lut[256] = {
@@ -162,8 +159,7 @@ static const uint16_t crc16_1021_msb_lut[256] = {
0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, 0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1, 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, 0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0};
};
// CRC-16-MAXIM G(X)=X16+X15+X2+1 // CRC-16-MAXIM G(X)=X16+X15+X2+1
const uint16_t crc16_8005_msb_lut[256] = { const uint16_t crc16_8005_msb_lut[256] = {
@@ -198,8 +194,7 @@ const uint16_t crc16_8005_msb_lut[256] = {
0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, 0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261, 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202};
};
// CRC-8-MAXIM G(X)=X8+X5+X4+1 // CRC-8-MAXIM G(X)=X8+X5+X4+1
const uint8_t crc8_31_msb_lut[256] = { const uint8_t crc8_31_msb_lut[256] = {
@@ -234,8 +229,7 @@ const uint8_t crc8_31_msb_lut[256] = {
0xC1, 0xF0, 0xA3, 0x92, 0x05, 0x34, 0x67, 0x56, 0xC1, 0xF0, 0xA3, 0x92, 0x05, 0x34, 0x67, 0x56,
0x78, 0x49, 0x1A, 0x2B, 0xBC, 0x8D, 0xDE, 0xEF, 0x78, 0x49, 0x1A, 0x2B, 0xBC, 0x8D, 0xDE, 0xEF,
0x82, 0xB3, 0xE0, 0xD1, 0x46, 0x77, 0x24, 0x15, 0x82, 0xB3, 0xE0, 0xD1, 0x46, 0x77, 0x24, 0x15,
0x3B, 0x0A, 0x59, 0x68, 0xFF, 0xCE, 0x9D, 0xAC 0x3B, 0x0A, 0x59, 0x68, 0xFF, 0xCE, 0x9D, 0xAC};
};
#endif #endif
/* CRC lookup table algorithm /* CRC lookup table algorithm
@@ -268,11 +262,13 @@ for(int i = 0; i < sizeof(dataBuff); i++)
* FAILURE * FAILURE
* *
******************************************************************************/ ******************************************************************************/
int crc8_gen(uint8_t* buffPtr, uint32_t dataLen, uint16_t poly, uint8_t* crcVal) { int crc8_gen(uint8_t *buffPtr, uint32_t dataLen, uint16_t poly, uint8_t *crcVal)
{
uint8_t crc = CRC8_INIT; uint8_t crc = CRC8_INIT;
const uint8_t *crc_lut = NULL; const uint8_t *crc_lut = NULL;
switch(poly){ switch (poly)
{
#ifdef __CRC_LSB #ifdef __CRC_LSB
case 0x31: case 0x31:
crc_lut = crc8_31_lsb_lut; crc_lut = crc8_31_lsb_lut;
@@ -288,14 +284,14 @@ int crc8_gen(uint8_t* buffPtr, uint32_t dataLen, uint16_t poly, uint8_t* crcVal)
#endif #endif
} }
for(int i = 0; i < dataLen; i++){ for (int i = 0; i < dataLen; i++)
{
crc = crc_lut[(uint8_t)(crc ^ buffPtr[i])]; crc = crc_lut[(uint8_t)(crc ^ buffPtr[i])];
} }
*crcVal = crc; *crcVal = crc;
return SUCCESS; return SUCCESS;
} }
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief CRC16 generator * @brief CRC16 generator
@@ -309,10 +305,12 @@ int crc8_gen(uint8_t* buffPtr, uint32_t dataLen, uint16_t poly, uint8_t* crcVal)
* FAILURE * FAILURE
* *
******************************************************************************/ ******************************************************************************/
int crc16_gen(uint8_t* buffPtr, uint32_t dataLen, uint16_t poly, uint16_t* crcVal) { int crc16_gen(uint8_t *buffPtr, uint32_t dataLen, uint16_t poly, uint16_t *crcVal)
{
uint16_t crc = CRC16_INIT; uint16_t crc = CRC16_INIT;
const uint16_t *crc_lut = NULL; const uint16_t *crc_lut = NULL;
switch(poly){ switch (poly)
{
#ifdef __CRC_LSB #ifdef __CRC_LSB
case 0x8005: case 0x8005:
crc_lut = crc16_8005_lsb_lut; crc_lut = crc16_8005_lsb_lut;
@@ -333,7 +331,8 @@ int crc16_gen(uint8_t* buffPtr, uint32_t dataLen, uint16_t poly, uint16_t* crcVa
return FAILURE; return FAILURE;
#endif #endif
} }
for(int i = 0; i < dataLen; i++){ for (int i = 0; i < dataLen; i++)
{
#ifdef __CRC_LSB #ifdef __CRC_LSB
crc = (crc >> 8) ^ crc_lut[(uint8_t)(crc ^ buffPtr[i])]; crc = (crc >> 8) ^ crc_lut[(uint8_t)(crc ^ buffPtr[i])];
#else #else
@@ -357,29 +356,34 @@ int crc16_gen(uint8_t* buffPtr, uint32_t dataLen, uint16_t poly, uint16_t* crcVa
* false * false
* *
******************************************************************************/ ******************************************************************************/
bool crc8_check(uint8_t* buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t poly) { bool crc8_check(uint8_t *buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t poly)
{
int Status = SUCCESS; int Status = SUCCESS;
uint8_t crc_comp; uint8_t crc_comp;
uint8_t crc; uint8_t crc;
if (dataLen == 0) { if (dataLen == 0)
{
return false; return false;
} }
crc_comp = (uint8_t)(*(buffPtr + crcOffset)); crc_comp = (uint8_t)(*(buffPtr + crcOffset));
Status = crc8_gen(buffPtr, dataLen, poly, &crc); Status = crc8_gen(buffPtr, dataLen, poly, &crc);
if(Status == SUCCESS){ if (Status == SUCCESS)
if (crc == crc_comp) { {
if (crc == crc_comp)
{
return true; return true;
} }
else { else
{
return false; return false;
} }
} }
else{ else
{
return false; return false;
} }
} }
/*****************************************************************************/ /*****************************************************************************/
/** /**
* @brief CRC16 compare cheker * @brief CRC16 compare cheker
@@ -393,25 +397,31 @@ bool crc8_check(uint8_t* buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t
* fasle * fasle
* *
******************************************************************************/ ******************************************************************************/
bool crc16_check(uint8_t* buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t poly) { bool crc16_check(uint8_t *buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t poly)
{
int Status = SUCCESS; int Status = SUCCESS;
uint16_t crc_comp; uint16_t crc_comp;
uint16_t crc; uint16_t crc;
if (dataLen == 0) { if (dataLen == 0)
{
return false; return false;
} }
crc_comp = (uint16_t)(*(buffPtr + crcOffset)); crc_comp = (uint16_t)(*(buffPtr + crcOffset));
crc_comp |= (uint16_t)((*(buffPtr + crcOffset + 1)) << 8); crc_comp |= (uint16_t)((*(buffPtr + crcOffset + 1)) << 8);
Status = crc16_gen(buffPtr, dataLen, poly, &crc); Status = crc16_gen(buffPtr, dataLen, poly, &crc);
if(Status == SUCCESS){ if (Status == SUCCESS)
if (crc == crc_comp) { {
if (crc == crc_comp)
{
return true; return true;
} }
else { else
{
return false; return false;
} }
} }
else{ else
{
return false; return false;
} }
} }
df_datalink/src/util/list.c
+34 -20
View File
@@ -1,4 +1,5 @@
/***************************************************************************//** /*******************************************************************************
//**
* @file list.c * @file list.c
* @brief List library implementation * @brief List library implementation
* @author Mihail Chindris (mihail.chindris@analog.com) * @author Mihail Chindris (mihail.chindris@analog.com)
@@ -53,7 +54,8 @@
* @struct list_elem * @struct list_elem
* @brief Format of each element of the list * @brief Format of each element of the list
*/ */
struct list_elem { struct list_elem
{
/** User data */ /** User data */
void *data; void *data;
/** Reference to previous element */ /** Reference to previous element */
@@ -66,7 +68,8 @@ struct list_elem {
* @struct list_iterator * @struct list_iterator
* @brief Structure used to iterate through the list * @brief Structure used to iterate through the list
*/ */
struct iterator { struct iterator
{
/** List reference */ /** List reference */
struct _list_desc *list; struct _list_desc *list;
/** Current element reference */ /** Current element reference */
@@ -77,7 +80,8 @@ struct iterator {
* @struct _list_desc * @struct _list_desc
* @brief List descriptor * @brief List descriptor
*/ */
struct _list_desc { struct _list_desc
{
/** Reference to first element in the list */ /** Reference to first element in the list */
struct list_elem *first; struct list_elem *first;
/** Reference to last element in the list*/ /** Reference to last element in the list*/
@@ -132,7 +136,8 @@ static inline void update_links(struct list_elem *prev, struct list_elem *elem,
{ {
if (prev) if (prev)
prev->next = elem ? elem : next; prev->next = elem ? elem : next;
if (elem) { if (elem)
{
elem->prev = prev; elem->prev = prev;
elem->next = next; elem->next = next;
} }
@@ -150,11 +155,14 @@ static inline void update_desc(struct _list_desc *list,
struct list_elem *new_first, struct list_elem *new_first,
struct list_elem *new_last) struct list_elem *new_last)
{ {
if (new_first == list->first) { if (new_first == list->first)
{
list->last = new_last; list->last = new_last;
if (new_first == NULL || new_last == NULL) if (new_first == NULL || new_last == NULL)
list->first = new_last; list->first = new_last;
} else { /* if (new_last == list->last) */ }
else
{ /* if (new_last == list->last) */
list->first = new_first; list->first = new_first;
if (new_last == NULL || new_first == NULL) if (new_last == NULL || new_first == NULL)
list->last = new_first; list->last = new_first;
@@ -168,7 +176,8 @@ static inline void update_desc(struct _list_desc *list,
*/ */
static inline void set_adapter(struct list_desc *ad, enum adapter_type type) static inline void set_adapter(struct list_desc *ad, enum adapter_type type)
{ {
switch (type) { switch (type)
{
case LIST_PRIORITY_LIST: case LIST_PRIORITY_LIST:
ad->push = list_add_find; ad->push = list_add_find;
ad->pop = list_get_first; ad->pop = list_get_first;
@@ -211,14 +220,14 @@ int32_t list_init(struct list_desc **list_desc, enum adapter_type type,
struct list_desc *l_desc; struct list_desc *l_desc;
struct _list_desc *list; struct _list_desc *list;
if (!list_desc) if (!list_desc)
return FAILURE; return FAILURE;
l_desc = (struct list_desc *)calloc(1, sizeof(*l_desc)); l_desc = (struct list_desc *)calloc(1, sizeof(*l_desc));
if (!l_desc) if (!l_desc)
return FAILURE; return FAILURE;
list = (struct _list_desc *)calloc(1, sizeof(*list)); list = (struct _list_desc *)calloc(1, sizeof(*list));
if (!list) { if (!list)
{
free(l_desc); free(l_desc);
return FAILURE; return FAILURE;
} }
@@ -373,22 +382,24 @@ int32_t list_add_find(struct list_desc *list_desc, void *data)
return FAILURE; return FAILURE;
list = list_desc->priv_desc; list = list_desc->priv_desc;
/* Based on place iterator */ /* Based on place iterator */
elem = list->first; elem = list->first;
while (elem) { while (elem)
{
if (0 < list->comparator(elem->data, data)) if (0 < list->comparator(elem->data, data))
break; break;
elem = elem->next; elem = elem->next;
} }
if (elem == NULL) { if (elem == NULL)
{
list->l_it.elem = list->last; list->l_it.elem = list->last;
return iterator_insert(&(list->l_it), data, 1); return iterator_insert(&(list->l_it), data, 1);
} else { }
else
{
list->l_it.elem = elem; list->l_it.elem = elem;
return iterator_insert(&(list->l_it), data, 0); return iterator_insert(&(list->l_it), data, 0);
} }
} }
/** @brief Edit the first element of the list. Refer to \ref f_edit */ /** @brief Edit the first element of the list. Refer to \ref f_edit */
@@ -702,7 +713,8 @@ int32_t iterator_move(struct iterator *iter, int32_t steps)
steps = abs(steps); steps = abs(steps);
elem = it->elem; elem = it->elem;
while (steps > 0 && elem) { while (steps > 0 && elem)
{
elem = dir > 0 ? elem->next : elem->prev; elem = dir > 0 ? elem->next : elem->prev;
steps--; steps--;
} }
@@ -733,7 +745,8 @@ int32_t iterator_move_to_idx(struct iterator *iter, int32_t idx)
idx = abs(idx); idx = abs(idx);
elem = dir > 0 ? iter->list->first : iter->list->last; elem = dir > 0 ? iter->list->first : iter->list->last;
while (idx > 0 && elem) { while (idx > 0 && elem)
{
elem = dir > 0 ? elem->next : elem->prev; elem = dir > 0 ? elem->next : elem->prev;
idx--; idx--;
} }
@@ -762,8 +775,10 @@ int32_t iterator_find(struct iterator *iter, void *cmp_data)
return FAILURE; return FAILURE;
elem = it->list->first; elem = it->list->first;
while (elem) { while (elem)
if (0 == it->list->comparator(elem->data, cmp_data)) { {
if (0 == it->list->comparator(elem->data, cmp_data))
{
it->elem = elem; it->elem = elem;
return SUCCESS; return SUCCESS;
} }
@@ -799,7 +814,6 @@ int32_t iterator_get(struct iterator *iter, void **data)
struct iterator *it = iter; struct iterator *it = iter;
struct list_elem *next; struct list_elem *next;
if (!it || !it->elem || !data) if (!it || !it->elem || !data)
return FAILURE; return FAILURE;
df_datalink/src/util/util.c
+35 -20
View File
@@ -43,20 +43,22 @@ static uint8_t byte_flipLR[256] = {
0x07, 0x87, 0x47, 0xC7, 0x27, 0xA7, 0x67, 0xE7, 0x07, 0x87, 0x47, 0xC7, 0x27, 0xA7, 0x67, 0xE7,
0x17, 0x97, 0x57, 0xD7, 0x37, 0xB7, 0x77, 0xF7, 0x17, 0x97, 0x57, 0xD7, 0x37, 0xB7, 0x77, 0xF7,
0x0F, 0x8F, 0x4F, 0xCF, 0x2F, 0xAF, 0x6F, 0xEF, 0x0F, 0x8F, 0x4F, 0xCF, 0x2F, 0xAF, 0x6F, 0xEF,
0x1F, 0x9F, 0x5F, 0xDF, 0x3F, 0xBF, 0x7F, 0xFF 0x1F, 0x9F, 0x5F, 0xDF, 0x3F, 0xBF, 0x7F, 0xFF};
};
#endif #endif
inline void IO_Out32(uintptr_t addr, uint32_t value){ inline void IO_Out32(uintptr_t addr, uint32_t value)
{
volatile uint32_t *localAddr = (volatile uint32_t *)addr; volatile uint32_t *localAddr = (volatile uint32_t *)addr;
*localAddr = value; *localAddr = value;
} }
inline uint32_t IO_In32(uintptr_t addr){ inline uint32_t IO_In32(uintptr_t addr)
{
return *(volatile uint32_t *)addr; return *(volatile uint32_t *)addr;
} }
uint8_t byte_bit_swap(uint8_t byte){ uint8_t byte_bit_swap(uint8_t byte)
{
#ifdef __BITSWAP_TABLE #ifdef __BITSWAP_TABLE
return byte_flipLR[byte]; return byte_flipLR[byte];
#else #else
@@ -64,7 +66,8 @@ uint8_t byte_bit_swap(uint8_t byte){
#endif #endif
} }
uint32_t word_bit_swap(uint32_t word){ uint32_t word_bit_swap(uint32_t word)
{
#ifdef __BITSWAP_TABLE #ifdef __BITSWAP_TABLE
uint32_t rv_word = (byte_flipLR[word & 0xff] << 24) | uint32_t rv_word = (byte_flipLR[word & 0xff] << 24) |
(byte_flipLR[(word >> 8) & 0xff] << 16) | (byte_flipLR[(word >> 8) & 0xff] << 16) |
@@ -80,12 +83,14 @@ uint32_t word_bit_swap(uint32_t word){
#endif #endif
} }
uint32_t count_ones(uint32_t word){ uint32_t count_ones(uint32_t word)
{
#ifdef __GNUC__ #ifdef __GNUC__
return __builtin_popcount(word); return __builtin_popcount(word);
#else #else
uint32_t one_count = 0; uint32_t one_count = 0;
while (word) { while (word)
{
if (word & 0x00000001) if (word & 0x00000001)
one_count++; one_count++;
word >>= 1; word >>= 1;
@@ -94,7 +99,8 @@ uint32_t count_ones(uint32_t word){
#endif #endif
} }
double fix2double(int32_t val, uint8_t frac_bits){ double fix2double(int32_t val, uint8_t frac_bits)
{
return (((double)val) / (BIT(frac_bits))); return (((double)val) / (BIT(frac_bits)));
} }
@@ -104,16 +110,19 @@ double fix2double(int32_t val, uint8_t frac_bits){
uint32_t find_first_set_bit(uint32_t word) uint32_t find_first_set_bit(uint32_t word)
{ {
#ifdef __GNUC__ #ifdef __GNUC__
if(word != 0){ if (word != 0)
{
return __builtin_ctz(word); return __builtin_ctz(word);
} }
else{ else
{
return 32; return 32;
} }
#else #else
uint32_t first_set_bit = 0; uint32_t first_set_bit = 0;
while (word) { while (word)
{
if (word & 0x1) if (word & 0x1)
return first_set_bit; return first_set_bit;
word >>= 1; word >>= 1;
@@ -130,17 +139,20 @@ uint32_t find_first_set_bit(uint32_t word)
uint32_t find_last_set_bit(uint32_t word) uint32_t find_last_set_bit(uint32_t word)
{ {
#ifdef __GNUC__ #ifdef __GNUC__
if(word != 0){ if (word != 0)
{
return (31 - __builtin_clz(word)); return (31 - __builtin_clz(word));
} }
else{ else
{
return 32; return 32;
} }
#else #else
uint32_t bit = 0; uint32_t bit = 0;
uint32_t last_set_bit = 32; uint32_t last_set_bit = 32;
while (word) { while (word)
{
if (word & 0x1) if (word & 0x1)
last_set_bit = bit; last_set_bit = bit;
word >>= 1; word >>= 1;
@@ -162,8 +174,10 @@ uint32_t find_closest(int32_t val,
uint32_t ret = 0; uint32_t ret = 0;
uint32_t i; uint32_t i;
for (i = 1; i < size; i++) { for (i = 1; i < size; i++)
if (abs(array[i] - val) < diff) { {
if (abs(array[i] - val) < diff)
{
diff = abs(array[i] - val); diff = abs(array[i] - val);
ret = i; ret = i;
} }
@@ -233,7 +247,8 @@ void rational_best_approximation(uint32_t given_numerator,
*best_denominator = given_denominator / gcd; *best_denominator = given_denominator / gcd;
if ((*best_numerator > max_numerator) || if ((*best_numerator > max_numerator) ||
(*best_denominator > max_denominator)) { (*best_denominator > max_denominator))
{
*best_numerator = 0; *best_numerator = 0;
*best_denominator = 0; *best_denominator = 0;
} }
@@ -246,7 +261,8 @@ uint32_t hweight8(uint32_t word)
{ {
uint32_t count = 0; uint32_t count = 0;
while (word) { while (word)
{
if (word & 0x1) if (word & 0x1)
count++; count++;
word >>= 1; word >>= 1;
@@ -348,4 +364,3 @@ uint32_t srt_to_uint32(const char *str)
else else
return value; return value;
} }