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
+242 -197
View File
@@ -40,30 +40,26 @@
#include "networking/link_layer/link_layer.h"
/************************** Function Prototypes ******************************/
int32_t SetupInterruptSystem(struct irq_ctrl_desc* descPtr);
int32_t SetupInterruptSystem(struct irq_ctrl_desc *descPtr);
void Phy_IntrHandler(void *context);
void ScuTimer_IntrHandler(void *context);
void Systembus_RxIntrHandler(void *context);
/************************** Constant Definitions *****************************/
struct callback_desc phy_callback_desc = {
.callback = Phy_IntrHandler,
.ctx = NULL
};
.ctx = NULL};
struct callback_desc sysbus_rx_callback_desc = {
.callback = Systembus_RxIntrHandler,
.ctx = NULL
};
.ctx = NULL};
struct callback_desc sysbus_tx_callback_desc = {
.callback = Systembus_RxIntrHandler,
.ctx = NULL
};
.ctx = NULL};
struct callback_desc scutm_callback_desc = {
.callback = ScuTimer_IntrHandler,
.ctx = NULL
};
.ctx = NULL};
static PHY_CONFIG phyConfig = {
.devId = 0,
@@ -71,8 +67,7 @@ static PHY_CONFIG phyConfig = {
.antBramId = PHY_ANTCONFIG_BRAM_DEVICE_ID,
.txBramId = PHY_TXBUFF_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 = {
.myType = L2_HEAD_NODE,
@@ -82,21 +77,20 @@ static L2_CONFIG linkConfig = {
.phyInstRef = NULL // pyhsical layer reference
};
//static L2_CONFIG linkConfig = {
// static L2_CONFIG linkConfig = {
// .myType = L2_MEMBER_NODE,
// .myId = 2,
// .headId = 1,
// .netScale = 2,
// .phyInstRef = NULL // pyhsical layer reference
//};
// };
static SYSBUS_CONFIG sysbusConfig = {
.baseAddr = SYSBUS_BASE_ADDR,
.baseAddr = SYSBUS_BASE_ADDR,
.txQueueMax = 16,
.rxQueueMax = 16
};
.rxQueueMax = 16};
/*********************** 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 SYSBUS *sysbusDescPtr;
static PHY_INST *phyInstPtr;
@@ -105,8 +99,8 @@ static L2_INST *linkInstPtr;
static struct hmc7044_dev *hmc7044_inst;
static struct ad9361_reg_dev *ad9361_1_reg_inst;
static struct ad9361_reg_dev *ad9361_2_reg_inst;
//static struct ad9361_rf_phy *ad9361_phy;
//static struct ad9361_rf_phy *ad9361_2_phy;
// static struct ad9361_rf_phy *ad9361_phy;
// static struct ad9361_rf_phy *ad9361_2_phy;
/****************************************************************************/
/**
* This function is responsible for initializing the SCUGIC instance, looking
@@ -119,75 +113,86 @@ static struct ad9361_reg_dev *ad9361_2_reg_inst;
* FAILURE if initialization failed.
*
*****************************************************************************/
int32_t SetupInterruptSystem(struct irq_ctrl_desc* descPtr) {
int32_t SetupInterruptSystem(struct irq_ctrl_desc *descPtr)
{
int32_t Status;
struct xil_irq_desc *xil_dev;
Status = irq_ctrl_init(&descPtr, &intc_init_param);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("Intc Init FAILED, ErrorCode: %ld.\n", Status);
return Status;
}
xil_dev = (struct xil_irq_desc*) (descPtr->extra);
xil_dev = (struct xil_irq_desc *)(descPtr->extra);
// Clear any pending interrupt flags
uint32_t intrState = XScuGic_CPUReadReg((XScuGic*)(xil_dev->instance), XSCUGIC_INT_ACK_OFFSET);
XScuGic_CPUWriteReg((XScuGic*)(xil_dev->instance), XSCUGIC_EOI_OFFSET, intrState);
uint32_t intrState = XScuGic_CPUReadReg((XScuGic *)(xil_dev->instance), XSCUGIC_INT_ACK_OFFSET);
XScuGic_CPUWriteReg((XScuGic *)(xil_dev->instance), XSCUGIC_EOI_OFFSET, intrState);
/* Initialize physical layer interrupt. */
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);
return Status;
}
// Set highest interrupt priority
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);
return Status;
}
// Connect the interrupt service routine isr1 to the interrupt controller
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);
return Status;
}
/* Initialize system bus interrupt. */
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);
return Status;
}
// Set interrupt priority
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);
return Status;
}
// Connect the interrupt service routine isr1 to the interrupt controller
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);
return Status;
}
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);
return Status;
}
// Set interrupt priority
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);
return Status;
}
// Connect the interrupt service routine isr1 to the interrupt controller
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);
return Status;
}
@@ -195,7 +200,8 @@ int32_t SetupInterruptSystem(struct irq_ctrl_desc* descPtr) {
/* Initialize Scu timer interrupt. */
// Connect the interrupt service routine isr1 to the interrupt controller
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);
return Status;
}
@@ -208,7 +214,8 @@ int32_t SetupInterruptSystem(struct irq_ctrl_desc* descPtr) {
// Enable Exceptions
Status = irq_global_enable(descPtr);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("IRQ global enable FAILED, ErrorCode: %ld.\n", Status);
return Status;
}
@@ -225,14 +232,16 @@ int32_t SetupInterruptSystem(struct irq_ctrl_desc* descPtr) {
* @return None.
*
*****************************************************************************/
void Phy_IntrHandler(void *context) {
void Phy_IntrHandler(void *context)
{
int32_t Status;
Status = Linklayer_Proc(linkInstPtr);
if(Status != SUCCESS){
if (Status != SUCCESS)
{
DBG_ERROR("Link layer runs failed, with error code: %ld\n", Status);
_nop_();
}
} // PhyIntr_Handler
} // PhyIntr_Handler
/*****************************************************************************/
/**
@@ -243,10 +252,12 @@ void Phy_IntrHandler(void *context) {
* @return None.
*
******************************************************************************/
void Systembus_RxIntrHandler(void *context) {
void Systembus_RxIntrHandler(void *context)
{
int32_t Status;
Status = SystemBus_RxIntrHandler(sysbusDescPtr);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("SystemBus_RxIntrHandler exit with error code %ld,\n", Status);
}
}
@@ -260,10 +271,12 @@ void Systembus_RxIntrHandler(void *context) {
* @return None.
*
******************************************************************************/
void Systembus_TxIntrHandler(void *context) {
void Systembus_TxIntrHandler(void *context)
{
int32_t Status;
Status = SystemBus_TxIntrHandler(sysbusDescPtr);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("SystemBus_TxIntrHandler exit with error code %ld,\n", Status);
}
}
@@ -277,20 +290,22 @@ void Systembus_TxIntrHandler(void *context) {
* @return None.
*
******************************************************************************/
void ScuTimer_IntrHandler(void *context) {
struct xil_timer_desc* xdesc;
xdesc = (struct xil_timer_desc*)(logTmPtr->extra);
if (XScuTimer_IsExpired((XScuTimer*)xdesc->instance)) {
void ScuTimer_IntrHandler(void *context)
{
struct xil_timer_desc *xdesc;
xdesc = (struct xil_timer_desc *)(logTmPtr->extra);
if (XScuTimer_IsExpired((XScuTimer *)xdesc->instance))
{
// Update log
Phy_UpdateHwSate(phyInstPtr);
XScuTimer_ClearInterruptStatus((XScuTimer*)xdesc->instance);
XScuTimer_ClearInterruptStatus((XScuTimer *)xdesc->instance);
}
else{
else
{
DBG_WARNING("UNEXPECTED CALL of ScuTimer_IntrHandler\n");
}
}
/*****************************************************************************/
/**
* @brief Hardware_SwitchFreq
@@ -301,155 +316,161 @@ void ScuTimer_IntrHandler(void *context) {
* @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;
uint8_t value = 0;
int32_t time_out_cnt = 0;
switch(freq){
case TX_FREQ_125: {
switch (freq)
{
case TX_FREQ_125:
{
//************************************************************
// Setup Tx Frequency-Dependent Syntheisizer Registers
//************************************************************
status = ad9361_reg_write(dev, 0x027A, 0x4A); // Set VCO Output level[3:0]
status = ad9361_reg_write(dev, 0x0279, 0xC1); // Set Init ALC Value[3:0] and VCO Varactor[3:0]
status = ad9361_reg_write(dev, 0x0282, 0x0E); // Set VCO Bias Tcf[1:0] and VCO Bias Ref[2:0]
status = ad9361_reg_write(dev, 0x0278, 0x78); // Set VCO Cal Offset[3:0]
status = ad9361_reg_write(dev, 0x0285, 0x00); // Set VCO Cal Ref Tcf[2:0]
status = ad9361_reg_write(dev, 0x0291, 0x0B); // Set VCO Varactor Reference[3:0]
status = ad9361_reg_write(dev, 0x0290, 0x70); // Set VCO Varactor Ref Tcf[2:0] and VCO Varactor Offset[3:0]
status = ad9361_reg_write(dev, 0x027B, 0x94); // Set Synth Loop Filter charge pump current (Icp)
status = ad9361_reg_write(dev, 0x027E, 0xF4); // Set Synth Loop Filter C2 and C1
status = ad9361_reg_write(dev, 0x027F, 0xDF); // Set Synth Loop Filter R1 and C3
status = ad9361_reg_write(dev, 0x0280, 0x0A); // Set Synth Loop Filter R3
status = ad9361_reg_write(dev, 0x027A, 0x4A); // Set VCO Output level[3:0]
status = ad9361_reg_write(dev, 0x0279, 0xC1); // Set Init ALC Value[3:0] and VCO Varactor[3:0]
status = ad9361_reg_write(dev, 0x0282, 0x0E); // Set VCO Bias Tcf[1:0] and VCO Bias Ref[2:0]
status = ad9361_reg_write(dev, 0x0278, 0x78); // Set VCO Cal Offset[3:0]
status = ad9361_reg_write(dev, 0x0285, 0x00); // Set VCO Cal Ref Tcf[2:0]
status = ad9361_reg_write(dev, 0x0291, 0x0B); // Set VCO Varactor Reference[3:0]
status = ad9361_reg_write(dev, 0x0290, 0x70); // Set VCO Varactor Ref Tcf[2:0] and VCO Varactor Offset[3:0]
status = ad9361_reg_write(dev, 0x027B, 0x94); // Set Synth Loop Filter charge pump current (Icp)
status = ad9361_reg_write(dev, 0x027E, 0xF4); // Set Synth Loop Filter C2 and C1
status = ad9361_reg_write(dev, 0x027F, 0xDF); // Set Synth Loop Filter R1 and C3
status = ad9361_reg_write(dev, 0x0280, 0x0A); // Set Synth Loop Filter R3
//************************************************************
// Write Tx Frequency Words
//************************************************************
status = ad9361_reg_write(dev, 0x0273, 0x00); // Write Tx Synth Fractional Freq Word[7:0]
status = ad9361_reg_write(dev, 0x0274, 0x00); // Write Tx Synth Fractional Freq Word[15:8]
status = ad9361_reg_write(dev, 0x0275, 0x00); // Write Tx Synth Fractional Freq Word[22:16]
status = ad9361_reg_write(dev, 0x0272, 0x00); // Write Tx Synth Integer Freq Word[10:8]
status = ad9361_reg_write(dev, 0x0271, 0xA0); // Write Tx Synth Integer Freq Word[7:0] (starts VCO cal)
status = ad9361_reg_write(dev, 0x0005, 0x57); // Set LO divider setting
status = ad9361_reg_write(dev, 0x0273, 0x00); // Write Tx Synth Fractional Freq Word[7:0]
status = ad9361_reg_write(dev, 0x0274, 0x00); // Write Tx Synth Fractional Freq Word[15:8]
status = ad9361_reg_write(dev, 0x0275, 0x00); // Write Tx Synth Fractional Freq Word[22:16]
status = ad9361_reg_write(dev, 0x0272, 0x00); // Write Tx Synth Integer Freq Word[10:8]
status = ad9361_reg_write(dev, 0x0271, 0xA0); // Write Tx Synth Integer Freq Word[7:0] (starts VCO cal)
status = ad9361_reg_write(dev, 0x0005, 0x57); // Set LO divider setting
//************************************************************
// Tx Quadrature Calibration Settings
//************************************************************
status = ad9361_reg_write(dev, 0x00A0, 0x75); // Set TxQuadcal NCO frequency
status = ad9361_reg_write(dev, 0x00A3, 0xC0); // Set TxQuadcal NCO frequency (Only update bits [7:6])
status = ad9361_reg_write(dev, 0x00A1, 0x7B); // Tx Quad Cal Configuration, Phase and Gain Cal Enable
status = ad9361_reg_write(dev, 0x00A9, 0xFF); // Set Tx Quad Cal Count
status = ad9361_reg_write(dev, 0x00A2, 0x7F); // Set Tx Quad Cal Kexp
status = ad9361_reg_write(dev, 0x00A5, 0x01); // Set Tx Quad Cal Magnitude Threshhold
status = ad9361_reg_write(dev, 0x00A6, 0x01); // Set Tx Quad Cal Magnitude Threshhold
status = ad9361_reg_write(dev, 0x00AA, 0x22); // Set Tx Quad Cal Gain Table index
status = ad9361_reg_write(dev, 0x00A4, 0xF0); // Set Tx Quad Cal Settle Count
status = ad9361_reg_write(dev, 0x00AE, 0x00); // Set Tx Quad Cal LPF Gain index incase Split table mode used
status = ad9361_reg_write(dev, 0x00A0, 0x75); // Set TxQuadcal NCO frequency
status = ad9361_reg_write(dev, 0x00A3, 0xC0); // Set TxQuadcal NCO frequency (Only update bits [7:6])
status = ad9361_reg_write(dev, 0x00A1, 0x7B); // Tx Quad Cal Configuration, Phase and Gain Cal Enable
status = ad9361_reg_write(dev, 0x00A9, 0xFF); // Set Tx Quad Cal Count
status = ad9361_reg_write(dev, 0x00A2, 0x7F); // Set Tx Quad Cal Kexp
status = ad9361_reg_write(dev, 0x00A5, 0x01); // Set Tx Quad Cal Magnitude Threshhold
status = ad9361_reg_write(dev, 0x00A6, 0x01); // Set Tx Quad Cal Magnitude Threshhold
status = ad9361_reg_write(dev, 0x00AA, 0x22); // Set Tx Quad Cal Gain Table index
status = ad9361_reg_write(dev, 0x00A4, 0xF0); // Set Tx Quad Cal Settle Count
status = ad9361_reg_write(dev, 0x00AE, 0x00); // Set Tx Quad Cal LPF Gain index incase Split table mode used
status = ad9361_reg_write(dev, 0x0169, 0xC0); // Disable Rx Quadrature Calibration before Running Tx Quadrature Calibration
status = ad9361_reg_write(dev, 0x0016, 0x10); // Start Tx Quad cal
status = ad9361_reg_write(dev, 0x0169, 0xC0); // Disable Rx Quadrature Calibration before Running Tx Quadrature Calibration
status = ad9361_reg_write(dev, 0x0016, 0x10); // Start Tx Quad cal
break;
}
case TX_FREQ_126: {
}
case TX_FREQ_126:
{
//************************************************************
// Setup Tx Frequency-Dependent Syntheisizer Registers
//************************************************************
status = ad9361_reg_write(dev, 0x027A, 0x4A); // Set VCO Output level[3:0]
status = ad9361_reg_write(dev, 0x0279, 0xC1); // Set Init ALC Value[3:0] and VCO Varactor[3:0]
status = ad9361_reg_write(dev, 0x0282, 0x0E); // Set VCO Bias Tcf[1:0] and VCO Bias Ref[2:0]
status = ad9361_reg_write(dev, 0x0278, 0x78); // Set VCO Cal Offset[3:0]
status = ad9361_reg_write(dev, 0x0285, 0x00); // Set VCO Cal Ref Tcf[2:0]
status = ad9361_reg_write(dev, 0x0291, 0x0B); // Set VCO Varactor Reference[3:0]
status = ad9361_reg_write(dev, 0x0290, 0x70); // Set VCO Varactor Ref Tcf[2:0] and VCO Varactor Offset[3:0]
status = ad9361_reg_write(dev, 0x027B, 0x93); // Set Synth Loop Filter charge pump current (Icp)
status = ad9361_reg_write(dev, 0x027E, 0xF4); // Set Synth Loop Filter C2 and C1
status = ad9361_reg_write(dev, 0x027F, 0xDF); // Set Synth Loop Filter R1 and C3
status = ad9361_reg_write(dev, 0x0280, 0x0A); // Set Synth Loop Filter R3
status = ad9361_reg_write(dev, 0x027A, 0x4A); // Set VCO Output level[3:0]
status = ad9361_reg_write(dev, 0x0279, 0xC1); // Set Init ALC Value[3:0] and VCO Varactor[3:0]
status = ad9361_reg_write(dev, 0x0282, 0x0E); // Set VCO Bias Tcf[1:0] and VCO Bias Ref[2:0]
status = ad9361_reg_write(dev, 0x0278, 0x78); // Set VCO Cal Offset[3:0]
status = ad9361_reg_write(dev, 0x0285, 0x00); // Set VCO Cal Ref Tcf[2:0]
status = ad9361_reg_write(dev, 0x0291, 0x0B); // Set VCO Varactor Reference[3:0]
status = ad9361_reg_write(dev, 0x0290, 0x70); // Set VCO Varactor Ref Tcf[2:0] and VCO Varactor Offset[3:0]
status = ad9361_reg_write(dev, 0x027B, 0x93); // Set Synth Loop Filter charge pump current (Icp)
status = ad9361_reg_write(dev, 0x027E, 0xF4); // Set Synth Loop Filter C2 and C1
status = ad9361_reg_write(dev, 0x027F, 0xDF); // Set Synth Loop Filter R1 and C3
status = ad9361_reg_write(dev, 0x0280, 0x0A); // Set Synth Loop Filter R3
//************************************************************
// Write Tx Frequency Words
//************************************************************
status = ad9361_reg_write(dev, 0x0273, 0x06); // Write Tx Synth Fractional Freq Word[7:0]
status = ad9361_reg_write(dev, 0x0274, 0xD7); // Write Tx Synth Fractional Freq Word[15:8]
status = ad9361_reg_write(dev, 0x0275, 0x23); // Write Tx Synth Fractional Freq Word[22:16]
status = ad9361_reg_write(dev, 0x0272, 0x00); // Write Tx Synth Integer Freq Word[10:8]
status = ad9361_reg_write(dev, 0x0271, 0xA1); // Write Tx Synth Integer Freq Word[7:0] (starts VCO cal)
status = ad9361_reg_write(dev, 0x0005, 0x57); // Set LO divider setting
status = ad9361_reg_write(dev, 0x0273, 0x06); // Write Tx Synth Fractional Freq Word[7:0]
status = ad9361_reg_write(dev, 0x0274, 0xD7); // Write Tx Synth Fractional Freq Word[15:8]
status = ad9361_reg_write(dev, 0x0275, 0x23); // Write Tx Synth Fractional Freq Word[22:16]
status = ad9361_reg_write(dev, 0x0272, 0x00); // Write Tx Synth Integer Freq Word[10:8]
status = ad9361_reg_write(dev, 0x0271, 0xA1); // Write Tx Synth Integer Freq Word[7:0] (starts VCO cal)
status = ad9361_reg_write(dev, 0x0005, 0x57); // Set LO divider setting
//************************************************************
// Tx Quadrature Calibration Settings
//************************************************************
status = ad9361_reg_write(dev, 0x00A0, 0x75); // Set TxQuadcal NCO frequency
status = ad9361_reg_write(dev, 0x00A3, 0xC0); // Set TxQuadcal NCO frequency (Only update bits [7:6])
status = ad9361_reg_write(dev, 0x00A1, 0x7B); // Tx Quad Cal Configuration, Phase and Gain Cal Enable
status = ad9361_reg_write(dev, 0x00A9, 0xFF); // Set Tx Quad Cal Count
status = ad9361_reg_write(dev, 0x00A2, 0x7F); // Set Tx Quad Cal Kexp
status = ad9361_reg_write(dev, 0x00A5, 0x01); // Set Tx Quad Cal Magnitude Threshhold
status = ad9361_reg_write(dev, 0x00A6, 0x01); // Set Tx Quad Cal Magnitude Threshhold
status = ad9361_reg_write(dev, 0x00AA, 0x22); // Set Tx Quad Cal Gain Table index
status = ad9361_reg_write(dev, 0x00A4, 0xF0); // Set Tx Quad Cal Settle Count
status = ad9361_reg_write(dev, 0x00AE, 0x00); // Set Tx Quad Cal LPF Gain index incase Split table mode used
status = ad9361_reg_write(dev, 0x00A0, 0x75); // Set TxQuadcal NCO frequency
status = ad9361_reg_write(dev, 0x00A3, 0xC0); // Set TxQuadcal NCO frequency (Only update bits [7:6])
status = ad9361_reg_write(dev, 0x00A1, 0x7B); // Tx Quad Cal Configuration, Phase and Gain Cal Enable
status = ad9361_reg_write(dev, 0x00A9, 0xFF); // Set Tx Quad Cal Count
status = ad9361_reg_write(dev, 0x00A2, 0x7F); // Set Tx Quad Cal Kexp
status = ad9361_reg_write(dev, 0x00A5, 0x01); // Set Tx Quad Cal Magnitude Threshhold
status = ad9361_reg_write(dev, 0x00A6, 0x01); // Set Tx Quad Cal Magnitude Threshhold
status = ad9361_reg_write(dev, 0x00AA, 0x22); // Set Tx Quad Cal Gain Table index
status = ad9361_reg_write(dev, 0x00A4, 0xF0); // Set Tx Quad Cal Settle Count
status = ad9361_reg_write(dev, 0x00AE, 0x00); // Set Tx Quad Cal LPF Gain index incase Split table mode used
status = ad9361_reg_write(dev, 0x0169, 0xC0); // Disable Rx Quadrature Calibration before Running Tx Quadrature Calibration
status = ad9361_reg_write(dev, 0x0016, 0x10); // Start Tx Quad cal
status = ad9361_reg_write(dev, 0x0169, 0xC0); // Disable Rx Quadrature Calibration before Running Tx Quadrature Calibration
status = ad9361_reg_write(dev, 0x0016, 0x10); // Start Tx Quad cal
break;
}
case TX_FREQ_4125: {
}
case TX_FREQ_4125:
{
//************************************************************
// Setup Tx Frequency-Dependent Syntheisizer Registers
//************************************************************
status = ad9361_reg_write(dev, 0x027A, 0x4A); // Set VCO Output level[3:0]
status = ad9361_reg_write(dev, 0x0279, 0xC1); // Set Init ALC Value[3:0] and VCO Varactor[3:0]
status = ad9361_reg_write(dev, 0x0282, 0x0E); // Set VCO Bias Tcf[1:0] and VCO Bias Ref[2:0]
status = ad9361_reg_write(dev, 0x0278, 0x78); // Set VCO Cal Offset[3:0]
status = ad9361_reg_write(dev, 0x0285, 0x00); // Set VCO Cal Ref Tcf[2:0]
status = ad9361_reg_write(dev, 0x0291, 0x0B); // Set VCO Varactor Reference[3:0]
status = ad9361_reg_write(dev, 0x0290, 0x70); // Set VCO Varactor Ref Tcf[2:0] and VCO Varactor Offset[3:0]
status = ad9361_reg_write(dev, 0x027B, 0x93); // Set Synth Loop Filter charge pump current (Icp)
status = ad9361_reg_write(dev, 0x027E, 0xF4); // Set Synth Loop Filter C2 and C1
status = ad9361_reg_write(dev, 0x027F, 0xDF); // Set Synth Loop Filter R1 and C3
status = ad9361_reg_write(dev, 0x0280, 0x0A); // Set Synth Loop Filter R3
status = ad9361_reg_write(dev, 0x027A, 0x4A); // Set VCO Output level[3:0]
status = ad9361_reg_write(dev, 0x0279, 0xC1); // Set Init ALC Value[3:0] and VCO Varactor[3:0]
status = ad9361_reg_write(dev, 0x0282, 0x0E); // Set VCO Bias Tcf[1:0] and VCO Bias Ref[2:0]
status = ad9361_reg_write(dev, 0x0278, 0x78); // Set VCO Cal Offset[3:0]
status = ad9361_reg_write(dev, 0x0285, 0x00); // Set VCO Cal Ref Tcf[2:0]
status = ad9361_reg_write(dev, 0x0291, 0x0B); // Set VCO Varactor Reference[3:0]
status = ad9361_reg_write(dev, 0x0290, 0x70); // Set VCO Varactor Ref Tcf[2:0] and VCO Varactor Offset[3:0]
status = ad9361_reg_write(dev, 0x027B, 0x93); // Set Synth Loop Filter charge pump current (Icp)
status = ad9361_reg_write(dev, 0x027E, 0xF4); // Set Synth Loop Filter C2 and C1
status = ad9361_reg_write(dev, 0x027F, 0xDF); // Set Synth Loop Filter R1 and C3
status = ad9361_reg_write(dev, 0x0280, 0x0A); // Set Synth Loop Filter R3
//************************************************************
// Write Rx Frequency Words
//************************************************************
status = ad9361_reg_write(dev, 0x0273, 0x00); // Write Tx Synth Fractional Freq Word[7:0]
status = ad9361_reg_write(dev, 0x0274, 0x00); // Write Tx Synth Fractional Freq Word[15:8]
status = ad9361_reg_write(dev, 0x0275, 0x00); // Write Tx Synth Fractional Freq Word[22:16]
status = ad9361_reg_write(dev, 0x0272, 0x00); // Write Tx Synth Integer Freq Word[10:8]
status = ad9361_reg_write(dev, 0x0271, 0xA5); // Write Tx Synth Integer Freq Word[7:0] (starts VCO cal)
status = ad9361_reg_write(dev, 0x0005, 0x07); // Set LO divider setting
status = ad9361_reg_write(dev, 0x0273, 0x00); // Write Tx Synth Fractional Freq Word[7:0]
status = ad9361_reg_write(dev, 0x0274, 0x00); // Write Tx Synth Fractional Freq Word[15:8]
status = ad9361_reg_write(dev, 0x0275, 0x00); // Write Tx Synth Fractional Freq Word[22:16]
status = ad9361_reg_write(dev, 0x0272, 0x00); // Write Tx Synth Integer Freq Word[10:8]
status = ad9361_reg_write(dev, 0x0271, 0xA5); // Write Tx Synth Integer Freq Word[7:0] (starts VCO cal)
status = ad9361_reg_write(dev, 0x0005, 0x07); // Set LO divider setting
//************************************************************
// Tx Quadrature Calibration Settings
//************************************************************
status = ad9361_reg_write(dev, 0x00A0, 0x75); // Set TxQuadcal NCO frequency
status = ad9361_reg_write(dev, 0x00A3, 0xC0); // Set TxQuadcal NCO frequency (Only update bits [7:6])
status = ad9361_reg_write(dev, 0x00A1, 0x7B); // Tx Quad Cal Configuration, Phase and Gain Cal Enable
status = ad9361_reg_write(dev, 0x00A9, 0xFF); // Set Tx Quad Cal Count
status = ad9361_reg_write(dev, 0x00A2, 0x7F); // Set Tx Quad Cal Kexp
status = ad9361_reg_write(dev, 0x00A5, 0x01); // Set Tx Quad Cal Magnitude Threshhold
status = ad9361_reg_write(dev, 0x00A6, 0x01); // Set Tx Quad Cal Magnitude Threshhold
status = ad9361_reg_write(dev, 0x00AA, 0x28); // Set Tx Quad Cal Gain Table index
status = ad9361_reg_write(dev, 0x00A4, 0xF0); // Set Tx Quad Cal Settle Count
status = ad9361_reg_write(dev, 0x00AE, 0x04); // Set Tx Quad Cal LPF Gain index incase Split table mode used
status = ad9361_reg_write(dev, 0x00A0, 0x75); // Set TxQuadcal NCO frequency
status = ad9361_reg_write(dev, 0x00A3, 0xC0); // Set TxQuadcal NCO frequency (Only update bits [7:6])
status = ad9361_reg_write(dev, 0x00A1, 0x7B); // Tx Quad Cal Configuration, Phase and Gain Cal Enable
status = ad9361_reg_write(dev, 0x00A9, 0xFF); // Set Tx Quad Cal Count
status = ad9361_reg_write(dev, 0x00A2, 0x7F); // Set Tx Quad Cal Kexp
status = ad9361_reg_write(dev, 0x00A5, 0x01); // Set Tx Quad Cal Magnitude Threshhold
status = ad9361_reg_write(dev, 0x00A6, 0x01); // Set Tx Quad Cal Magnitude Threshhold
status = ad9361_reg_write(dev, 0x00AA, 0x28); // Set Tx Quad Cal Gain Table index
status = ad9361_reg_write(dev, 0x00A4, 0xF0); // Set Tx Quad Cal Settle Count
status = ad9361_reg_write(dev, 0x00AE, 0x04); // Set Tx Quad Cal LPF Gain index incase Split table mode used
status = ad9361_reg_write(dev, 0x0169, 0xC0); // Disable Rx Quadrature Calibration before Running Tx Quadrature Calibration
status = ad9361_reg_write(dev, 0x0016, 0x10); // Start Tx Quad cal
status = ad9361_reg_write(dev, 0x0169, 0xC0); // Disable Rx Quadrature Calibration before Running Tx Quadrature Calibration
status = ad9361_reg_write(dev, 0x0016, 0x10); // Start Tx Quad cal
break;
}
}
// Wait for cal to complete (Done when 0x016[4]==0)
time_out_cnt = 0;
do{
do
{
status |= ad9361_reg_read(dev, 0x0016, &value);
mdelay(1);// Wait 1ms
mdelay(1); // Wait 1ms
time_out_cnt++;
} while((value & 0x10) != 0x00 && time_out_cnt <= AD9361_TIMEOUT_TH);
} while ((value & 0x10) != 0x00 && time_out_cnt <= AD9361_TIMEOUT_TH);
}
return status;
@@ -464,7 +485,8 @@ int Hardware_SwitchFreq(struct ad9361_reg_dev *dev, ad9361TxFreq freq){
* @return None.
*
******************************************************************************/
int Hardware_Init(void) {
int Hardware_Init(void)
{
int32_t Status;
XGpioPs Gpio;
XGpioPs_Config *ConfigPtr;
@@ -479,7 +501,8 @@ int Hardware_Init(void) {
/****************** HMC7044 Configuration *****************/
/**********************************************************/
Status = hmc7044_init(&hmc7044_inst, &hmc7044_default_init_param);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("hmc7044_init() error: %ld\n", Status);
return FAILURE;
}
@@ -497,14 +520,14 @@ int Hardware_Init(void) {
ad9361_reg_config(ad9361_1_reg_inst);
XGpioPs_WritePin(&Gpio, AD9361_1_ENABLE, 1);
// Status = ad9361_init(&ad9361_phy, &ad9361_default_init_param);
// if (Status != XST_SUCCESS){
// return XST_FAILURE;
// }else{
// printf("Configurate AD9361 Completely!\n");
// }
// ad9361_set_tx_fir_config(ad9361_phy, tx_fir_config);
// ad9361_set_rx_fir_config(ad9361_phy, rx_fir_config);
// Status = ad9361_init(&ad9361_phy, &ad9361_default_init_param);
// if (Status != XST_SUCCESS){
// return XST_FAILURE;
// }else{
// printf("Configurate AD9361 Completely!\n");
// }
// ad9361_set_tx_fir_config(ad9361_phy, tx_fir_config);
// ad9361_set_rx_fir_config(ad9361_phy, rx_fir_config);
/**********************************************************/
/***************** AD9361-2 Configuration *****************/
@@ -519,9 +542,9 @@ int Hardware_Init(void) {
ad9361_reg_config(ad9361_2_reg_inst);
XGpioPs_WritePin(&Gpio, AD9361_2_ENABLE, 1);
// uint8_t rd_back;
// Status = ad9361_reg_read(ad9361_1_reg_inst, 0x0002, &rd_back);
// Status = ad9361_reg_read(ad9361_2_reg_inst, 0x0002, &rd_back);
// uint8_t rd_back;
// Status = ad9361_reg_read(ad9361_1_reg_inst, 0x0002, &rd_back);
// Status = ad9361_reg_read(ad9361_2_reg_inst, 0x0002, &rd_back);
/**********************************************************/
/******* AD9361 Baseband Multichip Synchronization ********/
@@ -530,9 +553,9 @@ int Hardware_Init(void) {
XGpioPs_SetDirectionPin(&Gpio, AD9361_RX_FIFO_RST, AD9361_GPIO_OUTPUT);
// SYNC BBPLL and DIGITAL SAMPLING CLOCKS
ad9361_reg_write(ad9361_1_reg_inst, 0x0001, 0x0D); // MCS RF Enable; MCS BBPLL Enable; MCS BB Enable
ad9361_reg_write(ad9361_1_reg_inst, 0x0001, 0x0D); // MCS RF Enable; MCS BBPLL Enable; MCS BB Enable
ad9361_reg_write(ad9361_2_reg_inst, 0x0001, 0x0D);
ad9361_reg_write(ad9361_1_reg_inst, 0x0047, 0x80); // MCS refclk Scale Enable
ad9361_reg_write(ad9361_1_reg_inst, 0x0047, 0x80); // MCS refclk Scale Enable
ad9361_reg_write(ad9361_2_reg_inst, 0x0047, 0x80);
XGpioPs_WritePin(&Gpio, AD9361_SYNC, 0);
mdelay(20);
@@ -540,12 +563,12 @@ int Hardware_Init(void) {
mdelay(500);
XGpioPs_WritePin(&Gpio, AD9361_SYNC, 0);
mdelay(2000);
ad9361_reg_write(ad9361_1_reg_inst, 0x0001, 0x0B); // MCS RF Enable; MCS Digital CLK Enable ; MCS BB Enable
ad9361_reg_write(ad9361_1_reg_inst, 0x0001, 0x0B); // MCS RF Enable; MCS Digital CLK Enable ; MCS BB Enable
ad9361_reg_write(ad9361_2_reg_inst, 0x0001, 0x0B);
XGpioPs_WritePin(&Gpio, AD9361_SYNC, 1); // Input another rising-edge pulse to the SYNC_IN pin
mdelay(500);
XGpioPs_WritePin(&Gpio, AD9361_SYNC, 0);
ad9361_reg_write(ad9361_1_reg_inst, 0x0001, 0x08); // clear MCS bits
ad9361_reg_write(ad9361_1_reg_inst, 0x0001, 0x08); // clear MCS bits
ad9361_reg_write(ad9361_2_reg_inst, 0x0001, 0x08);
XGpioPs_WritePin(&Gpio, AD9361_RX_FIFO_RST, 0);
@@ -565,7 +588,7 @@ int Hardware_Init(void) {
// switch to lower frequency
Status = Hardware_SwitchFreq(ad9361_1_reg_inst, TX_FREQ_126);
XGpioPs_WritePin(&Gpio, AD9361_TX_SWITCH, 1);
XGpioPs_WritePin(&Gpio, AD9361_TX_SWITCH, 1);
mdelay(20);
// switch rx channel
@@ -573,26 +596,28 @@ int Hardware_Init(void) {
Status = ad9361_reg_write(ad9361_2_reg_inst, 0x0004, 0x30);
// enable dual ad9361 tx channels
Status = ad9361_reg_write(ad9361_1_reg_inst, 0x0002, 0xE4); // Setup Tx Digital Filters/ Channels
Status = ad9361_reg_write(ad9361_1_reg_inst, 0x0002, 0xE4); // Setup Tx Digital Filters/ Channels
// switch ad9361 tx channel
Status = ad9361_reg_write(ad9361_1_reg_inst, 0x0004, 0x70); // Select Rx input pin(A,B,C)/ Tx out pin (A,B)
mdelay(20);
Status = ad9361_reg_write(ad9361_1_reg_inst, 0x0004, 0x70); // Select Rx input pin(A,B,C)/ Tx out pin (A,B)
mdelay(20);
// begin to calibrate the 1st ad9361
XGpioPs_WritePin(&Gpio, AD9361_1_CAL_START, 0);
XGpioPs_WritePin(&Gpio, AD9361_1_CAL_START, 1);
XGpioPs_WritePin(&Gpio, AD9361_1_CAL_START, 0);
XGpioPs_WritePin(&Gpio, AD9361_1_CAL_START, 1);
mdelay(1);
XGpioPs_WritePin(&Gpio, AD9361_1_CAL_START, 0);
// read calibration status
int32_t time_out_cnt = 0;
do{
mdelay(1);// Wait 1ms
do
{
mdelay(1); // Wait 1ms
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;
}
@@ -603,17 +628,19 @@ int Hardware_Init(void) {
// read calibration status
time_out_cnt = 0;
do{
mdelay(1);// Wait 1ms
do
{
mdelay(1); // Wait 1ms
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;
}
// enable single ad9361 tx channels
Status = ad9361_reg_write(ad9361_1_reg_inst, 0x0002, 0xA4); // Setup Tx Digital Filters/ Channels
Status = ad9361_reg_write(ad9361_1_reg_inst, 0x0002, 0xA4); // Setup Tx Digital Filters/ Channels
// switch ad9361 tx & rx channel
Status = ad9361_reg_write(ad9361_1_reg_inst, 0x0004, 0x03);
@@ -638,35 +665,42 @@ int Hardware_Init(void) {
* @return
*
******************************************************************************/
int main(void) {
int main(void)
{
int Status;
DBG_PRINT("Application Version: %2d.%2d.%4d.\n", _APP_VER_MAIN, _APP_VER_MINOR, _APP_VER_PATCH);
/* System Monitor Initialize */
Status = SysMonitor_Init();
if (Status == SUCCESS) {
if (Status == SUCCESS)
{
DBG_PRINT("System monitor Init SUCCESS! \n");
}
else {
else
{
DBG_PRINT("System monitor Init FAILED! \n");
return Status;
}
Status = Hardware_Init();
if (Status == SUCCESS) {
if (Status == SUCCESS)
{
DBG_PRINT("Hardware Init SUCCESS! \n");
}
else {
else
{
DBG_PRINT("Hardware Init FAILED! \n");
return Status;
}
/* Physical Layer Initialize */
Status = Phy_InstInit(&phyInstPtr, &phyConfig);
if (Status == SUCCESS) {
if (Status == SUCCESS)
{
DBG_PRINT("Physical layer Init SUCCESS! \n");
}
else {
else
{
DBG_PRINT("Physical layer FAILED! \n");
return Status;
}
@@ -674,34 +708,40 @@ int main(void) {
/* Link Layer Initialize */
linkConfig.phyInstRef = phyInstPtr;
Status = Linklayer_Init(&linkInstPtr, &linkConfig);
if (Status == SUCCESS) {
if (Status == SUCCESS)
{
DBG_PRINT("LinkLayer Init SUCCESS! \n");
}
else {
else
{
DBG_PRINT("Linklayer Init FAILED! \n");
return Status;
}
/* System Bus Initialize */
Status = SystemBus_Init(&sysbusDescPtr, &sysbusConfig);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("Systembus init FAILED, with error code: %d\n", Status);
return Status;
}
/* System Monitor log timer Initialize */
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);
return Status;
}
/* interrupts Initialize */
Status = SetupInterruptSystem(intcDescPtr);
if (Status == SUCCESS) {
if (Status == SUCCESS)
{
DBG_INFO("Interrupt system setup SUCCESS!\n");
}
else {
else
{
DBG_ERROR("Interrupt system setup FAILED, ErrorCode: %d.\n", Status);
return Status;
}
@@ -712,19 +752,24 @@ int main(void) {
Phy_Start(phyInstPtr);
DBG_PRINT("Physical started...\n");
while (true) {
while (true)
{
// System bus handler
if (SystemBus_IsReqPendding(sysbusDescPtr)) {
if (SystemBus_IsReqPendding(sysbusDescPtr))
{
Status = SystemBus_HandleCmd(sysbusDescPtr);
if(Status != SUCCESS) {
if (Status != SUCCESS)
{
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)
udelay(200);
Status = SystemBus_MakeResp(sysbusDescPtr);
if(Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("SystemBus_HandleCmd exit with ErrorCode: %d.\n", Status);
}
}
df_datalink/src/networking/link_layer/framing.c
+28 -19
View File
@@ -12,7 +12,7 @@
* Copyright(c) 2022 Beijing Institute of Technology.
* Lab of Communication and Networking
*
******************************************************************************/
******************************************************************************/
/***************************** Include Files *********************************/
#include <stdint.h>
@@ -24,7 +24,6 @@
/************************** Constant Definitions *****************************/
/*****************************************************************************/
/**
* @brief Generate crc of specified data.
@@ -38,18 +37,20 @@
* 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);
int32_t Status;
switch (crcType){
switch (crcType)
{
case L2_CRC16_1021:
Status = crc16_gen(dataBuff, dataLen, 0x1021, (uint16_t*)crc);
Status = crc16_gen(dataBuff, dataLen, 0x1021, (uint16_t *)crc);
break;
case L2_CRC16_8005:
Status = crc16_gen(dataBuff, dataLen, 0x8005, (uint16_t*)crc);
Status = crc16_gen(dataBuff, dataLen, 0x8005, (uint16_t *)crc);
break;
case L2_CRC8_31:
Status = crc8_gen(dataBuff, dataLen, 0x31, (uint8_t*)crc);
Status = crc8_gen(dataBuff, dataLen, 0x31, (uint8_t *)crc);
break;
default:
Status = EPARAM;
@@ -71,23 +72,26 @@ static int32_t L2_CrcGen(uint8_t* dataBuff, uint32_t dataLen, L2_CRC_TYPE crcTyp
* 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);
int32_t Status;
uint32_t crcVal;
uint8_t* crcPtr;
uint8_t *crcPtr;
crcPtr = dataBuff + crcPos;
Status = L2_CrcGen(dataBuff,dataLen, crcType, &crcVal);
if(Status != SUCCESS){
Status = L2_CrcGen(dataBuff, dataLen, crcType, &crcVal);
if (Status != SUCCESS)
{
return Status;
}
switch (crcType){
switch (crcType)
{
case L2_CRC16_1021:
*(crcPtr+1) = (uint8_t)(crcVal>>8);
*(crcPtr + 1) = (uint8_t)(crcVal >> 8);
*crcPtr = (uint8_t)(crcVal);
break;
case L2_CRC16_8005:
*(crcPtr+1) = (uint8_t)(crcVal>>8);
*(crcPtr + 1) = (uint8_t)(crcVal >> 8);
*crcPtr = (uint8_t)(crcVal);
break;
case L2_CRC8_31:
@@ -112,21 +116,26 @@ int32_t L2_CrcAppend(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CR
* 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);
int32_t Status;
uint32_t crcVal;
crcVal = 0;
Status = L2_CrcGen(dataBuff, dataLen, crcType, &crcVal);
if(Status == SUCCESS){
if(crcVal == 0){
if (Status == SUCCESS)
{
if (crcVal == 0)
{
return true;
}
else{
else
{
return false;
}
}
else {
else
{
return false;
}
}
df_datalink/src/networking/link_layer/link_layer.c
+279 -166
View File
File diff suppressed because it is too large Load Diff
df_datalink/src/networking/link_layer/link_layer.h
+41 -38
View File
@@ -26,50 +26,53 @@
#include "../config/app_config.h"
#include "node_list.h"
/************************** Constant Definitions *****************************/
#define L2_HEAD_NODE_ID 1
#define L2_MEMBER_NODE_ID 2
#define L2_HEAD_NODE_ID 1
#define L2_MEMBER_NODE_ID 2
/***************** Macros (Inline Functions) Definitions *********************/
//#define AssertL2Event(inst, envent) Linklayer_EventHandler(inst, envent)
// #define AssertL2Event(inst, event) Linklayer_EventHandler(inst, event)
/**************************** Type Definitions *******************************/
typedef struct{
L2_RTNE_TABLE_ENTRY** rtneTable;
typedef struct
{
L2_RTNE_TABLE_ENTRY **rtneTable;
uint32_t rtnePrd;
uint32_t rtneStep;
L2_RTNE_TABLE_ENTRY* last;
L2_RTNE_TABLE_ENTRY* curr;
L2_RTNE_TABLE_ENTRY* next;
}L2_RTNE_CONFIG;
L2_RTNE_TABLE_ENTRY *last;
L2_RTNE_TABLE_ENTRY *curr;
L2_RTNE_TABLE_ENTRY *next;
} L2_RTNE_CONFIG;
typedef struct {
typedef struct
{
L2_NODE_TYPE myType;
// bool isInterHead;
// uint8_t clustId;
// bool isInterHead;
// uint8_t clustId;
uint8_t myId;
uint8_t headId;
// uint8_t bkpHeadId;
// uint8_t bkpHeadId;
uint8_t netScale;
PHY_INST* phyInstRef; // pyhsical layer reference
PHY_INST *phyInstRef; // pyhsical layer reference
} L2_CONFIG;
typedef struct {
typedef struct
{
L2_CONFIG config;
bool dataAvail; // if there is a valid received frame data in rx buffer
bool cxn2Net; // if the node is connected to the network
L2_RCD frameNo; // stores the last current and next frame number.
L2_RCD slotNo; // stores the last current and next slot number.
L2_RCD frame; // stores the last current and next link layer frame type.
L2_RCD torState; // stores the last current and next link layer send/receive state.
L2_NODE_STATE mainState; // the main state of link layer state
L2_NODE_STATE nextState; // next state of link layer state, which will be applied to mainState every second.
bool dataAvail; // if there is a valid received frame data in rx buffer
bool cxn2Net; // if the node is connected to the network
L2_RCD frameNo; // stores the last current and next frame number.
L2_RCD slotNo; // stores the last current and next slot number.
L2_RCD frame; // stores the last current and next link layer frame type.
L2_RCD torState; // stores the last current and next link layer send/receive state.
L2_NODE_STATE mainState; // the main state of link layer state
L2_NODE_STATE nextState; // next state of link layer state, which will be applied to mainState every second.
L2_RTNE_CONFIG rtneConfig; // current routine table configuration and status.
L2_SCHD_EVENT* schdEventPool[L2_SCHD_EVENT_MAX];
L2_SCHD_EVENT *schdEventPool[L2_SCHD_EVENT_MAX];
uint32_t schdEventCnt;
SCAN_FSM scanFsm;
SVC_FSM serviceFsm;
NODE_LIST *nodeInfoList;
uint8_t* sendPktData;
uint8_t* recvPktData;
uint8_t *sendPktData;
uint8_t *recvPktData;
// for system log
SYSINFO sysInfo;
SLOTLOG txSlotLog;
@@ -83,32 +86,32 @@ typedef struct {
} L2_INST;
/************************** Variable Definitions *****************************/
extern L2_RTNE_TABLE_ENTRY* normalTable[L2_RTNE_PERIOD];
extern L2_RTNE_TABLE_ENTRY *normalTable[L2_RTNE_PERIOD];
/************************** Function Prototypes ******************************/
// Main
int32_t Linklayer_Init(L2_INST **instPtr, L2_CONFIG *config);
int32_t Linklayer_Proc(L2_INST *instPtr);
int32_t Linklayer_EventHandler(L2_INST *instPtr, L2_EVENT* l2Event);
int32_t Linklayer_EventHandler(L2_INST *instPtr, L2_EVENT *l2Event);
int32_t Linklayer_PktHandler(L2_INST *instPtr);
int32_t L2_SteerBeam2Node(L2_INST *instPtr, uint8_t nodeId);
int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE frameType);
// Event
L2_SCHD_EVENT* L2_CreatNewSchdEvent(L2_INST *instPtr, L2_EVENT_TYPE event);
bool L2_isEventScheduled(L2_SCHD_EVENT* schdEvent);
int32_t L2_CancelEvent(L2_SCHD_EVENT* schdEvent);
int32_t L2_SchdEventAppendMsg(L2_SCHD_EVENT* schdEvent, void* msg);
int32_t L2_ScheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo);
int32_t L2_ScheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots);
int32_t L2_RecheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo);
int32_t L2_RecheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots);
L2_SCHD_EVENT *L2_CreatNewSchdEvent(L2_INST *instPtr, L2_EVENT_TYPE event);
bool L2_isEventScheduled(L2_SCHD_EVENT *schdEvent);
int32_t L2_CancelEvent(L2_SCHD_EVENT *schdEvent);
int32_t L2_SchdEventAppendMsg(L2_SCHD_EVENT *schdEvent, void *msg);
int32_t L2_ScheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t frameNo, uint32_t slotNo);
int32_t L2_ScheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t numSlots);
int32_t L2_RecheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t frameNo, uint32_t slotNo);
int32_t L2_RecheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t numSlots);
// Framing
void L2_FrameStateUpdate(L2_INST *instPtr);
L2_FRAME_TYPE L2_GetLastRcvFrameType(L2_INST *instPtr);
L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame);
int32_t L2_CrcAppend(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType);
bool L2_CrcCheck(uint8_t* dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType);
int32_t L2_CrcAppend(uint8_t *dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType);
bool L2_CrcCheck(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType);
// Miscellaneous
L2_NODE_STATE L2_GetMainState(L2_INST *instPtr);
df_datalink/src/networking/link_layer/link_layer_types.h
+38 -25
View File
@@ -11,7 +11,7 @@
* Copyright(c) 2022 Beijing Institute of Technology.
* Lab of Communication and Networking
*
******************************************************************************/
******************************************************************************/
#ifndef __LINK_LAYER_DEF_H_
#define __LINK_LAYER_DEF_H_
@@ -36,16 +36,16 @@
#define L2_FRAMENO_NONE 0
// Common number of slots definition
#define L2_FRAME_NO_MAX 256
#define L2_SLOTS_PER_FRAME 200
#define L2_SLOTS_PER_SUBFRAME 20
#define L2_FRAME_NO_MAX 256
#define L2_SLOTS_PER_FRAME 200
#define L2_SLOTS_PER_SUBFRAME 20
#define NUM_SLOTS_NONE UINT32_MAX
#define NUM_SLOTS_100MS 20
#define NUM_SLOTS_20MS 4
#define NUM_SLOTS_10MS 2
#define NUM_SLOTS_5MS 1
#define NUM_SLOTS_SKIP 0
#define NUM_SLOTS_100MS 20
#define NUM_SLOTS_20MS 4
#define NUM_SLOTS_10MS 2
#define NUM_SLOTS_5MS 1
#define NUM_SLOTS_SKIP 0
// Common schedule event definition
#define L2_SCHD_EVENT_MAX 16
@@ -54,13 +54,15 @@
#define AssertL2Event(inst, envent) Linklayer_EventHandler(inst, envent)
/**************************** Type Definitions *******************************/
typedef struct {
typedef struct
{
int32_t last;
int32_t curr;
int32_t next;
} L2_RCD;
typedef enum {
typedef enum
{
L2_TOR_SUSPEND = 0,
L2_TOR_TX = 1,
L2_TOR_RX = 2
@@ -68,7 +70,8 @@ typedef enum {
/** @name Prior information type definition
*/
typedef enum {
typedef enum
{
L2_NONE_TYPE = 0,
L2_HEAD_NODE = 1,
L2_MEMBER_NODE = 2,
@@ -77,17 +80,18 @@ typedef enum {
/** @name Node state type definition
*/
typedef enum {
typedef enum
{
NONE_STATE = 0,
INIT_STATE = 1,
NORMAL_STATE = 2
} L2_NODE_STATE;
/** @name Node information type definition
* The following types define the detail node info which needed by the link layer.
*/
typedef struct {
typedef struct
{
uint8_t nodeId;
uint8_t nodeType;
uint8_t beamNo;
@@ -99,7 +103,8 @@ typedef struct {
/** @name L2 event enum
* A enum of L2 event types.
*/
typedef enum {
typedef enum
{
L2_EV_NONE = 0,
L2_EV_SCAN_PROC_DONE = 1,
L2_EV_NET_SETUP_DONE = 2,
@@ -116,15 +121,17 @@ typedef enum {
/** @name L2 event
* A definition of L2 event instance.
*/
typedef struct {
typedef struct
{
L2_EVENT_TYPE event;
void* msg;
void *msg;
} L2_EVENT;
/** @name L2 event
* A definition of L2 event instance.
*/
typedef struct {
typedef struct
{
uint8_t eventId;
bool isSchd;
L2_EVENT l2Event;
@@ -135,7 +142,8 @@ typedef struct {
/** @name Slot types
* A definition of different slots.
*/
typedef enum {
typedef enum
{
L2_SLOT_NONE = 0,
L2_SLOT_IDLE = 1,
L2_SLOT_SCAN = 2,
@@ -146,7 +154,8 @@ typedef enum {
/** @name Frame types
* A definition of different frame types.
*/
typedef enum {
typedef enum
{
L2_NONE_FRAME = 0,
L2_SCAN_FRAME = 1,
L2_SVC_FRAME = 2,
@@ -156,7 +165,8 @@ typedef enum {
/** @name Frame types
* A definition of different frame types.
*/
typedef enum {
typedef enum
{
L2_CRC_NONE = 0,
L2_CRC16_1021 = 1,
L2_CRC16_8005 = 2,
@@ -166,7 +176,8 @@ typedef enum {
/** @name Schedule subtable entry
* A definition of Schedule subtable entry.
*/
typedef struct {
typedef struct
{
L2_SLOT_TYPE slotType : 8;
uint8_t slotSeq;
uint8_t src;
@@ -175,14 +186,16 @@ typedef struct {
/** @name Scan procedure FSM
*/
typedef struct {
typedef struct
{
uint8_t sacanCnt;
uint8_t beamNo;
} SCAN_FSM;
/** @name Service procedure FSM
*/
typedef struct {
typedef struct
{
uint32_t txSeqNum;
uint32_t rxSeqNum;
uint8_t rxLog[256];
df_datalink/src/networking/link_layer/msg_def.h
+18 -18
View File
@@ -26,10 +26,10 @@
// 1 slot number
// 26 payload
// 2 crc
#define L2_MSG_FRAME_NO 0
#define L2_MSG_SLOT_NO 1
#define L2_MSG_PAYLOAD 2
#define L2_MSG_CRC 28
#define L2_MSG_FRAME_NO 0
#define L2_MSG_SLOT_NO 1
#define L2_MSG_PAYLOAD 2
#define L2_MSG_CRC 28
// service frame structure:
// [bytes] [name]
@@ -38,13 +38,13 @@
// 3 time sync
// 23 payload
// 2 crc
#define L2_MSG_SVC_TMSYNC 2
#define L2_MSG_SVC_TMSYNC_LEN 3
#define L2_MSG_SVC_TMSYNC_MAX (8388607) //0x007FFFFF
#define L2_MSG_SVC_TMSYNC_MIN (-8388608) //0x00800000
#define L2_MSG_SVC_SEQ_ID 5
#define L2_MSG_SVC_PAYLOAD 6
#define L2_MSG_SVC_PAYLOAD_LEN 22
#define L2_MSG_SVC_TMSYNC 2
#define L2_MSG_SVC_TMSYNC_LEN 3
#define L2_MSG_SVC_TMSYNC_MAX (8388607) // 0x007FFFFF
#define L2_MSG_SVC_TMSYNC_MIN (-8388608) // 0x00800000
#define L2_MSG_SVC_SEQ_ID 5
#define L2_MSG_SVC_PAYLOAD 6
#define L2_MSG_SVC_PAYLOAD_LEN 22
// response frame structure:
// [bytes] [name]
@@ -58,11 +58,11 @@
// 4 speed z
// 2 dummy
// 2 crc
#define L2_MSG_INFO_POSX 2
#define L2_MSG_INFO_POSY 6
#define L2_MSG_INFO_POSZ 10
#define L2_MSG_INFO_SPEEDX 14
#define L2_MSG_INFO_SPEEDY 18
#define L2_MSG_INFO_SPEEDZ 22
#define L2_MSG_INFO_DUMMY 26
#define L2_MSG_INFO_POSX 2
#define L2_MSG_INFO_POSY 6
#define L2_MSG_INFO_POSZ 10
#define L2_MSG_INFO_SPEEDX 14
#define L2_MSG_INFO_SPEEDY 18
#define L2_MSG_INFO_SPEEDZ 22
#define L2_MSG_INFO_DUMMY 26
#endif //__L2_MSG_DEF_H_
df_datalink/src/networking/link_layer/node_list.c
+119 -69
View File
@@ -23,7 +23,8 @@
/************************** Constant Definitions *****************************/
/***************** Macros (Inline Functions) Definitions *********************/
inline int32_t GetNodeNum(NODE_LIST *list) {
inline int32_t GetNodeNum(NODE_LIST *list)
{
ASSERT(list != NULL);
return list->nodeNum;
}
@@ -43,14 +44,17 @@ inline int32_t GetNodeNum(NODE_LIST *list) {
* ENOMEM - not enough memory.
*
******************************************************************************/
int32_t NodeList_Init(NODE_LIST **list) {
int32_t NodeList_Init(NODE_LIST **list)
{
NODE_ENTRY *entry;
(*list) = malloc(sizeof(NODE_LIST));
if (*list == NULL) {
if (*list == NULL)
{
return ENOMEM;
}
entry = malloc(sizeof(NODE_ENTRY));
if(entry == NULL) {
if (entry == NULL)
{
free(*list);
return ENOMEM;
}
@@ -62,11 +66,13 @@ int32_t NodeList_Init(NODE_LIST **list) {
return SUCCESS;
}
void NodeList_Clear(NODE_LIST *list){
void NodeList_Clear(NODE_LIST *list)
{
ASSERT(list != NULL);
NODE_ENTRY *nodeEntry, *deletEntry;
nodeEntry = list->head->nextNode;
while (nodeEntry != list->head) {
while (nodeEntry != list->head)
{
deletEntry = nodeEntry;
nodeEntry = nodeEntry->nextNode;
deletEntry->preNode->nextNode = deletEntry->nextNode;
@@ -77,8 +83,10 @@ void NodeList_Clear(NODE_LIST *list){
}
}
void NodeList_Destroy(NODE_LIST *list){
if(list == NULL){
void NodeList_Destroy(NODE_LIST *list)
{
if (list == NULL)
{
return;
}
NodeList_Clear(list);
@@ -97,21 +105,27 @@ void NodeList_Destroy(NODE_LIST *list){
* 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);
NODE_ENTRY* nodeEntry = list->head->nextNode;
while (nodeEntry != list->head) {
if (nodeEntry->nodeInfo.nodeId == nodeId) {
NODE_ENTRY *nodeEntry = list->head->nextNode;
while (nodeEntry != list->head)
{
if (nodeEntry->nodeInfo.nodeId == nodeId)
{
break;
}
else {
else
{
nodeEntry = nodeEntry->nextNode;
}
}
if (nodeEntry == list->head) {
if (nodeEntry == list->head)
{
return NULL;
}
else {
else
{
return nodeEntry;
}
}
@@ -127,7 +141,8 @@ NODE_ENTRY* GetNode(NODE_LIST *list, uint32_t nodeId) {
* 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);
return (GetNode(list, nodeId) != NULL);
}
@@ -147,17 +162,19 @@ inline bool IsNodeInList(NODE_LIST *list, uint8_t nodeId) {
int32_t AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType)
{
ASSERT(list != NULL);
if(IsNodeInList(list, nodeId)){
if (IsNodeInList(list, nodeId))
{
DBG_WARNING("Node [%d] already exist.\n", nodeId);
return FAILURE;
}
DBG_INFO("Link layer appending node [%d] to node list.\n", nodeId);
NODE_ENTRY* nodeEntry = (NODE_ENTRY*) malloc(sizeof(NODE_ENTRY));
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = (NODE_ENTRY *)malloc(sizeof(NODE_ENTRY));
if (nodeEntry == NULL)
{
return ENOMEM;
}
nodeEntry->nodeInfo.nodeId = nodeId;
nodeEntry->nodeInfo.nodeType = (uint8_t) nodeType;
nodeEntry->nodeInfo.nodeType = (uint8_t)nodeType;
nodeEntry->nodeInfo.pos.x = 0;
nodeEntry->nodeInfo.pos.y = 0;
nodeEntry->nodeInfo.pos.z = 0;
@@ -186,10 +203,12 @@ int32_t AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType)
* 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);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if(nodeEntry == NULL){
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
DBG_WARNING("Node [%d] in not in node list, skip deleting process.\n", nodeId);
return SUCCESS;
}
@@ -201,7 +220,6 @@ int32_t DeletNode(NODE_LIST *list, uint8_t nodeId) {
return SUCCESS;
}
/*****************************************************************************/
/**
* @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);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
*dst = Coord_Distance(myCoord, nodeEntry->nodeInfo.pos);
return SUCCESS;
}
@@ -239,19 +260,21 @@ int32_t GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double* dst)
* 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);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
memcpy(&(nodeEntry->nodeInfo.pos), newPos, sizeof(COORD));
}
return SUCCESS;
}
/*****************************************************************************/
/**
* @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.
*
******************************************************************************/
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);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
memcpy(pos, &(nodeEntry->nodeInfo.pos), sizeof(COORD));
}
return SUCCESS;
@@ -288,19 +314,21 @@ int32_t GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *pos) {
* 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);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
memcpy(&(nodeEntry->nodeInfo.speed), newSpeed, sizeof(COORD));
}
return SUCCESS;
}
/*****************************************************************************/
/**
* @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.
*
******************************************************************************/
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);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
memcpy(speed, &(nodeEntry->nodeInfo.speed), sizeof(COORD));
}
return SUCCESS;
@@ -336,13 +367,16 @@ int32_t GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed) {
* 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);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
nodeEntry->timeDevi = devi;
}
return SUCCESS;
@@ -359,13 +393,16 @@ int32_t UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi) {
* @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);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
nodeEntry->ttlCnt += inc;
}
return SUCCESS;
@@ -382,13 +419,16 @@ int32_t IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc) {
* 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);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
nodeEntry->ttlCnt = 0;
}
return SUCCESS;
@@ -405,13 +445,16 @@ int32_t RstNodeTTL(NODE_LIST *list, uint8_t nodeId) {
* @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);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
*ttl = nodeEntry->ttlCnt;
}
return SUCCESS;
@@ -430,13 +473,16 @@ int32_t GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl) {
* 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);
int32_t Status = SUCCESS;
NODE_ENTRY *nodeEntry = list->head->nextNode;
while (nodeEntry != list->head) {
while (nodeEntry != list->head)
{
Status = operation(nodeEntry, param);
if(Status != SUCCESS){
if (Status != SUCCESS)
{
break;
}
nodeEntry = nodeEntry->nextNode;
@@ -455,12 +501,15 @@ int32_t IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)(NO
* @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);
NODE_ENTRY *nodeEntry, *deletEntry;
nodeEntry = list->head->nextNode;
while (nodeEntry != list->head) {
if (condition(nodeEntry) == false) {
while (nodeEntry != list->head)
{
if (condition(nodeEntry) == false)
{
deletEntry = nodeEntry;
nodeEntry = nodeEntry->nextNode;
deletEntry->preNode->nextNode = deletEntry->nextNode;
@@ -469,7 +518,8 @@ int32_t IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *)) {
free(deletEntry);
list->nodeNum--;
}
else {
else
{
nodeEntry = nodeEntry->nextNode;
}
}
df_datalink/src/networking/link_layer/node_list.h
+6 -4
View File
@@ -26,7 +26,8 @@
/************************** Constant Definitions *****************************/
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
typedef struct NODE_LIST_ENTRY {
typedef struct NODE_LIST_ENTRY
{
L2_NODE_INFO nodeInfo;
uint32_t ttlCnt;
int32_t timeDevi;
@@ -34,7 +35,8 @@ typedef struct NODE_LIST_ENTRY {
struct NODE_LIST_ENTRY *preNode;
} NODE_ENTRY;
typedef struct {
typedef struct
{
uint32_t nodeNum;
NODE_ENTRY *head;
} NODE_LIST;
@@ -53,12 +55,12 @@ int32_t GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *newPos);
int32_t UpdateNodeSpeed(NODE_LIST *list, uint8_t nodeId, const COORD *newSpeed);
int32_t GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed);
int32_t UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi);
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);
int32_t GetNodeNum(NODE_LIST *list);
int32_t RstNodeTTL(NODE_LIST *list, uint8_t nodeId);
int32_t IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc);
int32_t GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl);
bool IsNodeInList(NODE_LIST *list, uint8_t nodeId);
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 *));
int32_t IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *));
#endif /* __L2_LOCAL_RECORD_H_ */
df_datalink/src/networking/link_layer/routine_table.c
+843 -451
View File
File diff suppressed because it is too large Load Diff
df_datalink/src/networking/link_layer/scan.c
+105 -69
View File
@@ -45,45 +45,52 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type);
* 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);
int32_t Status;
uint8_t *buff;
uint32_t packLen;
switch (type) {
case L2_INFO_FRAME:{
packLen = RSP_PACK_LEN;
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
buff = malloc(packLen);
if (buff == NULL) {
DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM;
}
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
memcpy(buff+L2_MSG_INFO_POSX, &(instPtr->sysInfo.posX), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSY, &(instPtr->sysInfo.posY), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSZ, &(instPtr->sysInfo.posZ), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDX, &(instPtr->sysInfo.vX), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDY, &(instPtr->sysInfo.vY), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDZ, &(instPtr->sysInfo.vZ), sizeof(float));
switch (type)
{
case L2_INFO_FRAME:
{
packLen = RSP_PACK_LEN;
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
buff = malloc(packLen);
if (buff == NULL)
{
DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM;
}
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
memcpy(buff + L2_MSG_INFO_POSX, &(instPtr->sysInfo.posX), sizeof(float));
memcpy(buff + L2_MSG_INFO_POSY, &(instPtr->sysInfo.posY), sizeof(float));
memcpy(buff + L2_MSG_INFO_POSZ, &(instPtr->sysInfo.posZ), sizeof(float));
memcpy(buff + L2_MSG_INFO_SPEEDX, &(instPtr->sysInfo.vX), sizeof(float));
memcpy(buff + L2_MSG_INFO_SPEEDY, &(instPtr->sysInfo.vY), 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);
if (Status != SUCCESS) {
DBG_ERROR("Fail to append CRC, error code: %ld.\n", Status);
return Status;
}
if(CircQueuePush(instPtr->txQueue, buff) == false){
DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE;
}
break;
Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
if (Status != SUCCESS)
{
DBG_ERROR("Fail to append CRC, error code: %ld.\n", Status);
return Status;
}
default :{
DBG_ERROR("Unexpected frame type: %d.\n", type);
return EPARAM;
break;
if (CircQueuePush(instPtr->txQueue, buff) == false)
{
DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE;
}
break;
}
default:
{
DBG_ERROR("Unexpected frame type: %d.\n", type);
return EPARAM;
break;
}
}
return SUCCESS;
}
@@ -97,7 +104,8 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
* @return
*
******************************************************************************/
int32_t L2_Scan_Init(L2_INST *instPtr) {
int32_t L2_Scan_Init(L2_INST *instPtr)
{
ASSERT(instPtr != NULL);
instPtr->scanFsm.sacanCnt = 0;
instPtr->scanFsm.beamNo = 0;
@@ -113,25 +121,32 @@ int32_t L2_Scan_Init(L2_INST *instPtr) {
* @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(msg != NULL);
int32_t Status;
switch (instPtr->frame.last) {
case L2_SCAN_FRAME: {
if (instPtr->config.myType == L2_HEAD_NODE) {
switch (instPtr->frame.last)
{
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");
Status = EUNEXP;
break;
}
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);
}
break;
}
case L2_INFO_FRAME: {
if (instPtr->config.myType != L2_HEAD_NODE) {
case L2_INFO_FRAME:
{
if (instPtr->config.myType != L2_HEAD_NODE)
{
DBG_ERROR("Unexpected call from member node, Only Head Node can handle L2_SVC_FRAME.\n");
Status = EUNEXP;
break;
@@ -139,47 +154,55 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) {
COORD coordTemp;
float tmpVal;
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);
break;
}
memcpy(&tmpVal, msg+L2_MSG_INFO_POSX, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_POSX, sizeof(float));
coordTemp.x = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_POSY, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_POSY, sizeof(float));
coordTemp.y = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_POSZ, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_POSZ, sizeof(float));
coordTemp.z = tmpVal;
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);
}
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDX, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDX, sizeof(float));
coordTemp.x = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDY, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDY, sizeof(float));
coordTemp.y = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDZ, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDZ, sizeof(float));
coordTemp.z = tmpVal;
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);
}
// Update Time deviation
int64_t devi, deviTrunc;
Status = L2_TimeSync_GetDeviatoion(instPtr, &devi);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("Failed to compute slot deviation.\n");
UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, 0);
}
else {
if(devi > INT32_MAX || devi < INT32_MIN) {
else
{
if (devi > INT32_MAX || devi < INT32_MIN)
{
DBG_ERROR("Illegal deviation value: %lld.\n", devi);
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);
deviTrunc = (int32_t)devi;
}
else {
else
{
deviTrunc = (int32_t)devi;
}
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.
if(L2_GetMainState(instPtr) == INIT_STATE && Status == SUCCESS){
if (L2_GetMainState(instPtr) == INIT_STATE && Status == SUCCESS)
{
L2_EVENT l2Event;
L2_RTNE_CONFIG newRtneConfig;
@@ -204,7 +228,7 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) {
// Assert Routine Table Update...
newRtneConfig.rtneTable = normalTable;
newRtneConfig.rtnePrd = ARRAY_SIZE(normalTable)*L2_SLOTS_PER_FRAME;
newRtneConfig.rtnePrd = ARRAY_SIZE(normalTable) * L2_SLOTS_PER_FRAME;
newRtneConfig.rtneStep = 0;
l2Event.event = L2_ROUTINE_UPDATE;
l2Event.msg = &newRtneConfig;
@@ -222,21 +246,25 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) {
* @return used slot (5ms slot).
*
******************************************************************************/
int32_t L2_Scan_Proc(L2_INST *instPtr) {
int32_t L2_Scan_Proc(L2_INST *instPtr)
{
int32_t Status;
/* instPtr->nextState == NORMAL_STATE indicates network set up procedure was finished.
* Skip 5ms for nodes for node configuration update.
*/
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");
if(instPtr->config.myType != L2_HEAD_NODE) {
if (instPtr->config.myType != L2_HEAD_NODE)
{
Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_120);
Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_SCAN_FRAME);
}
else {
else
{
DBG_ERROR("Illegal call from Head Node.\n");
}
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
* it reaches the boundary.
*/
if (instPtr->config.myType == L2_HEAD_NODE) {
if (instPtr->config.myType == L2_HEAD_NODE)
{
// Apply a scan procedure
if (instPtr->scanFsm.sacanCnt % 2 == 0){
if (instPtr->scanFsm.sacanCnt % 2 == 0)
{
// Antenna configure
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetBeamNo(instPtr->config.phyInstRef, instPtr->scanFsm.beamNo);
Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(instPtr, L2_TOR_TX, L2_SCAN_FRAME);
}
else {
else
{
// Antenna configure
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8);
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
* to the listening configuration.
*/
else {
else
{
uint8_t *sendData;
// Pack node information.
Status = ScanMsg_Packing(instPtr, L2_INFO_FRAME);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
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);
free(sendData);
}
else {
else
{
DBG_INFO("Failed to pop tx Queue.\n");
}
// Antenna configure
@@ -297,7 +332,7 @@ int32_t L2_Scan_Proc(L2_INST *instPtr) {
}
// Increase scan counter (for debug purpose).
instPtr->scanFsm.sacanCnt ++;
instPtr->scanFsm.sacanCnt++;
return NUM_SLOTS_SKIP;
}
@@ -311,7 +346,8 @@ int32_t L2_Scan_Proc(L2_INST *instPtr) {
* @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");
ASSERT(false); // Not support.
return 0;
df_datalink/src/networking/link_layer/service.c
+220 -160
View File
@@ -25,10 +25,10 @@
#include "../config/app_config.h"
/************************** Constant Definitions *****************************/
#define NUM_SERVICE_SLOTS 20
#define NUM_RESPONSE_SLOTS 20
#define NUM_NONE_SERVICE_SLOTS 0
#define NUM_SERVICE_PRD_SLOTS 60
#define NUM_SERVICE_SLOTS 20
#define NUM_RESPONSE_SLOTS 20
#define NUM_NONE_SERVICE_SLOTS 0
#define NUM_SERVICE_PRD_SLOTS 60
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
@@ -42,8 +42,7 @@ static uint8_t berTestData[SVC_PACK_LEN] = {
0x27, 0x4E, 0xFB, 0x39, 0x11, 0xE7, 0x12, 0xDA,
0xDF, 0xD2, 0x27, 0x8F, 0xC9, 0x42, 0xCD, 0x0C,
0x76, 0x1C, 0x81, 0x87, 0x69, 0xC9, 0x60, 0xFD,
0xC4, 0x27, 0xA5, 0x09, 0xE5, 0x71
};
0xC4, 0x27, 0xA5, 0x09, 0xE5, 0x71};
#endif
/*****************************************************************************/
@@ -58,116 +57,134 @@ static uint8_t berTestData[SVC_PACK_LEN] = {
* 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
ASSERT(instPtr != NULL);
int32_t Status;
uint8_t *buff;
uint32_t packLen;
switch (type) {
case L2_SVC_FRAME:{
const NODE_ENTRY *nodeEntry;
int32_t timeDevi;
packLen = SVC_PACK_LEN;
switch (type)
{
case L2_SVC_FRAME:
{
const NODE_ENTRY *nodeEntry;
int32_t timeDevi;
packLen = SVC_PACK_LEN;
buff = malloc(packLen);
if (buff == NULL) {
DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM;
}
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
// TODO: Check Pack TimeSync info, and better approch?.
nodeEntry = GetNode(instPtr->nodeInfoList, L2_MEMBER_NODE_ID);
if(nodeEntry != NULL) {
// check if deviation info outdated.
if(nodeEntry->ttlCnt <= NUM_SERVICE_PRD_SLOTS) {
// Member node should adjust in the opposite way.
timeDevi = -nodeEntry->timeDevi;
}
else {
DBG_WARNING("Time deviation is outdated, omitted it.\n");
}
}
else {
DBG_ERROR("Fail to get time deviation...\n");
timeDevi = 0;
}
if(timeDevi > L2_MSG_SVC_TMSYNC_MAX || timeDevi < L2_MSG_SVC_TMSYNC_MIN) {
do {
timeDevi /= 2;
} 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);
}
memcpy(buff+L2_MSG_SVC_TMSYNC, &timeDevi, L2_MSG_SVC_TMSYNC_LEN);
// Pack Seq ID
buff[L2_MSG_SVC_SEQ_ID] = instPtr->serviceFsm.txSeqNum;
// Pack payload
memcpy(buff+L2_MSG_SVC_PAYLOAD,
instPtr->serviceFsm.txLog + L2_MSG_SVC_PAYLOAD_LEN*instPtr->serviceFsm.txSeqNum,
L2_MSG_SVC_PAYLOAD_LEN);
if (instPtr->config.myType == L2_HEAD_NODE) {
// if last packet reset counting
instPtr->serviceFsm.txSeqNum = (instPtr->serviceFsm.txSeqNum
>= (PAYLOAD_FLJ-1)/L2_MSG_SVC_PAYLOAD+1)
? 0 : instPtr->serviceFsm.txSeqNum + 1;
}
else{
// if last packet reset counting
instPtr->serviceFsm.txSeqNum = (instPtr->serviceFsm.txSeqNum
>= (PAYLOAD_LJ-1)/L2_MSG_SVC_PAYLOAD+1)
? 0 : instPtr->serviceFsm.txSeqNum + 1;
}
Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
if (Status != SUCCESS) {
DBG_ERROR("Fail to append CRC...\n");
return Status;
}
if(CircQueuePush(instPtr->txQueue, buff) == false){
DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE;
}
break;
buff = malloc(packLen);
if (buff == NULL)
{
DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM;
}
case L2_INFO_FRAME:{
packLen = RSP_PACK_LEN;
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
buff = malloc(packLen);
if (buff == NULL) {
DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM;
}
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
memcpy(buff+L2_MSG_INFO_POSX, &(instPtr->sysInfo.posX), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSY, &(instPtr->sysInfo.posY), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSZ, &(instPtr->sysInfo.posZ), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDX, &(instPtr->sysInfo.vX), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDY, &(instPtr->sysInfo.vY), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDZ, &(instPtr->sysInfo.vZ), sizeof(float));
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
if (Status != SUCCESS) {
DBG_ERROR("Fail to append CRC...\n");
return Status;
// TODO: Check Pack TimeSync info, and better approch?.
nodeEntry = GetNode(instPtr->nodeInfoList, L2_MEMBER_NODE_ID);
if (nodeEntry != NULL)
{
// check if deviation info outdated.
if (nodeEntry->ttlCnt <= NUM_SERVICE_PRD_SLOTS)
{
// Member node should adjust in the opposite way.
timeDevi = -nodeEntry->timeDevi;
}
if(CircQueuePush(instPtr->txQueue, buff) == false){
DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE;
else
{
DBG_WARNING("Time deviation is outdated, omitted it.\n");
}
break;
}
default :{
DBG_ERROR("Unexpected frame type: %d.\n", type);
return EPARAM;
break;
else
{
DBG_ERROR("Fail to get time deviation...\n");
timeDevi = 0;
}
if (timeDevi > L2_MSG_SVC_TMSYNC_MAX || timeDevi < L2_MSG_SVC_TMSYNC_MIN)
{
do
{
timeDevi /= 2;
} 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);
}
memcpy(buff + L2_MSG_SVC_TMSYNC, &timeDevi, L2_MSG_SVC_TMSYNC_LEN);
// Pack Seq ID
buff[L2_MSG_SVC_SEQ_ID] = instPtr->serviceFsm.txSeqNum;
// Pack payload
memcpy(buff + L2_MSG_SVC_PAYLOAD,
instPtr->serviceFsm.txLog + L2_MSG_SVC_PAYLOAD_LEN * instPtr->serviceFsm.txSeqNum,
L2_MSG_SVC_PAYLOAD_LEN);
if (instPtr->config.myType == L2_HEAD_NODE)
{
// if last packet reset counting
instPtr->serviceFsm.txSeqNum = (instPtr->serviceFsm.txSeqNum >= (PAYLOAD_FLJ - 1) / L2_MSG_SVC_PAYLOAD + 1)
? 0
: instPtr->serviceFsm.txSeqNum + 1;
}
else
{
// if last packet reset counting
instPtr->serviceFsm.txSeqNum = (instPtr->serviceFsm.txSeqNum >= (PAYLOAD_LJ - 1) / L2_MSG_SVC_PAYLOAD + 1)
? 0
: instPtr->serviceFsm.txSeqNum + 1;
}
Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
if (Status != SUCCESS)
{
DBG_ERROR("Fail to append CRC...\n");
return Status;
}
if (CircQueuePush(instPtr->txQueue, buff) == false)
{
DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE;
}
break;
}
case L2_INFO_FRAME:
{
packLen = RSP_PACK_LEN;
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
buff = malloc(packLen);
if (buff == NULL)
{
DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM;
}
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
memcpy(buff + L2_MSG_INFO_POSX, &(instPtr->sysInfo.posX), sizeof(float));
memcpy(buff + L2_MSG_INFO_POSY, &(instPtr->sysInfo.posY), sizeof(float));
memcpy(buff + L2_MSG_INFO_POSZ, &(instPtr->sysInfo.posZ), sizeof(float));
memcpy(buff + L2_MSG_INFO_SPEEDX, &(instPtr->sysInfo.vX), sizeof(float));
memcpy(buff + L2_MSG_INFO_SPEEDY, &(instPtr->sysInfo.vY), 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);
if (Status != SUCCESS)
{
DBG_ERROR("Fail to append CRC...\n");
return Status;
}
if (CircQueuePush(instPtr->txQueue, buff) == false)
{
DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE;
}
break;
}
default:
{
DBG_ERROR("Unexpected frame type: %d.\n", type);
return EPARAM;
break;
}
}
return SUCCESS;
#else
@@ -176,7 +193,6 @@ static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
#endif
}
/*****************************************************************************/
/**
* @brief Initialize scan FSM.
@@ -186,7 +202,8 @@ static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
* @return
*
******************************************************************************/
int32_t L2_Service_Init(L2_INST *instPtr) {
int32_t L2_Service_Init(L2_INST *instPtr)
{
ASSERT(instPtr != NULL);
instPtr->serviceFsm.txSeqNum = 0;
instPtr->serviceFsm.rxSeqNum = 0;
@@ -211,13 +228,17 @@ int32_t L2_Service_Init(L2_INST *instPtr) {
* 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(msg != NULL);
int32_t Status;
switch (instPtr->frame.last) {
case L2_SVC_FRAME: {
if (instPtr->config.myType == L2_HEAD_NODE) {
switch (instPtr->frame.last)
{
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");
Status = EUNEXP;
break;
@@ -241,57 +262,72 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) {
timeAdj |= (timeAdj & 0x0000000000800000) ? 0xFFFFFFFFFF000000 : 0x0000000000000000;
// FIXME: better approach?
// 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);
}
#endif
#ifndef DATALINK_ENABLE_BERTEST
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) {
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 last packet reset counting
if(seqNum >= (PAYLOAD_FLJ-1)/L2_MSG_SVC_PAYLOAD+1) {
if(seqNum == instPtr->serviceFsm.rxSeqNum) {
if (seqNum >= (PAYLOAD_FLJ - 1) / L2_MSG_SVC_PAYLOAD + 1)
{
if (seqNum == instPtr->serviceFsm.rxSeqNum)
{
SysMonitor_UpdateRecvPayloadLJRaw(instPtr->serviceFsm.rxLog);
}
instPtr->serviceFsm.rxSeqNum = 0;
}
else {
if(instPtr->serviceFsm.rxSeqNum == seqNum){
else
{
if (instPtr->serviceFsm.rxSeqNum == seqNum)
{
instPtr->serviceFsm.rxSeqNum++;
}
}
}
else{
else
{
// if last packet reset counting
if(seqNum >= (PAYLOAD_LJ-1)/L2_MSG_SVC_PAYLOAD+1) {
if(seqNum == instPtr->serviceFsm.rxSeqNum) {
if (seqNum >= (PAYLOAD_LJ - 1) / L2_MSG_SVC_PAYLOAD + 1)
{
if (seqNum == instPtr->serviceFsm.rxSeqNum)
{
SysMonitor_UpdateRecvPayloadFLJRaw(instPtr->serviceFsm.rxLog);
}
instPtr->serviceFsm.rxSeqNum = 0;
}
else {
if(instPtr->serviceFsm.rxSeqNum == seqNum){
else
{
if (instPtr->serviceFsm.rxSeqNum == seqNum)
{
instPtr->serviceFsm.rxSeqNum++;
}
}
}
#else
for(int i = 0; i < SVC_PACK_LEN; i++) {
instPtr->serviceFsm.bitError += count_ones(msg[i]^berTestData[i]);
for (int i = 0; i < SVC_PACK_LEN; i++)
{
instPtr->serviceFsm.bitError += count_ones(msg[i] ^ berTestData[i]);
}
instPtr->serviceFsm.bitTotal += SVC_PACK_LEN*8;
instPtr->serviceFsm.bitTotal += SVC_PACK_LEN * 8;
#endif
instPtr->serviceFsm.pktRecved++;
// Reset Node TTL
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);
}
break;
}
case L2_INFO_FRAME: {
if (instPtr->config.myType != L2_HEAD_NODE) {
case L2_INFO_FRAME:
{
if (instPtr->config.myType != L2_HEAD_NODE)
{
DBG_ERROR("Unexpected call, Only Head Node can handle L2_SVC_FRAME.\n");
Status = EUNEXP;
break;
@@ -299,24 +335,26 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) {
#ifndef DATALINK_ENABLE_BERTEST
COORD coordTemp;
float tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_POSX, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_POSX, sizeof(float));
coordTemp.x = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_POSY, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_POSY, sizeof(float));
coordTemp.y = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_POSZ, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_POSZ, sizeof(float));
coordTemp.z = tmpVal;
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);
}
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDX, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDX, sizeof(float));
coordTemp.x = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDY, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDY, sizeof(float));
coordTemp.y = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDZ, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDZ, sizeof(float));
coordTemp.z = tmpVal;
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);
}
@@ -324,38 +362,46 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) {
// Update Time deviation
int64_t devi, deviTrunc;
Status = L2_TimeSync_GetDeviatoion(instPtr, &devi);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("Failed to compute slot deviation.\n");
deviTrunc = 0;
}
else {
if(devi > INT32_MAX || devi < INT32_MIN) {
else
{
if (devi > INT32_MAX || devi < INT32_MIN)
{
DBG_WARNING("Illegal deviation value: %lld, omit it.\n", devi);
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);
deviTrunc = (int32_t)devi;
}
else {
else
{
deviTrunc = (int32_t)devi;
}
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);
}
#else
for(int i = 0; i < SVC_PACK_LEN; i++) {
instPtr->serviceFsm.bitError += count_ones(msg[i]^berTestData[i]);
for (int i = 0; i < SVC_PACK_LEN; i++)
{
instPtr->serviceFsm.bitError += count_ones(msg[i] ^ berTestData[i]);
}
instPtr->serviceFsm.bitTotal += SVC_PACK_LEN*8;
instPtr->serviceFsm.bitTotal += SVC_PACK_LEN * 8;
#endif
instPtr->serviceFsm.pktRecved++;
// Reset Node TTL
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);
}
break;
@@ -377,42 +423,53 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) {
* @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);
DBG_INFO("-------- ServiceNet Routine --------\n");
uint8_t *sendData;
int32_t usedSlots;
L2_TOR_TYPE torState;
if(schedEntry->src == instPtr->config.myId){
if (schedEntry->src == instPtr->config.myId)
{
torState = L2_TOR_TX;
}
else if(schedEntry->dst == instPtr->config.myId){
else if (schedEntry->dst == instPtr->config.myId)
{
torState = L2_TOR_RX;
instPtr->serviceFsm.pktRecv++;
}
else{
else
{
torState = L2_TOR_SUSPEND;
}
switch(schedEntry->slotType){
switch (schedEntry->slotType)
{
case L2_SLOT_SERVICE:
if(torState == L2_TOR_TX){
if (torState == L2_TOR_TX)
{
#ifndef DATALINK_ENABLE_BERTEST
if (instPtr->serviceFsm.txSeqNum == 0) {
if(instPtr->config.myType == L2_HEAD_NODE){
if (instPtr->serviceFsm.txSeqNum == 0)
{
if (instPtr->config.myType == L2_HEAD_NODE)
{
SysMonitor_GetSendPayloadFLJRaw(instPtr->serviceFsm.txLog);
}
else {
else
{
SysMonitor_GetSendPayloadLJRaw(instPtr->serviceFsm.txLog);
}
}
// Packing service data
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);
free(sendData);
}
else {
else
{
DBG_ERROR("Failed to pop tx queue.\n");
}
#else
@@ -428,14 +485,17 @@ int32_t L2_Service_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry) {
break;
#ifndef DATALINK_ENABLE_BERTEST
case L2_SLOT_ADVERTISE:
if(torState == L2_TOR_TX){
if (torState == L2_TOR_TX)
{
// Packing response data.
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);
free(sendData);
}
else {
else
{
DBG_ERROR("Failed to pop tx queue.\n");
}
}
df_datalink/src/networking/link_layer/time_sync.c
+23 -20
View File
@@ -23,25 +23,25 @@
#include "link_layer.h"
#include "../phy_layer/phy_reg.h"
/************************** Constant Definitions *****************************/
#define MAX_FREQ_EST_NUM 128 // 32bit word
#define SVC_FREQ_EST_NUM 512 // byte
#define MGMT_FREQ_EST_NUM 512 // byte
#define MAX_FREQ_EST_NUM 128 // 32bit word
#define SVC_FREQ_EST_NUM 512 // byte
#define MGMT_FREQ_EST_NUM 512 // byte
#define BISAR_FREQ_EST_NUM 480 // byte
#define SYS_CLK_FREQ 192000000.0 // Hz
#define SIG_CLK_FREQ 245760000.0 // Hz
#define CXR_FREQ 3800000000.0 // Hz
#define SYS_CLK_FREQ 192000000.0 // Hz
#define SIG_CLK_FREQ 245760000.0 // Hz
#define CXR_FREQ 3800000000.0 // Hz
#define NORMAL_SLOT_CLK_NUM 960000.0 // 9600000= 5e-3*192e6
#define COUNTER_10NS_MAX 500000 // how many 10ns in a 5ms
#define COUNTER_10NS_MAX 500000 // how many 10ns in a 5ms
#define LIGHT_SPPED_AIR 299702547.0 // m/s
#define LIGHT_SPPED_VAC 299792458.0 // m/s
//#define PHY_SYNCT2FEND_200K 904052UL
//#define PHY_SYNCT2FEND_200K 924232UL
// #define PHY_SYNCT2FEND_200K 904052UL
// #define PHY_SYNCT2FEND_200K 924232UL
#define PHY_SYNCT2FEND_2K_20MS 276336UL // 1.919ms
#define PHY_SYNCT2FEND_2K_20MS 276336UL // 1.919ms
#define PHY_SYNCT2FEND_2K_100MS 11796336UL // 81.919ms
#define PHY_LOCAL_CNT_MAX 0x0000FFFFFFFFFFFFULL
@@ -60,7 +60,8 @@
* @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
Phy_SetTimeWord(instPtr->config.phyInstRef, adjustment, PHY_TIME_WORD_LOCAL_ADJ);
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
*
******************************************************************************/
int32_t L2_TimeSync_SlotAlign(L2_INST *instPtr) {
int32_t L2_TimeSync_SlotAlign(L2_INST *instPtr)
{
int32_t Status;
int64_t slotAdj;
int64_t slotAdj;
Status = L2_TimeSync_GetDeviatoion(instPtr, &slotAdj);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("Failed to compute slot deviation.\n");
return Status;
}
@@ -97,17 +100,19 @@ int32_t L2_TimeSync_SlotAlign(L2_INST *instPtr) {
* @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
int64_t currentTarget, lastFrameSync, slotMeasure;
int64_t currentTarget, lastFrameSync, slotMeasure;
currentTarget = PhyRead(instPtr->config.phyInstRef, TM_TARGET_L);
currentTarget |= ((int64_t)PhyRead(instPtr->config.phyInstRef, TM_TARGET_H)) << 32;
lastFrameSync = PhyRead(instPtr->config.phyInstRef, FSTIME_L);
lastFrameSync |= ((int64_t)PhyRead(instPtr->config.phyInstRef, FSTIME_H)) << 32;
slotMeasure = (currentTarget > lastFrameSync) ? (currentTarget - lastFrameSync)
: (currentTarget - lastFrameSync + PHY_LOCAL_CNT_MAX);
switch (Phy_GetLastDetectedFrameType(instPtr->config.phyInstRef)) {
case L1_2KBPS_20MS_FRAME :
switch (Phy_GetLastDetectedFrameType(instPtr->config.phyInstRef))
{
case L1_2KBPS_20MS_FRAME:
*devi = ((int64_t)PHY_SYNCT2FEND_2K_20MS) - slotMeasure;
break;
case L1_2KBPS_100MS_FRAME:
@@ -119,5 +124,3 @@ int32_t L2_TimeSync_GetDeviatoion(L2_INST *instPtr, int64_t *devi) {
}
return SUCCESS;
}
df_datalink/src/networking/phy_layer/phy_antenna.c
+15 -8
View File
@@ -230,11 +230,14 @@ int32_t Phy_AntConfigInit(PHY_ANT_COINFO *antConfigPtr)
int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo)
{
if(instPtr->beamWd.next == PHY_ANT_BEAM_8){
if (beamNo >= L1_NUM_ANT_NRRW_PATTERN) {
if (instPtr->beamWd.next == PHY_ANT_BEAM_8)
{
if (beamNo >= L1_NUM_ANT_NRRW_PATTERN)
{
return FAILURE;
}
else {
else
{
instPtr->beamNo.next = beamNo;
instPtr->antConfigPtr.oaxis[1] = (uint8_t)(antNrrwPattern[beamNo].oaxis);
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;
}
}
else if(instPtr->beamWd.next == PHY_ANT_BEAM_25){
if (beamNo >= L1_NUM_ANT_SUBNRRW_PATTERN) {
else if (instPtr->beamWd.next == PHY_ANT_BEAM_25)
{
if (beamNo >= L1_NUM_ANT_SUBNRRW_PATTERN)
{
return FAILURE;
}
else {
else
{
instPtr->beamNo.next = beamNo;
instPtr->antConfigPtr.oaxis[1] = (uint8_t)(antSubnrrwPattern[beamNo].oaxis);
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;
}
}
else{
else
{
// Beam number assign not support.
// Assign default angle (0,0)
instPtr->beamNo.next = 0;
@@ -306,7 +313,7 @@ int32_t Phy_SetBeamAngle(PHY_INST *phyInstPtr, double offAx, double rndAx)
if (offAxDeg > 60)
{
DBG_ERROR("Angle out of range, off axis angle: %u (should be 0~60), round axis angle: %u (should be 0~360).\n",
offAxDeg, rndAxDeg);
offAxDeg, rndAxDeg);
return EPARAM;
}
offAxDeg = offAxDeg * 10;
df_datalink/src/networking/phy_layer/phy_layer.c
+187 -132
View File
@@ -30,39 +30,39 @@
#include "../config/app_config.h"
/************************** Constant Definitions *****************************/
#define TOR_BUFF_MAX_SIZE 1024 //8192bits
#define TMWORD_FRAC_MAX 500000
#define ANTCONFIG_MAX_LEN (8192/2)
#define PAYLOAD_MAX_LEN (4096/2)
#define TOR_BUFF_MAX_SIZE 1024 // 8192bits
#define TMWORD_FRAC_MAX 500000
#define ANTCONFIG_MAX_LEN (8192 / 2)
#define PAYLOAD_MAX_LEN (4096 / 2)
#define INIT_BEAM_NO 0
#define INIT_ANT_WD PHY_ANT_BEAM_120
#define INIT_ANT_NO 0
#define L1_FRAME_NONE_LEN UINT32_MAX// byte
#define L1_FRAME_2KBPS_20MS_LEN 0 // byte
#define L1_FRAME_2KBPS_100MS_LEN (240/8 )// byte
#define L1_FRAME_10KBPS_10MS_LEN 0 // byte
#define L1_FRAME_10KBPS_20MS_LEN (240/8) // byte
#define L1_FRAME_40KBPS_20MS_LEN (2160/8)// byte
#define L1_FRAME_40KBPS_10MS_LEN (720/8) // byte
#define L1_FRAME_200KBPS_5MS_LEN (2048/8)// byte
#define L1_FRAME_1M2BPS_5MS_LEN (8184/8)// byte
#define L1_FRAME_NONE_LEN UINT32_MAX // byte
#define L1_FRAME_2KBPS_20MS_LEN 0 // byte
#define L1_FRAME_2KBPS_100MS_LEN (240 / 8) // byte
#define L1_FRAME_10KBPS_10MS_LEN 0 // byte
#define L1_FRAME_10KBPS_20MS_LEN (240 / 8) // byte
#define L1_FRAME_40KBPS_20MS_LEN (2160 / 8) // byte
#define L1_FRAME_40KBPS_10MS_LEN (720 / 8) // byte
#define L1_FRAME_200KBPS_5MS_LEN (2048 / 8) // byte
#define L1_FRAME_1M2BPS_5MS_LEN (8184 / 8) // byte
#define L1_FRAME_NONE_COMP UINT32_MAX // *10ns
#define L1_FRAME_2KBPS_20MS_COMP 1622400 // *10ns
#define L1_FRAME_2KBPS_100MS_COMP 1622400 // *10ns
#define L1_FRAME_10KBPS_10MS_COMP 747366 // *10ns
#define L1_FRAME_10KBPS_20MS_COMP 747366 // *10ns
#define L1_FRAME_40KBPS_20MS_COMP 97066 // *10ns
#define L1_FRAME_40KBPS_10MS_COMP 97066 // *10ns
#define L1_FRAME_200KBPS_5MS_COMP 17067 // *10ns
#define L1_FRAME_1M2BPS_5MS_COMP 17067 // *10ns
#define L1_FRAME_NONE_COMP UINT32_MAX // *10ns
#define L1_FRAME_2KBPS_20MS_COMP 1622400 // *10ns
#define L1_FRAME_2KBPS_100MS_COMP 1622400 // *10ns
#define L1_FRAME_10KBPS_10MS_COMP 747366 // *10ns
#define L1_FRAME_10KBPS_20MS_COMP 747366 // *10ns
#define L1_FRAME_40KBPS_20MS_COMP 97066 // *10ns
#define L1_FRAME_40KBPS_10MS_COMP 97066 // *10ns
#define L1_FRAME_200KBPS_5MS_COMP 17067 // *10ns
#define L1_FRAME_1M2BPS_5MS_COMP 17067 // *10ns
/***************** Macros (Inline Functions) Definitions *********************/
/************************** Function Prototypes ******************************/
static void Phy_RcdShift(PHY_RCD* rcd, int32_t netxVal);
static int32_t SetSendData(PHY_INST* instPtr, L1_FRAME_TYPE frameType, uint8_t* buff);
static void Phy_RcdShift(PHY_RCD *rcd, int32_t netxVal);
static int32_t SetSendData(PHY_INST *instPtr, L1_FRAME_TYPE frameType, uint8_t *buff);
/**************************** Type Definitions *******************************/
@@ -79,22 +79,24 @@ static int32_t SetSendData(PHY_INST* instPtr, L1_FRAME_TYPE frameType, uint8_t*
* 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);
uint32_t frameLen, recvBramAddr;
frameLen = Phy_GetFrameLen(instPtr->frameType.last);
if (frameLen == L1_FRAME_NONE_LEN) {
if (frameLen == L1_FRAME_NONE_LEN)
{
return FAILURE;
}
else {
else
{
// Ping-pong flow.
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);
}
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Setup data will be sent in next slot.
@@ -105,18 +107,21 @@ int32_t Phy_RetrieveRecvData(PHY_INST* instPtr, uint8_t* buff) {
* 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);
uint32_t frameLen, sendCmd;
frameLen = Phy_GetFrameLen(frameType);
if (frameLen == L1_FRAME_NONE_LEN) {
if (frameLen == L1_FRAME_NONE_LEN)
{
return FAILURE;
}
else {
else
{
// 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;
memcpy((uint8_t*) (instPtr->txDataPtr + instPtr->sendOffset), buff, frameLen);
memcpy((uint8_t *)(instPtr->txDataPtr + instPtr->sendOffset), buff, frameLen);
// Trigger Physical layer to start fetching data.
sendCmd = frameLen & SEND_DATA_CMD_LEN_MASK;
sendCmd |= (instPtr->sendOffset << SEND_DATA_CMD_OFFSET_SHIFT) & SEND_DATA_CMD_OFFSET_MASK;
@@ -125,7 +130,6 @@ static int32_t SetSendData(PHY_INST* instPtr, L1_FRAME_TYPE frameType, uint8_t*
return SUCCESS;
}
/*****************************************************************************/
/**
* @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
*
******************************************************************************/
inline bool Phy_IsFrameDetected(PHY_INST* instPtr) {
inline bool Phy_IsFrameDetected(PHY_INST *instPtr)
{
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;
}
else {
else
{
return false;
}
}
@@ -158,13 +165,15 @@ inline bool Phy_IsFrameDetected(PHY_INST* instPtr) {
* false
*
******************************************************************************/
inline bool Phy_IsFrameReceived(PHY_INST* instPtr) {
inline bool Phy_IsFrameReceived(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
if(instPtr->TOR.last == PHY_TOR_RX && instPtr->isAcq
&& instPtr->isFrameSync && instPtr->isDecode) {
if (instPtr->TOR.last == PHY_TOR_RX && instPtr->isAcq && instPtr->isFrameSync && instPtr->isDecode)
{
return true;
}
else {
else
{
return false;
}
}
@@ -180,13 +189,15 @@ inline bool Phy_IsFrameReceived(PHY_INST* instPtr) {
* false
*
******************************************************************************/
inline bool Phy_IsFrameValid(PHY_INST* instPtr) {
inline bool Phy_IsFrameValid(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
if (instPtr->TOR.last == PHY_TOR_RX && instPtr->isAcq && instPtr->isFrameSync
&& instPtr->isDecode && instPtr->isValid) {
if (instPtr->TOR.last == PHY_TOR_RX && instPtr->isAcq && instPtr->isFrameSync && instPtr->isDecode && instPtr->isValid)
{
return true;
}
else {
else
{
return false;
}
}
@@ -200,12 +211,15 @@ inline bool Phy_IsFrameValid(PHY_INST* instPtr) {
* @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);
if (Phy_IsFrameDetected(instPtr) == false) {
if (Phy_IsFrameDetected(instPtr) == false)
{
return L1_NONE_FRAME;
}
else {
else
{
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"
*
******************************************************************************/
inline L1_FRAME_TYPE Phy_GetLastReceivedFrameType(PHY_INST* instPtr) {
inline L1_FRAME_TYPE Phy_GetLastReceivedFrameType(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
if (Phy_IsFrameReceived(instPtr) == false) {
if (Phy_IsFrameReceived(instPtr) == false)
{
return L1_NONE_FRAME;
}
else {
else
{
return instPtr->frameType.last;
}
}
/*****************************************************************************/
/**
* @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"
*
******************************************************************************/
inline L1_FRAME_TYPE Phy_GetLastSentFrameType(PHY_INST* instPtr) {
inline L1_FRAME_TYPE Phy_GetLastSentFrameType(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
if (instPtr->TOR.last != PHY_TOR_TX) {
if (instPtr->TOR.last != PHY_TOR_TX)
{
return L1_NONE_FRAME;
}
else {
else
{
return instPtr->frameType.last;
}
}
@@ -258,7 +277,8 @@ inline L1_FRAME_TYPE Phy_GetLastSentFrameType(PHY_INST* instPtr) {
* @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);
rcd->last = rcd->curr;
rcd->curr = rcd->next;
@@ -275,12 +295,14 @@ static void Phy_RcdShift(PHY_RCD* rcd, int32_t netxVal){
* L1_FRAME_NONE_LEN if the frame type code cannot be recognized.
*
******************************************************************************/
uint32_t Phy_GetFrameLen(L1_FRAME_TYPE frameType) {
switch (frameType) {
uint32_t Phy_GetFrameLen(L1_FRAME_TYPE frameType)
{
switch (frameType)
{
case L1_2KBPS_20MS_FRAME:
// FIXME: for DUIFEI net setup
return L1_FRAME_2KBPS_100MS_LEN;
// return L1_FRAME_2KBPS_20MS_LEN;
// return L1_FRAME_2KBPS_20MS_LEN;
case L1_2KBPS_100MS_FRAME:
return L1_FRAME_2KBPS_100MS_LEN;
case L1_10KBPS_10MS_FRAME:
@@ -300,8 +322,6 @@ uint32_t Phy_GetFrameLen(L1_FRAME_TYPE frameType) {
}
}
/*****************************************************************************/
/**
* @brief Update Physical Layer reception status.
@@ -311,31 +331,33 @@ uint32_t Phy_GetFrameLen(L1_FRAME_TYPE frameType) {
* @return
*
******************************************************************************/
void Phy_UpdateState(PHY_INST* instPtr) {
void Phy_UpdateState(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
Phy_RcdShift(&(instPtr->TOR), PHY_TOR_SUSPEND);
Phy_RcdShift(&(instPtr->frameType),L1_NONE_FRAME);
Phy_RcdShift(&(instPtr->antNo),INIT_ANT_NO);
Phy_RcdShift(&(instPtr->beamNo),INIT_BEAM_NO);
Phy_RcdShift(&(instPtr->beamWd),INIT_ANT_WD);
Phy_RcdShift(&(instPtr->frameType), L1_NONE_FRAME);
Phy_RcdShift(&(instPtr->antNo), INIT_ANT_NO);
Phy_RcdShift(&(instPtr->beamNo), INIT_BEAM_NO);
Phy_RcdShift(&(instPtr->beamWd), INIT_ANT_WD);
/* Update Frame Sate. */
uint32_t txrxSate = PhyRead(instPtr, TXRX_STATE);
instPtr->isAcq = ((txrxSate & TXRX_STATE_ACQ_MASK) == TXRX_STATE_ACQ_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;
if (Phy_IsFrameReceived(instPtr)) {
if (Phy_IsFrameReceived(instPtr))
{
DBG_INFO("Physical layer finished reception procedure, retrieving received data...\n");
instPtr->isValid = true;
}
else {
else
{
instPtr->isValid = false;
}
return;
}
/*****************************************************************************/
/**
* @brief Initialize Physical Layer instance, allocate memory for buffers,
@@ -347,12 +369,14 @@ void Phy_UpdateState(PHY_INST* instPtr) {
* @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(phyConfig != NULL);
int32_t Status = SUCCESS;
*instPtr = (PHY_INST*) malloc(sizeof(PHY_INST));
if ((*instPtr) == NULL) {
*instPtr = (PHY_INST *)malloc(sizeof(PHY_INST));
if ((*instPtr) == NULL)
{
return ENOMEM;
}
@@ -374,7 +398,8 @@ int32_t Phy_InstInit(PHY_INST** instPtr, PHY_CONFIG *phyConfig) {
memset(&((*instPtr)->hwSate), 0x00, sizeof(HWSTATE));
Status = Phy_AntConfigInit(&((*instPtr)->antConfigPtr));
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
goto init_error;
}
@@ -383,57 +408,65 @@ int32_t Phy_InstInit(PHY_INST** instPtr, PHY_CONFIG *phyConfig) {
XBram txDataBram;
XBram rxDataBram;
bramConfigPtr = XBram_LookupConfig(phyConfig->antBramId);
if (bramConfigPtr == NULL) {
if (bramConfigPtr == NULL)
{
Status = EDCONF;
goto init_error;
}
Status = XBram_CfgInitialize(&antConfigBram, bramConfigPtr,
bramConfigPtr->CtrlBaseAddress);
if (Status != SUCCESS) {
bramConfigPtr->CtrlBaseAddress);
if (Status != SUCCESS)
{
goto init_error;
}
/*
* Execute the BRAM driver selftest.
*/
Status = XBram_SelfTest(&antConfigBram, 0);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
goto init_error;
}
bramConfigPtr = XBram_LookupConfig(phyConfig->txBramId);
if (bramConfigPtr == NULL) {
if (bramConfigPtr == NULL)
{
Status = EDCONF;
goto init_error;
}
Status = XBram_CfgInitialize(&txDataBram, bramConfigPtr,
bramConfigPtr->CtrlBaseAddress);
if (Status != SUCCESS) {
bramConfigPtr->CtrlBaseAddress);
if (Status != SUCCESS)
{
goto init_error;
}
/*
* Execute the BRAM driver selftest.
*/
Status = XBram_SelfTest(&txDataBram, 0);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
goto init_error;
}
bramConfigPtr = XBram_LookupConfig(phyConfig->rxBramId);
if (bramConfigPtr == NULL) {
if (bramConfigPtr == NULL)
{
Status = EDCONF;
goto init_error;
}
Status = XBram_CfgInitialize(&rxDataBram, bramConfigPtr,
bramConfigPtr->CtrlBaseAddress);
if (Status != SUCCESS) {
bramConfigPtr->CtrlBaseAddress);
if (Status != SUCCESS)
{
goto init_error;
}
/*
* Execute the BRAM driver selftest.
*/
Status = XBram_SelfTest(&rxDataBram, 0);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
goto init_error;
}
@@ -445,7 +478,8 @@ int32_t Phy_InstInit(PHY_INST** instPtr, PHY_CONFIG *phyConfig) {
XAdcPs_Config *xadcConfigPtr;
xadcConfigPtr = XAdcPs_LookupConfig(phyConfig->hwXadcId);
if (xadcConfigPtr == NULL) {
if (xadcConfigPtr == NULL)
{
Status = EDCONF;
goto init_error;
}
@@ -453,7 +487,8 @@ int32_t Phy_InstInit(PHY_INST** instPtr, PHY_CONFIG *phyConfig) {
/* Self Test the XADC/ADC device */
Status = XAdcPs_SelfTest(&((*instPtr)->hwXadc));
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
goto init_error;
}
@@ -475,10 +510,12 @@ init_error:
* @return NONE
*
******************************************************************************/
void Phy_Start(PHY_INST* instPtr) {
void Phy_Start(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
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");
}
instPtr->isPhyEn = true;
@@ -495,17 +532,18 @@ void Phy_Start(PHY_INST* instPtr) {
* @return NONE
*
******************************************************************************/
void Phy_Halt(PHY_INST* instPtr) {
void Phy_Halt(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
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");
}
instPtr->isPhyEn = false;
PhyWrite(instPtr, PHY_MAIN_CTRL, (phyMainCtrl & (~PHY_MAIN_CTRL_EN_MASK)));
}
/*****************************************************************************/
/**
* @brief Update Physical Layer mode.
@@ -516,10 +554,12 @@ void Phy_Halt(PHY_INST* instPtr) {
* @return
*
******************************************************************************/
int32_t Phy_SetMode(PHY_INST* instPtr, PHY_MODE modeCod) {
int32_t Phy_SetMode(PHY_INST *instPtr, PHY_MODE modeCod)
{
ASSERT(instPtr != NULL);
uint32_t phyMainCtrl = PhyRead(instPtr, PHY_MAIN_CTRL) & ~PHY_MAIN_CTRL_MODE_MASK;
switch (modeCod) {
switch (modeCod)
{
case PHY_MODE_NONE:
phyMainCtrl |= PHY_MAIN_CTRL_MODE_NONE;
break;
@@ -551,12 +591,13 @@ int32_t Phy_SetMode(PHY_INST* instPtr, PHY_MODE modeCod) {
* @return
*
******************************************************************************/
int32_t Phy_SetupNextSlot(PHY_INST* instPtr, PHY_TOR_TYPE txrxMode,
L1_FRAME_TYPE frameType, uint8_t* buff) {
int32_t Phy_SetupNextSlot(PHY_INST *instPtr, PHY_TOR_TYPE txrxMode,
L1_FRAME_TYPE frameType, uint8_t *buff)
{
ASSERT(instPtr != NULL);
int32_t Status = SUCCESS;
uint32_t phyModeCtrl, antConfigBramAddr, hopNum;
uint16_t* hopPattern;
uint16_t *hopPattern;
// Implement a ping-pong flow, offset always is 0 or half of buffer depth.
instPtr->antConfigOffset = (instPtr->antConfigOffset == ANTCONFIG_MAX_LEN) ? 0 : ANTCONFIG_MAX_LEN;
antConfigBramAddr = instPtr->antConfigDataPtr + instPtr->antConfigOffset;
@@ -564,7 +605,8 @@ int32_t Phy_SetupNextSlot(PHY_INST* instPtr, PHY_TOR_TYPE txrxMode,
instPtr->TOR.next = txrxMode;
instPtr->frameType.next = frameType;
switch (txrxMode) {
switch (txrxMode)
{
case PHY_TOR_SUSPEND:
phyModeCtrl = PHY_MODE_CTRL_TOR_SUSPEND;
break;
@@ -577,16 +619,19 @@ int32_t Phy_SetupNextSlot(PHY_INST* instPtr, PHY_TOR_TYPE txrxMode,
default:
return EPARAM;
}
if (instPtr->isPresetHop == false) {
if (instPtr->isPresetHop == false)
{
hopPattern = hopPattern_Config;
}
else {
switch (frameType) {
else
{
switch (frameType)
{
case L1_2KBPS_20MS_FRAME:
phyModeCtrl |= PHY_MODE_CTRL_FRAME_2KBPS_20MS;
hopNum = L1_FRAME_2KBPS_20MS_HOPS;
// FIXME: For DUIFEI net setup.
// hopPattern = (txrxMode == TOR_TX) ? hopPattern_2KBPS_20MS;
// hopPattern = (txrxMode == TOR_TX) ? hopPattern_2KBPS_20MS;
hopPattern = hopPattern_2KBPS_100MS;
break;
case L1_2KBPS_100MS_FRAME:
@@ -632,25 +677,26 @@ int32_t Phy_SetupNextSlot(PHY_INST* instPtr, PHY_TOR_TYPE txrxMode,
PhyWrite(instPtr, PHY_MODE_CTRL, phyModeCtrl);
//TODO: Set antenna stand by, when suspend.
const uint8_t* dataSrcPtr = (uint8_t*)(&(instPtr->antConfigPtr));
for (int32_t i = 0; i < hopNum; i++) {
// TODO: Set antenna stand by, when suspend.
const uint8_t *dataSrcPtr = (uint8_t *)(&(instPtr->antConfigPtr));
for (int32_t i = 0; i < hopNum; i++)
{
instPtr->antConfigPtr.antFreq[1] = (uint8_t)(hopPattern[i] & 0x00FF);
instPtr->antConfigPtr.antFreq[0] = (uint8_t)((hopPattern[i] >> 8) & 0x00FF);
instPtr->antConfigPtr.rfFreq[1] = (uint8_t)(hopPattern[i] & 0x00FF);
instPtr->antConfigPtr.rfFreq[0] = (uint8_t)((hopPattern[i] >> 8) & 0x00FF);
memcpy((uint8_t*) (antConfigBramAddr + i * ANT_COINFO_LENGTH),
dataSrcPtr, sizeof(PHY_ANT_COINFO));
memcpy((uint8_t *)(antConfigBramAddr + i * ANT_COINFO_LENGTH),
dataSrcPtr, sizeof(PHY_ANT_COINFO));
}
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);
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);
PhyWrite(instPtr, ANT_CONFIG_CMD, antCtrl);
// 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);
if(Status != SUCCESS) {
if (Status != SUCCESS)
{
return Status;
}
}
@@ -671,8 +717,10 @@ int32_t Phy_SetupNextSlot(PHY_INST* instPtr, PHY_TOR_TYPE txrxMode,
* FAILURE frame type cannot be recognized
*
******************************************************************************/
uint32_t Phy_GetHopLength(L1_FRAME_TYPE frameType){
switch (frameType) {
uint32_t Phy_GetHopLength(L1_FRAME_TYPE frameType)
{
switch (frameType)
{
case L1_2KBPS_20MS_FRAME:
return L1_FRAME_2KBPS_20MS_HOPS;
case L1_2KBPS_100MS_FRAME:
@@ -703,8 +751,10 @@ uint32_t Phy_GetHopLength(L1_FRAME_TYPE frameType){
* @return 32bit unsigned integer (clock periods)
*
******************************************************************************/
uint32_t Phy_GetTimeComp(L1_FRAME_TYPE frameType){
switch (frameType) {
uint32_t Phy_GetTimeComp(L1_FRAME_TYPE frameType)
{
switch (frameType)
{
case L1_2KBPS_20MS_FRAME:
return L1_FRAME_2KBPS_20MS_COMP;
case L1_2KBPS_100MS_FRAME:
@@ -735,17 +785,18 @@ uint32_t Phy_GetTimeComp(L1_FRAME_TYPE frameType){
* @return 32bit signed integer (Hz)
*
******************************************************************************/
int32_t Phy_GetFreqEst(PHY_INST *instPtr){
int32_t Phy_GetFreqEst(PHY_INST *instPtr)
{
uint32_t temp;
int32_t acqCh, fsCh;
double freqEst;
temp = PhyRead(instPtr, COASE_FREQEST);
acqCh = temp & 0x000000FF;
acqCh = 80 - acqCh;
fsCh = (temp>>8) & 0x000000FF;
fsCh = fsCh -128;
freqEst = acqCh*(31.25e3/4) + fsCh*(31.25e3/256);
return ((int32_t) freqEst);
fsCh = (temp >> 8) & 0x000000FF;
fsCh = fsCh - 128;
freqEst = acqCh * (31.25e3 / 4) + fsCh * (31.25e3 / 256);
return ((int32_t)freqEst);
}
/*****************************************************************************/
@@ -759,12 +810,14 @@ int32_t Phy_GetFreqEst(PHY_INST *instPtr){
* 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);
if(patternLen > L1_FRAME_MAX_HOPS){
if (patternLen > L1_FRAME_MAX_HOPS)
{
return FAILURE;
}
memcpy(hopPattern_Config, patternBuff, patternLen*sizeof(uint16_t));
memcpy(hopPattern_Config, patternBuff, patternLen * sizeof(uint16_t));
return SUCCESS;
}
@@ -780,16 +833,18 @@ int32_t Phy_SetHopPattern(void* patternBuff, uint32_t patternLen){
* 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);
switch(tmType){
switch (tmType)
{
case PHY_TIME_WORD_POLL:
PhyWrite(instPtr, POLLTIME_L, (uint32_t)tmWord);
PhyWrite(instPtr, POLLTIME_H, (uint32_t)(tmWord>>32));
PhyWrite(instPtr, POLLTIME_H, (uint32_t)(tmWord >> 32));
break;
case PHY_TIME_WORD_LOCAL_ADJ:
PhyWrite(instPtr, LOCALTM_ADJ_L, (uint32_t)tmWord);
PhyWrite(instPtr, LOCALTM_ADJ_H, (uint32_t)(tmWord>>32));
PhyWrite(instPtr, LOCALTM_ADJ_H, (uint32_t)(tmWord >> 32));
break;
case PHY_TIME_WORD_STEP:
PhyWrite(instPtr, CNT_STEP_OPT, (uint32_t)tmWord);
@@ -800,7 +855,6 @@ int32_t Phy_SetTimeWord(PHY_INST* instPtr, uint64_t tmWord, PHY_TIME_WORD_TYPE t
return SUCCESS;
}
/*****************************************************************************/
/**
* @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.
*
******************************************************************************/
void Phy_UpdateHwSate(PHY_INST *instPtr){
void Phy_UpdateHwSate(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
uint32_t rawData;
@@ -824,7 +879,7 @@ void Phy_UpdateHwSate(PHY_INST *instPtr){
rawData = XAdcPs_GetAdcData(&(instPtr->hwXadc), XADCPS_CH_TEMP);
instPtr->hwSate.spTemp = (uint16_t)(XAdcPs_RawToTemperature(rawData) * 100);
//FIXME: Chip status determine.
// FIXME: Chip status determine.
instPtr->hwSate.chipSt = 0;
/*** Voltage and Status ***/
df_datalink/src/networking/phy_layer/phy_layer.h
+50 -42
View File
@@ -30,24 +30,25 @@
#include "phy_reg.h"
#include "../../system_monitor.h"
/************************** Constant Definitions *****************************/
#define L1_FRAME_NONE_HOPS UINT32_MAX
#define L1_FRAME_MAX_HOPS 81
#define L1_FRAME_2KBPS_20MS_HOPS 1
#define L1_FRAME_2KBPS_100MS_HOPS 81
#define L1_FRAME_10KBPS_10MS_HOPS 1
#define L1_FRAME_10KBPS_20MS_HOPS 11
#define L1_FRAME_40KBPS_20MS_HOPS 19
#define L1_FRAME_40KBPS_10MS_HOPS 9
#define L1_FRAME_200KBPS_5MS_HOPS 20
#define L1_FRAME_1M2BPS_5MS_HOPS 20
#define L1_FRAME_NONE_HOPS UINT32_MAX
#define L1_FRAME_MAX_HOPS 81
#define L1_FRAME_2KBPS_20MS_HOPS 1
#define L1_FRAME_2KBPS_100MS_HOPS 81
#define L1_FRAME_10KBPS_10MS_HOPS 1
#define L1_FRAME_10KBPS_20MS_HOPS 11
#define L1_FRAME_40KBPS_20MS_HOPS 19
#define L1_FRAME_40KBPS_10MS_HOPS 9
#define L1_FRAME_200KBPS_5MS_HOPS 20
#define L1_FRAME_1M2BPS_5MS_HOPS 20
#define L1_NUM_ANT_SUBNRRW_PATTERN 37
#define L1_NUM_ANT_NRRW_PATTERN 61
#define L1_NUM_ANT_SUBNRRW_PATTERN 37
#define L1_NUM_ANT_NRRW_PATTERN 61
#define ANT_COINFO_LENGTH 32 //bytes
#define ANT_COINFO_LENGTH 32 // bytes
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
typedef enum {
typedef enum
{
L1_NONE_FRAME = 0,
L1_2KBPS_20MS_FRAME = 1,
L1_2KBPS_100MS_FRAME = 2,
@@ -59,7 +60,8 @@ typedef enum {
L1_1M2BPS_5MS_FRAME = 8
} L1_FRAME_TYPE;
typedef struct {
typedef struct
{
/*** Ant Control ***/
/* Antenna control frame head 0xAA */
/* default 0, reserved for further use */
@@ -113,8 +115,8 @@ typedef struct {
uint8_t rfReserved1;
} PHY_ANT_COINFO;
typedef struct {
typedef struct
{
/* off-axis angle range is 0~60,
* 0.1 degree each LSB */
uint16_t oaxis;
@@ -123,15 +125,16 @@ typedef struct {
uint16_t raxis;
} PHY_ANT_ANGLE;
typedef enum {
typedef enum
{
PHY_TIME_WORD_NONE = 0,
PHY_TIME_WORD_POLL = 1,
PHY_TIME_WORD_LOCAL_ADJ = 2,
PHY_TIME_WORD_STEP = 3,
} PHY_TIME_WORD_TYPE;
typedef enum {
typedef enum
{
PHY_ANT_BEAM_NONE = 0,
PHY_ANT_BEAM_120 = 1,
PHY_ANT_BEAM_60 = 2,
@@ -139,20 +142,23 @@ typedef enum {
PHY_ANT_BEAM_8 = 4
} PHY_ANT_BEAM_WIDTH;
typedef enum {
typedef enum
{
PHY_TOR_SUSPEND = 0,
PHY_TOR_TX = 1,
PHY_TOR_RX = 2
} PHY_TOR_TYPE;
typedef enum {
typedef enum
{
PHY_MODE_NONE = 0,
PHY_MODE_LISTEN = 1,
PHY_MODE_SCHEDULE = 2,
PHY_MODE_PULL = 3
} PHY_MODE;
typedef struct {
typedef struct
{
uint32_t devId;
uint32_t txBramId;
uint32_t rxBramId;
@@ -161,27 +167,29 @@ typedef struct {
uint32_t baseAddr;
} PHY_CONFIG;
typedef struct {
typedef struct
{
int32_t last;
int32_t curr;
int32_t next;
} PHY_RCD;
typedef struct {
PHY_CONFIG phyCofig; // Device address configs
bool isPhyEn; // is physical instance enable
bool isAcq; // is last reception acquisition success
bool isFrameSync; // is last reception frame sync success
bool isDecode; // is last reception decode success
bool isValid; // is last reception frame data valid (crc check pass)
bool isPresetHop; // is using preset frequency hop
PHY_RCD antNo; // antenna number record
PHY_RCD beamNo; // beam number record
PHY_RCD beamWd; // beam width record
PHY_RCD TOR; // tx rx state record
PHY_RCD frameType; // frame type record
uint32_t antConfigOffset; // antenna configuration bram offset (to implement ping-pong data flow)
uint32_t sendOffset; // antenna configuration bram offset (to implement ping-pong data flow)
typedef struct
{
PHY_CONFIG phyCofig; // Device address configs
bool isPhyEn; // is physical instance enable
bool isAcq; // is last reception acquisition success
bool isFrameSync; // is last reception frame sync success
bool isDecode; // is last reception decode success
bool isValid; // is last reception frame data valid (crc check pass)
bool isPresetHop; // is using preset frequency hop
PHY_RCD antNo; // antenna number record
PHY_RCD beamNo; // beam number record
PHY_RCD beamWd; // beam width record
PHY_RCD TOR; // tx rx state record
PHY_RCD frameType; // frame type record
uint32_t antConfigOffset; // antenna configuration bram offset (to implement ping-pong data flow)
uint32_t sendOffset; // antenna configuration bram offset (to implement ping-pong data flow)
PHY_ANT_COINFO antConfigPtr; // antenna configuration pointer
uintptr_t txDataPtr;
uintptr_t rxDataPtr;
@@ -213,15 +221,15 @@ void Phy_Start(PHY_INST *instPtr);
void Phy_Halt(PHY_INST *instPtr);
int32_t Phy_SetMode(PHY_INST *instPtr, PHY_MODE modeCod);
int32_t Phy_SetupNextSlot(PHY_INST *instPtr, PHY_TOR_TYPE txrxMode, L1_FRAME_TYPE frameType, uint8_t* buff);
int32_t Phy_RetrieveRecvData(PHY_INST* instPtr, uint8_t* buff);
int32_t Phy_SetupNextSlot(PHY_INST *instPtr, PHY_TOR_TYPE txrxMode, L1_FRAME_TYPE frameType, uint8_t *buff);
int32_t Phy_RetrieveRecvData(PHY_INST *instPtr, uint8_t *buff);
bool Phy_IsFrameDetected(PHY_INST *instPtr);
bool Phy_IsFrameReceived(PHY_INST *instPtr);
bool Phy_IsFrameValid(PHY_INST *instPtr);
uint32_t Phy_GetFrameLen(L1_FRAME_TYPE frameType);
L1_FRAME_TYPE Phy_GetLastDetectedFrameType(PHY_INST* instPtr);
L1_FRAME_TYPE Phy_GetLastDetectedFrameType(PHY_INST *instPtr);
L1_FRAME_TYPE Phy_GetLastReceivedFrameType(PHY_INST *instPtr);
L1_FRAME_TYPE Phy_GetLastSentFrameType(PHY_INST *instPtr);
df_datalink/src/networking/phy_layer/phy_reg.h
+188 -192
View File
@@ -28,311 +28,307 @@
/***************** Macros (Inline Functions) Definitions *********************/
// Read Physical Layer register (0x00000000~0x000000FF)
#define PhyRead(instPtr, addr) IO_In32(instPtr->phyCofig.baseAddr + addr)
#define PhyRead(instPtr, addr) IO_In32(instPtr->phyCofig.baseAddr + addr)
// Write Physical Layer register (0x000000100~0x00000200)
#define PhyWrite(instPtr, addr, value) IO_Out32(instPtr->phyCofig.baseAddr + addr, value)
#define PhyWrite(instPtr, addr, value) IO_Out32(instPtr->phyCofig.baseAddr + addr, value)
/************************** Constant Definitions *****************************/
/************************* Ps2Pl Regs PS Write Only ***************************/
/**
* Register: PHY_MAIN_CTRL
*/
#define PHY_MAIN_CTRL 0X00000000U
#define PHY_MAIN_CTRL_EN_SHIFT 0
#define PHY_MAIN_CTRL_EN_WIDTH 1
#define PHY_MAIN_CTRL_EN_MASK 0x00000001U
#define PHY_MAIN_CTRL_MODE_SHIFT 8
#define PHY_MAIN_CTRL_MODE_WIDTH 8
#define PHY_MAIN_CTRL_MODE_MASK 0x0000FF00U
#define PHY_MAIN_CTRL 0X00000000U
#define PHY_MAIN_CTRL_EN_SHIFT 0
#define PHY_MAIN_CTRL_EN_WIDTH 1
#define PHY_MAIN_CTRL_EN_MASK 0x00000001U
#define PHY_MAIN_CTRL_MODE_SHIFT 8
#define PHY_MAIN_CTRL_MODE_WIDTH 8
#define PHY_MAIN_CTRL_MODE_MASK 0x0000FF00U
#define PHY_MAIN_CTRL_EN_EN 0x00000001U
#define PHY_MAIN_CTRL_EN_DSBL 0x00000000U
#define PHY_MAIN_CTRL_MODE_NONE 0x00000000U
#define PHY_MAIN_CTRL_MODE_LISTEN 0x00000100U
#define PHY_MAIN_CTRL_MODE_SCHEDULE 0x00000200U
#define PHY_MAIN_CTRL_MODE_PULL 0x00000300U
#define PHY_MAIN_CTRL_EN_EN 0x00000001U
#define PHY_MAIN_CTRL_EN_DSBL 0x00000000U
#define PHY_MAIN_CTRL_MODE_NONE 0x00000000U
#define PHY_MAIN_CTRL_MODE_LISTEN 0x00000100U
#define PHY_MAIN_CTRL_MODE_SCHEDULE 0x00000200U
#define PHY_MAIN_CTRL_MODE_PULL 0x00000300U
/**
* Register: TXRX_CTRL
*/
#define PHY_MODE_CTRL 0x00000004U
#define PHY_MODE_CTRL_TOR_SHIFT 0
#define PHY_MODE_CTRL_TOR_WIDTH 2
#define PHY_MODE_CTRL_TOR_MASK 0x00000003U
#define PHY_MODE_CTRL_FRAME_SHIFT 16
#define PHY_MODE_CTRL_FRAME_WIDTH 8
#define PHY_MODE_CTRL_FRAME_MASK 0x00FF0000U
#define PHY_MODE_CTRL 0x00000004U
#define PHY_MODE_CTRL_TOR_SHIFT 0
#define PHY_MODE_CTRL_TOR_WIDTH 2
#define PHY_MODE_CTRL_TOR_MASK 0x00000003U
#define PHY_MODE_CTRL_FRAME_SHIFT 16
#define PHY_MODE_CTRL_FRAME_WIDTH 8
#define PHY_MODE_CTRL_FRAME_MASK 0x00FF0000U
#define PHY_MODE_CTRL_TOR_SUSPEND 0x00000000U
#define PHY_MODE_CTRL_TOR_TX 0x00000001U
#define PHY_MODE_CTRL_TOR_RX 0x00000002U
#define PHY_MODE_CTRL_TOR_SUSPEND 0x00000000U
#define PHY_MODE_CTRL_TOR_TX 0x00000001U
#define PHY_MODE_CTRL_TOR_RX 0x00000002U
#define PHY_MODE_CTRL_FRAME_NONE 0x00000000U
#define PHY_MODE_CTRL_FRAME_2KBPS_20MS 0x00000100U
#define PHY_MODE_CTRL_FRAME_2KBPS_100MS 0x00000200U
#define PHY_MODE_CTRL_FRAME_10KBPS_10MS 0x00000300U // Initial Scan Frame
#define PHY_MODE_CTRL_FRAME_10KBPS_20MS 0x00000400U // Benchmark Frame
#define PHY_MODE_CTRL_FRAME_40KBPS_20MS 0x00000500U
#define PHY_MODE_CTRL_FRAME_40KBPS_10MS 0x00000600U
#define PHY_MODE_CTRL_FRAME_200KBPS_5MS 0x00000700U // 2-Way ACQ Frame, Management Frame, Service Frame
#define PHY_MODE_CTRL_FRAME_1M2BPS_5MS 0x00000800U // Bistatic Synthetic Aperture Radar Frame
#define PHY_MODE_CTRL_FRAME_NONE 0x00000000U
#define PHY_MODE_CTRL_FRAME_2KBPS_20MS 0x00000100U
#define PHY_MODE_CTRL_FRAME_2KBPS_100MS 0x00000200U
#define PHY_MODE_CTRL_FRAME_10KBPS_10MS 0x00000300U // Initial Scan Frame
#define PHY_MODE_CTRL_FRAME_10KBPS_20MS 0x00000400U // Benchmark Frame
#define PHY_MODE_CTRL_FRAME_40KBPS_20MS 0x00000500U
#define PHY_MODE_CTRL_FRAME_40KBPS_10MS 0x00000600U
#define PHY_MODE_CTRL_FRAME_200KBPS_5MS 0x00000700U // 2-Way ACQ Frame, Management Frame, Service Frame
#define PHY_MODE_CTRL_FRAME_1M2BPS_5MS 0x00000800U // Bistatic Synthetic Aperture Radar Frame
/**
* Register: POLLTIME_L
*/
#define POLLTIME_L 0x00000008U
#define POLLTIME_L_SHIFT 0
#define POLLTIME_L_WIDTH 32
#define POLLTIME_L_MASK 0xFFFFFFFFU
#define POLLTIME_L 0x00000008U
#define POLLTIME_L_SHIFT 0
#define POLLTIME_L_WIDTH 32
#define POLLTIME_L_MASK 0xFFFFFFFFU
/**
* Register: POLLTIME_H
*/
#define POLLTIME_H 0x0000000CU
#define POLLTIME_H_SHIFT 0
#define POLLTIME_H_WIDTH 32
#define POLLTIME_H_MASK 0xFFFFFFFFU
#define POLLTIME_H 0x0000000CU
#define POLLTIME_H_SHIFT 0
#define POLLTIME_H_WIDTH 32
#define POLLTIME_H_MASK 0xFFFFFFFFU
/**
* Register: TM_ADJ_MODE
*/
#define TM_ADJ_MODE 0x00000010U
#define TM_ADJ_MODE_SHIFT 0
#define TM_ADJ_MODE_WIDTH 8
#define TM_ADJ_MODE_MASK 0x000000FFU
#define TM_ADJ_MODE_NONE 0x00000000U
#define TM_ADJ_MODE_ADD 0x00000001U
#define TM_ADJ_MODE_SET 0x00000002U
#define TM_ADJ_MODE 0x00000010U
#define TM_ADJ_MODE_SHIFT 0
#define TM_ADJ_MODE_WIDTH 8
#define TM_ADJ_MODE_MASK 0x000000FFU
#define TM_ADJ_MODE_NONE 0x00000000U
#define TM_ADJ_MODE_ADD 0x00000001U
#define TM_ADJ_MODE_SET 0x00000002U
#define TM_ADJ_MODE_LOCAL_SHIFT 8
#define TM_ADJ_MODE_LOCAL_WIDTH 8
#define TM_ADJ_MODE_LOCAL_MASK 0x0000FF00U
#define TM_ADJ_MODE_LOCAL_NONE 0x00000000U
#define TM_ADJ_MODE_LOCAL_ADD 0x00000100U
#define TM_ADJ_MODE_LOCAL_SET 0x00000200U
#define TM_ADJ_MODE_LOCAL_SHIFT 8
#define TM_ADJ_MODE_LOCAL_WIDTH 8
#define TM_ADJ_MODE_LOCAL_MASK 0x0000FF00U
#define TM_ADJ_MODE_LOCAL_NONE 0x00000000U
#define TM_ADJ_MODE_LOCAL_ADD 0x00000100U
#define TM_ADJ_MODE_LOCAL_SET 0x00000200U
/**
* Register: LOCALTM_ADJ_L
*/
#define LOCALTM_ADJ_L 0x00000014U
#define LOCALTM_ADJ_L_SHIFT 0
#define LOCALTM_ADJ_L_WIDTH 32
#define LOCALTM_ADJ_L_MASK 0xFFFFFFFFU
#define LOCALTM_ADJ_L 0x00000014U
#define LOCALTM_ADJ_L_SHIFT 0
#define LOCALTM_ADJ_L_WIDTH 32
#define LOCALTM_ADJ_L_MASK 0xFFFFFFFFU
/**
* Register: LOCALTM_ADJ_H
*/
#define LOCALTM_ADJ_H 0x00000018U
#define LOCALTM_ADJ_H_SHIFT 0
#define LOCALTM_ADJ_H_WIDTH 32
#define LOCALTM_ADJ_H_MASK 0xFFFFFFFFU
#define LOCALTM_ADJ_H 0x00000018U
#define LOCALTM_ADJ_H_SHIFT 0
#define LOCALTM_ADJ_H_WIDTH 32
#define LOCALTM_ADJ_H_MASK 0xFFFFFFFFU
/**
* Register: CNT_STEP_TOPT
*/
#define CNT_STEP_OPT 0x0000001CU
#define CNT_STEP_OPT_SHIFT 0
#define CNT_STEP_OPT_WIDTH 32
#define CNT_STEP_OPT_MASK 0xFFFFFFFFU
#define CNT_STEP_OPT 0x0000001CU
#define CNT_STEP_OPT_SHIFT 0
#define CNT_STEP_OPT_WIDTH 32
#define CNT_STEP_OPT_MASK 0xFFFFFFFFU
/**
* Register: SEND_DATA_CMD
*/
#define SEND_DATA_CMD 0x00000020U
#define SEND_DATA_CMD_LEN_SHIFT 0
#define SEND_DATA_CMD_LEN_WIDTH 16
#define SEND_DATA_CMD_LEN_MASK 0x0000FFFFU
#define SEND_DATA_CMD_OFFSET_SHIFT 16
#define SEND_DATA_CMD_OFFSET_WIDTH 16
#define SEND_DATA_CMD_OFFSET_MASK 0xFFFF0000U
#define SEND_DATA_CMD 0x00000020U
#define SEND_DATA_CMD_LEN_SHIFT 0
#define SEND_DATA_CMD_LEN_WIDTH 16
#define SEND_DATA_CMD_LEN_MASK 0x0000FFFFU
#define SEND_DATA_CMD_OFFSET_SHIFT 16
#define SEND_DATA_CMD_OFFSET_WIDTH 16
#define SEND_DATA_CMD_OFFSET_MASK 0xFFFF0000U
/**
* Register: ANT_CONFIG_CMD
*/
#define ANT_CONFIG_CMD 0x00000024U
#define ANT_CONFIG_CMD_OFFSET_SHIFT 0
#define ANT_CONFIG_CMD_OFFSET_WIDTH 16
#define ANT_CONFIG_CMD_OFFSET_MASK 0x0000FFFFU
#define ANT_CONFIG_CMD_LEN_SHIFT 16
#define ANT_CONFIG_CMD_LEN_WIDTH 8
#define ANT_CONFIG_CMD_LEN_MASK 0x00FF0000U
#define ANT_CONFIG_CMD_NUM_SHIFT 24
#define ANT_CONFIG_CMD_NUM_WIDTH 8
#define ANT_CONFIG_CMD_NUM_MASK 0xFF000000U
#define ANT_CONFIG_CMD 0x00000024U
#define ANT_CONFIG_CMD_OFFSET_SHIFT 0
#define ANT_CONFIG_CMD_OFFSET_WIDTH 16
#define ANT_CONFIG_CMD_OFFSET_MASK 0x0000FFFFU
#define ANT_CONFIG_CMD_LEN_SHIFT 16
#define ANT_CONFIG_CMD_LEN_WIDTH 8
#define ANT_CONFIG_CMD_LEN_MASK 0x00FF0000U
#define ANT_CONFIG_CMD_NUM_SHIFT 24
#define ANT_CONFIG_CMD_NUM_WIDTH 8
#define ANT_CONFIG_CMD_NUM_MASK 0xFF000000U
/**
* Register: SLOT_CONFIG_CMD
*/
#define SLOT_CONFIG_CMD 0x00000028U
#define SLOT_CONFIG_CMD_SHIFT 0
#define SLOT_CONFIG_CMD_WIDTH 32
#define SLOT_CONFIG_CMD_MASK 0xFFFFFFFFU
#define SLOT_CONFIG_CMD_VAULE 0x4846414E
#define SLOT_CONFIG_CMD 0x00000028U
#define SLOT_CONFIG_CMD_SHIFT 0
#define SLOT_CONFIG_CMD_WIDTH 32
#define SLOT_CONFIG_CMD_MASK 0xFFFFFFFFU
#define SLOT_CONFIG_CMD_VAULE 0x4846414E
/************************* Ps2Pl Regs PS Read Only ***************************/
/**
* Register: TXRX_STATE
*/
#define TXRX_STATE 0x00000100U
#define TXRX_STATE_ACQ_SHIFT 0
#define TXRX_STATE_ACQ_WIDTH 8
#define TXRX_STATE_ACQ_MASK 0x000000FFU
#define TXRX_STATE_ACQ_SUCCESS 0x00000001U
#define TXRX_STATE_ACQ_FAIL 0x00000002U
#define TXRX_STATE 0x00000100U
#define TXRX_STATE_ACQ_SHIFT 0
#define TXRX_STATE_ACQ_WIDTH 8
#define TXRX_STATE_ACQ_MASK 0x000000FFU
#define TXRX_STATE_ACQ_SUCCESS 0x00000001U
#define TXRX_STATE_ACQ_FAIL 0x00000002U
#define TXRX_STATE_FSYNC_SHIFT 8
#define TXRX_STATE_FSYNC_WIDTH 8
#define TXRX_STATE_FSYNC_MASK 0x0000FF00U
#define TXRX_STATE_FSYNC_SUCCESS 0x00000100U
#define TXRX_STATE_FSYNC_FAIL 0x00000200U
#define TXRX_STATE_FSYNC_SHIFT 8
#define TXRX_STATE_FSYNC_WIDTH 8
#define TXRX_STATE_FSYNC_MASK 0x0000FF00U
#define TXRX_STATE_FSYNC_SUCCESS 0x00000100U
#define TXRX_STATE_FSYNC_FAIL 0x00000200U
#define TXRX_STATE_DECODE_SHIFT 16
#define TXRX_STATE_DECODE_WIDTH 8
#define TXRX_STATE_DECODE_MASK 0x00FF0000U
#define TXRX_STATE_DECODE_SUCCESS 0x00010000U
#define TXRX_STATE_DECODE_FAIL 0x00020000U
#define TXRX_STATE_DECODE_SHIFT 16
#define TXRX_STATE_DECODE_WIDTH 8
#define TXRX_STATE_DECODE_MASK 0x00FF0000U
#define TXRX_STATE_DECODE_SUCCESS 0x00010000U
#define TXRX_STATE_DECODE_FAIL 0x00020000U
/**
* Register: RECV_SATE
*/
#define RECV_DATA_SATE 0x00000104U
#define RECV_DATA_LEN_SHIFT 0
#define RECV_DATA_LEN_WIDTH 16
#define RECV_DATA_LEN_MASK 0x0000FFFFU
#define RECV_DATA_OFFSET_SHIFT 16
#define RECV_DATA_OFFSET_WIDTH 16
#define RECV_DATA_OFFSET_MASK 0xFFFFF0000U
#define RECV_DATA_SATE 0x00000104U
#define RECV_DATA_LEN_SHIFT 0
#define RECV_DATA_LEN_WIDTH 16
#define RECV_DATA_LEN_MASK 0x0000FFFFU
#define RECV_DATA_OFFSET_SHIFT 16
#define RECV_DATA_OFFSET_WIDTH 16
#define RECV_DATA_OFFSET_MASK 0xFFFFF0000U
/**
* Register: FSTIME_L
*/
#define FSTIME_L 0x00000108U
#define FSTIME_L_SHIFT 0
#define FSTIME_L_WIDTH 32
#define FSTIME_L_MASK 0xFFFFFFFFU
#define FSTIME_L 0x00000108U
#define FSTIME_L_SHIFT 0
#define FSTIME_L_WIDTH 32
#define FSTIME_L_MASK 0xFFFFFFFFU
/**
* Register: FSTIME_INT1
*/
#define FSTIME_H 0x0000010CU
#define FSTIME_H_SHIFT 0
#define FSTIME_H_WIDTH 32
#define FSTIME_H_MASK 0xFFFFFFFFU
#define FSTIME_H 0x0000010CU
#define FSTIME_H_SHIFT 0
#define FSTIME_H_WIDTH 32
#define FSTIME_H_MASK 0xFFFFFFFFU
/**
* Register: TM_TARGET_L
*/
#define TM_TARGET_L 0x00000110U
#define TM_TARGET_L_SHIFT 0
#define TM_TARGET_L_WIDTH 32
#define TM_TARGET_L_MASK 0xFFFFFFFFU
#define TM_TARGET_L 0x00000110U
#define TM_TARGET_L_SHIFT 0
#define TM_TARGET_L_WIDTH 32
#define TM_TARGET_L_MASK 0xFFFFFFFFU
/**
* Register: TM_TARGET_H
*/
#define TM_TARGET_H 0x00000114U
#define TM_TARGET_H_SHIFT 0
#define TM_TARGET_H_WIDTH 32
#define TM_TARGET_H_MASK 0xFFFFFFFFU
#define TM_TARGET_H 0x00000114U
#define TM_TARGET_H_SHIFT 0
#define TM_TARGET_H_WIDTH 32
#define TM_TARGET_H_MASK 0xFFFFFFFFU
/**
* Register: LOCALTM_L
*/
#define TM_LOCAL_L 0x00000118U
#define TM_LOCAL_L_SHIFT 0
#define TM_LOCAL_L_WIDTH 32
#define TM_LOCAL_L_MASK 0xFFFFFFFFU
#define TM_LOCAL_L 0x00000118U
#define TM_LOCAL_L_SHIFT 0
#define TM_LOCAL_L_WIDTH 32
#define TM_LOCAL_L_MASK 0xFFFFFFFFU
/**
* Register: LOCALTM_H
*/
#define TM_LOCAL_H 0x0000011CU
#define TM_LOCAL_H_SHIFT 0
#define TM_LOCAL_H_WIDTH 32
#define TM_LOCAL_H_MASK 0xFFFFFFFFU
#define TM_LOCAL_H 0x0000011CU
#define TM_LOCAL_H_SHIFT 0
#define TM_LOCAL_H_WIDTH 32
#define TM_LOCAL_H_MASK 0xFFFFFFFFU
/**
* Register: COASEFREQEST
*/
#define COASE_FREQEST 0x00000120U
#define COASE_FREQEST_SHIFT 0
#define COASE_FREQEST_WIDTH 32
#define COASE_FREQEST_MASK 0xFFFFFFFFU
#define COASE_FREQEST 0x00000120U
#define COASE_FREQEST_SHIFT 0
#define COASE_FREQEST_WIDTH 32
#define COASE_FREQEST_MASK 0xFFFFFFFFU
/**
* Register: INTR_TYPE
*/
#define INTR_TYPE 0x00000124U
#define INTR_TYPE 0x00000124U
/**
* Register: ANT_STATUS_0
*/
#define ANT_STATUS 0x00000128U
#define ANT_STATUS_CODE_SHIFT 0
#define ANT_STATUS_CODE_WIDTH 8
#define ANT_STATUS_CODE_MASK 0x000000FFU
#define ANT_STATUS_NIL_VOLT_SHIFT 8
#define ANT_STATUS_NIL_VOLT_WIDTH 8
#define ANT_STATUS_NIL_VOLT_MASK 0x0000FF00U
#define ANT_STATUS_RF_VOLT_SHIFT 16
#define ANT_STATUS_RF_VOLT_WIDTH 8
#define ANT_STATUS_RF_VOLT_MASK 0x00FF0000U
#define ANT_STATUS_ANT_VOLT_SHIFT 24
#define ANT_STATUS_ANT_VOLT_WIDTH 8
#define ANT_STATUS_ANT_VOLT_MASK 0xFF000000U
#define ANT_STATUS 0x00000128U
#define ANT_STATUS_CODE_SHIFT 0
#define ANT_STATUS_CODE_WIDTH 8
#define ANT_STATUS_CODE_MASK 0x000000FFU
#define ANT_STATUS_NIL_VOLT_SHIFT 8
#define ANT_STATUS_NIL_VOLT_WIDTH 8
#define ANT_STATUS_NIL_VOLT_MASK 0x0000FF00U
#define ANT_STATUS_RF_VOLT_SHIFT 16
#define ANT_STATUS_RF_VOLT_WIDTH 8
#define ANT_STATUS_RF_VOLT_MASK 0x00FF0000U
#define ANT_STATUS_ANT_VOLT_SHIFT 24
#define ANT_STATUS_ANT_VOLT_WIDTH 8
#define ANT_STATUS_ANT_VOLT_MASK 0xFF000000U
/**
* Register: ANT_FRAME_CNT
*/
#define ANT_FRAME_CNT 0x0000012CU
#define ANT_FRAME_CNT_SHIFT 0
#define ANT_FRAME_CNT_WIDTH 32
#define ANT_FRAME_CNT_MASK 0xFFFFFFFFU
#define ANT_FRAME_CNT 0x0000012CU
#define ANT_FRAME_CNT_SHIFT 0
#define ANT_FRAME_CNT_WIDTH 32
#define ANT_FRAME_CNT_MASK 0xFFFFFFFFU
/**
* Register: RF_STATUS
*/
#define RF_STATUS 0x00000130U
#define RF_STATUS_TEMP_SHIFT 0
#define RF_STATUS_TEMP_WIDTH 8
#define RF_STATUS_TEMP_MASK 0x000000FFU
#define RF_STATUS_BDLOCK_SHIFT 8
#define RF_STATUS_BDLOCK_WIDTH 8
#define RF_STATUS_BDLOCK_MASK 0x0000FF00U
#define RF_STATUS_PLLA_SHIFT 16
#define RF_STATUS_PLLA_WIDTH 8
#define RF_STATUS_PLLA_MASK 0x00FF0000U
#define RF_STATUS_PLLB_SHIFT 24
#define RF_STATUS_PLLB_WIDTH 8
#define RF_STATUS_PLLB_MASK 0xFF000000U
#define RF_STATUS 0x00000130U
#define RF_STATUS_TEMP_SHIFT 0
#define RF_STATUS_TEMP_WIDTH 8
#define RF_STATUS_TEMP_MASK 0x000000FFU
#define RF_STATUS_BDLOCK_SHIFT 8
#define RF_STATUS_BDLOCK_WIDTH 8
#define RF_STATUS_BDLOCK_MASK 0x0000FF00U
#define RF_STATUS_PLLA_SHIFT 16
#define RF_STATUS_PLLA_WIDTH 8
#define RF_STATUS_PLLA_MASK 0x00FF0000U
#define RF_STATUS_PLLB_SHIFT 24
#define RF_STATUS_PLLB_WIDTH 8
#define RF_STATUS_PLLB_MASK 0xFF000000U
/**
* Register: RF_MGC
*/
#define RF_MGC 0x00000134U
#define RF_MGC_CH1_SHIFT 0
#define RF_MGC_CH1_WIDTH 4
#define RF_MGC_CH1_MASK 0x0000000FU
#define RF_MGC_CH2_SHIFT 8
#define RF_MGC_CH2_WIDTH 4
#define RF_MGC_CH2_MASK 0x00000F00U
#define RF_MGC_CH3_SHIFT 16
#define RF_MGC_CH3_WIDTH 4
#define RF_MGC_CH3_MASK 0x000F0000U
#define RF_MGC_CH4_SHIFT 24
#define RF_MGC_CH4_WIDTH 4
#define RF_MGC_CH4_MASK 0x0F000000U
#define RF_MGC 0x00000134U
#define RF_MGC_CH1_SHIFT 0
#define RF_MGC_CH1_WIDTH 4
#define RF_MGC_CH1_MASK 0x0000000FU
#define RF_MGC_CH2_SHIFT 8
#define RF_MGC_CH2_WIDTH 4
#define RF_MGC_CH2_MASK 0x00000F00U
#define RF_MGC_CH3_SHIFT 16
#define RF_MGC_CH3_WIDTH 4
#define RF_MGC_CH3_MASK 0x000F0000U
#define RF_MGC_CH4_SHIFT 24
#define RF_MGC_CH4_WIDTH 4
#define RF_MGC_CH4_MASK 0x0F000000U
/**
* Register: RF_FRAME_CNT
*/
#define RF_FRAME_CNT 0x00000138U
#define RF_FRAME_CNT_SHIFT 0
#define RF_FRAME_CNT_WIDTH 32
#define RF_FRAME_CNT_MASK 0xFFFFFFFFU
#define RF_FRAME_CNT 0x00000138U
#define RF_FRAME_CNT_SHIFT 0
#define RF_FRAME_CNT_WIDTH 32
#define RF_FRAME_CNT_MASK 0xFFFFFFFFU
#endif /* PHY_CTRLREG_H_ */
df_datalink/src/system_monitor.c
+111 -79
View File
@@ -4,23 +4,25 @@
#include "common.h"
#include "system_monitor.h"
#define BUFF_A 0x00
#define BUFF_B 0x01
#define BUFF_A 0x00
#define BUFF_B 0x01
#define BUFF_NONE 0xFF
#define SYSINFO_PACK_LEN 54 // bytes
#define PAYLOAD_PACK_LEN 46 // bytes
#define SLOTLOG_PACK_LEN 88 // bytes
#define SYSINFO_PACK_LEN 54 // bytes
#define PAYLOAD_PACK_LEN 46 // bytes
#define SLOTLOG_PACK_LEN 88 // bytes
#define PRTCLLOG_PACK_ELN 24 // bytes
#define HWSTATE_PACK_ELN 12 // bytes
#define HWSTATE_PACK_ELN 12 // bytes
typedef struct{
typedef struct
{
int8_t latest;
int8_t rBusy;
int8_t wBusy;
} PINGPONG;
typedef struct{
typedef struct
{
PINGPONG recvPLCtrl;
PAYLOAD recvPL[2];
PINGPONG sendPLCtrl;
@@ -42,25 +44,29 @@ static SYSMONITOR *sysMonitorInst;
uint8_t GetWritable(PINGPONG ctrllor);
uint8_t GetReadable(PINGPONG ctrllor);
inline uint8_t GetWritable(PINGPONG ctrllor) {
ASSERT(ctrllor.wBusy !=BUFF_NONE);
inline uint8_t GetWritable(PINGPONG ctrllor)
{
ASSERT(ctrllor.wBusy != 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);
else
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)
return ctrllor.latest;
return ctrllor.latest;
else
return ((ctrllor.wBusy == BUFF_B) ? BUFF_A : BUFF_B);
return ((ctrllor.wBusy == BUFF_B) ? BUFF_A : BUFF_B);
}
// System monitor initialise
int32_t SysMonitor_Init(void){
int32_t SysMonitor_Init(void)
{
sysMonitorInst = calloc(1, sizeof(SYSMONITOR));
if (sysMonitorInst == NULL) {
if (sysMonitorInst == NULL)
{
return ENOMEM;
}
memset(&(sysMonitorInst->recvPLCtrl), BUFF_NONE, sizeof(PINGPONG));
@@ -74,7 +80,8 @@ int32_t SysMonitor_Init(void){
}
// @System Info
void SysMonitor_UpdateSysinfo(SYSINFO *info) {
void SysMonitor_UpdateSysinfo(SYSINFO *info)
{
uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->sysInfoCtrl);
sysMonitorInst->sysInfoCtrl.wBusy = buffIndx;
@@ -83,7 +90,8 @@ void SysMonitor_UpdateSysinfo(SYSINFO *info) {
sysMonitorInst->sysInfoCtrl.wBusy = BUFF_NONE;
}
void SysMonitor_UpdateSysinfoRaw(uint8_t *buff){
void SysMonitor_UpdateSysinfoRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->sysInfoCtrl);
@@ -125,26 +133,28 @@ void SysMonitor_UpdateSysinfoRaw(uint8_t *buff){
sysMonitorInst->sysInfoCtrl.wBusy = BUFF_NONE;
}
void SysMonitor_GetSysinfo(SYSINFO *info){
void SysMonitor_GetSysinfo(SYSINFO *info)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->sysInfoCtrl.rBusy;
if (buffIndex_old != BUFF_NONE)
buffIndx = buffIndex_old; // Keep it busy
buffIndx = buffIndex_old; // Keep it busy
else
buffIndx = GetReadable(sysMonitorInst->sysInfoCtrl);
buffIndx = GetReadable(sysMonitorInst->sysInfoCtrl);
sysMonitorInst->sysInfoCtrl.rBusy = buffIndx;
memcpy(info, &(sysMonitorInst->sysInfo[buffIndx]), sizeof(SYSINFO));
sysMonitorInst->sysInfoCtrl.rBusy = buffIndex_old;
}
void SysMonitor_GetSysinfoRaw(uint8_t *buff) {
void SysMonitor_GetSysinfoRaw(uint8_t *buff)
{
uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->sysInfoCtrl.rBusy;
if (buffIndex_old != BUFF_NONE)
buffIndx = buffIndex_old; // Keep it busy
buffIndx = buffIndex_old; // Keep it busy
else
buffIndx = GetReadable(sysMonitorInst->sysInfoCtrl);
buffIndx = GetReadable(sysMonitorInst->sysInfoCtrl);
sysMonitorInst->sysInfoCtrl.rBusy = buffIndx;
// Encapsulate Systeminfo
@@ -186,12 +196,13 @@ void SysMonitor_GetSysinfoRaw(uint8_t *buff) {
/**
* Update send FLJ payload by raw data.
*/
void SysMonitor_UpdateSendPayloadFLJRaw(uint8_t *buff){
void SysMonitor_UpdateSendPayloadFLJRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->sendPLCtrl);
sysMonitorInst->sendPLCtrl.wBusy = buffIndx;
memcpy(sysMonitorInst->sendPL[buffIndx].data_flj, buff, sizeof(uint8_t)*PAYLOAD_FLJ);
memcpy(sysMonitorInst->sendPL[buffIndx].data_flj, buff, sizeof(uint8_t) * PAYLOAD_FLJ);
sysMonitorInst->sendPLCtrl.latest = buffIndx;
sysMonitorInst->sendPLCtrl.wBusy = BUFF_NONE;
}
@@ -199,28 +210,30 @@ void SysMonitor_UpdateSendPayloadFLJRaw(uint8_t *buff){
/**
* Get send FLJ payload as raw data.
*/
void SysMonitor_GetSendPayloadFLJRaw(uint8_t *buff) {
void SysMonitor_GetSendPayloadFLJRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->sendPLCtrl.rBusy;
if (buffIndex_old != BUFF_NONE)
buffIndx = buffIndex_old; // Keep it busy
buffIndx = buffIndex_old; // Keep it busy
else
buffIndx = GetReadable(sysMonitorInst->sendPLCtrl);
buffIndx = GetReadable(sysMonitorInst->sendPLCtrl);
sysMonitorInst->sendPLCtrl.rBusy = buffIndx;
memcpy(buff, sysMonitorInst->sendPL[buffIndx].data_flj, sizeof(uint8_t)*PAYLOAD_FLJ);
memcpy(buff, sysMonitorInst->sendPL[buffIndx].data_flj, sizeof(uint8_t) * PAYLOAD_FLJ);
sysMonitorInst->sendPLCtrl.rBusy = buffIndex_old;
}
/**
* Update LJ send payload by raw data.
*/
void SysMonitor_UpdateSendPayloadLJRaw(uint8_t *buff){
void SysMonitor_UpdateSendPayloadLJRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->sendPLCtrl);
sysMonitorInst->sendPLCtrl.wBusy = buffIndx;
memcpy(sysMonitorInst->sendPL[buffIndx].data_lj, buff, sizeof(uint8_t)*PAYLOAD_LJ);
memcpy(sysMonitorInst->sendPL[buffIndx].data_lj, buff, sizeof(uint8_t) * PAYLOAD_LJ);
sysMonitorInst->sendPLCtrl.latest = buffIndx;
sysMonitorInst->sendPLCtrl.wBusy = BUFF_NONE;
}
@@ -228,16 +241,17 @@ void SysMonitor_UpdateSendPayloadLJRaw(uint8_t *buff){
/**
* Get send LJ payload as raw data.
*/
void SysMonitor_GetSendPayloadLJRaw(uint8_t *buff) {
void SysMonitor_GetSendPayloadLJRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->sendPLCtrl.rBusy;
if (buffIndex_old != BUFF_NONE)
buffIndx = buffIndex_old; // Keep it busy
buffIndx = buffIndex_old; // Keep it busy
else
buffIndx = GetReadable(sysMonitorInst->sendPLCtrl);
buffIndx = GetReadable(sysMonitorInst->sendPLCtrl);
sysMonitorInst->sendPLCtrl.rBusy = buffIndx;
memcpy(buff, sysMonitorInst->sendPL[buffIndx].data_lj, sizeof(uint8_t)*PAYLOAD_LJ);
memcpy(buff, sysMonitorInst->sendPL[buffIndx].data_lj, sizeof(uint8_t) * PAYLOAD_LJ);
sysMonitorInst->sendPLCtrl.rBusy = buffIndex_old;
}
@@ -245,12 +259,13 @@ void SysMonitor_GetSendPayloadLJRaw(uint8_t *buff) {
/**
* Update receive FLJ payload by raw data.
*/
void SysMonitor_UpdateRecvPayloadFLJRaw(uint8_t *buff){
void SysMonitor_UpdateRecvPayloadFLJRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->recvPLCtrl);
sysMonitorInst->recvPLCtrl.wBusy = buffIndx;
memcpy(sysMonitorInst->recvPL[buffIndx].data_flj, buff, sizeof(uint8_t)*PAYLOAD_FLJ);
memcpy(sysMonitorInst->recvPL[buffIndx].data_flj, buff, sizeof(uint8_t) * PAYLOAD_FLJ);
sysMonitorInst->recvPLCtrl.latest = buffIndx;
sysMonitorInst->recvPLCtrl.wBusy = BUFF_NONE;
}
@@ -258,28 +273,30 @@ void SysMonitor_UpdateRecvPayloadFLJRaw(uint8_t *buff){
/**
* Get send FLJ payload as raw data.
*/
void SysMonitor_GetRecvPayloadFLJRaw(uint8_t *buff){
void SysMonitor_GetRecvPayloadFLJRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->recvPLCtrl.rBusy;
if (buffIndex_old != BUFF_NONE)
buffIndx = buffIndex_old; // Keep it busy
buffIndx = buffIndex_old; // Keep it busy
else
buffIndx = GetReadable(sysMonitorInst->recvPLCtrl);
buffIndx = GetReadable(sysMonitorInst->recvPLCtrl);
sysMonitorInst->recvPLCtrl.rBusy = buffIndx;
memcpy(buff, sysMonitorInst->recvPL[buffIndx].data_flj, sizeof(uint8_t)*PAYLOAD_FLJ);
memcpy(buff, sysMonitorInst->recvPL[buffIndx].data_flj, sizeof(uint8_t) * PAYLOAD_FLJ);
sysMonitorInst->recvPLCtrl.rBusy = buffIndex_old;
}
/**
* Update receive LJ payload by raw data.
*/
void SysMonitor_UpdateRecvPayloadLJRaw(uint8_t *buff){
void SysMonitor_UpdateRecvPayloadLJRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->recvPLCtrl);
sysMonitorInst->recvPLCtrl.wBusy = buffIndx;
memcpy(sysMonitorInst->recvPL[buffIndx].data_lj, buff, sizeof(uint8_t)*PAYLOAD_LJ);
memcpy(sysMonitorInst->recvPL[buffIndx].data_lj, buff, sizeof(uint8_t) * PAYLOAD_LJ);
sysMonitorInst->recvPLCtrl.latest = buffIndx;
sysMonitorInst->recvPLCtrl.wBusy = BUFF_NONE;
}
@@ -287,16 +304,17 @@ void SysMonitor_UpdateRecvPayloadLJRaw(uint8_t *buff){
/**
* Get send LJ payload as raw data.
*/
void SysMonitor_GetRecvPayloadLJRaw(uint8_t *buff){
void SysMonitor_GetRecvPayloadLJRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->recvPLCtrl.rBusy;
if (buffIndex_old != BUFF_NONE)
buffIndx = buffIndex_old; // Keep it busy
buffIndx = buffIndex_old; // Keep it busy
else
buffIndx = GetReadable(sysMonitorInst->recvPLCtrl);
buffIndx = GetReadable(sysMonitorInst->recvPLCtrl);
sysMonitorInst->recvPLCtrl.rBusy = buffIndx;
memcpy(buff, sysMonitorInst->recvPL[buffIndx].data_lj, sizeof(uint8_t)*PAYLOAD_LJ);
memcpy(buff, sysMonitorInst->recvPL[buffIndx].data_lj, sizeof(uint8_t) * PAYLOAD_LJ);
sysMonitorInst->recvPLCtrl.rBusy = buffIndex_old;
}
@@ -304,7 +322,8 @@ void SysMonitor_GetRecvPayloadLJRaw(uint8_t *buff){
/**
* Log send slot information
*/
void SysMonitor_UpdateSendSlotLog(SLOTLOG *log){
void SysMonitor_UpdateSendSlotLog(SLOTLOG *log)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->sendLogCtrl);
@@ -317,14 +336,15 @@ void SysMonitor_UpdateSendSlotLog(SLOTLOG *log){
/**
* Get the latest send slot log
*/
void SysMonitor_GetSendSlotLog(SLOTLOG *log){
void SysMonitor_GetSendSlotLog(SLOTLOG *log)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->sendLogCtrl.rBusy;
if (buffIndex_old != BUFF_NONE)
buffIndx = buffIndex_old; // Keep it busy
buffIndx = buffIndex_old; // Keep it busy
else
buffIndx = GetReadable(sysMonitorInst->sendLogCtrl);
buffIndx = GetReadable(sysMonitorInst->sendLogCtrl);
sysMonitorInst->sendLogCtrl.rBusy = buffIndx;
memcpy(log, &(sysMonitorInst->sendLog[buffIndx]), sizeof(SLOTLOG));
sysMonitorInst->sendLogCtrl.rBusy = buffIndex_old;
@@ -333,14 +353,15 @@ void SysMonitor_GetSendSlotLog(SLOTLOG *log){
/**
* Get the latest send slot log as raw data
*/
void SysMonitor_GetSendSlotLogRaw(uint8_t *buff){
void SysMonitor_GetSendSlotLogRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->sendLogCtrl.rBusy;
if (buffIndex_old != BUFF_NONE)
buffIndx = buffIndex_old; // Keep it busy
buffIndx = buffIndex_old; // Keep it busy
else
buffIndx = GetReadable(sysMonitorInst->sendLogCtrl);
buffIndx = GetReadable(sysMonitorInst->sendLogCtrl);
sysMonitorInst->sendLogCtrl.rBusy = buffIndx;
// Encapsulate log
@@ -371,7 +392,8 @@ void SysMonitor_GetSendSlotLogRaw(uint8_t *buff){
/**
* Log receive slot information
*/
void SysMonitor_UpdateRecvSlotLog(SLOTLOG *log){
void SysMonitor_UpdateRecvSlotLog(SLOTLOG *log)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->recvLogCtrl);
@@ -384,14 +406,15 @@ void SysMonitor_UpdateRecvSlotLog(SLOTLOG *log){
/**
* Get the latest receive slot information
*/
void SysMonitor_GetRecvSlotLog(SLOTLOG *log){
void SysMonitor_GetRecvSlotLog(SLOTLOG *log)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->recvLogCtrl.rBusy;
if (buffIndex_old != BUFF_NONE)
buffIndx = buffIndex_old; // Keep it busy
buffIndx = buffIndex_old; // Keep it busy
else
buffIndx = GetReadable(sysMonitorInst->recvLogCtrl);
buffIndx = GetReadable(sysMonitorInst->recvLogCtrl);
sysMonitorInst->recvLogCtrl.rBusy = buffIndx;
memcpy(log, &(sysMonitorInst->recvLog[buffIndx]), sizeof(SLOTLOG));
sysMonitorInst->recvLogCtrl.rBusy = buffIndex_old;
@@ -400,14 +423,15 @@ void SysMonitor_GetRecvSlotLog(SLOTLOG *log){
/**
* Get the latest receive slot information as raw data
*/
void SysMonitor_GetRecvSlotLogRaw(uint8_t *buff) {
void SysMonitor_GetRecvSlotLogRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->recvLogCtrl.rBusy;
if (buffIndex_old != BUFF_NONE)
buffIndx = buffIndex_old; // Keep it busy
buffIndx = buffIndex_old; // Keep it busy
else
buffIndx = GetReadable(sysMonitorInst->recvLogCtrl);
buffIndx = GetReadable(sysMonitorInst->recvLogCtrl);
sysMonitorInst->recvLogCtrl.rBusy = buffIndx;
// Encapsulate log
@@ -438,7 +462,8 @@ void SysMonitor_GetRecvSlotLogRaw(uint8_t *buff) {
/**
* Log protocol running status.
*/
void SysMonitor_UpdateProtocolLog(PRTCLLOG *log){
void SysMonitor_UpdateProtocolLog(PRTCLLOG *log)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->protocolLogCtrl);
@@ -451,14 +476,15 @@ void SysMonitor_UpdateProtocolLog(PRTCLLOG *log){
/**
* Get the protocol running log.
*/
void SysMonitor_GetProtocolLog(PRTCLLOG *log){
void SysMonitor_GetProtocolLog(PRTCLLOG *log)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->protocolLogCtrl.rBusy;
if (buffIndex_old != BUFF_NONE)
buffIndx = buffIndex_old; // Keep it busy
buffIndx = buffIndex_old; // Keep it busy
else
buffIndx = GetReadable(sysMonitorInst->protocolLogCtrl);
buffIndx = GetReadable(sysMonitorInst->protocolLogCtrl);
sysMonitorInst->protocolLogCtrl.rBusy = buffIndx;
memcpy(log, &(sysMonitorInst->protocolLog[buffIndx]), sizeof(PRTCLLOG));
sysMonitorInst->protocolLogCtrl.rBusy = buffIndex_old;
@@ -467,14 +493,15 @@ void SysMonitor_GetProtocolLog(PRTCLLOG *log){
/**
* Get the protocol running log as raw data.
*/
void SysMonitor_GetProtocolLogRaw(uint8_t *buff){
void SysMonitor_GetProtocolLogRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->protocolLogCtrl.rBusy;
if (buffIndex_old != BUFF_NONE)
buffIndx = buffIndex_old; // Keep it busy
buffIndx = buffIndex_old; // Keep it busy
else
buffIndx = GetReadable(sysMonitorInst->protocolLogCtrl);
buffIndx = GetReadable(sysMonitorInst->protocolLogCtrl);
sysMonitorInst->protocolLogCtrl.rBusy = buffIndx;
// Encapsulate log
@@ -507,7 +534,8 @@ void SysMonitor_GetProtocolLogRaw(uint8_t *buff){
/**
* Log hardware running status.
*/
void SysMonitor_UpdateHardwareStatus(HWSTATE *state){
void SysMonitor_UpdateHardwareStatus(HWSTATE *state)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx;
buffIndx = GetWritable(sysMonitorInst->hwStateCtrl);
@@ -520,14 +548,15 @@ void SysMonitor_UpdateHardwareStatus(HWSTATE *state){
/**
* Get the latest hardware running status.
*/
void SysMonitor_GetHardwareStatus(HWSTATE *state){
void SysMonitor_GetHardwareStatus(HWSTATE *state)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->hwStateCtrl.rBusy;
if (buffIndex_old != BUFF_NONE)
buffIndx = buffIndex_old; // Keep it busy
buffIndx = buffIndex_old; // Keep it busy
else
buffIndx = GetReadable(sysMonitorInst->hwStateCtrl);
buffIndx = GetReadable(sysMonitorInst->hwStateCtrl);
sysMonitorInst->hwStateCtrl.rBusy = buffIndx;
memcpy(state, &(sysMonitorInst->hwState[buffIndx]), sizeof(HWSTATE));
sysMonitorInst->hwStateCtrl.rBusy = buffIndex_old;
@@ -536,14 +565,15 @@ void SysMonitor_GetHardwareStatus(HWSTATE *state){
/**
* Get the latest hardware running status as raw data.
*/
void SysMonitor_GetHardwareStatusRaw(uint8_t *buff){
void SysMonitor_GetHardwareStatusRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL);
uint8_t buffIndx, buffIndex_old;
buffIndex_old = sysMonitorInst->hwStateCtrl.rBusy;
if (buffIndex_old != BUFF_NONE)
buffIndx = buffIndex_old; // Keep it busy
buffIndx = buffIndex_old; // Keep it busy
else
buffIndx = GetReadable(sysMonitorInst->hwStateCtrl);
buffIndx = GetReadable(sysMonitorInst->hwStateCtrl);
sysMonitorInst->hwStateCtrl.rBusy = buffIndx;
// Encapsulate state
@@ -570,7 +600,8 @@ void SysMonitor_GetHardwareStatusRaw(uint8_t *buff){
/**
* Get the latest system running log.
*/
void SysMonitor_GetSyslog(SYSLOG *log){
void SysMonitor_GetSyslog(SYSLOG *log)
{
ASSERT(sysMonitorInst != NULL);
SysMonitor_GetSendSlotLog(&(log->sendLog));
SysMonitor_GetRecvSlotLog(&(log->recvLog));
@@ -581,7 +612,8 @@ void SysMonitor_GetSyslog(SYSLOG *log){
/**
* Get the latest system running log as raw data.
*/
void SysMonitor_GetSyslogRaw(uint8_t *buff){
void SysMonitor_GetSyslogRaw(uint8_t *buff)
{
ASSERT(sysMonitorInst != NULL);
SysMonitor_GetSendSlotLogRaw(buff);
buff += SLOTLOG_PACK_LEN;
df_datalink/src/system_monitor.h
+17 -12
View File
@@ -1,19 +1,19 @@
#ifndef __SYSTEM_MONITOR_H_
#define __SYSTEM_MONITOR_H_
#include <stdint.h>
#include <stdbool.h>
#define PAYLOAD_FLJ 78
#define PAYLOAD_LJ 156
# define PAYLOAD_FLJ 78
# define PAYLOAD_LJ 156
typedef struct{
// FIXME: To Complete
typedef struct
{
// FIXME: To Complete
} SYSCONFIG;
typedef struct{
typedef struct
{
// -- unit: ms, 0~604799999
uint32_t psBDS;
// -- unit: week, 0~0~8191
@@ -51,12 +51,14 @@ typedef struct{
float roll;
} SYSINFO;
typedef struct{
typedef struct
{
uint8_t data_lj[PAYLOAD_LJ];
uint8_t data_flj[PAYLOAD_FLJ];
} PAYLOAD;
typedef struct{
typedef struct
{
// Network log
uint32_t tuNo;
uint8_t slotNo;
@@ -72,7 +74,8 @@ typedef struct{
uint32_t freqDevi;
} SLOTLOG;
typedef struct{
typedef struct
{
// Protocol log
// -- System time
uint32_t tmSys;
@@ -91,7 +94,8 @@ typedef struct{
uint8_t dstId;
} PRTCLLOG;
typedef struct{
typedef struct
{
// Hardware log
// -- Signal processor status
uint16_t spVolt;
@@ -105,7 +109,8 @@ typedef struct{
uint8_t antSt;
} HWSTATE;
typedef struct{
typedef struct
{
// Send slot log
SLOTLOG sendLog;
df_datalink/src/systembus/systembus.c
+128 -93
View File
@@ -13,7 +13,7 @@
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Network
*
******************************************************************************/
******************************************************************************/
/***************************** Include Files *********************************/
@@ -32,13 +32,13 @@
// Common Field
// -- Command word
#define WD_CMD 0x00
#define WD_CMD 0x00
// -- Production type
#define WD_PRD 0x01
#define WD_PRD 0x01
#define PRD_DL_K 0xDD
#define PRD_DL_S 0xDE
#define PRD_ALL 0xFF
#define PRD_ALL 0xFF
// Self-Test
#define CMD_SELF_TEST 0xE1
@@ -46,9 +46,9 @@
// -- -- --
// -- Response
// -- -- Self-Test Result
#define ST_RSP_LEN 16
#define WD_ST_RET 0x02
#define ST_RET_OK 0xAA
#define ST_RSP_LEN 16
#define WD_ST_RET 0x02
#define ST_RET_OK 0xAA
#define ST_RET_ERROR 0x55
// -- -- Signal processor voltage
#define WD_SP_VOLT_L 0x03
@@ -57,21 +57,21 @@
#define WD_SP_TEMP_L 0x05
#define WD_SP_TEMP_H 0x06
// -- -- Signal processor chip status
#define WD_SP_CS_L 0x07
#define WD_SP_CS_H 0x08
#define WD_SP_CS_L 0x07
#define WD_SP_CS_H 0x08
// -- -- Signal processor antenna voltage
#define WD_AS_VOLT 0x09
#define WD_AS_VOLT 0x09
// -- -- Signal processor antenna temperature
#define WD_AS_TEMP 0x0A
#define WD_AS_TEMP 0x0A
// -- -- Signal processor antenna PLL lock status
#define WD_AS_PLL 0x0B
#define WD_AS_PLL 0x0B
// -- -- Signal processor antenna MGC
#define WD_AS_MGC_L 0x0C
#define WD_AS_MGC_H 0x0D
#define WD_AS_MGC_L 0x0C
#define WD_AS_MGC_H 0x0D
// -- -- Signal processor antenna status
#define WD_AS_ST 0x0E.
#define WD_AS_ST 0x0E.
// -- -- Dummy must be 0x00
#define WD_ST_DUMMY 0x0F
#define WD_ST_DUMMY 0x0F
// Data Send Test Command
#define CMD_DATA_TEST_A 0xE2
@@ -127,7 +127,7 @@
// FLJ Poll
#define CMD_FLJ_POLL 0xE5
// -- Request
#define WD_FLJ_POLL 0x02
#define WD_FLJ_POLL 0x02
#define FLJ_POLL_CODE 0x77
// -- Response
#define FLJ_POLL_RESP_LEN 78
@@ -153,22 +153,25 @@
// My Production Type
#define MY_PRD_ID PRD_DL_K
inline bool SystemBus_IsReqPendding(SYSBUS *instPtr){
inline bool SystemBus_IsReqPendding(SYSBUS *instPtr)
{
ASSERT(instPtr != NULL);
return (!isCircQueueEmpty(instPtr->rxQueue));
}
inline bool SystemBus_IsRsqPendding(SYSBUS *instPtr){
inline bool SystemBus_IsRsqPendding(SYSBUS *instPtr)
{
ASSERT(instPtr != NULL);
return (!isCircQueueEmpty(instPtr->txQueue));
}
inline bool SystemBus_IsTxBusy(SYSBUS *instPtr) {
inline bool SystemBus_IsTxBusy(SYSBUS *instPtr)
{
ASSERT(instPtr != NULL);
return (!instPtr->isTxDone);
}
inline bool SystemBus_IsRxBusy(SYSBUS *instPtr) {
inline bool SystemBus_IsRxBusy(SYSBUS *instPtr)
{
ASSERT(instPtr != NULL);
return SysbusUart_IsRxBusy(instPtr->uartInst);
}
@@ -182,14 +185,16 @@ inline bool SystemBus_IsRxBusy(SYSBUS *instPtr) {
*
* @return None.
*
******************************************************************************/
int32_t SystemBus_Init(SYSBUS **instPtr, SYSBUS_CONFIG *config) {
******************************************************************************/
int32_t SystemBus_Init(SYSBUS **instPtr, SYSBUS_CONFIG *config)
{
ASSERT(instPtr != NULL);
ASSERT(config != NULL);
SYSBUS_UART_CONFIG uartConfig;
int32_t Status;
(*instPtr) = malloc(sizeof(SYSBUS));
if ((*instPtr) == NULL) {
if ((*instPtr) == NULL)
{
return FAILURE;
}
(*instPtr)->rxQueue = NULL;
@@ -198,20 +203,23 @@ int32_t SystemBus_Init(SYSBUS **instPtr, SYSBUS_CONFIG *config) {
memcpy(&((*instPtr)->config), config, sizeof(SYSBUS_CONFIG));
uartConfig.baseAddr = config->baseAddr;
Status = SysbusUart_Init(&((*instPtr)->uartInst), &uartConfig);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
Status = ENOMEM;
goto sysbus_init_error;
}
if(CircQueueCreate(&((*instPtr)->txQueue), (*instPtr)->config.txQueueMax) == false){
if (CircQueueCreate(&((*instPtr)->txQueue), (*instPtr)->config.txQueueMax) == false)
{
Status = ENOMEM;
goto sysbus_init_error;
}
if(CircQueueCreate(&((*instPtr)->rxQueue), (*instPtr)->config.rxQueueMax) == false){
if (CircQueueCreate(&((*instPtr)->rxQueue), (*instPtr)->config.rxQueueMax) == false)
{
Status = ENOMEM;
goto sysbus_init_error;
}
(*instPtr)->isRxDone = false; // nothing received.
(*instPtr)->isTxDone = true; // nothing to be sent.
(*instPtr)->isTxDone = true; // nothing to be sent.
return SUCCESS;
sysbus_init_error:
@@ -231,32 +239,38 @@ sysbus_init_error:
*
* @return None
*
******************************************************************************/
int32_t SystemBus_HandleCmd(SYSBUS* instPtr) {
******************************************************************************/
int32_t SystemBus_HandleCmd(SYSBUS *instPtr)
{
ASSERT(instPtr != NULL);
int32_t Status;
SYSBUS_UART_FRAME *sysbusReq, *sysbusRsp;
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");
return EIGNORE;
}
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");
return EIGNORE;
}
switch(data[WD_CMD]){
case CMD_SELF_TEST : {
switch (data[WD_CMD])
{
case CMD_SELF_TEST:
{
sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
sysbusRsp->len = ST_RSP_LEN;
memset(sysbusRsp->data,0x00,ST_RSP_LEN);
memset(sysbusRsp->data, 0x00, ST_RSP_LEN);
sysbusRsp->data[WD_CMD] = CMD_SELF_TEST;
sysbusRsp->data[WD_PRD] = MY_PRD_ID;
// FIXME: run self-test and insert result
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");
Status = EQUEUE;
break;
@@ -264,11 +278,13 @@ int32_t SystemBus_HandleCmd(SYSBUS* instPtr) {
Status = SUCCESS;
break;
}
case CMD_DATA_TEST_A : {
case CMD_DATA_TEST_A:
{
sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
sysbusRsp->len = 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");
Status = EQUEUE;
break;
@@ -276,11 +292,13 @@ int32_t SystemBus_HandleCmd(SYSBUS* instPtr) {
Status = SUCCESS;
break;
}
case CMD_DATA_TEST_B : {
case CMD_DATA_TEST_B:
{
sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
sysbusRsp->len = 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");
Status = EQUEUE;
break;
@@ -288,7 +306,8 @@ int32_t SystemBus_HandleCmd(SYSBUS* instPtr) {
Status = SUCCESS;
break;
}
case CMD_CONFIG : {
case CMD_CONFIG:
{
// FIXME: apply configurations
// Power + Mode
// -- No response
@@ -296,22 +315,27 @@ int32_t SystemBus_HandleCmd(SYSBUS* instPtr) {
Status = SUCCESS;
break;
}
case CMD_SYS_INFO : {
SysMonitor_UpdateSysinfoRaw(data+2);
case CMD_SYS_INFO:
{
SysMonitor_UpdateSysinfoRaw(data + 2);
Status = SUCCESS;
break;
}
// TODO
case CMD_FLJ_SEND : {
SysMonitor_UpdateSendPayloadFLJRaw(data+2); // ommit 2 bytes control info
case CMD_FLJ_SEND:
{
SysMonitor_UpdateSendPayloadFLJRaw(data + 2); // ommit 2 bytes control info
break;
}
case CMD_LJ_SEND : {
SysMonitor_UpdateSendPayloadLJRaw(data+2); // ommit 2 bytes control info
case CMD_LJ_SEND:
{
SysMonitor_UpdateSendPayloadLJRaw(data + 2); // ommit 2 bytes control info
break;
}
case CMD_FLJ_POLL : {
if (data[WD_FLJ_POLL] != FLJ_POLL_CODE) {
case CMD_FLJ_POLL:
{
if (data[WD_FLJ_POLL] != FLJ_POLL_CODE)
{
DBG_WARNING("SystemBus::Invalid roll call code!\n");
Status = EIGNORE;
break;
@@ -321,28 +345,31 @@ int32_t SystemBus_HandleCmd(SYSBUS* instPtr) {
sysbusRsp->data[WD_CMD] = WD_FLJ_POLL;
sysbusRsp->data[WD_PRD] = MY_PRD_ID;
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");
Status = EQUEUE;
break;
}
//FIXME: Confirm
// sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
// sysbusRsp->len = RM_TEST_LEN;
// sysbusRsp->data[WD_CMD] = CMD_RM_TEST;
// sysbusRsp->data[WD_PRD] = MY_PRD_ID;
// SysMonitor_GetSyslogRaw(sysbusRsp->data + 2); //ommit 2 bytes control info
// if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false){
// DBG_WARNING("SystemBus::TX Queue overflowed!\n");
// Status = EQUEUE;
// break;
// }
// FIXME: Confirm
// sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
// sysbusRsp->len = RM_TEST_LEN;
// sysbusRsp->data[WD_CMD] = CMD_RM_TEST;
// sysbusRsp->data[WD_PRD] = MY_PRD_ID;
// SysMonitor_GetSyslogRaw(sysbusRsp->data + 2); //ommit 2 bytes control info
// if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false){
// DBG_WARNING("SystemBus::TX Queue overflowed!\n");
// Status = EQUEUE;
// break;
// }
Status = SUCCESS;
break;
}
case CMD_LJ_POLL : {
if (data[WD_LJ_POLL] != LJ_POLL_CODE) {
case CMD_LJ_POLL:
{
if (data[WD_LJ_POLL] != LJ_POLL_CODE)
{
DBG_WARNING("SystemBus::Invalid roll call code!\n");
Status = EIGNORE;
break;
@@ -352,48 +379,49 @@ int32_t SystemBus_HandleCmd(SYSBUS* instPtr) {
sysbusRsp->data[WD_CMD] = WD_LJ_POLL;
sysbusRsp->data[WD_PRD] = MY_PRD_ID;
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");
Status = EQUEUE;
break;
}
//FIXME: Confirm
// sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
// sysbusRsp->len = RM_TEST_LEN;
// sysbusRsp->data[WD_CMD] = CMD_RM_TEST;
// sysbusRsp->data[WD_PRD] = MY_PRD_ID;
// SysMonitor_GetSyslogRaw(sysbusRsp->data + 2); //ommit 2 bytes control info
// if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false){
// DBG_WARNING("SystemBus::TX Queue overflowed!\n");
// Status = EQUEUE;
// break;
// }
// FIXME: Confirm
// sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
// sysbusRsp->len = RM_TEST_LEN;
// sysbusRsp->data[WD_CMD] = CMD_RM_TEST;
// sysbusRsp->data[WD_PRD] = MY_PRD_ID;
// SysMonitor_GetSyslogRaw(sysbusRsp->data + 2); //ommit 2 bytes control info
// if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false){
// DBG_WARNING("SystemBus::TX Queue overflowed!\n");
// Status = EQUEUE;
// break;
// }
Status = SUCCESS;
break;
}
default:{
default:
{
DBG_ERROR("SystemBus::Unknown command code!\n");
break;
}
}
free(sysbusReq);
return Status;
return Status;
}
int32_t SystemBus_FLJ_PeriodUpdate(SYSBUS *instPtr){
int32_t SystemBus_FLJ_PeriodUpdate(SYSBUS *instPtr)
{
// TODO
return SUCCESS;
}
int32_t SystemBus_LJ_PeriodUpdate(SYSBUS *instPtr){
int32_t SystemBus_LJ_PeriodUpdate(SYSBUS *instPtr)
{
// TODO
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief This function send a repose frame from system bus tx queue.
@@ -402,12 +430,14 @@ int32_t SystemBus_LJ_PeriodUpdate(SYSBUS *instPtr){
*
* @return None
*
******************************************************************************/
int32_t SystemBus_MakeResp(SYSBUS *instPtr){
******************************************************************************/
int32_t SystemBus_MakeResp(SYSBUS *instPtr)
{
ASSERT(instPtr != NULL);
int32_t Status;
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");
return EIGNORE;
}
@@ -425,8 +455,9 @@ int32_t SystemBus_MakeResp(SYSBUS *instPtr){
*
* @return None
*
******************************************************************************/
int32_t SystemBus_TxIntrHandler(SYSBUS *instPtr){
******************************************************************************/
int32_t SystemBus_TxIntrHandler(SYSBUS *instPtr)
{
ASSERT(instPtr != NULL);
// nothing special...
instPtr->isTxDone = true; // A transmission procedure finished.
@@ -441,21 +472,25 @@ int32_t SystemBus_TxIntrHandler(SYSBUS *instPtr){
*
* @return None
*
******************************************************************************/
int32_t SystemBus_RxIntrHandler(SYSBUS *instPtr){
******************************************************************************/
int32_t SystemBus_RxIntrHandler(SYSBUS *instPtr)
{
ASSERT(instPtr != NULL);
int32_t Status;
SYSBUS_UART_FRAME *sysbusReq;
instPtr->isRxDone = true; // whether reception successes or not, receiving procedure ended.
sysbusReq = malloc(sizeof(SYSBUS_UART_FRAME));
if (sysbusReq == NULL) {
if (sysbusReq == NULL)
{
return ENOMEM;
}
Status = SysbusUart_Recv(instPtr->uartInst, sysbusReq);
if(Status != SUCCESS) {
if (Status != SUCCESS)
{
goto recv_error;
}
if (CircQueuePush(instPtr->rxQueue, (void *)sysbusReq) == false){
if (CircQueuePush(instPtr->rxQueue, (void *)sysbusReq) == false)
{
DBG_WARNING("SystemBus::RX Queue overflowed!\n");
Status = EQUEUE;
goto recv_error;
df_datalink/src/systembus/systembus.h
+6 -6
View File
@@ -11,7 +11,7 @@
*
* Copyright(c) 2021 Beijing Institute of Technology.
*
******************************************************************************/
******************************************************************************/
#ifndef __SYSTEMBUS_H_
#define __SYSTEMBUS_H_
@@ -22,18 +22,19 @@
#include <stdint.h>
#include <stdbool.h>
/************************** Constant Definitions *****************************/
/************************** Data Type Definitions ****************************/
// Definition of SystemBus instance.
typedef struct {
typedef struct
{
uintptr_t baseAddr;
uint32_t txQueueMax;
uint32_t rxQueueMax;
} SYSBUS_CONFIG;
typedef struct {
SYSBUS_CONFIG config;
typedef struct
{
SYSBUS_CONFIG config;
SYSBUS_UART *uartInst;
CIRC_QUEUE *txQueue;
CIRC_QUEUE *rxQueue;
@@ -61,5 +62,4 @@ bool SystemBus_IsRsqPendding(SYSBUS *instPtr);
bool SystemBus_IsTxBusy(SYSBUS *instPtr);
bool SystemBus_IsRxBusy(SYSBUS *instPtr);
#endif //__SYSTEMBUS_H_
df_datalink/src/systembus/systembus_uart.c
+65 -45
View File
@@ -21,32 +21,32 @@
#include "common.h"
// Control and status register
#define MODULE_VERSION 0
#define UART_STATUS 4
#define UART_SEND_CMD 8
#define UART_RECV_CNT 12
#define MODULE_VERSION 0
#define UART_STATUS 4
#define UART_SEND_CMD 8
#define UART_RECV_CNT 12
// Status code
#define UART_STATUS_TX_BUSY 0x00010000
#define UART_STATUS_RX_BUSY 0x01000000
// Data buffer register
#define UART_TXBUFF_LOW 1024
#define UART_TXBUFF_MAX 1024
#define UART_RXBUFF_LOW 2048
#define UART_RXBUFF_MAX 1024
#define UART_TXBUFF_LOW 1024
#define UART_TXBUFF_MAX 1024
#define UART_RXBUFF_LOW 2048
#define UART_RXBUFF_MAX 1024
// Frame Data offsets
#define FRAME_HEAD_LOW 0x5A
#define FRAME_HEAD_HIGH 0x54
#define FRAME_TAIL_LOW 0x5A
#define FRAME_TAIL_HIGH 0xFE
#define FRAME_HEAD_LOW 0x5A
#define FRAME_HEAD_HIGH 0x54
#define FRAME_TAIL_LOW 0x5A
#define FRAME_TAIL_HIGH 0xFE
#define FRAME_HEAD_LEN 2
#define FRAME_TAIL_LEN 2
#define FRAME_DUMMY_LEN 1 // Dummy
#define FRAME_CHECK_LEN 1 // CheckSum
#define FRAME_INFO_LEN 3 // CheckSum, Dummy and DataLen
#define FRAME_OVERHEAD_LEN 7 // CheckSum, Dummy and DataLen
#define FRAME_HEAD_LEN 2
#define FRAME_TAIL_LEN 2
#define FRAME_DUMMY_LEN 1 // Dummy
#define FRAME_CHECK_LEN 1 // CheckSum
#define FRAME_INFO_LEN 3 // CheckSum, Dummy and DataLen
#define FRAME_OVERHEAD_LEN 7 // CheckSum, Dummy and DataLen
/*************************** Function Definitions ****************************/
/*****************************************************************************/
@@ -59,11 +59,13 @@
* @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(config != NULL);
(*instPtr) = malloc(sizeof(SYSBUS_UART));
if ((*instPtr) == NULL){
if ((*instPtr) == NULL)
{
return FAILURE;
}
(*instPtr)->config.baseAddr = config->baseAddr;
@@ -81,36 +83,41 @@ int SysbusUart_Init(SYSBUS_UART **instPtr, SYSBUS_UART_CONFIG *config) {
* @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(frame != NULL);
uint8_t dLen, checkSum;
uint8_t sendLen;
uint8_t *sendBuff;
if (frame->len > UART_TXBUFF_MAX || frame->len == 0) {
if (frame->len > UART_TXBUFF_MAX || frame->len == 0)
{
return FAILURE;
}
sendBuff = malloc(sizeof(uint8_t) * UART_TXBUFF_MAX);
if (sendBuff == NULL) {
if (sendBuff == NULL)
{
return FAILURE;
}
sendBuff[0] = FRAME_HEAD_LOW;
sendBuff[1] = FRAME_HEAD_HIGH;
dLen = 0;
checkSum = 0;
for (int i = 0; i < frame->len - 1; i++) {
if (frame->data[i] == FRAME_TAIL_LOW && frame->data[i+1] == FRAME_TAIL_HIGH) {
for (int i = 0; i < frame->len - 1; i++)
{
if (frame->data[i] == FRAME_TAIL_LOW && frame->data[i + 1] == FRAME_TAIL_HIGH)
{
sendBuff[FRAME_HEAD_LEN + dLen] = 0x00;
dLen ++;
dLen++;
}
sendBuff[FRAME_HEAD_LEN + dLen] = frame->data[i];
checkSum += sendBuff[FRAME_HEAD_LEN + dLen];
dLen ++;
dLen++;
}
// Last Byte
sendBuff[FRAME_HEAD_LEN + dLen] = frame->data[frame->len - 1];
checkSum += sendBuff[FRAME_HEAD_LEN + dLen];
dLen ++;
dLen++;
// Dummy Byte
sendBuff[FRAME_HEAD_LEN + dLen] = 0x00;
// Check Sum
@@ -121,11 +128,12 @@ int SysbusUart_Send(SYSBUS_UART* instPtr, SYSBUS_UART_FRAME *frame) {
sendBuff[FRAME_HEAD_LEN + dLen + 3] = FRAME_TAIL_LOW;
sendBuff[FRAME_HEAD_LEN + dLen + 4] = FRAME_TAIL_HIGH;
sendLen = dLen + FRAME_OVERHEAD_LEN;
if (sendLen > UART_TXBUFF_MAX) {
if (sendLen > UART_TXBUFF_MAX)
{
free(sendBuff);
return FAILURE;
}
memcpy((void *)(instPtr->config.baseAddr + UART_TXBUFF_LOW), sendBuff, (sendLen + sizeof(uint32_t) - 1)/sizeof(uint32_t) * sizeof(uint32_t));
memcpy((void *)(instPtr->config.baseAddr + UART_TXBUFF_LOW), sendBuff, (sendLen + sizeof(uint32_t) - 1) / sizeof(uint32_t) * sizeof(uint32_t));
free(sendBuff);
// Start Sending
SYSTEMBUS_UART_mWriteReg(instPtr->config.baseAddr, UART_SEND_CMD, sendLen);
@@ -142,29 +150,34 @@ int SysbusUart_Send(SYSBUS_UART* instPtr, SYSBUS_UART_FRAME *frame) {
* @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(frame != NULL);
uint8_t dLen, dWLen, plLen, checkSum, checkSumCal;
uint8_t *plBuff, *dBuff;
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;
}
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);
if (dBuff == NULL){
dBuff = malloc(sizeof(uint32_t) * dWLen);
if (dBuff == NULL)
{
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);
plBuff = dBuff + FRAME_HEAD_LEN;
checkSumCal = 0;
checkSum = plBuff[dLen + FRAME_DUMMY_LEN];
plLen = 0;
for(int i = 0; i < dLen; i++){
if (plBuff[i+1] == FRAME_TAIL_LOW && plBuff[i+2] == FRAME_TAIL_HIGH) {
for (int i = 0; i < dLen; i++)
{
if (plBuff[i + 1] == FRAME_TAIL_LOW && plBuff[i + 2] == FRAME_TAIL_HIGH)
{
continue;
}
checkSumCal += plBuff[i];
@@ -174,7 +187,8 @@ int SysbusUart_Recv(SYSBUS_UART* instPtr, SYSBUS_UART_FRAME *frame) {
free(dBuff);
frame->len = plLen;
frame->checkSum = checkSumCal;
if (checkSumCal != checkSum){
if (checkSumCal != checkSum)
{
frame->isValid = false;
return FAILURE;
}
@@ -192,15 +206,18 @@ int SysbusUart_Recv(SYSBUS_UART* instPtr, SYSBUS_UART_FRAME *frame) {
* false - SYSBUS_UART rx channel is idle.
*
******************************************************************************/
bool SysbusUart_IsRxBusy(SYSBUS_UART* instPtr) {
bool SysbusUart_IsRxBusy(SYSBUS_UART *instPtr)
{
ASSERT(instPtr != NULL);
uint32_t Status;
Status = SYSTEMBUS_UART_mReadReg(instPtr->config.baseAddr, UART_STATUS);
if (Status & UART_STATUS_RX_BUSY) {
if (Status & UART_STATUS_RX_BUSY)
{
return true;
}
else {
else
{
return false;
}
}
@@ -215,14 +232,17 @@ bool SysbusUart_IsRxBusy(SYSBUS_UART* instPtr) {
* false - SYSBUS_UART tx channel is idle.
*
******************************************************************************/
bool SysbusUart_IsTxBusy(SYSBUS_UART* instPtr) {
bool SysbusUart_IsTxBusy(SYSBUS_UART *instPtr)
{
ASSERT(instPtr != NULL);
uint32_t Status;
Status = SYSTEMBUS_UART_mReadReg(instPtr->config.baseAddr, UART_STATUS);
if (Status & UART_STATUS_TX_BUSY) {
if (Status & UART_STATUS_TX_BUSY)
{
return true;
}
else {
else
{
return false;
}
}
df_datalink/src/systembus/systembus_uart.h
+8 -5
View File
@@ -38,7 +38,7 @@
*
*/
#define SYSTEMBUS_UART_mWriteReg(baseAddr, offset, data) \
IO_Out32((baseAddr) + (offset), (uint32_t)(data))
IO_Out32((baseAddr) + (offset), (uint32_t)(data))
/**
*
@@ -58,18 +58,21 @@
*
*/
#define SYSTEMBUS_UART_mReadReg(baseAddr, offset) \
IO_In32((baseAddr) + (offset))
IO_In32((baseAddr) + (offset))
/************************** Data Type Definitions ****************************/
typedef struct {
typedef struct
{
uintptr_t baseAddr;
} SYSBUS_UART_CONFIG;
typedef struct {
typedef struct
{
SYSBUS_UART_CONFIG config;
} SYSBUS_UART;
typedef struct {
typedef struct
{
uint8_t len;
uint8_t checkSum;
uint8_t isValid;
df_datalink/src/util/circ_queue.c
+33 -24
View File
@@ -27,7 +27,7 @@
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
******************************************************************************/
******************************************************************************/
#include "circ_queue.h"
#include "common.h"
@@ -35,21 +35,23 @@
#include <stdlib.h>
#include <string.h>
inline bool isCircQueueFull(CIRC_QUEUE *queue){
ASSERT(queue != NULL);
return ((queue)->nelts == (queue)->bounds);
}
inline bool isCircQueueEmpty(CIRC_QUEUE *queue){
ASSERT(queue != NULL);
return ((queue)->nelts == 0);
}
void CircQueueFree(CIRC_QUEUE * queue)
inline bool isCircQueueFull(CIRC_QUEUE *queue)
{
ASSERT(queue != NULL);
if (queue->data) free(queue->data);
return ((queue)->nelts == (queue)->bounds);
}
inline bool isCircQueueEmpty(CIRC_QUEUE *queue)
{
ASSERT(queue != NULL);
return ((queue)->nelts == 0);
}
void CircQueueFree(CIRC_QUEUE *queue)
{
ASSERT(queue != NULL);
if (queue->data)
free(queue->data);
free(queue);
}
@@ -61,15 +63,17 @@ bool CircQueueCreate(CIRC_QUEUE **q, uint32_t queueCapacity)
ASSERT(q != NULL);
CIRC_QUEUE *queue;
queue = malloc(sizeof(CIRC_QUEUE));
if (!queue) {
if (!queue)
{
return false;
}
*q = queue;
memset(queue, 0, sizeof(CIRC_QUEUE));
/* Set all the data in the queue to NULL */
queue->data = malloc(queueCapacity * sizeof(void*));
if(queue->data == NULL){
queue->data = malloc(queueCapacity * sizeof(void *));
if (queue->data == NULL)
{
goto error;
}
queue->bounds = queueCapacity;
@@ -89,13 +93,15 @@ bool CircQueuePush(CIRC_QUEUE *queue, void *data)
{
ASSERT(queue != NULL);
if (isCircQueueFull(queue)) {
if (isCircQueueFull(queue))
{
return false;
}
queue->data[queue->in] = data;
queue->in++;
if (queue->in >= queue->bounds) {
if (queue->in >= queue->bounds)
{
queue->in -= queue->bounds;
}
queue->nelts++;
@@ -106,7 +112,8 @@ bool CircQueuePush(CIRC_QUEUE *queue, void *data)
/**
* Return current number of element in queue.
*/
inline uint32_t CircQueueSize(CIRC_QUEUE *queue) {
inline uint32_t CircQueueSize(CIRC_QUEUE *queue)
{
return queue->nelts;
}
@@ -117,15 +124,17 @@ bool CircQueuePop(CIRC_QUEUE *queue, void **data)
{
ASSERT(queue != NULL);
if (isCircQueueEmpty(queue)) {
return false;
}
if (isCircQueueEmpty(queue))
{
return false;
}
*data = queue->data[queue->out];
queue->nelts--;
queue->out++;
if (queue->out >= queue->bounds) {
if (queue->out >= queue->bounds)
{
queue->out -= queue->bounds;
}
return true;
df_datalink/src/util/common.c
+5 -3
View File
@@ -38,7 +38,9 @@
* @return NONE.
*
******************************************************************************/
void AppAssert(const char * file, int line){
DBG_PRINT("in %s #%4d [assert] .\n",file, line);
while(1);
void AppAssert(const char *file, int line)
{
DBG_PRINT("in %s #%4d [assert] .\n", file, line);
while (1)
;
}
df_datalink/src/util/crc.c
+79 -69
View File
@@ -54,8 +54,7 @@ static const uint16_t crc16_1021_lsb_lut[256] = {
0xE70E, 0xF687, 0xC41C, 0xD595, 0xA12A, 0xB0A3, 0x8238, 0x93B1,
0x6B46, 0x7ACF, 0x4854, 0x59DD, 0x2D62, 0x3CEB, 0x0E70, 0x1FF9,
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
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,
0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41,
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
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,
0x2B, 0x75, 0x97, 0xC9, 0x4A, 0x14, 0xF6, 0xA8,
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
// CRC-16-CCITT G(X)=X16+X12+X5+1
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,
0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
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
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,
0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
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
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,
0x78, 0x49, 0x1A, 0x2B, 0xBC, 0x8D, 0xDE, 0xEF,
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
/* CRC lookup table algorithm
@@ -268,34 +262,36 @@ for(int i = 0; i < sizeof(dataBuff); i++)
* 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;
const uint8_t* crc_lut = NULL;
const uint8_t *crc_lut = NULL;
switch(poly){
switch (poly)
{
#ifdef __CRC_LSB
case 0x31:
crc_lut = crc8_31_lsb_lut;
break;
default:
return FAILURE;
case 0x31:
crc_lut = crc8_31_lsb_lut;
break;
default:
return FAILURE;
#else
case 0x31:
crc_lut = crc8_31_msb_lut;
break;
default:
return FAILURE;
case 0x31:
crc_lut = crc8_31_msb_lut;
break;
default:
return FAILURE;
#endif
}
for(int i = 0; i < dataLen; i++){
crc = crc_lut[(uint8_t)(crc^buffPtr[i])];
for (int i = 0; i < dataLen; i++)
{
crc = crc_lut[(uint8_t)(crc ^ buffPtr[i])];
}
*crcVal = crc;
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief CRC16 generator
@@ -309,35 +305,38 @@ int crc8_gen(uint8_t* buffPtr, uint32_t dataLen, uint16_t poly, uint8_t* crcVal)
* 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;
const uint16_t* crc_lut = NULL;
switch(poly){
const uint16_t *crc_lut = NULL;
switch (poly)
{
#ifdef __CRC_LSB
case 0x8005:
crc_lut = crc16_8005_lsb_lut;
break;
case 0x1021:
crc_lut = crc16_1021_lsb_lut;
break;
default:
return FAILURE;
case 0x8005:
crc_lut = crc16_8005_lsb_lut;
break;
case 0x1021:
crc_lut = crc16_1021_lsb_lut;
break;
default:
return FAILURE;
#else
case 0x8005:
crc_lut = crc16_8005_msb_lut;
break;
case 0x1021:
crc_lut = crc16_1021_msb_lut;
break;
default:
return FAILURE;
case 0x8005:
crc_lut = crc16_8005_msb_lut;
break;
case 0x1021:
crc_lut = crc16_1021_msb_lut;
break;
default:
return FAILURE;
#endif
}
for(int i = 0; i < dataLen; i++){
for (int i = 0; i < dataLen; i++)
{
#ifdef __CRC_LSB
crc = (crc>>8) ^ crc_lut[(uint8_t)(crc^buffPtr[i])];
crc = (crc >> 8) ^ crc_lut[(uint8_t)(crc ^ buffPtr[i])];
#else
crc = (crc<<8) ^ crc_lut[(uint8_t)((crc >> 8)^buffPtr[i])];
crc = (crc << 8) ^ crc_lut[(uint8_t)((crc >> 8) ^ buffPtr[i])];
#endif
}
*crcVal = crc;
@@ -356,30 +355,35 @@ int crc16_gen(uint8_t* buffPtr, uint32_t dataLen, uint16_t poly, uint16_t* crcVa
* @return true
* 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;
uint8_t crc_comp;
uint8_t crc;
if (dataLen == 0) {
return false;
}
if (dataLen == 0)
{
return false;
}
crc_comp = (uint8_t)(*(buffPtr + crcOffset));
Status = crc8_gen(buffPtr, dataLen, poly, &crc);
if(Status == SUCCESS){
if (crc == crc_comp) {
if (Status == SUCCESS)
{
if (crc == crc_comp)
{
return true;
}
else {
else
{
return false;
}
}
else{
else
{
return false;
}
}
/*****************************************************************************/
/**
* @brief CRC16 compare cheker
@@ -392,26 +396,32 @@ bool crc8_check(uint8_t* buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t
* @return true
* 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;
uint16_t crc_comp;
uint16_t crc;
if (dataLen == 0) {
return false;
}
if (dataLen == 0)
{
return false;
}
crc_comp = (uint16_t)(*(buffPtr + crcOffset));
crc_comp |= (uint16_t)((*(buffPtr + crcOffset + 1)) << 8);
Status = crc16_gen(buffPtr, dataLen, poly, &crc);
if(Status == SUCCESS){
if (crc == crc_comp) {
if (Status == SUCCESS)
{
if (crc == crc_comp)
{
return true;
}
else {
else
{
return false;
}
}
else{
else
{
return false;
}
}
df_datalink/src/util/list.c
+101 -87
View File
@@ -1,4 +1,5 @@
/***************************************************************************//**
/*******************************************************************************
//**
* @file list.c
* @brief List library implementation
* @author Mihail Chindris ([email protected])
@@ -53,43 +54,46 @@
* @struct list_elem
* @brief Format of each element of the list
*/
struct list_elem {
struct list_elem
{
/** User data */
void *data;
void *data;
/** Reference to previous element */
struct list_elem *prev;
struct list_elem *prev;
/** Reference to next element */
struct list_elem *next;
struct list_elem *next;
};
/**
* @struct list_iterator
* @brief Structure used to iterate through the list
*/
struct iterator {
struct iterator
{
/** List reference */
struct _list_desc *list;
struct _list_desc *list;
/** Current element reference */
struct list_elem *elem;
struct list_elem *elem;
};
/**
* @struct _list_desc
* @brief List descriptor
*/
struct _list_desc {
struct _list_desc
{
/** Reference to first element in the list */
struct list_elem *first;
struct list_elem *first;
/** Reference to last element in the list*/
struct list_elem *last;
struct list_elem *last;
/** Number of elements in the list */
uint32_t nb_elements;
uint32_t nb_elements;
/** Function used to compare elements */
f_cmp comparator;
f_cmp comparator;
/** Number of current active iterators */
uint32_t nb_iterators;
uint32_t nb_iterators;
/** Internal list iterator */
struct iterator l_it;
struct iterator l_it;
};
/** @brief Default function used to compare element in the list ( \ref f_cmp) */
@@ -106,8 +110,8 @@ static int32_t default_comparator(void *data1, void *data2)
* @return Address of the new element or NULL if allocation fails.
*/
static inline struct list_elem *create_element(void *data,
struct list_elem *prev,
struct list_elem *next)
struct list_elem *prev,
struct list_elem *next)
{
struct list_elem *elem;
@@ -128,11 +132,12 @@ static inline struct list_elem *create_element(void *data,
* @param next - High element
*/
static inline void update_links(struct list_elem *prev, struct list_elem *elem,
struct list_elem *next)
struct list_elem *next)
{
if (prev)
prev->next = elem ? elem : next;
if (elem) {
if (elem)
{
elem->prev = prev;
elem->next = next;
}
@@ -147,14 +152,17 @@ static inline void update_links(struct list_elem *prev, struct list_elem *elem,
* @param new_last - New last element
*/
static inline void update_desc(struct _list_desc *list,
struct list_elem *new_first,
struct list_elem *new_last)
struct list_elem *new_first,
struct list_elem *new_last)
{
if (new_first == list->first) {
if (new_first == list->first)
{
list->last = new_last;
if (new_first == NULL || new_last == NULL)
list->first = new_last;
} else { /* if (new_last == list->last) */
}
else
{ /* if (new_last == list->last) */
list->first = new_first;
if (new_last == NULL || new_first == NULL)
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)
{
switch (type) {
switch (type)
{
case LIST_PRIORITY_LIST:
ad->push = list_add_find;
ad->pop = list_get_first;
@@ -206,11 +215,10 @@ static inline void set_adapter(struct list_desc *ad, enum adapter_type type)
* - \ref FAILURE : Otherwise
*/
int32_t list_init(struct list_desc **list_desc, enum adapter_type type,
f_cmp comparator)
f_cmp comparator)
{
struct list_desc *l_desc;
struct _list_desc *list;
struct list_desc *l_desc;
struct _list_desc *list;
if (!list_desc)
return FAILURE;
@@ -218,7 +226,8 @@ int32_t list_init(struct list_desc **list_desc, enum adapter_type type,
if (!l_desc)
return FAILURE;
list = (struct _list_desc *)calloc(1, sizeof(*list));
if (!list) {
if (!list)
{
free(l_desc);
return FAILURE;
}
@@ -246,8 +255,8 @@ int32_t list_init(struct list_desc **list_desc, enum adapter_type type,
*/
int32_t list_remove(struct list_desc *list_desc)
{
void *data;
struct _list_desc *list;
void *data;
struct _list_desc *list;
if (!list_desc)
return FAILURE;
@@ -275,7 +284,7 @@ int32_t list_remove(struct list_desc *list_desc)
*/
int32_t list_get_size(struct list_desc *list_desc, uint32_t *out_size)
{
struct _list_desc *list;
struct _list_desc *list;
if (!list_desc || !out_size)
return FAILURE;
@@ -289,10 +298,10 @@ int32_t list_get_size(struct list_desc *list_desc, uint32_t *out_size)
/** @brief Add element at the begining of the list. Refer to \ref f_add */
int32_t list_add_first(struct list_desc *list_desc, void *data)
{
struct list_elem *prev;
struct list_elem *next;
struct list_elem *elem;
struct _list_desc *list;
struct list_elem *prev;
struct list_elem *next;
struct list_elem *elem;
struct _list_desc *list;
if (!list_desc)
return FAILURE;
@@ -317,10 +326,10 @@ int32_t list_add_first(struct list_desc *list_desc, void *data)
/** @brief Add element at the end of the list. Refer to \ref f_add */
int32_t list_add_last(struct list_desc *list_desc, void *data)
{
struct list_elem *prev;
struct list_elem *next;
struct list_elem *elem;
struct _list_desc *list;
struct list_elem *prev;
struct list_elem *next;
struct list_elem *elem;
struct _list_desc *list;
if (!list_desc)
return FAILURE;
@@ -344,7 +353,7 @@ int32_t list_add_last(struct list_desc *list_desc, void *data)
/** @brief Add element at the specified idx. Refer to \ref f_add */
int32_t list_add_idx(struct list_desc *list_desc, void *data, uint32_t idx)
{
struct _list_desc *list;
struct _list_desc *list;
if (!list_desc)
return FAILURE;
@@ -366,35 +375,37 @@ int32_t list_add_idx(struct list_desc *list_desc, void *data, uint32_t idx)
/** @brief Add element in ascending order. Refer to \ref f_add */
int32_t list_add_find(struct list_desc *list_desc, void *data)
{
struct list_elem *elem;
struct _list_desc *list;
struct list_elem *elem;
struct _list_desc *list;
if (!list_desc)
return FAILURE;
list = list_desc->priv_desc;
/* Based on place iterator */
elem = list->first;
while (elem) {
while (elem)
{
if (0 < list->comparator(elem->data, data))
break;
elem = elem->next;
}
if (elem == NULL) {
if (elem == NULL)
{
list->l_it.elem = list->last;
return iterator_insert(&(list->l_it), data, 1);
} else {
}
else
{
list->l_it.elem = elem;
return iterator_insert(&(list->l_it), data, 0);
}
}
/** @brief Edit the first element of the list. Refer to \ref f_edit */
int32_t list_edit_first(struct list_desc *list_desc, void *new_data)
{
struct _list_desc *list;
struct _list_desc *list;
if (!list_desc)
return FAILURE;
@@ -408,7 +419,7 @@ int32_t list_edit_first(struct list_desc *list_desc, void *new_data)
/** @brief Edit the last element of the list. Refer to \ref f_edit */
int32_t list_edit_last(struct list_desc *list_desc, void *new_data)
{
struct _list_desc *list;
struct _list_desc *list;
if (!list_desc)
return FAILURE;
@@ -422,7 +433,7 @@ int32_t list_edit_last(struct list_desc *list_desc, void *new_data)
/** @brief Edit the element at the specified idx. Refer to \ref f_edit */
int32_t list_edit_idx(struct list_desc *list_desc, void *new_data, uint32_t idx)
{
struct _list_desc *list;
struct _list_desc *list;
if (!list_desc)
return FAILURE;
@@ -437,9 +448,9 @@ int32_t list_edit_idx(struct list_desc *list_desc, void *new_data, uint32_t idx)
/** @brief Edit the element which match with cmp_data. Refer to \ref f_edit */
int32_t list_edit_find(struct list_desc *list_desc, void *new_data,
void *cmp_data)
void *cmp_data)
{
struct _list_desc *list;
struct _list_desc *list;
if (!list_desc)
return FAILURE;
@@ -455,7 +466,7 @@ int32_t list_edit_find(struct list_desc *list_desc, void *new_data,
/** @brief Read the first element of the list. Refer to \ref f_read */
int32_t list_read_first(struct list_desc *list_desc, void **data)
{
struct _list_desc *list;
struct _list_desc *list;
if (!list_desc || !data)
return FAILURE;
@@ -473,7 +484,7 @@ int32_t list_read_first(struct list_desc *list_desc, void **data)
/** @brief Read the last element of the list. Refer to \ref f_read */
int32_t list_read_last(struct list_desc *list_desc, void **data)
{
struct _list_desc *list;
struct _list_desc *list;
if (!list_desc || !data)
return FAILURE;
@@ -491,7 +502,7 @@ int32_t list_read_last(struct list_desc *list_desc, void **data)
/** @brief Read the element at the specified idx. Refer to \ref f_read */
int32_t list_read_idx(struct list_desc *list_desc, void **data, uint32_t idx)
{
struct _list_desc *list;
struct _list_desc *list;
if (!list_desc || !data)
return FAILURE;
@@ -513,9 +524,9 @@ int32_t list_read_idx(struct list_desc *list_desc, void **data, uint32_t idx)
/** @brief Read the element which match with cmp_data. Refer to \ref f_read */
int32_t list_read_find(struct list_desc *list_desc, void **data,
void *cmp_data)
void *cmp_data)
{
struct _list_desc *list;
struct _list_desc *list;
if (!list_desc || !data)
return FAILURE;
@@ -536,10 +547,10 @@ int32_t list_read_find(struct list_desc *list_desc, void **data,
/** @brief Read and delete the first element of the list. Refer to \ref f_get */
int32_t list_get_first(struct list_desc *list_desc, void **data)
{
struct list_elem *prev;
struct list_elem *next;
struct list_elem *elem;
struct _list_desc *list;
struct list_elem *prev;
struct list_elem *next;
struct list_elem *elem;
struct _list_desc *list;
if (!list_desc || !data)
return FAILURE;
@@ -566,10 +577,10 @@ int32_t list_get_first(struct list_desc *list_desc, void **data)
/** @brief Read and delete the last element of the list. Refer to \ref f_get */
int32_t list_get_last(struct list_desc *list_desc, void **data)
{
struct list_elem *prev;
struct list_elem *next;
struct list_elem *elem;
struct _list_desc *list;
struct list_elem *prev;
struct list_elem *next;
struct list_elem *elem;
struct _list_desc *list;
if (!list_desc || !data)
return FAILURE;
@@ -596,7 +607,7 @@ int32_t list_get_last(struct list_desc *list_desc, void **data)
/** @brief Read and delete the element at idx. Refer to \ref f_get */
int32_t list_get_idx(struct list_desc *list_desc, void **data, uint32_t idx)
{
struct _list_desc *list;
struct _list_desc *list;
if (!list_desc || !data)
return FAILURE;
@@ -616,7 +627,7 @@ int32_t list_get_idx(struct list_desc *list_desc, void **data, uint32_t idx)
*/
int32_t list_get_find(struct list_desc *list_desc, void **data, void *cmp_data)
{
struct _list_desc *list;
struct _list_desc *list;
if (!list_desc || !data)
return FAILURE;
@@ -641,9 +652,9 @@ int32_t list_get_find(struct list_desc *list_desc, void **data, void *cmp_data)
* - \ref FAILURE : Otherwise
*/
int32_t iterator_init(struct iterator **iter, struct list_desc *list_desc,
bool start)
bool start)
{
struct iterator *it;
struct iterator *it;
if (!list_desc)
return FAILURE;
@@ -693,16 +704,17 @@ int32_t iterator_remove(struct iterator *iter)
*/
int32_t iterator_move(struct iterator *iter, int32_t steps)
{
struct iterator *it = iter;
struct list_elem *elem;
int32_t dir = (steps < 0) ? -1 : 1;
struct iterator *it = iter;
struct list_elem *elem;
int32_t dir = (steps < 0) ? -1 : 1;
if (!it)
return FAILURE;
steps = abs(steps);
elem = it->elem;
while (steps > 0 && elem) {
while (steps > 0 && elem)
{
elem = dir > 0 ? elem->next : elem->prev;
steps--;
}
@@ -725,15 +737,16 @@ int32_t iterator_move(struct iterator *iter, int32_t steps)
*/
int32_t iterator_move_to_idx(struct iterator *iter, int32_t idx)
{
struct list_elem *elem;
int32_t dir = (idx < 0) ? -1 : 1;
struct list_elem *elem;
int32_t dir = (idx < 0) ? -1 : 1;
if (!iter)
return FAILURE;
idx = abs(idx);
elem = dir > 0 ? iter->list->first : iter->list->last;
while (idx > 0 && elem) {
while (idx > 0 && elem)
{
elem = dir > 0 ? elem->next : elem->prev;
idx--;
}
@@ -755,15 +768,17 @@ int32_t iterator_move_to_idx(struct iterator *iter, int32_t idx)
*/
int32_t iterator_find(struct iterator *iter, void *cmp_data)
{
struct iterator *it = iter;
struct list_elem *elem;
struct iterator *it = iter;
struct list_elem *elem;
if (!it)
return FAILURE;
elem = it->list->first;
while (elem) {
if (0 == it->list->comparator(elem->data, cmp_data)) {
while (elem)
{
if (0 == it->list->comparator(elem->data, cmp_data))
{
it->elem = elem;
return SUCCESS;
}
@@ -796,9 +811,8 @@ int32_t iterator_edit(struct iterator *iter, void *new_data)
*/
int32_t iterator_get(struct iterator *iter, void **data)
{
struct iterator *it = iter;
struct list_elem *next;
struct iterator *it = iter;
struct list_elem *next;
if (!it || !it->elem || !data)
return FAILURE;
@@ -845,9 +859,9 @@ int32_t iterator_read(struct iterator *iter, void **data)
*/
int32_t iterator_insert(struct iterator *iter, void *data, bool after)
{
struct iterator *it = iter;
struct list_elem *elem;
struct list_desc list_desc;
struct iterator *it = iter;
struct list_elem *elem;
struct list_desc list_desc;
if (!it)
return FAILURE;
df_datalink/src/util/util.c
+55 -40
View File
@@ -43,20 +43,22 @@ static uint8_t byte_flipLR[256] = {
0x07, 0x87, 0x47, 0xC7, 0x27, 0xA7, 0x67, 0xE7,
0x17, 0x97, 0x57, 0xD7, 0x37, 0xB7, 0x77, 0xF7,
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
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;
*localAddr = value;
}
inline uint32_t IO_In32(uintptr_t addr){
return *(volatile uint32_t*) addr;
inline uint32_t IO_In32(uintptr_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
return byte_flipLR[byte];
#else
@@ -64,38 +66,42 @@ uint8_t byte_bit_swap(uint8_t byte){
#endif
}
uint32_t word_bit_swap(uint32_t word){
uint32_t word_bit_swap(uint32_t word)
{
#ifdef __BITSWAP_TABLE
uint32_t rv_word = (byte_flipLR[word & 0xff] << 24) |
(byte_flipLR[(word >> 8) & 0xff] << 16) |
(byte_flipLR[(word >> 16) & 0xff] << 8) |
(byte_flipLR[(word >> 24) & 0xff]);
(byte_flipLR[(word >> 8) & 0xff] << 16) |
(byte_flipLR[(word >> 16) & 0xff] << 8) |
(byte_flipLR[(word >> 24) & 0xff]);
return rv_word;
#else
word = (((word & 0xaaaaaaaa) >> 1) | ((word & 0x55555555) << 1));
word = (((word & 0xcccccccc) >> 2) | ((word & 0x33333333) << 2));
word = (((word & 0xf0f0f0f0) >> 4) | ((word & 0x0f0f0f0f) << 4));
word = (((word & 0xff00ff00) >> 8) | ((word & 0x00ff00ff) << 8));
return((word >> 16) | (word << 16));
return ((word >> 16) | (word << 16));
#endif
}
uint32_t count_ones(uint32_t word){
uint32_t count_ones(uint32_t word)
{
#ifdef __GNUC__
return __builtin_popcount(word);
#else
uint32_t one_count = 0;
while (word) {
while (word)
{
if (word & 0x00000001)
one_count ++;
one_count++;
word >>= 1;
}
return one_count;
#endif
}
double fix2double(int32_t val, uint8_t frac_bits){
return (((double)val)/(BIT(frac_bits)));
double fix2double(int32_t val, uint8_t frac_bits)
{
return (((double)val) / (BIT(frac_bits)));
}
/**
@@ -104,20 +110,23 @@ double fix2double(int32_t val, uint8_t frac_bits){
uint32_t find_first_set_bit(uint32_t word)
{
#ifdef __GNUC__
if(word != 0){
if (word != 0)
{
return __builtin_ctz(word);
}
else{
else
{
return 32;
}
#else
uint32_t first_set_bit = 0;
while (word) {
while (word)
{
if (word & 0x1)
return first_set_bit;
word >>= 1;
first_set_bit ++;
first_set_bit++;
}
return 32;
@@ -130,21 +139,24 @@ uint32_t find_first_set_bit(uint32_t word)
uint32_t find_last_set_bit(uint32_t word)
{
#ifdef __GNUC__
if(word != 0){
return (31-__builtin_clz(word));
if (word != 0)
{
return (31 - __builtin_clz(word));
}
else{
else
{
return 32;
}
#else
uint32_t bit = 0;
uint32_t last_set_bit = 32;
while (word) {
while (word)
{
if (word & 0x1)
last_set_bit = bit;
word >>= 1;
bit ++;
bit++;
}
return last_set_bit;
@@ -155,15 +167,17 @@ uint32_t find_last_set_bit(uint32_t word)
* Locate the closest element in an array.
*/
uint32_t find_closest(int32_t val,
const int32_t *array,
uint32_t size)
const int32_t *array,
uint32_t size)
{
int32_t diff = abs(array[0] - val);
uint32_t ret = 0;
uint32_t i;
for (i = 1; i < size; i++) {
if (abs(array[i] - val) < diff) {
for (i = 1; i < size; i++)
{
if (abs(array[i] - val) < diff)
{
diff = abs(array[i] - val);
ret = i;
}
@@ -200,7 +214,7 @@ int32_t log_base_2(uint32_t x)
* Find greatest common divisor of the given two numbers.
*/
uint32_t greatest_common_divisor(uint32_t a,
uint32_t b)
uint32_t b)
{
uint32_t div;
uint32_t common_div = 1;
@@ -219,11 +233,11 @@ uint32_t greatest_common_divisor(uint32_t a,
* Calculate best rational approximation for a given fraction.
*/
void rational_best_approximation(uint32_t given_numerator,
uint32_t given_denominator,
uint32_t max_numerator,
uint32_t max_denominator,
uint32_t *best_numerator,
uint32_t *best_denominator)
uint32_t given_denominator,
uint32_t max_numerator,
uint32_t max_denominator,
uint32_t *best_numerator,
uint32_t *best_denominator)
{
uint32_t gcd;
@@ -233,7 +247,8 @@ void rational_best_approximation(uint32_t given_numerator,
*best_denominator = given_denominator / gcd;
if ((*best_numerator > max_numerator) ||
(*best_denominator > max_denominator)) {
(*best_denominator > max_denominator))
{
*best_numerator = 0;
*best_denominator = 0;
}
@@ -246,7 +261,8 @@ uint32_t hweight8(uint32_t word)
{
uint32_t count = 0;
while (word) {
while (word)
{
if (word & 0x1)
count++;
word >>= 1;
@@ -258,8 +274,8 @@ uint32_t hweight8(uint32_t word)
/**
* Calculate the quotient and the remainder of an integer division.
*/
uint64_t do_div(uint64_t* n,
uint64_t base)
uint64_t do_div(uint64_t *n,
uint64_t base)
{
uint64_t mod = 0;
@@ -348,4 +364,3 @@ uint32_t srt_to_uint32(const char *str)
else
return value;
}