0xha0/df_datalink
1
0
Fork 1
Code Issues Pull Requests Packages Projects Releases 4 Wiki Activity

merged datalink814.

Browse Source
This commit is contained in:
Gatsby Van
2024-01-22 19:08:52 +08:00
parent add250cdcb
commit a116e2e27e
47 changed files with 3972 additions and 3110 deletions
Download Patch File Download Diff File Expand all files Collapse all files
df_datalink_sp/src/config/app_config.h
+5 -4
View File
@@ -53,18 +53,19 @@
// Physical Layer Configs
#define PHY_DEVICE_ID 0
#define PHY_BASEADDR XPAR_PHY_BASEADDR
#define PHY_BASEADDR XPAR_PHYC_BASEADDR
#define PHY_BEAM_SET_PRESET
#define PHY_BEAM_FORMING_ENABLE
// -- Antenna Bram Configs
#define PHY_ANTCONFIG_BRAM_DEVICE_ID XPAR_PHY_DATA_INTERFACE_ANT_CONFIG_AXI_BRAM_CTRL_DEVICE_ID
#define PHY_ANTCONFIG_BRAM_DEVICE_ID XPAR_PHY_DATA_INTERFACE_ANT_INFO_BRAM_CTRL_DEVICE_ID
// -- Buffers
#define PHY_TXBUFF_BRAM_DEVICE_ID XPAR_PHY_DATA_INTERFACE_PHY_TX_BUFF_AXI_BRAM_CTRL_DEVICE_ID
#define PHY_RXBUFF_BRAM_DEVICE_ID XPAR_PHY_DATA_INTERFACE_PHY_RX_BUFF_AXI_BRAM_CTRL_DEVICE_ID
#define PHY_TXBUFF_BRAM_DEVICE_ID XPAR_PHY_DATA_INTERFACE_TX_FRAME_BRAM_CTRL_DEVICE_ID
#define PHY_RXBUFF_BRAM_DEVICE_ID XPAR_PHY_DATA_INTERFACE_RX_FRAME_BRAM_CTRL_DEVICE_ID
// -- Xadc
#define PHY_HW_XADC_DEVICE_ID XPAR_PS7_XADC_0_DEVICE_ID
// -- Interrupt id
#define PHY_INTR_ID XPAR_FABRIC_PHY_IRQ_INTR
#define PERIODIC_5MS_INTR_ID XPAR_FABRIC_PHY_5MS_DELAY_2MS_INTR
// System Bus Configs
#define SYSBUS_BASE_ADDR XPAR_SYSTEMBUS_BASEADDR
df_datalink_sp/src/dev_profile/dev_profile.h
-3
View File
@@ -1,9 +1,6 @@
#ifndef _DEV_PROFILE_H_
#define _DEV_PROFILE_H_
#include "spi.h"
#include "i2c.h"
#include "delay.h"
#include "irq.h"
#include "error.h"
#include "../drivers/rf-transceiver/ad9361/ad9361_api.h"
df_datalink_sp/src/dev_profile/intc_profile.c
+1
View File
@@ -2,6 +2,7 @@
#include "irq.h"
#include "../config/app_config.h"
#include "../drivers/platform/xilinx_irq.h"
struct xil_irq_init_param xil_intc_init_param = {
.type = IRQ_PS
};
df_datalink_sp/src/lscript.ld
+3 -3
View File
@@ -23,9 +23,9 @@ _UNDEF_STACK_SIZE = DEFINED(_UNDEF_STACK_SIZE) ? _UNDEF_STACK_SIZE : 1024;
MEMORY
{
phy_data_interface_phy_rx_buff_axi_bram_ctrl_Mem0 : ORIGIN = 0x40000000, LENGTH = 0x1000
phy_data_interface_phy_tx_buff_axi_bram_ctrl_Mem0 : ORIGIN = 0x42000000, LENGTH = 0x1000
phy_data_interface_ant_config_axi_bram_ctrl_Mem0 : ORIGIN = 0x44000000, LENGTH = 0x2000
phy_data_interface_rx_frame_bram_ctrl_Mem0 : ORIGIN = 0x40000000, LENGTH = 0x2000
phy_data_interface_tx_frame_bram_ctrl_Mem0 : ORIGIN = 0x40002000, LENGTH = 0x2000
phy_data_interface_ant_info_bram_ctrl_Mem0 : ORIGIN = 0x40004000, LENGTH = 0x2000
ps7_ddr_0 : ORIGIN = 0x100000, LENGTH = 0x1FF00000
ps7_qspi_linear_0 : ORIGIN = 0xFC000000, LENGTH = 0x1000000
ps7_ram_0 : ORIGIN = 0x0, LENGTH = 0x30000
df_datalink_sp/src/main.c
+106 -27
View File
@@ -42,24 +42,57 @@
/************************** Function Prototypes ******************************/
int32_t SetupInterruptSystem(struct irq_ctrl_desc *descPtr);
void Phy_IntrHandler(void *context);
void PeriodicPull_Handler(void *callBackRef);
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 periodicpull_callback_desc = {
.callback = PeriodicPull_Handler,
.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 L2_CONFIG linkConfig = {
.myType = L2_NODE_HEAD,
.clustId = 1,
.myId = 1,
.headId = 1,
.bkpHeadId = 0,
.netScale = 2,
.phyInstPtr = NULL, // pyhsical layer reference
.tmcInstPtr = NULL
};
// static L2_CONFIG linkConfig = {
// .myType = L2_NODE_MEMBER,
// .clustId = 1,
// .myId = 2,
// .headId = 1,
// .bkpHeadId = 0,
// .netScale = 2,
// .phyInstPtr = NULL, // pyhsical layer reference
// .tmcInstPtr = NULL
// };
static PHY_CONFIG phyConfig = {
.devId = 0,
@@ -67,33 +100,25 @@ 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};
static L2_CONFIG linkConfig = {
.myType = L2_HEAD_NODE,
.myId = 1,
.headId = 1,
.netScale = 2,
.phyInstRef = NULL // pyhsical layer reference
.hwXadcId = PHY_HW_XADC_DEVICE_ID
};
// static L2_CONFIG linkConfig = {
// .myType = L2_MEMBER_NODE,
// .myId = 2,
// .headId = 1,
// .netScale = 2,
// .phyInstRef = NULL // pyhsical layer reference
// };
TIMECTRL_CONFIG tmcConfig = {
.baseAddr = 0, //TIME_CONTROLLER_BASEADDR,
.reset = 10
};
static SYSBUS_CONFIG sysbusConfig = {
.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 timer_desc *logTmPtr;
static SYSBUS *sysbusDescPtr;
static PHY_INST *phyInstPtr;
static TIMECTRL_INST *tmcInstPtr = NULL;
static L2_INST *linkInstPtr;
static struct hmc7044_dev *hmc7044_inst;
@@ -139,7 +164,7 @@ int32_t SetupInterruptSystem(struct irq_ctrl_desc *descPtr)
return Status;
}
// Set highest interrupt priority
Status = irq_set_priority(descPtr, PHY_INTR_ID, 0x00);
Status = irq_set_priority(descPtr, PHY_INTR_ID, 0x08);
if (Status != SUCCESS)
{
DBG_ERROR("PHY_INTR priority level set FAILED, ErrorCode: %ld.\n", Status);
@@ -148,6 +173,28 @@ int32_t SetupInterruptSystem(struct irq_ctrl_desc *descPtr)
// Connect the interrupt service routine isr1 to the interrupt controller
Status = irq_register_callback(descPtr, PHY_INTR_ID, &phy_callback_desc);
if (Status != SUCCESS)
{
DBG_ERROR("PHY_INTR register callback FAILED, ErrorCode: %ld.\n", Status);
return Status;
}
/* Initialize linklayer layer interrupt. */
Status = irq_trigger_level_set(descPtr, PERIODIC_5MS_INTR_ID, IRQ_EDGE_RISING);
if (Status != SUCCESS)
{
DBG_ERROR("PERIODIC_5MS_INTR_ID trigger level set FAILED, ErrorCode: %ld.\n", Status);
return Status;
}
// Set highest interrupt priority
Status = irq_set_priority(descPtr, PERIODIC_5MS_INTR_ID, 0x00);
if (Status != SUCCESS)
{
DBG_ERROR("PERIODIC_5MS_INTR_ID 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, &periodicpull_callback_desc);
if (Status != SUCCESS)
{
DBG_ERROR("PHY_INTR register callback FAILED, ErrorCode: %ld.\n", Status);
return Status;
@@ -161,7 +208,7 @@ int32_t SetupInterruptSystem(struct irq_ctrl_desc *descPtr)
return Status;
}
// Set interrupt priority
Status = irq_set_priority(descPtr, SYSBUS_TX_INTR_ID, 0x08);
Status = irq_set_priority(descPtr, SYSBUS_TX_INTR_ID, 0x10);
if (Status != SUCCESS)
{
DBG_ERROR("PHY_INTR priority level set FAILED, ErrorCode: %ld.\n", Status);
@@ -182,7 +229,7 @@ int32_t SetupInterruptSystem(struct irq_ctrl_desc *descPtr)
return Status;
}
// Set interrupt priority
Status = irq_set_priority(descPtr, SYSBUS_RX_INTR_ID, 0x10);
Status = irq_set_priority(descPtr, SYSBUS_RX_INTR_ID, 0x18);
if (Status != SUCCESS)
{
DBG_ERROR("PHY_INTR priority level set FAILED, ErrorCode: %ld.\n", Status);
@@ -208,6 +255,7 @@ int32_t SetupInterruptSystem(struct irq_ctrl_desc *descPtr)
// Enable interrupts
irq_enable(descPtr, PHY_INTR_ID);
irq_enable(descPtr, PERIODIC_5MS_INTR_ID);
irq_enable(descPtr, SYSBUS_RX_INTR_ID);
irq_enable(descPtr, SYSBUS_TX_INTR_ID);
irq_enable(descPtr, SCUTIMER_INTR_ID);
@@ -233,6 +281,21 @@ int32_t SetupInterruptSystem(struct irq_ctrl_desc *descPtr)
*
*****************************************************************************/
void Phy_IntrHandler(void *context)
{
Phy_UpdateState(phyInstPtr);
} // PhyIntr_Handler
/****************************************************************************/
/**
* Handler of periodic 5ms interrupt
*
* @param callBackRef is the linklayer instance pointer.
*
* @return None.
*
*****************************************************************************/
void PeriodicPull_Handler(void *callBackRef)
{
int32_t Status;
Status = Linklayer_Proc(linkInstPtr);
@@ -241,7 +304,7 @@ void Phy_IntrHandler(void *context)
DBG_ERROR("Link layer runs failed, with error code: %ld\n", Status);
_nop_();
}
} // PhyIntr_Handler
} // PeriodicPull_Handler
/*****************************************************************************/
/**
@@ -681,7 +744,8 @@ int main(void)
DBG_PRINT("System monitor Init FAILED! \n");
return Status;
}
/* Hardware Initialize */
Status = Hardware_Init();
if (Status == SUCCESS)
{
@@ -693,6 +757,18 @@ int main(void)
return Status;
}
/* Time controller Initialize */
Status = TimeCtrl_Init(&tmcInstPtr, &tmcConfig);
if (Status == SUCCESS)
{
DBG_PRINT("TmcCtrl Init SUCCESS! \n");
}
else
{
DBG_PRINT("TmcCtrl Init FAILED! \n");
return Status;
}
/* Physical Layer Initialize */
Status = Phy_InstInit(&phyInstPtr, &phyConfig);
if (Status == SUCCESS)
@@ -706,7 +782,8 @@ int main(void)
}
/* Link Layer Initialize */
linkConfig.phyInstRef = phyInstPtr;
linkConfig.phyInstPtr = phyInstPtr;
linkConfig.tmcInstPtr = tmcInstPtr;
Status = Linklayer_Init(&linkInstPtr, &linkConfig);
if (Status == SUCCESS)
{
@@ -749,8 +826,10 @@ int main(void)
// Start System Monitor logging.
timer_start(logTmPtr);
Phy_Start(phyInstPtr);
DBG_PRINT("Physical started...\n");
Phy_Start(phyInstPtr);
Linklayer_Start(linkInstPtr);
while (true)
{
df_datalink_sp/src/networking/link_layer/framing.c
+38
View File
@@ -24,6 +24,44 @@
/************************** Constant Definitions *****************************/
/*****************************************************************************/
/**
* @brief Insert a data chunk to a bit array.
*
* @param frame pointer to target bit array.
* @param offset offset of insert start position.
* @param bytes number of insert bytes
* @param chunk pointer to data chunk
*
* @return NONE
*
******************************************************************************/
void L2_FrameInsertChunk(uint8_t *frame, size_t offset, size_t bytes, const void *chunk)
{
ASSERT(frame != NULL);
ASSERT(chunk != NULL);
memcpy(frame + offset, chunk, bytes);
}
/*****************************************************************************/
/**
* @brief Get a data chunk to a frame buff.
*
* @param frame pointer to target bit array.
* @param offset offset of data chunk start position.
* @param bytes number of data bytes
* @param chunk pointer to data chunk
*
* @return NONE
*
******************************************************************************/
void L2_FrameGetChunk(uint8_t *frame, size_t offset, size_t bytes, void *chunk)
{
ASSERT(frame != NULL);
ASSERT(chunk != NULL);
memcpy(chunk, frame + offset, bytes);
}
/*****************************************************************************/
/**
* @brief Generate crc of specified data.
df_datalink_sp/src/networking/link_layer/link_layer.c
+623 -573
View File
File diff suppressed because it is too large Load Diff
df_datalink_sp/src/networking/link_layer/link_layer.h
+98 -71
View File
@@ -19,127 +19,154 @@
/***************************** Include Files *********************************/
#include <stdint.h>
#include "circ_queue.h"
#include "link_layer_types.h"
#include "schdevent.h"
#include "node_list.h"
#include "circ_queue.h"
#include "../phy_layer/phy_layer.h"
#include "../../system_monitor.h"
#include "../config/app_config.h"
#include "node_list.h"
/************************** Constant Definitions *****************************/
#define L2_HEAD_NODE_ID 1
#define L2_MEMBER_NODE_ID 2
/***************** Macros (Inline Functions) Definitions *********************/
// #define AssertL2Event(inst, event) Linklayer_EventHandler(inst, event)
#define AssertL2Event(instPtr, event) Linklayer_EventHandler(instPtr, event)
/**************************** Type Definitions *******************************/
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;
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 *phyInstPtr;
TIMECTRL_INST *tmcInstPtr;
} L2_CONFIG;
typedef struct
{
uint32_t frameRx;
uint32_t frameTx;
uint32_t frameRecv;
uint32_t frameCorrect;
uint32_t advRxCnt;
uint32_t advTxCnt;
uint32_t advRecvCnt;
uint32_t advCorrectCnt;
uint32_t cfmRxCnt;
uint32_t cfmTxCnt;
uint32_t cfmRecvCnt;
uint32_t cfmCorrectCnt;
uint32_t svcRxCnt;
uint32_t svcTxCnt;
uint32_t svcRecvCnt;
uint32_t svcCorrectCnt;
} L2_LOG;
typedef struct
{
L2_CONFIG config;
L2_NODE_INFO myInfo;
NODE_LIST *nodeList;
L2_SCHD_POOL *schdEventPool;
uint32_t netStartTime;
bool isEnable; // if the node is connected to the network
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 frameNum; // stores the last current and next frame number.
L2_RCD slotNum; // 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_RCD tor; // stores the last current and next link layer tx/rx 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];
uint32_t schdEventCnt;
SCAN_FSM scanFsm;
SVC_FSM serviceFsm;
NODE_LIST *nodeInfoList;
L2_SCAN_FSM *scanFSM; // FSM of scan net (link set up)
L2_SVC_FSM *serviceFSM; // FSM of service net (data service)
uint8_t *sendPktData;
uint8_t *recvPktData;
L2_LOG log;
// for system log
SYSINFO sysInfo;
SLOTLOG txSlotLog;
SLOTLOG rxSlotLog;
PRTCLLOG prtcLog;
CIRC_QUEUE *txQueue;
// for debug
uint32_t recvCnt;
uint32_t recvFineCnt;
uint32_t sentCnt;
} L2_INST;
/************************** Variable Definitions *****************************/
extern L2_RTNE_TABLE_ENTRY *normalTable[L2_RTNE_PERIOD];
extern L2_RTNE_TABLE_ENTRY *initTable[L2_INIT_RTNE_PERIOD];
extern L2_RTNE_TABLE_ENTRY *normalTable[L2_NORMAL_RTNE_PERIOD];
/************************** Function Prototypes ******************************/
// Main
int32_t Linklayer_Init(L2_INST **instPtr, L2_CONFIG *config);
int32_t Linklayer_Init(L2_INST **instPtr, const L2_CONFIG *config);
int32_t Linklayer_Reconfig(L2_INST *instPtr, const L2_CONFIG *config);
void Linklayer_Destroy(L2_INST *instPtr);
int32_t Linklayer_Start(L2_INST *instPtr);
int32_t Linklayer_Halt(L2_INST *instPtr);
int32_t Linklayer_Proc(L2_INST *instPtr);
int32_t Linklayer_EventHandler(L2_INST *instPtr, L2_EVENT *l2Event);
int32_t Linklayer_PktHandler(L2_INST *instPtr);
int32_t Linklayer_EventHandler(L2_INST *instPtr, const L2_EVENT *l2Event);
int32_t Linklayer_PktHandler(L2_INST *instPtr, L2_FRAME_TYPE frameType);
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);
// Framing
void L2_FrameStateUpdate(L2_INST *instPtr);
L2_FRAME_TYPE L2_GetLastRcvFrameType(L2_INST *instPtr);
L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame);
// Scheduling Event
void L2_EventPoolHandler(L2_INST *instPtr);
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_RescheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t frameNo, uint32_t slotNo);
int32_t L2_RescheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t numSlots);
// Framing
void L2_FrameInsertChunk(uint8_t *frame, size_t offset, size_t bytes, const void *chunk);
void L2_FrameGetChunk(uint8_t *frame, size_t offset, size_t bytes, void *chunk);
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);
// Scanning
int32_t L2_Scan_Init(L2_SCAN_FSM **scanFSMPtr);
void L2_Scan_Reset(L2_SCAN_FSM *scanFSMPtr);
void L2_Scan_Destroy(L2_SCAN_FSM *scanFSMPtr);
int32_t L2_Scan_Proc(L2_INST *instPtr);
int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg);
int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *schedEntry);
// Service
int32_t L2_Service_Init(L2_SVC_FSM **svcFSMPtr);
void L2_Service_Reset(L2_SVC_FSM *svcFSMPtr);
void L2_Service_Destroy(L2_SVC_FSM *svcFSMPtr);
int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg);
int32_t L2_Service_Proc(L2_INST *instPtr);
int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *schedEntry);
// Miscellaneous
uint8_t L2_GetNodeId(L2_INST *instPtr);
uint8_t L2_GetClusterId(L2_INST *instPtr);
uint8_t L2_GetHeadId(L2_INST *instPtr);
L2_NODE_TYPE L2_GetNodeType(L2_INST *instPtr);
L2_NODE_STATE L2_GetMainState(L2_INST *instPtr);
bool L2_isDataAvail(L2_INST *instPtr);
bool L2_isCxn2Net(L2_INST *instPtr);
int32_t L2_GetFrameLen(L2_FRAME_TYPE l2FrameType);
uint32_t L2_GetLastFrameNo(L2_INST *instPtr);
uint32_t L2_GetLastSlotNo(L2_INST *instPtr);
uint32_t L2_GetCurrentFrameNo(L2_INST *instPtr);
uint32_t L2_GetCurrentSlotNo(L2_INST *instPtr);
uint32_t L2_GetNextFrameNo(L2_INST *instPtr);
uint32_t L2_GetNextSlotNo(L2_INST *instPtr);
uint32_t L2_GetFrameLen(L2_FRAME_TYPE l2FrameType);
uint32_t L2_GetLastFrameNum(L2_INST *instPtr);
uint32_t L2_GetLastSlotNum(L2_INST *instPtr);
L2_FRAME_TYPE L2_GetLastFrameType(L2_INST *instPtr);
L2_TOR_TYPE L2_GetLastTor(L2_INST *instPtr);
uint32_t L2_GetCurrentFrameNum(L2_INST *instPtr);
uint32_t L2_GetCurrentSlotNum(L2_INST *instPtr);
L2_FRAME_TYPE L2_GetCurrentFrameType(L2_INST *instPtr);
L2_TOR_TYPE L2_GetCurrentTor(L2_INST *instPtr);
uint32_t L2_GetNextFrameNum(L2_INST *instPtr);
uint32_t L2_GetNextSlotNum(L2_INST *instPtr);
L2_FRAME_TYPE L2_GetNextFrameType(L2_INST *instPtr);
L2_TOR_TYPE L2_GetNextTor(L2_INST *instPtr);
// Scanning
int32_t L2_Scan_Init(L2_INST *instPtr);
int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg);
int32_t L2_Scan_Proc(L2_INST *instPtr);
int32_t L2_Scan_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry);
// Time syncronization
int32_t L2_TimeAlign(L2_INST *instPtr, uint32_t frameNum, uint32_t slotNum, bool relative);
// Service
int32_t L2_Service_Init(L2_INST *instPtr);
int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg);
int32_t L2_Service_Proc(L2_INST *instPtr);
int32_t L2_Service_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry);
// Time Sync
int32_t L2_TimeSync_LocalTimerAdd(L2_INST *instPtr, int64_t adjustment);
int32_t L2_TimeSync_SlotAlign(L2_INST *instPtr);
int32_t L2_TimeSync_GetDeviatoion(L2_INST *instPtr, int64_t *devi);
#endif /* __LINK_LAYER_H_ */
df_datalink_sp/src/networking/link_layer/link_layer_types.h
+76 -61
View File
@@ -20,47 +20,43 @@
#include <stdbool.h>
#include <stdint.h>
#include "geometry/coord.h"
#include "geometry/eulerangles.h"
#include "geometry/quaternion.h"
#include "../config/app_config.h"
#include "../phy_layer/phy_layer.h"
/************************** Constant Definitions *****************************/
// Common node ID definition
#define BROADCAST_ID 0xFF
#define L2_BROADCAST_ID 0xFF
#define L2_NONE_ID 0
// Routine configure
#define L2_RTNE_PERIOD 60
#define L2_INIT_RTNE_PERIOD 1
#define L2_NORMAL_RTNE_PERIOD 1
// Common slot number definition
#define L2_SLOTNO_NONE 0
#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 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 L2_NUM_SLOTS_NONE UINT32_MAX
#define L2_NUM_SLOTS_100MS 20
#define L2_NUM_SLOTS_20MS 4
#define L2_NUM_SLOTS_10MS 2
#define L2_NUM_SLOTS_5MS 1
#define L2_NUM_SLOTS_SKIP 0
// Common schedule event definition
#define L2_SCHD_EVENT_MAX 16
/***************** Macros (Inline Functions) Definitions *********************/
#define AssertL2Event(inst, envent) Linklayer_EventHandler(inst, envent)
/**************************** Type Definitions *******************************/
typedef struct
{
int32_t last;
int32_t curr;
int32_t next;
uint32_t last;
uint32_t curr;
uint32_t next;
} L2_RCD;
/** @name Tx/Rx mode code
*/
typedef enum
{
L2_TOR_SUSPEND = 0,
@@ -72,19 +68,22 @@ typedef enum
*/
typedef enum
{
L2_NONE_TYPE = 0,
L2_HEAD_NODE = 1,
L2_MEMBER_NODE = 2,
L2_BACKUP_HEAD = 3
L2_NODE_NONE = 0,
L2_NODE_HEAD = 1,
L2_NODE_MEMBER = 2,
L2_NODE_BACKUP = 3
} L2_NODE_TYPE;
/** @name Node state type definition
*/
typedef enum
{
NONE_STATE = 0,
INIT_STATE = 1,
NORMAL_STATE = 2
L2_NONE_STATE = 0,
L2_INIT_STATE = 1,
L2_NORMAL_STATE = 2,
L2_INTRA_SEEK_STATE = 3,
L2_INTER_SEEK_STATE = 4,
L2_SEEK_STATE = 5
} L2_NODE_STATE;
/** @name Node information type definition
@@ -93,7 +92,9 @@ typedef enum
typedef struct
{
uint8_t nodeId;
uint8_t nodeType;
uint8_t clustId;
uint8_t headId;
L2_NODE_TYPE nodeType;
uint8_t beamNo;
COORD pos;
COORD speed;
@@ -107,15 +108,16 @@ typedef enum
{
L2_EV_NONE = 0,
L2_EV_SCAN_PROC_DONE = 1,
L2_EV_NET_SETUP_DONE = 2,
L2_EV_TIME_SYNC = 3,
L2_EV_LOST_CXN = 4,
L2_EV_LOST_NODE = 5,
L2_EV_FRAME_NO_UPDATE = 6,
L2_EV_SLOT_NO_UPDATE = 7,
L2_EV_MAIN_STATE_UPDATE = 8,
L2_ROUTINE_FINISH = 9,
L2_ROUTINE_UPDATE = 10
L2_EV_NET_ADV_UPDATE = 2,
L2_EV_NET_SETUP_DONE = 3,
L2_EV_TIME_SYNC = 4,
L2_EV_LOST_CXN = 5,
L2_EV_LOST_NODE = 6,
L2_EV_SET_FRAME_NUM = 7,
L2_EV_SET_SLOT_NUM = 8,
L2_EV_MAIN_STATE_UPDATE = 9,
L2_ROUTINE_FINISH = 10,
L2_ROUTINE_UPDATE = 11
} L2_EVENT_TYPE;
/** @name L2 event
@@ -127,18 +129,6 @@ typedef struct
void *msg;
} L2_EVENT;
/** @name L2 event
* A definition of L2 event instance.
*/
typedef struct
{
uint8_t eventId;
bool isSchd;
L2_EVENT l2Event;
uint32_t schdFrame;
uint32_t schdSlot;
} L2_SCHD_EVENT;
/** @name Slot types
* A definition of different slots.
*/
@@ -147,8 +137,9 @@ typedef enum
L2_SLOT_NONE = 0,
L2_SLOT_IDLE = 1,
L2_SLOT_SCAN = 2,
L2_SLOT_SERVICE = 3,
L2_SLOT_ADVERTISE = 4,
L2_SLOT_ADVERTISE = 3,
L2_SLOT_CONFIRM = 4,
L2_SLOT_SERVICE = 5,
} L2_SLOT_TYPE;
/** @name Frame types
@@ -158,8 +149,9 @@ typedef enum
{
L2_NONE_FRAME = 0,
L2_SCAN_FRAME = 1,
L2_SVC_FRAME = 2,
L2_INFO_FRAME = 3
L2_ADV_FRAME = 2,
L2_CFM_FRAME = 3,
L2_SVC_FRAME = 4
} L2_FRAME_TYPE;
/** @name Frame types
@@ -184,29 +176,52 @@ typedef struct
uint8_t dst;
} L2_RTNE_TABLE_ENTRY;
typedef enum
{
L2_SCAN_NONE_STAGE = 0,
L2_SCAN_INIT_STAGE = 1,
L2_SCAN_ADV_STAGE = 2,
L2_SCAN_CFM_STAGE = 3
} SCAN_STAGE;
/** @name Scan procedure FSM
*/
typedef struct
{
uint8_t sacanCnt;
bool isLocked; // init acq done.
bool isAdvRecvd; // bm received.
bool isAcked; // 2way acq done.
uint8_t scanCnt;
uint8_t beamNo;
} SCAN_FSM;
uint32_t advTimeout;
uint32_t cfmTimeout;
SCAN_STAGE scanStage; // only for member node
} L2_SCAN_FSM;
/** @name Service procedure FSM
*/
typedef struct
{
uint32_t txSeqNum;
uint32_t rxSeqNum;
uint8_t rxLog[256];
uint8_t txLog[256];
PAYLOAD rxLog;
PAYLOAD txLog;
uint32_t pktRecv;
uint32_t pktRecved;
uint32_t pktCorrect;
#ifdef DATALINK_ENABLE_BERTEST
uint32_t bitTotal;
uint32_t bitError;
#endif
} SVC_FSM;
} L2_SVC_FSM;
typedef struct
{
L2_RTNE_TABLE_ENTRY **rtneTable;
uint32_t rtnePrd;
L2_RTNE_TABLE_ENTRY *last;
L2_RTNE_TABLE_ENTRY *curr;
L2_RTNE_TABLE_ENTRY *next;
} L2_RTNE_CONFIG;
/************************** Variable Definitions *****************************/
#endif /* __LINK_LAYER_DEF_H_ */
df_datalink_sp/src/networking/link_layer/msg_def.h
+84 -39
View File
@@ -15,54 +15,99 @@
#ifndef __L2_MSG_DEF_H_
#define __L2_MSG_DEF_H_
#define PACK_LEN_MAX 512
#define L2_PACK_LEN_MAX 512 // bytes
#define SVC_PACK_LEN 30 // bytes(240 bits)
#define RSP_PACK_LEN 30 // bytes(240 bits)
#define L2_ADV_PACK_LEN 28 // bytes
#define L2_CMF_PACK_LEN 268 // bytes
#define L2_SVC_PACK_LEN 268 // bytes
// Common structure:
// [bytes] [name]
// 1 fame number
// 4 fame number
// 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
// 1 source id
// 1 destination id
// 1 message type
#define L2_MSG_FRAME_NUM 0
#define L2_MSG_FRAME_NUM_LEN 4
#define L2_MSG_SLOT_NUM 4
#define L2_MSG_SLOT_NUM_LEN 1
#define L2_MSG_SRC_ID 5
#define L2_MSG_SRC_ID_LEN 1
#define L2_MSG_DST_ID 6
#define L2_MSG_DST_ID_LEN 1
#define L2_MSG_TYPE 7
#define L2_MSG_TYPE_LEN 1
#define L2_MSG_PAYLOAD 8
// service frame structure:
// advertisement payload structure:
// [bytes] [name]
// 1 fame number
// 1 slot number
// 3 time sync
// 23 payload
// 4 position x
// 4 position y
// 4 position z
// 4 net start time
// 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_ADV_SRC_POSX 8
#define L2_MSG_ADV_SRC_POSX_LEN 4
#define L2_MSG_ADV_SRC_POSY 12
#define L2_MSG_ADV_SRC_POSY_LEN 4
#define L2_MSG_ADV_SRC_POSZ 16
#define L2_MSG_ADV_SRC_POSZ_LEN 4
#define L2_MSG_ADV_SRC_NETSTART_TIME 20
#define L2_MSG_ADV_SRC_NETSTART_TIME_LEN 4
#define L2_MSG_ADV_SVC_CRC 26
#define L2_MSG_ADV_SVC_CRC_LEN 2
// response frame structure:
// confirm payload structure:
// [bytes] [name]
// 1 fame number
// 1 slot number
// 4 position x
// 4 position y
// 4 position z
// 4 speed x
// 4 speed y
// 4 speed z
// 2 dummy
// 4 position x
// 4 position y
// 4 position z
// 4 speed x
// 4 speed y
// 4 speed z
// 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_CFM_SRC_POSX 8
#define L2_MSG_CFM_SRC_POSX_LEN 4
#define L2_MSG_CFM_SRC_POSY 12
#define L2_MSG_CFM_SRC_POSY_LEN 4
#define L2_MSG_CFM_SRC_POSZ 16
#define L2_MSG_CFM_SRC_POSZ_LEN 4
#define L2_MSG_CFM_SRC_SPEEDX 20
#define L2_MSG_CFM_SRC_SPEEDX_LEN 4
#define L2_MSG_CFM_SRC_SPEEDY 24
#define L2_MSG_CFM_SRC_SPEEDY_LEN 4
#define L2_MSG_CFM_SRC_SPEEDZ 28
#define L2_MSG_CFM_SRC_SPEEDZ_LEN 4
#define L2_MSG_CFM_CRC 266
#define L2_MSG_CFM_CRC_LEN 2
// service payload structure:
// [bytes] [name]
// 4 position x
// 4 position y
// 4 position z
// 4 speed x
// 4 speed y
// 4 speed z
// 234 payload
// 2 crc
#define L2_MSG_SVC_SRC_POSX 8
#define L2_MSG_SVC_SRC_POSX_LEN 4
#define L2_MSG_SVC_SRC_POSY 12
#define L2_MSG_SVC_SRC_POSY_LEN 4
#define L2_MSG_SVC_SRC_POSZ 16
#define L2_MSG_SVC_SRC_POSZ_LEN 4
#define L2_MSG_SVC_SRC_SPEEDX 20
#define L2_MSG_SVC_SRC_SPEEDX_LEN 4
#define L2_MSG_SVC_SRC_SPEEDY 24
#define L2_MSG_SVC_SRC_SPEEDY_LEN 4
#define L2_MSG_SVC_SRC_SPEEDZ 28
#define L2_MSG_SVC_SRC_SPEEDZ_LEN 4
#define L2_MSG_SVC_PAYLOAD 32
#define L2_MSG_SVC_PAYLOAD_LEN 234
#define L2_MSG_SVC_CRC 266
#define L2_MSG_SVC_CRC_LEN 2
#endif //__L2_MSG_DEF_H_
df_datalink_sp/src/networking/link_layer/node_list.c
+159 -143
View File
@@ -1,8 +1,9 @@
/*****************************************************************************/
/**
* Description: Implementations of df linklayer local record.
* Description: Implementations of 814 linklayer local record.
*
* Date: 2022-07-14 00:08
* LastEditors: Fan Hao
* LastEditTime: 2022-07-14 22:52
* Revisions: Rev 0.0
* History:
@@ -14,26 +15,58 @@
******************************************************************************/
/***************************** Include Files *********************************/
#include "common.h"
#include "error.h"
#include "link_layer.h"
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <stdbool.h>
#include "link_layer_types.h"
#include "node_list.h"
/************************** Constant Definitions *****************************/
#include "clist.h"
#include "common.h"
#include "error.h"
/***************** Macros (Inline Functions) Definitions *********************/
inline int32_t GetNodeNum(NODE_LIST *list)
inline uint32_t L2_GetNodeListLen(NODE_LIST *list)
{
ASSERT(list != NULL);
return list->nodeNum;
return list->len;
}
/**************************** Type Definitions *******************************/
/************************** Variable Definitions *****************************/
static NODE_ENTRY util_entry;
/************************** Function Prototypes ******************************/
/**
* @brief helper function to free node entry.
*/
static void entry_free(void *val)
{
free(val);
}
/**
* @brief helper function to match node entries by nodeIds
*/
static int8_t entry_compare(void *a, void *b)
{
ASSERT(a != NULL);
ASSERT(b != NULL);
register uint8_t idA, idB;
idA = ((NODE_ENTRY *)a)->nodeInfo.nodeId;
idB = ((NODE_ENTRY *)b)->nodeInfo.nodeId;
if (idA == idB)
{
return 0;
}
else if (idA < idB)
{
return -1;
}
else
{
return 1;
}
}
/*****************************************************************************/
/**
* @brief Initialize node list
@@ -44,59 +77,48 @@ inline int32_t GetNodeNum(NODE_LIST *list)
* ENOMEM - not enough memory.
*
******************************************************************************/
int32_t NodeList_Init(NODE_LIST **list)
int32_t L2_NodeList_Init(NODE_LIST **list)
{
NODE_ENTRY *entry;
(*list) = malloc(sizeof(NODE_LIST));
(*list) = clist_new();
if (*list == NULL)
{
return ENOMEM;
}
entry = malloc(sizeof(NODE_ENTRY));
if (entry == NULL)
{
free(*list);
return ENOMEM;
}
entry->nodeInfo.nodeId = 0;
entry->nextNode = entry;
entry->preNode = entry;
(*list)->head = entry;
(*list)->nodeNum = 0;
(*list)->free = entry_free;
(*list)->compare = entry_compare;
return SUCCESS;
}
void NodeList_Clear(NODE_LIST *list)
{
ASSERT(list != NULL);
NODE_ENTRY *nodeEntry, *deletEntry;
nodeEntry = list->head->nextNode;
while (nodeEntry != list->head)
{
deletEntry = nodeEntry;
nodeEntry = nodeEntry->nextNode;
deletEntry->preNode->nextNode = deletEntry->nextNode;
deletEntry->nextNode->preNode = deletEntry->preNode;
DBG_INFO("Deleting node [%d].\n", deletEntry->nodeInfo.nodeId);
free(deletEntry);
list->nodeNum--;
}
}
void NodeList_Destroy(NODE_LIST *list)
{
if (list == NULL)
{
return;
}
NodeList_Clear(list);
free(list->head);
free(list);
}
/*****************************************************************************/
/**
* @brief Get node entry frome node list base on node id.
* @brief Clear all nodes in the node list, free allocated memory.
*
* @param list
*NONE
*
******************************************************************************/
void L2_NodeList_Clear(NODE_LIST *list)
{
ASSERT(list != NULL);
clist_clear(list);
}
/*****************************************************************************/
/**
* @brief Completely destroy node list.
*
* @param list
*
* @return NONE
*
******************************************************************************/
void L2_NodeList_Destroy(NODE_LIST *list)
{
clist_destroy(list);
}
/*****************************************************************************/
/**
* @brief Get node entry from node list base on node id.
*
* @param list
* @param nodeId
@@ -105,48 +127,21 @@ void NodeList_Destroy(NODE_LIST *list)
* NULL if node dose not exist.
*
******************************************************************************/
NODE_ENTRY *GetNode(NODE_LIST *list, uint32_t nodeId)
NODE_ENTRY *L2_GetNodeById(NODE_LIST *list, uint8_t nodeId)
{
ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = list->head->nextNode;
while (nodeEntry != list->head)
{
if (nodeEntry->nodeInfo.nodeId == nodeId)
{
break;
}
else
{
nodeEntry = nodeEntry->nextNode;
}
}
if (nodeEntry == list->head)
util_entry.nodeInfo.nodeId = nodeId;
clist_node_t *node = clist_find(list, &util_entry);
if (node == NULL)
{
return NULL;
}
else
{
return nodeEntry;
return ((NODE_ENTRY *)node->val);
}
}
/*****************************************************************************/
/**
* @brief Check whether the node is already in the node list.
*
* @param list
* @param nodeId
*
* @return true node in the list
* false node not in the list
*
******************************************************************************/
inline bool IsNodeInList(NODE_LIST *list, uint8_t nodeId)
{
ASSERT(list != NULL);
return (GetNode(list, nodeId) != NULL);
}
/*****************************************************************************/
/**
* @brief Append a new node entry to the node list
@@ -159,19 +154,27 @@ inline bool IsNodeInList(NODE_LIST *list, uint8_t nodeId)
* Other Error occurred.(Node already exist, memory alloc error ect.)
*
******************************************************************************/
int32_t AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType)
NODE_ENTRY *L2_AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType)
{
ASSERT(list != NULL);
if (IsNodeInList(list, nodeId))
if (L2_isNodeInList(list, nodeId))
{
DBG_WARNING("Node [%d] already exist.\n", nodeId);
return FAILURE;
return NULL;
}
DBG_INFO("Link layer appending node [%d] to node list.\n", nodeId);
NODE_ENTRY *nodeEntry = (NODE_ENTRY *)malloc(sizeof(NODE_ENTRY));
if (nodeEntry == NULL)
{
return ENOMEM;
DBG_ERROR("Failed to allocated memory for nodeEntry.\n");
return NULL;
}
clist_node_t *node = clist_node_new(nodeEntry);
if (node == NULL)
{
DBG_ERROR("Failed to allocated memory for list node.\n");
free(nodeEntry);
return NULL;
}
nodeEntry->nodeInfo.nodeId = nodeId;
nodeEntry->nodeInfo.nodeType = (uint8_t)nodeType;
@@ -183,13 +186,8 @@ int32_t AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType)
nodeEntry->nodeInfo.speed.z = 0;
nodeEntry->nodeInfo.beamNo = 0;
nodeEntry->ttlCnt = 0;
nodeEntry->nextNode = list->head;
nodeEntry->preNode = list->head->nextNode;
list->head->nextNode = nodeEntry;
list->nodeNum++;
return SUCCESS;
clist_rpush(list, node);
return nodeEntry;
}
/*****************************************************************************/
@@ -203,23 +201,38 @@ 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 L2_DeleteNodeById(NODE_LIST *list, uint8_t nodeId)
{
ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
util_entry.nodeInfo.nodeId = nodeId;
clist_node_t *node = clist_find(list, &util_entry);
if (node == NULL)
{
DBG_WARNING("Node [%d] in not in node list, skip deleting process.\n", nodeId);
return SUCCESS;
}
nodeEntry->preNode->nextNode = nodeEntry->nextNode;
nodeEntry->nextNode->preNode = nodeEntry->preNode;
DBG_INFO("Deleting node [%d].\n", nodeId);
free(nodeEntry);
list->nodeNum--;
clist_remove(list, node);
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Check whether the node is already in the node list.
*
* @param list
* @param nodeId
*
* @return true node in the list
* false node not in the list
*
******************************************************************************/
bool L2_isNodeInList(NODE_LIST *list, uint8_t nodeId)
{
ASSERT(list != NULL);
return (L2_GetNodeById(list, nodeId) != NULL);
}
/*****************************************************************************/
/**
* @brief Get the distance between me and the node.
@@ -233,10 +246,10 @@ int32_t DeletNode(NODE_LIST *list, uint8_t nodeId)
*
******************************************************************************/
int32_t GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double *dst)
int32_t L2_GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double *dst)
{
ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
NODE_ENTRY *nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
@@ -260,10 +273,10 @@ 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 L2_UpdateNodePos(NODE_LIST *list, uint8_t nodeId, const COORD *newPos)
{
ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
NODE_ENTRY *nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
@@ -287,10 +300,10 @@ 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 L2_GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *pos)
{
ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
NODE_ENTRY *nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
@@ -314,10 +327,10 @@ 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 L2_UpdateNodeSpeed(NODE_LIST *list, uint8_t nodeId, const COORD *newSpeed)
{
ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
NODE_ENTRY *nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
@@ -341,10 +354,10 @@ 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 L2_GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed)
{
ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
NODE_ENTRY *nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
@@ -361,16 +374,16 @@ int32_t GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed)
*
* @param list
* @param nodeId
* @param defl
* @param devi
*
* @return SUCCESS update successfully.
* FAILURE Node not exist.
*
******************************************************************************/
int32_t UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi)
int32_t L2_UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi)
{
ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
NODE_ENTRY *nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
@@ -393,10 +406,10 @@ 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 L2_IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc)
{
ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
NODE_ENTRY *nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
@@ -419,10 +432,10 @@ 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 L2_RstNodeTTL(NODE_LIST *list, uint8_t nodeId)
{
ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
NODE_ENTRY *nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
@@ -445,10 +458,10 @@ 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 L2_GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl)
{
ASSERT(list != NULL);
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
NODE_ENTRY *nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
@@ -473,20 +486,27 @@ 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 L2_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)
int32_t Status;
clist_node_t *node;
clist_iterator_t *iter = clist_iterator_new(list, LIST_HEAD);
if (iter == NULL)
{
Status = operation(nodeEntry, param);
DBG_ERROR("Failed to initialize list iteration process.\n");
return ENOMEM;
}
while ((node = clist_iterator_next(iter)))
{
Status = operation((NODE_ENTRY *)node->val, param);
if (Status != SUCCESS)
{
DBG_ERROR("Error occurred when iteratively edit node entry.\n");
break;
}
nodeEntry = nodeEntry->nextNode;
}
clist_iterator_destroy(iter);
return Status;
}
@@ -501,27 +521,23 @@ 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 L2_IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *))
{
ASSERT(list != NULL);
NODE_ENTRY *nodeEntry, *deletEntry;
nodeEntry = list->head->nextNode;
while (nodeEntry != list->head)
clist_node_t *node;
clist_iterator_t *iter = clist_iterator_new(list, LIST_HEAD);
if (iter == NULL)
{
if (condition(nodeEntry) == false)
DBG_ERROR("Failed to initialize list iteration process.\n");
return ENOMEM;
}
while ((node = clist_iterator_next(iter)))
{
if (condition((NODE_ENTRY *)node->val) == false)
{
deletEntry = nodeEntry;
nodeEntry = nodeEntry->nextNode;
deletEntry->preNode->nextNode = deletEntry->nextNode;
deletEntry->nextNode->preNode = deletEntry->preNode;
DBG_INFO("Deleting node [%d].\n", deletEntry->nodeInfo.nodeId);
free(deletEntry);
list->nodeNum--;
}
else
{
nodeEntry = nodeEntry->nextNode;
clist_remove(list, node);
}
}
clist_iterator_destroy(iter);
return SUCCESS;
}
df_datalink_sp/src/networking/link_layer/node_list.h
+28 -30
View File
@@ -3,6 +3,7 @@
* Description: Header of 814 linklayer local record.
*
* Date: 2022-07-14 00:08
* LastEditors: Fan Hao
* LastEditTime: 2022-07-14 22:52
* Revisions: Rev 0.0
* History:
@@ -14,14 +15,16 @@
******************************************************************************/
/***************************** Include Files *********************************/
#ifndef __L2_LOCAL_RECORD_H_
#define __L2_LOCAL_RECORD_H_
#ifndef NODE_LIST_H_
#define NODE_LIST_H_
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <stdbool.h>
#include "util.h"
#include "link_layer_types.h"
#include "geometry/coord.h"
#include "util.h"
#include "clist.h"
/************************** Constant Definitions *****************************/
/***************** Macros (Inline Functions) Definitions *********************/
@@ -31,36 +34,31 @@ typedef struct NODE_LIST_ENTRY
L2_NODE_INFO nodeInfo;
uint32_t ttlCnt;
int32_t timeDevi;
struct NODE_LIST_ENTRY *nextNode;
struct NODE_LIST_ENTRY *preNode;
} NODE_ENTRY;
typedef struct
{
uint32_t nodeNum;
NODE_ENTRY *head;
} NODE_LIST;
typedef clist_t NODE_LIST;
/************************** Variable Definitions *****************************/
/************************** Function Prototypes ******************************/
int32_t NodeList_Init(NODE_LIST **list);
void NodeList_Clear(NODE_LIST *list);
void NodeList_Destroy(NODE_LIST *list);
NODE_ENTRY *GetNode(NODE_LIST *list, uint32_t nodeId);
int32_t AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType);
int32_t DeletNode(NODE_LIST *list, uint8_t nodeId);
int32_t UpdateNodePos(NODE_LIST *list, uint8_t nodeId, const COORD *newPos);
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 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 IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *));
#endif /* __L2_LOCAL_RECORD_H_ */
int32_t L2_NodeList_Init(NODE_LIST **list);
void L2_NodeList_Clear(NODE_LIST *list);
void L2_NodeList_Destroy(NODE_LIST *list);
uint32_t L2_GetNodeListLen(NODE_LIST *list);
NODE_ENTRY *L2_GetNodeById(NODE_LIST *list, uint8_t nodeId);
NODE_ENTRY *L2_AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType);
int32_t L2_DeleteNodeById(NODE_LIST *list, uint8_t nodeId);
int32_t L2_UpdateNodePos(NODE_LIST *list, uint8_t nodeId, const COORD *newPos);
bool L2_isNodeInList(NODE_LIST *list, uint8_t nodeId);
int32_t L2_GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *newPos);
int32_t L2_UpdateNodeSpeed(NODE_LIST *list, uint8_t nodeId, const COORD *newSpeed);
int32_t L2_GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed);
int32_t L2_UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi);
int32_t L2_GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double *dst);
int32_t L2_RstNodeTTL(NODE_LIST *list, uint8_t nodeId);
int32_t L2_IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc);
int32_t L2_GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl);
int32_t L2_IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)(NODE_ENTRY *, const void *));
int32_t L2_IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *));
#endif /* NODE_LIST_H_ */
df_datalink_sp/src/networking/link_layer/routine_table.c
+410 -876
View File
File diff suppressed because it is too large Load Diff
df_datalink_sp/src/networking/link_layer/scan.c
+506 -212
View File
@@ -16,24 +16,196 @@
/***************************** Include Files *********************************/
#include <string.h>
#include "error.h"
#include "common.h"
#include "msg_def.h"
#include "link_layer.h"
#include "link_layer_types.h"
#include "../phy_layer/phy_layer.h"
#include "msg_def.h"
#include "node_list.h"
#include "error.h"
#include "common.h"
#include "../phy_layer/phy_layer.h"
/************************** Constant Definitions *****************************/
/***************** Macros (Inline Functions) Definitions *********************/
#define ADV_LISTEN_TIMEOUT 40
#define CFM_LISTEN_TIMEOUT 40
/***************** Macros (In-line Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
/************************** Variable Definitions *****************************/
/************************** Function Prototypes ******************************/
static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type);
static void LockStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state);
static void AdvrecvStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state);
static void CfmStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state);
static void StageUpdate(L2_SCAN_FSM *scanFsmPtr, SCAN_STAGE newStage);
static int32_t Scan_InitScanHandler(L2_INST *instPtr);
static int32_t Scan_AdvHandler(L2_INST *instPtr);
static int32_t Scan_CfmHandler(L2_INST *instPtr);
static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
/**
* @brief Update Init-Scan lock state.
*/
static inline void LockStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state)
{
ASSERT(scanFsmPtr != NULL);
DBG_INFO("Update BeamLockState from [%d] to [%d].\n",
scanFsmPtr->isLocked, state);
scanFsmPtr->isLocked = state;
return;
}
/**
* @brief Update Base-info Advertisement state.
*/
static void AdvrecvStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state)
{
ASSERT(scanFsmPtr != NULL);
DBG_INFO("Update AdvRecvState from [%d] to [%d].\n",
scanFsmPtr->isBmRecvd, state);
scanFsmPtr->isAdvRecvd = state;
return;
}
/**
* @brief Update 2-Way Acquisition ack state.
*/
static void CfmStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state)
{
ASSERT(scanFsmPtr != NULL);
DBG_INFO("Update CfmAckState from [%d] to [%d].\n",
scanFsmPtr->isAcked, state);
scanFsmPtr->isAcked = state;
return;
}
/*****************************************************************************/
/**
* @brief Update current Scan-Net stage
*
* @param scanFsmPtr A pointer to scan net fsm.
* newStage New state.
*
* @return NONE.
*
******************************************************************************/
static void StageUpdate(L2_SCAN_FSM *scanFsmPtr, SCAN_STAGE newStage)
{
ASSERT(newStage >= L2_SCAN_NONE_STAGE && newStage <= L2_SCAN_CFM_STAGE);
DBG_INFO("ScanNet current stage: %u -> ScanNet new stage: %u \n",
scanFsmPtr->scanStage, newStage);
scanFsmPtr->scanStage = newStage;
}
/*****************************************************************************/
/**
* @brief Member node scan net InitScan stage handler.
*
* @param instPtr A pointer to l2 instance
*
* @return used slot (5ms slot).
*
******************************************************************************/
static int32_t Scan_InitScanHandler(L2_INST *instPtr)
{
ASSERT(instPtr != NULL);
L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM;
DBG_INFO("** Netsetup - InitScan Procedure (Attempts: %u)**\n", scanFsmPtr->scanCnt);
if (scanFsmPtr->isLocked == true)
{
StageUpdate(scanFsmPtr, L2_SCAN_ADV_STAGE);
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_ADV_FRAME);
} /* Otherwise remain L2_SCAN_INIT_STAGE */
else
{
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_SCAN_FRAME);
scanFsmPtr->scanCnt++;
}
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Member node scan net BmAdvt stage handler.
*
* @param scanFsmPtr A pointer to scan net fsm.
* mainState A pointer to main node state.
*
* @return used slot (5ms slot).
*
******************************************************************************/
static int32_t Scan_AdvHandler(L2_INST *instPtr)
{
L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM;
DBG_INFO("** Netsetup - Advertisement Procedure (Timeout: %u) **\n", scanFsmPtr->advTimeout);
// if received a valid bm frame.
if (scanFsmPtr->isAdvRecvd == true)
{
scanFsmPtr->advTimeout = 0; /* reset listen counter. */
DBG_INFO("Received Adv frame, skipping 10ms for turn around...\n");
// TODO: check functionality.
StageUpdate(scanFsmPtr, L2_SCAN_CFM_STAGE);
return SUCCESS;
}
// no valid bm frame was received.
else
{
/* if time out reached, go back to init scan stage. */
if (scanFsmPtr->advTimeout > ADV_LISTEN_TIMEOUT)
{
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_SCAN_FRAME);
DBG_WARNING("Adv timeout, restart net scan from L2_SCAN_INIT_STAGE.\n");
L2_Scan_Reset(scanFsmPtr);
}
else
{
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_ADV_FRAME);
scanFsmPtr->advTimeout++;
}
return SUCCESS;
}
}
/*****************************************************************************/
/**
* @brief Scan net 2Way-Acq stage handler.
*
* @param instPtr a pointer to linklayer layer instance
*
* @return used slot (5ms slot).
*
******************************************************************************/
static int32_t Scan_CfmHandler(L2_INST *instPtr)
{
L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM;
DBG_INFO("** Netsetup - 2WAcq Procedure (Timeout: %u) **\n", scanFsmPtr->w2acqTimeout);
// 2 way acq failed, back to init stage.
if (scanFsmPtr->cfmTimeout >= CFM_LISTEN_TIMEOUT)
{
DBG_WARNING("Cmf timeout, restart net scan from L2_SCAN_INIT_STAGE.\n");
L2_Scan_Reset(scanFsmPtr);
}
else
{
scanFsmPtr->cfmTimeout++; // increase counter.
}
// using pull mode to receive confirm frame
// since first time sync procedure is accomplished in BM stage.
Phy_SetMode(instPtr->config.phyInstPtr, PHY_MODE_PULL);
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_CFM_FRAME);
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Packing Scan message to a bit array.
@@ -45,43 +217,71 @@ 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, uint8_t dstId)
{
ASSERT(instPtr != NULL);
int32_t Status;
uint8_t *buff;
uint32_t packLen;
uint32_t dataChunk;
uint8_t *buff = instPtr->sendPktData;
if(type < L2_ADV_FRAME || type > L2_CFM_FRAME)
{
DBG_ERROR("Unexpected frame type: %d.\n", type);
return EPARAM;
}
dataChunk = L2_GetNextFrameNum(instPtr);
L2_FrameInsertChunk(buff, L2_MSG_FRAME_NUM, L2_MSG_FRAME_NUM_LEN, &dataChunk);
dataChunk = L2_GetNextSlotNum(instPtr);
L2_FrameInsertChunk(buff, L2_MSG_SLOT_NUM, L2_MSG_SLOT_NUM_LEN, &dataChunk);
dataChunk = L2_GetNodeId(instPtr);
L2_FrameInsertChunk(buff, L2_MSG_SRC_ID, L2_MSG_SRC_ID_LEN, &dataChunk);
dataChunk = dstId;
L2_FrameInsertChunk(buff, L2_MSG_DST_ID, L2_MSG_DST_ID_LEN, &dataChunk);
switch (type)
{
case L2_INFO_FRAME:
case L2_ADV_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));
dataChunk = L2_ADV_FRAME;
L2_FrameInsertChunk(buff, L2_MSG_TYPE, L2_MSG_TYPE_LEN, &dataChunk);
Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
// insert position and speed info
L2_FrameInsertChunk(buff, L2_MSG_ADV_SRC_POSX, L2_MSG_ADV_SRC_POSX_LEN, &(instPtr->sysInfo.posX));
L2_FrameInsertChunk(buff, L2_MSG_ADV_SRC_POSY, L2_MSG_ADV_SRC_POSY_LEN, &(instPtr->sysInfo.posY));
L2_FrameInsertChunk(buff, L2_MSG_ADV_SRC_POSZ, L2_MSG_ADV_SRC_POSZ_LEN, &(instPtr->sysInfo.posZ));
// Insert net start time
L2_FrameInsertChunk(buff, L2_MSG_ADV_SRC_NETSTART_TIME, L2_MSG_ADV_SRC_NETSTART_TIME_LEN, &(instPtr->netStartTime));
// Append CRC
Status = L2_CrcAppend(buff, L2_ADV_PACK_LEN - L2_MSG_ADV_SVC_CRC_LEN, L2_MSG_ADV_SVC_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)
break;
}
case L2_CFM_FRAME:
{
dataChunk = L2_CFM_FRAME;
L2_FrameInsertChunk(buff, L2_MSG_TYPE, L2_MSG_TYPE_LEN, &dataChunk);
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
// insert position and speed info
L2_FrameInsertChunk(buff, L2_MSG_CFM_SRC_POSX, L2_MSG_CFM_SRC_POSX_LEN, &(instPtr->sysInfo.posX));
L2_FrameInsertChunk(buff, L2_MSG_CFM_SRC_POSY, L2_MSG_CFM_SRC_POSY_LEN, &(instPtr->sysInfo.posY));
L2_FrameInsertChunk(buff, L2_MSG_CFM_SRC_POSZ, L2_MSG_CFM_SRC_POSZ_LEN, &(instPtr->sysInfo.posZ));
L2_FrameInsertChunk(buff, L2_MSG_CFM_SRC_SPEEDX, L2_MSG_CFM_SRC_SPEEDX_LEN, &(instPtr->sysInfo.vX));
L2_FrameInsertChunk(buff, L2_MSG_CFM_SRC_SPEEDY, L2_MSG_CFM_SRC_SPEEDY_LEN, &(instPtr->sysInfo.vY));
L2_FrameInsertChunk(buff, L2_MSG_CFM_SRC_SPEEDZ, L2_MSG_CFM_SRC_SPEEDZ_LEN, &(instPtr->sysInfo.vZ));
// Append CRC
Status = L2_CrcAppend(buff, L2_CMF_PACK_LEN - L2_MSG_CFM_CRC_LEN, L2_MSG_CFM_CRC, L2_CRC16_1021);
if (Status != SUCCESS)
{
DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE;
DBG_ERROR("Fail to append CRC, error code: %ld.\n", Status);
return Status;
}
break;
}
@@ -89,7 +289,6 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type)
{
DBG_ERROR("Unexpected frame type: %d.\n", type);
return EPARAM;
break;
}
}
return SUCCESS;
@@ -104,14 +303,55 @@ 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_SCAN_FSM **scanFSMPtr)
{
ASSERT(instPtr != NULL);
instPtr->scanFsm.sacanCnt = 0;
instPtr->scanFsm.beamNo = 0;
ASSERT(scanFSMPtr != NULL);
*scanFSMPtr = (L2_SCAN_FSM *)malloc(sizeof(L2_SCAN_FSM));
if (*scanFSMPtr == NULL)
{
DBG_ERROR("Failed to allocate memory for scanFsmPtr.\n");
return ENOMEM;
}
L2_Scan_Reset(*scanFSMPtr);
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Reset ScanNet FSM
*
* @param scanFsmPtr a pointer to scan net finite state machine
*
* @return NONE
*
******************************************************************************/
void L2_Scan_Reset(L2_SCAN_FSM *scanFSMPtr)
{
ASSERT(scanFSMPtr != NULL);
scanFSMPtr->isLocked = false;
scanFSMPtr->isAdvRecvd = false;
scanFSMPtr->isAcked = false;
scanFSMPtr->scanCnt = 0;
scanFSMPtr->beamNo = 0;
scanFSMPtr->advTimeout = 0;
scanFSMPtr->cfmTimeout = 0;
scanFSMPtr->scanStage = L2_SCAN_NONE_STAGE;
}
/*****************************************************************************/
/**
* @brief Destroy ScanNet FSM, free memory
*
* @param scanFsmPtr a pointer to scan net finite state machine
*
* @return NONE
*
******************************************************************************/
void L2_Scan_Destroy(L2_SCAN_FSM *scanFSMPtr)
{
free(scanFSMPtr);
}
/*****************************************************************************/
/**
* @brief Scan net packet handler.
@@ -126,214 +366,172 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
ASSERT(instPtr != NULL);
ASSERT(msg != NULL);
int32_t Status;
switch (instPtr->frame.last)
L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM;
switch (L2_GetLastFrameType(instPtr))
{
case L2_SCAN_FRAME:
LockStateUpdate(scanFsmPtr, true);
StageUpdate(scanFsmPtr, L2_SCAN_ADV_STAGE);
case L2_ADV_FRAME:
{
if (instPtr->config.myType == L2_HEAD_NODE)
if (instPtr->config.myType == L2_NODE_HEAD)
{
DBG_ERROR("Unexpected call from head node, Only None-Head Node can handle L2_SVC_FRAME.\n");
DBG_ERROR("Unexpected call from head node, only member Node can handle L2_SCAN_FRAME and L2_ADV_FRAME.\n");
Status = EUNEXP;
break;
}
Status = AppendNode(instPtr->nodeInfoList, L2_HEAD_NODE_ID, L2_HEAD_NODE);
if (Status != SUCCESS)
uint8_t nodeId;
L2_FrameGetChunk(msg, L2_MSG_SRC_ID, L2_MSG_SRC_ID_LEN, &nodeId);
NODE_ENTRY *node;
node = L2_GetNodeById(instPtr->nodeList, nodeId);
if (node == NULL)
{
DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
node = L2_AppendNode(instPtr->nodeList, nodeId, L2_NODE_MEMBER);
if(node == NULL)
{
DBG_ERROR("Failed to append new node [id: %d]", nodeId);
Status = ENOMEM;
break;
}
}
break;
// Update node info.
float nodePos;
L2_FrameGetChunk(msg, L2_MSG_ADV_SRC_POSX, L2_MSG_ADV_SRC_POSX_LEN, &nodePos);
node->nodeInfo.pos.x = (double) nodePos;
L2_FrameGetChunk(msg, L2_MSG_ADV_SRC_POSY, L2_MSG_ADV_SRC_POSY_LEN, &nodePos);
node->nodeInfo.pos.y = (double) nodePos;
L2_FrameGetChunk(msg, L2_MSG_ADV_SRC_POSZ, L2_MSG_ADV_SRC_POSZ_LEN, &nodePos);
node->nodeInfo.pos.z = (double) nodePos;
if (L2_isCxn2Net(instPtr) == false)
{
uint32_t netstartTime, frameNum, slotNum;
// Update net start time.
L2_FrameGetChunk(msg, L2_MSG_ADV_SRC_NETSTART_TIME, L2_MSG_ADV_SRC_NETSTART_TIME_LEN, &netstartTime);
L2_EVENT netSetupDoneEvent = {
.event = L2_EV_NET_ADV_UPDATE,
.msg = &netstartTime};
AssertL2Event(instPtr, &netSetupDoneEvent);
// Time sync.
L2_FrameGetChunk(msg, L2_MSG_FRAME_NUM, L2_MSG_FRAME_NUM_LEN, &frameNum);
L2_FrameGetChunk(msg, L2_MSG_SLOT_NUM, L2_MSG_SLOT_NUM_LEN, &slotNum);
// TODO: Check
L2_TimeAlign(instPtr, frameNum, slotNum, true);
}
// trigger flag
AdvrecvStateUpdate(scanFsmPtr, true);
}
case L2_INFO_FRAME:
case L2_CFM_FRAME:
{
if (instPtr->config.myType != L2_HEAD_NODE)
uint8_t nodeId;
L2_FrameGetChunk(msg, L2_MSG_SRC_ID, L2_MSG_SRC_ID_LEN, &nodeId);
NODE_ENTRY *node;
node = L2_GetNodeById(instPtr->nodeList, nodeId);
if (node == NULL)
{
DBG_ERROR("Unexpected call from member node, Only Head Node can handle L2_SVC_FRAME.\n");
Status = EUNEXP;
break;
}
COORD coordTemp;
float tmpVal;
Status = AppendNode(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, L2_MEMBER_NODE);
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));
coordTemp.x = tmpVal;
memcpy(&tmpVal, msg + L2_MSG_INFO_POSY, sizeof(float));
coordTemp.y = tmpVal;
memcpy(&tmpVal, msg + L2_MSG_INFO_POSZ, sizeof(float));
coordTemp.z = tmpVal;
Status = UpdateNodePos(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
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));
coordTemp.x = tmpVal;
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDY, sizeof(float));
coordTemp.y = tmpVal;
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDZ, sizeof(float));
coordTemp.z = tmpVal;
Status = UpdateNodeSpeed(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
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)
{
DBG_ERROR("Failed to compute slot deviation.\n");
UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, 0);
}
else
{
if (devi > INT32_MAX || devi < INT32_MIN)
node = L2_AppendNode(instPtr->nodeList, nodeId, L2_NODE_MEMBER);
if(node == NULL)
{
DBG_ERROR("Illegal deviation value: %lld.\n", devi);
deviTrunc = 0;
DBG_ERROR("Failed to append new node [id: %d]", nodeId);
Status = ENOMEM;
break;
}
}
// Update node info.
float nodePos;
L2_FrameGetChunk(msg, L2_MSG_CFM_SRC_POSX, L2_MSG_CFM_SRC_POSX_LEN, &nodePos);
node->nodeInfo.pos.x = (double) nodePos;
L2_FrameGetChunk(msg, L2_MSG_CFM_SRC_POSY, L2_MSG_CFM_SRC_POSY_LEN, &nodePos);
node->nodeInfo.pos.y = (double) nodePos;
L2_FrameGetChunk(msg, L2_MSG_CFM_SRC_POSZ, L2_MSG_CFM_SRC_POSZ_LEN, &nodePos);
node->nodeInfo.pos.z = (double) nodePos;
float nodeSpeed;
L2_FrameGetChunk(msg, L2_MSG_CFM_SRC_SPEEDX, L2_MSG_CFM_SRC_SPEEDX_LEN, &nodeSpeed);
node->nodeInfo.speed.x = (double) nodePos;
L2_FrameGetChunk(msg, L2_MSG_CFM_SRC_SPEEDY, L2_MSG_CFM_SRC_SPEEDY_LEN, &nodeSpeed);
node->nodeInfo.speed.y = (double) nodePos;
L2_FrameGetChunk(msg, L2_MSG_CFM_SRC_SPEEDZ, L2_MSG_CFM_SRC_SPEEDZ_LEN, &nodeSpeed);
node->nodeInfo.speed.z = (double) nodePos;
// Net setup finished.
if (L2_GetMainState(instPtr) == L2_INIT_STATE)
{
// Assert Net-Setup Done.
L2_EVENT l2Event;
l2Event.event = L2_EV_NET_SETUP_DONE;
l2Event.msg = NULL;
AssertL2Event(instPtr, &l2Event);
}
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
{
// check if net setup finished.
if (L2_isCxn2Net(instPtr) == false)
{
// Assert update routine event...
L2_RTNE_CONFIG newRtneConfig = {0};
L2_EVENT rtneUpEvent =
{
.event = L2_ROUTINE_UPDATE,
.msg = &newRtneConfig
};
newRtneConfig.rtneTable = initTable;
newRtneConfig.rtnePrd = ARRAY_SIZE(initTable);
AssertL2Event(instPtr, &rtneUpEvent);
}
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
{
deviTrunc = (int32_t)devi;
}
UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc);
}
break;
}
default:
DBG_ERROR("Unsupported frame type: %ld.\n", instPtr->frame.last);
DBG_ERROR("Unsupported frame type: %d.\n", L2_GetLastFrameType(instPtr));
Status = EUNEXP;
break;
}
// Net setup finished.
if (L2_GetMainState(instPtr) == INIT_STATE && Status == SUCCESS)
{
L2_EVENT l2Event;
L2_RTNE_CONFIG newRtneConfig;
// Assert Net-Setup Done.
l2Event.event = L2_EV_NET_SETUP_DONE;
l2Event.msg = NULL;
AssertL2Event(instPtr, &l2Event);
// Assert Routine Table Update...
newRtneConfig.rtneTable = normalTable;
newRtneConfig.rtnePrd = ARRAY_SIZE(normalTable) * L2_SLOTS_PER_FRAME;
newRtneConfig.rtneStep = 0;
l2Event.event = L2_ROUTINE_UPDATE;
l2Event.msg = &newRtneConfig;
AssertL2Event(instPtr, &l2Event);
}
return Status;
}
/*****************************************************************************/
/**
* @brief Execute a member node scan net procedure.
*
* @param instPtr A pointer to scan net fsm.
* @param instPtr a pointer to linklayer layer instance
*
* @return used slot (5ms slot).
*
******************************************************************************/
int32_t L2_Scan_Proc(L2_INST *instPtr)
{
L2_SCAN_FSM *scanFSM = instPtr->scanFSM;
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
{
DBG_ERROR("Only member node needs to execute L2_Scan_Proc. \n");
return FAILURE;
}
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)
DBG_INFO("-------- ScanNet Process --------\n");
switch (scanFSM->scanStage)
{
DBG_INFO("NETSET DONE, halt 20ms before normal routine.\n");
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
{
DBG_ERROR("Illegal call from Head Node.\n");
}
return NUM_SLOTS_SKIP;
case L2_SCAN_NONE_STAGE:
/* Start Scan. */
StageUpdate(scanFSM, L2_SCAN_INIT_STAGE);
Status = Scan_InitScanHandler(instPtr);
break;
case L2_SCAN_INIT_STAGE:
Status = Scan_InitScanHandler(instPtr);
break;
case L2_SCAN_ADV_STAGE:
Status = Scan_AdvHandler(instPtr);
break;
case L2_SCAN_CFM_STAGE:
Status = Scan_CfmHandler(instPtr);
break;
default:
DBG_ERROR("Unexpected scan net stage: %d.\n", scanFSM->scanStage);
Status = FAILURE;
break;
}
/* Head node implement a periodical scan pattern
* | Tx 20ms | Rx 100ms | Tx 20ms | Rx 100ms ... ...
* Send pilot signal in Tx slots with narrow beam width.
* Wait for response in Rx slots with the same beam width and beam number.
* Increase beam number after a Tx-Rx procedure pair, reset beam number when
* it reaches the boundary.
*/
if (instPtr->config.myType == L2_HEAD_NODE)
{
// Apply a scan procedure
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
{
// 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_RX, L2_INFO_FRAME);
// Update Scan beam number
instPtr->scanFsm.beamNo = (instPtr->scanFsm.beamNo + 1) % L1_NUM_ANT_NRRW_PATTERN;
}
}
/* Member node listening the head node pilot signal with wide beam width.
* Once is received a pilot signal, physical layer will perform a response
* procedure automatically, witch should contains the basic node information
* (position, speed, etc.).
* So that, a extra datagram with the up-to-date node information is attached
* to the listening configuration.
*/
else
{
uint8_t *sendData;
// Pack node information.
Status = ScanMsg_Packing(instPtr, L2_INFO_FRAME);
if (Status != SUCCESS)
{
DBG_INFO("Failed to pack response.\n");
}
if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true)
{
memcpy(instPtr->sendPktData, sendData, RSP_PACK_LEN);
free(sendData);
}
else
{
DBG_INFO("Failed to pop tx Queue.\n");
}
// Antenna configure
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_LISTEN);
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_SCAN_FRAME);
}
// Increase scan counter (for debug purpose).
instPtr->scanFsm.sacanCnt++;
return NUM_SLOTS_SKIP;
return Status;
}
/*****************************************************************************/
@@ -341,14 +539,110 @@ int32_t L2_Scan_Proc(L2_INST *instPtr)
/**
* @brief Execute a scan net Routine procedure.
*
* @param schedEntry Routine configuration
* @param rtneEntry Routine configuration
*
* @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, const L2_RTNE_TABLE_ENTRY *rtneEntry)
{
ASSERT(instPtr != NULL);
L2_TOR_TYPE torState;
int32_t Status;
DBG_INFO("-------- Scan Routine --------\n");
ASSERT(false); // Not support.
return 0;
if (rtneEntry->slotSeq != 0)
{
return SUCCESS; // skip
}
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
{
if (rtneEntry->dst == L2_GetNodeId(instPtr) || rtneEntry->dst == L2_BROADCAST_ID)
{
torState = L2_TOR_RX;
}
else if (rtneEntry->src == L2_GetNodeId(instPtr) || rtneEntry->src == L2_BROADCAST_ID)
{
torState = L2_TOR_TX;
}
else
{
torState = L2_TOR_SUSPEND;
}
}
else
{
if (rtneEntry->dst == L2_GetNodeId(instPtr))
{
torState = L2_TOR_RX;
}
else if (rtneEntry->src == L2_GetNodeId(instPtr))
{
torState = L2_TOR_TX;
}
else
{
torState = L2_TOR_SUSPEND;
}
}
switch (rtneEntry->slotType)
{
case L2_SLOT_SCAN:
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD && torState == L2_TOR_TX)
{
DBG_ERROR("Only head node can send init-scan Frame.\n");
Status = FAILURE;
goto routine_err;
}
L2_SetupNextSlot(instPtr, torState, L2_SCAN_FRAME);
break;
case L2_SLOT_ADVERTISE:
if (torState == L2_TOR_TX)
{
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
{
DBG_ERROR("Only head node can send ADV Frame.\n");
Status = FAILURE;
goto routine_err;
}
memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX);
if (ScanMsg_Packing(instPtr, L2_ADV_FRAME, rtneEntry->dst) != SUCCESS)
{
Status = FAILURE;
goto routine_err;
}
}
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, torState, L2_ADV_FRAME);
break;
case L2_SLOT_CONFIRM:
if (torState == L2_TOR_TX)
{
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD && L2_isDataAvail(instPtr) == false)
{
DBG_INFO("Member node is not ready to send CFM frame.\n");
break;
}
memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX);
if (ScanMsg_Packing(instPtr, L2_CFM_FRAME, rtneEntry->dst) != SUCCESS)
{
Status = FAILURE;
goto routine_err;
}
}
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, torState, L2_CFM_FRAME);
break;
default:
DBG_ERROR("Cannot start a routine for slot type [%d] properly.\n", rtneEntry->slotType);
Status = FAILURE;
goto routine_err;
}
return SUCCESS;
routine_err:
return Status;
}
df_datalink_sp/src/networking/link_layer/schdevent.c
+430
View File
@@ -0,0 +1,430 @@
/*****************************************************************************/
/**
* Description: Event scheduling implementation
*
* Date: 2023-12-21 13:32
* LastEditors: Fan Hao
* LastEditTime: 2023-12-21 13:32
* Revisions: Rev 0.0
* History:
* [2023-12-21]: Initial version.
*
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Networking
*
******************************************************************************/
/***************************** Include Files *********************************/
#include <stdlib.h>
#include "schdevent.h"
#include "link_layer.h"
#include "link_layer_types.h"
#include "error.h"
#include "clist.h"
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
/************************** Variable Definitions *****************************/
static L2_SCHD_EVENT util_schdevent;
/************************** Function Prototypes ******************************/
/**
* @brief helper function to free schedule events.
*/
static void schdevent_free(void *val)
{
if (val == NULL)
{
return;
}
free(((L2_SCHD_EVENT *)val)->l2Event.msg);
free(val);
}
/**
* @brief helper function to match events by ids.
*/
static int8_t schdevent_compare(void *a, void *b)
{
ASSERT(a == NULL);
ASSERT(b != NULL);
register uint32_t idA, idB;
idA = ((L2_SCHD_EVENT *)a)->eventId;
idB = ((L2_SCHD_EVENT *)b)->eventId;
if (idA == idB)
{
return 0;
}
else if (idA < idB)
{
return -1;
}
else
{
return 1;
}
}
/*****************************************************************************/
/**
* @brief Creat a schedule event pool.
*
* @param schdPool a pointer to schedule pool instance.
*
* @return SUCCESS
*
******************************************************************************/
int32_t L2_CreateSchedEventPool(L2_SCHD_POOL **schdPool)
{
(*schdPool) = malloc(sizeof(L2_SCHD_POOL));
if (*schdPool == NULL)
{
DBG_ERROR("Failed to allocated memory for schdPool.\n");
return ENOMEM;
}
(*schdPool)->schdList = clist_new();
if ((*schdPool)->schdList == NULL)
{
DBG_ERROR("Failed to allocated memory for schdPool->schdList.\n");
free(*schdPool);
return ENOMEM;
}
(*schdPool)->schdList->free = schdevent_free;
(*schdPool)->schdList->compare = schdevent_compare;
(*schdPool)->envCnt = 0;
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Clear schedule event pool, delete all events in pool and
* free allocated memory.
*
* @param schdPool a pointer to schedule pool instance.
*
* @return SUCCESS
*
******************************************************************************/
void L2_ClearSchedEventPool(L2_SCHD_POOL *schdPool)
{
ASSERT(schdPool != NULL);
clist_clear(schdPool->schdList);
schdPool->envCnt = 0;
}
/*****************************************************************************/
/**
* @brief Completely destroy schedule event pool.
*
* @param schdPool a pointer to schedule pool instance.
*
* @return SUCCESS
*
******************************************************************************/
void L2_DestroySchedEventPool(L2_SCHD_POOL *schdPool)
{
if (schdPool == NULL)
{
return;
}
clist_destroy(schdPool->schdList);
free(schdPool);
}
/*****************************************************************************/
/**
* @brief Creat a new schedule event, register event in schedule event pool.
*
* @param schdPool a pointer to schedule pool instance.
* @param event event type of the new schedule event.
* @param isVolatile event type of the new schedule event.
*
* @return NULL error occurred
* Pointer to schedule event.
*
******************************************************************************/
L2_SCHD_EVENT *L2_CreatNewSchdEvent(L2_SCHD_POOL *schdPool, L2_EVENT_TYPE event, bool isVolatile)
{
ASSERT(schdPool != NULL);
if (event < L2_EV_SCAN_PROC_DONE || event > L2_ROUTINE_UPDATE)
{
DBG_ERROR("Unknown event type: %d.\n", event);
return NULL;
}
L2_SCHD_EVENT *schdEvent = (L2_SCHD_EVENT *)malloc(sizeof(L2_SCHD_EVENT));
if (schdEvent == NULL)
{
DBG_ERROR("Failed to allocate schedule event.\n");
return NULL;
}
clist_node_t *node = clist_node_new(schdEvent);
if (node == NULL)
{
DBG_ERROR("Failed to allocate schedule event list node.\n");
free(schdEvent);
return NULL;
}
schdEvent->eventId = schdPool->envCnt++;
schdEvent->occurCnt = 0;
schdEvent->l2Event.event = event;
schdEvent->l2Event.msg = NULL;
schdEvent->isSchd = false;
schdEvent->isVolatile = isVolatile;
schdEvent->schdSlot = L2_SLOTNO_NONE;
schdEvent->schdFrame = L2_FRAMENO_NONE;
clist_rpush(schdPool->schdList, node);
return schdEvent;
}
/*****************************************************************************/
/**
* @brief Get a schedule event by its id.
*
* @param schdPool a pointer to schedule pool instance.
* @param id ID of the event
*
* @return NULL
* Pointer to the schedule event.
*
******************************************************************************/
L2_SCHD_EVENT *L2_GetEventById(L2_SCHD_POOL *schdPool, uint32_t id)
{
ASSERT(schdPool != NULL);
util_schdevent.eventId = id;
clist_node_t *node = clist_find(schdPool->schdList, &util_schdevent);
if (node != NULL)
{
DBG_WARNING("Event with id=%d, dose not exist.\n", id);
return NULL;
}
return ((L2_SCHD_EVENT *)node->val);
}
/*****************************************************************************/
/**
* @brief Delete a schedule event, remove the event from event pool,
* release memory.
*
* @param schdPool a pointer to schedule pool instance.
* @param id ID of the event
*
* @return SUCCESS
* error code
*
******************************************************************************/
int32_t L2_DeleteSchdEventById(L2_SCHD_POOL *schdPool, uint32_t id)
{
ASSERT(schdPool != NULL);
util_schdevent.eventId = id;
clist_node_t *node = clist_find(schdPool->schdList, &util_schdevent);
if (node != NULL)
{
DBG_ERROR("Event with id=%d, dose not exist.\n", id);
return EIGNORE;
}
clist_remove(schdPool->schdList, node);
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Schedule a event at a specific time(indexed by frame and slot number)
*
* @param instPtr a pointer to linklayer instance
* @param schdEvent pointer to the schedule event.
* @param frameNum schedule frame number
* @param slotNum schedule slot number
*
* @return FAILURE, event has already been scheduled/schedule time not allowed.
* SUCCESS
*
******************************************************************************/
int32_t L2_ScheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t frameNum, uint32_t slotNum)
{
ASSERT(schdEvent != NULL);
if (L2_isEventScheduled(schdEvent) == true)
{
DBG_ERROR("Event %d has already been scheduled.\n", schdEvent->eventId);
return FAILURE;
}
if (frameNum <= L2_GetCurrentFrameNum(instPtr) && slotNum < L2_GetCurrentSlotNum(instPtr))
{
DBG_ERROR("Cannot schedule event %d to the past(current: [%u-%u], schedule: [%u-%u]).\n",
schdEvent->eventId, L2_GetCurrentFrameNum(instPtr), L2_GetCurrentSlotNum(instPtr), frameNum, slotNum);
return FAILURE;
}
DBG_INFO("Schedule event (type:%d) [%u] at %u-%u.\n",
schdEvent->l2Event.event, schdEvent->eventId,
frameNum, slotNum);
schdEvent->isSchd = true;
schdEvent->schdFrame = frameNum;
schdEvent->schdSlot = slotNum;
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Schedule a event after a specific time period.
*
* @param instPtr a pointer to linklayer instance
* @param schdEvent pointer to the schedule event.
* @param numSlots how many slot later the event should be scheduled
*
* @return FAILURE, event has already been scheduled/schedule time not allowed.
* SUCCESS
*
******************************************************************************/
int32_t L2_ScheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t numSlots)
{
ASSERT(schdEvent != NULL);
if (L2_isEventScheduled(schdEvent) == true)
{
DBG_ERROR("Event %d has already been scheduled.\n", schdEvent->eventId);
return FAILURE;
}
uint32_t frameNum = L2_GetCurrentFrameNum(instPtr) + (numSlots / L2_SLOTS_PER_FRAME);
uint32_t slotNum = L2_GetCurrentSlotNum(instPtr) + (numSlots % L2_SLOTS_PER_FRAME);
return L2_ScheduleEventAt(instPtr, schdEvent, frameNum, slotNum);
}
/*****************************************************************************/
/**
* @brief Reschedule a event at a specific time(indexed by frame and slot number)
*
* @param instPtr a pointer to linklayer instance
* @param schdEvent pointer to the schedule event.
* @param frameNum schedule frame number
* @param slotNum schedule slot number
*
* @return FAILURE, event has already been scheduled/schedule time not allowed.
* SUCCESS
*
******************************************************************************/
int32_t L2_RescheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t frameNum, uint32_t slotNum)
{
ASSERT(schdEvent != NULL);
if (schdEvent->isSchd == false)
{
DBG_WARNING("Event %d has not been scheduled yet.\n", schdEvent->eventId);
}
schdEvent->isSchd = false;
return L2_ScheduleEventAt(instPtr, schdEvent, frameNum, slotNum);
}
/*****************************************************************************/
/**
* @brief Reschedule a event after a specific time period.
*
* @param instPtr a pointer to linklayer instance
* @param schdEvent pointer to the schedule event.
* @param numSlots how many slot later the event should be scheduled
*
* @return FAILURE, event has already been scheduled/schedule time not allowed.
* SUCCESS
*
******************************************************************************/
int32_t L2_RescheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t numSlots)
{
ASSERT(schdEvent != NULL);
if (schdEvent->isSchd == false)
{
DBG_WARNING("Event %d has not been scheduled yet.\n", schdEvent->eventId);
}
return L2_ScheduleEventAfter(instPtr, schdEvent, numSlots);
}
/*****************************************************************************/
/**
* @brief Check and excute schedule event in event pool illiterate.
*
* @param instPtr a pointer to linklayer instance
*
* @return NONE.
*
******************************************************************************/
void L2_EventPoolHandler(L2_INST *instPtr)
{
ASSERT(instPtr != NULL);
clist_node_t *node;
L2_SCHD_EVENT *event;
L2_SCHD_POOL *pool = instPtr->schdEventPool;
clist_iterator_t *iter = clist_iterator_new(pool->schdList, LIST_HEAD);
if (iter == NULL)
{
DBG_ERROR("Failed to initialize schedule event pool iteration.\n");
return;
}
while ((node = clist_iterator_next(iter)))
{
event = (L2_SCHD_EVENT *)node->val;
if (L2_isEventScheduled(event))
{
if (event->schdFrame == L2_GetCurrentFrameNum(instPtr) && event->schdSlot == L2_GetCurrentSlotNum(instPtr))
{
DBG_INFO("Asserting scheduled event [%d].\n", event->eventId);
AssertL2Event(instPtr, &(event->l2Event));
if (event->isVolatile == true)
{
DBG_INFO("Deleting scheduled event [%d], since it is volatile.\n", event->eventId);
clist_remove(pool->schdList, node);
}
else
{
event->occurCnt++;
}
}
}
}
clist_iterator_destroy(iter);
}
/*****************************************************************************/
/**
* @brief Append extra message content to specific schedule event.
*
* @param schdEvent pointer to the schedule event.
* msg pointer to the append content.
*
* @return SUCCESS.
* error code.
*
******************************************************************************/
inline void L2_SchdEventAppendMsg(L2_SCHD_EVENT *schdEvent, void *msg)
{
ASSERT(schdEvent != NULL);
schdEvent->l2Event.msg = msg;
}
/*****************************************************************************/
/**
* @brief Cancel a scheduled event.
*
* @param schdEvent pointer to the schedule event.
*
* @return SUCCESS.
*
******************************************************************************/
inline int32_t L2_CancelEvent(L2_SCHD_EVENT *schdEvent)
{
ASSERT(schdEvent != NULL);
if (schdEvent->isSchd == false)
{
DBG_WARNING("Event %d has not been scheduled yet.\n", schdEvent->eventId);
}
schdEvent->isSchd = false;
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Check if the event was scheduled.
*
* @param schdEvent pointer to the schedule event.
*
* @return true.
* false.
*
******************************************************************************/
inline bool L2_isEventScheduled(const L2_SCHD_EVENT *schdEvent)
{
ASSERT(schdEvent != NULL);
return schdEvent->isSchd;
}
df_datalink_sp/src/networking/link_layer/schdevent.h
+65
View File
@@ -0,0 +1,65 @@
/*****************************************************************************/
/**
* Description: Event scheduling implementation
*
* Date: 2023-12-21 13:32
* LastEditors: Fan Hao
* LastEditTime: 2023-12-21 13:32
* Revisions: Rev 0.0
* History:
* [2023-12-21]: Initial version.
*
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Networking
*
******************************************************************************/
#ifndef SCHDEVENT_H_
#define SCHDEVENT_H_
#define SCHDEVENT_ID_NONE UINT32_MAX
/***************************** Include Files *********************************/
#include <stdlib.h>
#include "link_layer_types.h"
#include "error.h"
#include "clist.h"
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
/** @name L2 event
* A definition of L2 event instance.
*/
typedef struct
{
bool isSchd;
bool isVolatile;
L2_EVENT l2Event;
uint32_t eventId;
uint32_t occurCnt;
uint32_t schdFrame;
uint32_t schdSlot;
} L2_SCHD_EVENT;
/** @name L2 event
* A definition of L2 event instance.
*/
typedef struct
{
clist_t *schdList;
uint32_t envCnt;
} L2_SCHD_POOL;
/************************** Variable Definitions *****************************/
/************************** Function Prototypes ******************************/
// Scheduling Event
int32_t L2_CreateSchedEventPool(L2_SCHD_POOL **schdPool);
void L2_ClearSchedEventPool(L2_SCHD_POOL *schdPool);
void L2_DestroySchedEventPool(L2_SCHD_POOL *schdPool);
L2_SCHD_EVENT *L2_CreatNewSchdEvent(L2_SCHD_POOL *schdPool, L2_EVENT_TYPE event, bool isVolatile);
L2_SCHD_EVENT *L2_GetEventById(L2_SCHD_POOL *schdPool, uint32_t id);
int32_t L2_DeleteSchdEventById(L2_SCHD_POOL *schdPool, uint32_t id);
void L2_SchdEventAppendMsg(L2_SCHD_EVENT *schdEvent, void *msg);
bool L2_isEventScheduled(const L2_SCHD_EVENT *schdEvent);
int32_t L2_CancelEvent(L2_SCHD_EVENT *schdEvent);
#endif // SCHDEVENT_H_
df_datalink_sp/src/networking/link_layer/service.c
+171 -362
View File
@@ -16,167 +16,110 @@
/***************************** Include Files *********************************/
#include <string.h>
#include <stdint.h>
#include "link_layer.h"
#include "link_layer_types.h"
#include "msg_def.h"
#include "error.h"
#include "common.h"
#include "circ_queue.h"
#include "msg_def.h"
#include "link_layer.h"
#include "link_layer_types.h"
#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
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
/************************** Function Prototypes ******************************/
static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type);
static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
/************************** Variable Definitions *****************************/
#ifdef DATALINK_ENABLE_BERTEST
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};
#endif
static uint8_t testData[L2_PACK_LEN_MAX] =
{
0x39, 0x85, 0x02, 0xF0, 0xC7, 0x6A, 0xE8, 0xB4, 0xE7, 0xAB, 0xC9, 0xDC, 0x0B, 0x41, 0x42, 0xE0,
0xED, 0x21, 0x07, 0x62, 0x4E, 0x80, 0xA0, 0xE1, 0xAC, 0x77, 0x9B, 0x1F, 0x48, 0x0B, 0x58, 0xB9,
0x45, 0xA5, 0x24, 0xEF, 0xF3, 0x71, 0x2E, 0x84, 0x6C, 0x22, 0xB3, 0x05, 0xEB, 0x75, 0xF2, 0x0B,
0x45, 0x73, 0x01, 0xA8, 0x1E, 0x00, 0x21, 0x3E, 0x03, 0x86, 0x55, 0x59, 0x98, 0x5C, 0x61, 0xA2,
0xE8, 0x20, 0x0E, 0x7A, 0xD5, 0x72, 0xAF, 0xF4, 0xBC, 0xB3, 0x6F, 0xFE, 0xB3, 0x78, 0x6A, 0x59,
0x73, 0xBD, 0x97, 0xBE, 0x44, 0xA4, 0x9B, 0x71, 0x11, 0x45, 0x37, 0xFE, 0xB3, 0x71, 0xEF, 0x1E,
0x3A, 0x2B, 0x58, 0x07, 0xF6, 0x01, 0x1C, 0x73, 0x28, 0x11, 0xA8, 0x15, 0xA2, 0x58, 0x32, 0x2E,
0x7D, 0x35, 0x9C, 0xE4, 0x98, 0x7F, 0x9C, 0x81, 0x32, 0xAA, 0xC4, 0x87, 0x16, 0x26, 0x23, 0x95,
0x57, 0xC9, 0xD4, 0x34, 0xCE, 0x63, 0x78, 0x7E, 0xD4, 0x3C, 0x16, 0x1D, 0x80, 0xC0, 0x55, 0xBF,
0xB5, 0xD6, 0x0C, 0xA5, 0xB9, 0x0A, 0x14, 0x5B, 0x4D, 0x52, 0xF2, 0xA9, 0x7A, 0x08, 0x67, 0x8E,
0xEB, 0x67, 0x34, 0x7F, 0x7A, 0x35, 0xAF, 0xD9, 0x07, 0x52, 0x30, 0x38, 0x70, 0xC9, 0x41, 0xF8,
0x36, 0x61, 0x32, 0x72, 0xA4, 0x7A, 0xEA, 0x21, 0xB4, 0xBF, 0xAA, 0xCF, 0x93, 0x4F, 0xE1, 0x5C,
0xA0, 0xFB, 0xD0, 0x2B, 0xA3, 0x82, 0x2E, 0x69, 0x7D, 0xB7, 0x76, 0x88, 0xF7, 0x5C, 0xF3, 0x82,
0xC6, 0x6D, 0x49, 0x92, 0x64, 0x88, 0x9A, 0x43, 0xEE, 0xE9, 0x93, 0xC3, 0x70, 0x50, 0x22, 0xF1,
0xD3, 0x2B, 0xCF, 0xA9, 0xE5, 0x69, 0x22, 0xF9, 0xBF, 0x78, 0x9A, 0xA4, 0x5A, 0x3D, 0x2E, 0x2A,
0x23, 0xE3, 0x51, 0x8F, 0x1A, 0xC1, 0xAF, 0x47, 0x9C, 0x14, 0xCE, 0xB9, 0xCD, 0xD0, 0x02, 0xB7,
0xB8, 0x81, 0xCC, 0x44, 0xE7, 0xA2, 0x6F, 0x32, 0x8C, 0x78, 0x79, 0x10, 0xF3, 0xE1, 0xB0, 0x7C,
0xAB, 0x40, 0xDB, 0xF9, 0x99, 0xE7, 0x27, 0x30, 0x53, 0x67, 0x00, 0xE8, 0x33, 0x6D, 0xFC, 0x4D,
0xDC, 0xB0, 0xFC, 0x3D, 0xEF, 0x8D, 0x43, 0xAF, 0x7E, 0x41, 0x69, 0xDA, 0xFD, 0xDF, 0x2F, 0xF8,
0xEC, 0xFB, 0x6A, 0x65, 0xE7, 0xEC, 0xC7, 0x53, 0x1B, 0x14, 0xDF, 0xBD, 0x0C, 0x54, 0x57, 0xD2,
0x56, 0x15, 0xF4, 0x34, 0xE2, 0xFB, 0x12, 0xA0, 0xA2, 0xDE, 0x82, 0xF5, 0x04, 0x81, 0x9B, 0x08,
0xB6, 0x8C, 0x7B, 0xE0, 0xAA, 0xDF, 0xC9, 0x92, 0x9D, 0x5F, 0x2D, 0xF7, 0x4A, 0x0B, 0xFF, 0xA5,
0xAA, 0xDC, 0xD6, 0x3F, 0x56, 0xB2, 0x6A, 0x3C, 0x6F, 0xAA, 0xAC, 0x84, 0xAC, 0x3F, 0x3D, 0x05,
0x96, 0x12, 0x44, 0x39, 0x1C, 0xD1, 0xFA, 0xBE, 0x6B, 0x1A, 0xCD, 0x31, 0x29, 0x41, 0x31, 0xA3,
0xA8, 0x0D, 0x87, 0xEB, 0x96, 0x21, 0x39, 0xD6, 0x3B, 0x11, 0x50, 0xB5, 0xFE, 0x44, 0x0E, 0x74,
0x31, 0x12, 0xA1, 0x66, 0x49, 0xFD, 0x2D, 0x75, 0x72, 0xF9, 0xC8, 0xD9, 0x6C, 0xC9, 0x11, 0x74,
0x2E, 0xEB, 0x20, 0x33, 0x3D, 0x2C, 0x5E, 0xEA, 0x39, 0x31, 0x88, 0xC7, 0xD1, 0x93, 0x95, 0xD8,
0x9E, 0x87, 0x93, 0xBC, 0x09, 0x17, 0xC9, 0xA2, 0x59, 0x18, 0x51, 0x8F, 0xBF, 0x55, 0x45, 0x81,
0x8D, 0xFD, 0xAF, 0xB2, 0x21, 0xB7, 0xD8, 0xA8, 0x00, 0x2E, 0x70, 0x73, 0x2F, 0x74, 0xBC, 0x61,
0x0A, 0x8E, 0xE3, 0x0A, 0x92, 0x82, 0xD2, 0x82, 0xBA, 0xE4, 0x3E, 0x8E, 0xBB, 0xC7, 0xA2, 0x8F,
0x68, 0xE1, 0x29, 0x34, 0x7C, 0xB4, 0x22, 0xCF, 0x36, 0x8F, 0x9D, 0x63, 0x9D, 0x04, 0x8F, 0x4A,
0xFB, 0x84, 0xBB, 0x6B, 0x97, 0x2C, 0x15, 0xC9, 0x37, 0x4F, 0x40, 0xD7, 0x47, 0x50, 0x39, 0x97
};
/*****************************************************************************/
/**
* @brief Packing UDU Header to a bit array.
* @brief Packing Service message
*
* @param bitarr point32_ter to bit array buffer.
* dst Destination node address.
* dataLen Length of UDU data.
* @param instPtr pointer to linklayer instance
* type message type.
* dstId destination node address.
*
* @return EPARAM
* 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, uint8_t dstId)
{
#ifndef DATALINK_ENABLE_BERTEST
ASSERT(instPtr != NULL);
int32_t Status;
uint8_t *buff;
uint32_t packLen;
uint32_t dataChunk;
uint8_t *buff = instPtr->sendPktData;
dataChunk = L2_GetNextFrameNum(instPtr);
L2_FrameInsertChunk(buff, L2_MSG_FRAME_NUM, L2_MSG_FRAME_NUM_LEN, &dataChunk);
dataChunk = L2_GetNextSlotNum(instPtr);
L2_FrameInsertChunk(buff, L2_MSG_SLOT_NUM, L2_MSG_SLOT_NUM_LEN, &dataChunk);
dataChunk = L2_GetNodeId(instPtr);
L2_FrameInsertChunk(buff, L2_MSG_SRC_ID, L2_MSG_SRC_ID_LEN, &dataChunk);
dataChunk = dstId;
L2_FrameInsertChunk(buff, L2_MSG_DST_ID, L2_MSG_DST_ID_LEN, &dataChunk);
switch (type)
{
case L2_SVC_FRAME:
{
const NODE_ENTRY *nodeEntry;
int32_t timeDevi;
packLen = SVC_PACK_LEN;
dataChunk = L2_SVC_FRAME;
L2_FrameInsertChunk(buff, L2_MSG_TYPE, L2_MSG_TYPE_LEN, &dataChunk);
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;
}
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));
// insert position and speed info
L2_FrameInsertChunk(buff, L2_MSG_SVC_SRC_POSX, L2_MSG_SVC_SRC_POSX_LEN, &(instPtr->sysInfo.posX));
L2_FrameInsertChunk(buff, L2_MSG_SVC_SRC_POSY, L2_MSG_SVC_SRC_POSY_LEN, &(instPtr->sysInfo.posY));
L2_FrameInsertChunk(buff, L2_MSG_SVC_SRC_POSZ, L2_MSG_SVC_SRC_POSZ_LEN, &(instPtr->sysInfo.posZ));
L2_FrameInsertChunk(buff, L2_MSG_SVC_SRC_SPEEDX, L2_MSG_SVC_SRC_SPEEDX_LEN, &(instPtr->sysInfo.vX));
L2_FrameInsertChunk(buff, L2_MSG_SVC_SRC_SPEEDY, L2_MSG_SVC_SRC_SPEEDY_LEN, &(instPtr->sysInfo.vY));
L2_FrameInsertChunk(buff, L2_MSG_SVC_SRC_SPEEDZ, L2_MSG_SVC_SRC_SPEEDZ_LEN, &(instPtr->sysInfo.vZ));
Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
// packing payload
L2_FrameInsertChunk(buff, L2_MSG_SVC_PAYLOAD, L2_MSG_SVC_PAYLOAD_LEN, testData);
Status = L2_CrcAppend(buff, L2_SVC_PACK_LEN - L2_MSG_SVC_CRC_LEN, L2_MSG_SVC_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:
@@ -195,29 +138,60 @@ static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type)
/*****************************************************************************/
/**
* @brief Initialize scan FSM.
* @brief Initialize service net FSM
*
* @param instPtr
* @param serviceFSM A pointer to service FSM instance.
*
* @return
* @return SUCCESS Initialize successfully.
* Other Initialize failed.
*
******************************************************************************/
int32_t L2_Service_Init(L2_INST *instPtr)
int32_t L2_Service_Init(L2_SVC_FSM **svcFSMPtr)
{
ASSERT(instPtr != NULL);
instPtr->serviceFsm.txSeqNum = 0;
instPtr->serviceFsm.rxSeqNum = 0;
memset((&instPtr->serviceFsm.rxLog), 0x00, sizeof(PAYLOAD));
memset((&instPtr->serviceFsm.txLog), 0x00, sizeof(PAYLOAD));
instPtr->serviceFsm.pktRecv = 0;
instPtr->serviceFsm.pktRecved = 0;
#ifdef DATALINK_ENABLE_BERTEST
instPtr->serviceFsm.bitTotal = 0;
instPtr->serviceFsm.bitError = 0;
#endif
ASSERT(svcFSMPtr != NULL);
*svcFSMPtr = (L2_SVC_FSM *)malloc(sizeof(L2_SVC_FSM));
if (*svcFSMPtr == NULL)
{
return ENOMEM;
}
L2_Service_Reset(*svcFSMPtr);
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Reset ServiceNet FSM
*
* @param scanFsmPtr a pointer to service net finite state machine
*
* @return NONE
*
******************************************************************************/
void L2_Service_Reset(L2_SVC_FSM *svcFSMPtr)
{
ASSERT(svcFSMPtr != NULL);
memset((&svcFSMPtr->rxLog), 0x00, sizeof(PAYLOAD));
memset((&svcFSMPtr->txLog), 0x00, sizeof(PAYLOAD));
#ifdef DATALINK_ENABLE_BERTEST
svcFSMPtr->bitTotal = 0;
svcFSMPtr->bitError = 0;
#endif
}
/*****************************************************************************/
/**
* @brief Destroy ServiceNet FSM, free memory
*
* @param scanFsmPtr a pointer to service net finite state machine
*
* @return NONE
*
******************************************************************************/
void L2_Service_Destroy(L2_SVC_FSM *serviceFSM)
{
free(serviceFSM);
}
/*****************************************************************************/
/**
* @brief Handle service received packet. Send data up to MPC
@@ -232,178 +206,51 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg)
{
ASSERT(instPtr != NULL);
ASSERT(msg != NULL);
#ifndef DATALINK_ENABLE_BERTEST
int32_t Status;
switch (instPtr->frame.last)
uint8_t frameType;
L2_FrameGetChunk(msg, L2_MSG_TYPE, L2_MSG_TYPE_LEN, &frameType);
switch (frameType)
{
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;
}
// Implement a auto time sync.
uint32_t seqNum;
L2_EVENT l2Event;
l2Event.event = L2_EV_TIME_SYNC;
seqNum = msg[L2_MSG_SVC_SEQ_ID];
#ifdef DATALINK_ENABLE_AUTOTIMESYNC
l2Event.msg = NULL;
AssertL2Event(instPtr, &l2Event);
#else
int64_t timeAdj;
l2Event.msg = &timeAdj;
// Get Time Deviation
timeAdj = 0;
memcpy(&timeAdj, msg + L2_MSG_SVC_TMSYNC, L2_MSG_SVC_TMSYNC_LEN);
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)
{
AssertL2Event(instPtr, &l2Event);
}
// Implement a auto time sync.
uint32_t frameNum, slotNum;
L2_FrameGetChunk(msg, L2_MSG_FRAME_NUM, L2_MSG_FRAME_NUM_LEN, &frameNum);
L2_FrameGetChunk(msg, L2_MSG_SLOT_NUM, L2_MSG_SLOT_NUM_LEN, &slotNum);
L2_TimeAlign(instPtr, frameNum, slotNum, true);
#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)
uint8_t nodeId;
L2_FrameGetChunk(msg, L2_MSG_SRC_ID, L2_MSG_SRC_ID_LEN, &nodeId);
NODE_ENTRY *node;
node = L2_GetNodeById(instPtr->nodeList, nodeId);
if (node == NULL)
{
// if last packet reset counting
if (seqNum >= (PAYLOAD_FLJ - 1) / L2_MSG_SVC_PAYLOAD + 1)
node = L2_AppendNode(instPtr->nodeList, nodeId, L2_NODE_MEMBER);
if(node == NULL)
{
if (seqNum == instPtr->serviceFsm.rxSeqNum)
{
SysMonitor_UpdateRecvPayloadLJRaw(instPtr->serviceFsm.rxLog);
}
instPtr->serviceFsm.rxSeqNum = 0;
}
else
{
if (instPtr->serviceFsm.rxSeqNum == seqNum)
{
instPtr->serviceFsm.rxSeqNum++;
}
}
}
else
{
// if last packet reset counting
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)
{
instPtr->serviceFsm.rxSeqNum++;
}
DBG_ERROR("Failed to append new node [id: %d]", nodeId);
Status = ENOMEM;
break;
}
}
#else
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;
#endif
instPtr->serviceFsm.pktRecved++;
// Reset Node TTL
Status = RstNodeTTL(instPtr->nodeInfoList, L2_HEAD_NODE_ID);
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)
{
DBG_ERROR("Unexpected call, Only Head Node can handle L2_SVC_FRAME.\n");
Status = EUNEXP;
break;
}
#ifndef DATALINK_ENABLE_BERTEST
COORD coordTemp;
float tmpVal;
memcpy(&tmpVal, msg + L2_MSG_INFO_POSX, sizeof(float));
coordTemp.x = tmpVal;
memcpy(&tmpVal, msg + L2_MSG_INFO_POSY, sizeof(float));
coordTemp.y = tmpVal;
memcpy(&tmpVal, msg + L2_MSG_INFO_POSZ, sizeof(float));
coordTemp.z = tmpVal;
Status = UpdateNodePos(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
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));
coordTemp.x = tmpVal;
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDY, sizeof(float));
coordTemp.y = tmpVal;
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDZ, sizeof(float));
coordTemp.z = tmpVal;
Status = UpdateNodeSpeed(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS)
{
DBG_ERROR("Failed to update node %d speed, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
}
// TODO: Check
// Update Time deviation
int64_t devi, deviTrunc;
Status = L2_TimeSync_GetDeviatoion(instPtr, &devi);
if (Status != SUCCESS)
{
DBG_ERROR("Failed to compute slot deviation.\n");
deviTrunc = 0;
}
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)
{
DBG_WARNING("Deviation value: %lld is out of adjustable range.\n", devi);
deviTrunc = (int32_t)devi;
}
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)
{
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]);
}
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)
{
DBG_ERROR("Failed to reset node %d ttl, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
}
// Update node info.
float nodePos;
L2_FrameGetChunk(msg, L2_MSG_SVC_SRC_POSX, L2_MSG_SVC_SRC_POSX_LEN, &nodePos);
node->nodeInfo.pos.x = (double) nodePos;
L2_FrameGetChunk(msg, L2_MSG_SVC_SRC_POSY, L2_MSG_SVC_SRC_POSY_LEN, &nodePos);
node->nodeInfo.pos.y = (double) nodePos;
L2_FrameGetChunk(msg, L2_MSG_SVC_SRC_POSZ, L2_MSG_SVC_SRC_POSZ_LEN, &nodePos);
node->nodeInfo.pos.z = (double) nodePos;
float nodeSpeed;
L2_FrameGetChunk(msg, L2_MSG_SVC_SRC_SPEEDX, L2_MSG_SVC_SRC_SPEEDX_LEN, &nodeSpeed);
node->nodeInfo.speed.x = (double) nodePos;
L2_FrameGetChunk(msg, L2_MSG_SVC_SRC_SPEEDY, L2_MSG_SVC_SRC_SPEEDY_LEN, &nodeSpeed);
node->nodeInfo.speed.y = (double) nodePos;
L2_FrameGetChunk(msg, L2_MSG_SVC_SRC_SPEEDZ, L2_MSG_SVC_SRC_SPEEDZ_LEN, &nodeSpeed);
node->nodeInfo.speed.z = (double) nodePos;
break;
}
default:
@@ -411,6 +258,16 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg)
break;
}
return Status;
#else
uint32_t errorBits = 0;
for (int i = 0; i < SVC_PACK_LEN; i++)
{
errorBits += count_ones(msg[i] ^ testData[i]);
}
instPtr->serviceFsm.bitError += errorBits;
instPtr->serviceFsm.bitTotal += SVC_PACK_LEN * 8;
#endif
}
/*****************************************************************************/
@@ -423,102 +280,54 @@ 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, const L2_RTNE_TABLE_ENTRY *rtneEntry)
{
ASSERT(instPtr != NULL);
DBG_INFO("-------- ServiceNet Routine --------\n");
uint8_t *sendData;
int32_t usedSlots;
PHY_INST* phyRef = instPtr->config.phyInstPtr;
L2_TOR_TYPE torState;
if (schedEntry->src == instPtr->config.myId)
if (rtneEntry->src == L2_GetNodeId(instPtr))
{
torState = L2_TOR_TX;
}
else if (schedEntry->dst == instPtr->config.myId)
else if (rtneEntry->dst == L2_GetNodeId(instPtr))
{
torState = L2_TOR_RX;
instPtr->serviceFsm.pktRecv++;
}
else
{
torState = L2_TOR_SUSPEND;
}
switch (schedEntry->slotType)
switch (rtneEntry->slotType)
{
case L2_SLOT_SERVICE:
if (torState == L2_TOR_TX)
{
#ifndef DATALINK_ENABLE_BERTEST
if (instPtr->serviceFsm.txSeqNum == 0)
{
if (instPtr->config.myType == L2_HEAD_NODE)
{
SysMonitor_GetSendPayloadFLJRaw(instPtr->serviceFsm.txLog);
}
else
{
SysMonitor_GetSendPayloadLJRaw(instPtr->serviceFsm.txLog);
}
}
// Packing service data
Service_Packing(instPtr, L2_SVC_FRAME);
if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true)
{
memcpy(instPtr->sendPktData, sendData, SVC_PACK_LEN);
free(sendData);
}
else
{
DBG_ERROR("Failed to pop tx queue.\n");
}
Service_Packing(instPtr, L2_SVC_FRAME, rtneEntry->dst);
#else
memcpy(instPtr->config.phyInstRef->sendFrameBuff, berTestData, SVC_PACK_LEN);
memcpy(instPtr->sendPktData, testData, SVC_PACK_LEN);
#endif
}
// FIXME: Steer antenna to point to destination.
Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
Phy_SetBeamAngle(phyRef, 0.0, 0.0);
Phy_SetBeamWidth(phyRef, PHY_ANT_BEAM_8);
L2_SetupNextSlot(instPtr, torState, L2_SVC_FRAME);
usedSlots = NUM_SERVICE_SLOTS;
break;
#ifndef DATALINK_ENABLE_BERTEST
case L2_SLOT_ADVERTISE:
if (torState == L2_TOR_TX)
{
// Packing response data.
Service_Packing(instPtr, L2_INFO_FRAME);
if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true)
{
memcpy(instPtr->sendPktData, sendData, RSP_PACK_LEN);
free(sendData);
}
else
{
DBG_ERROR("Failed to pop tx queue.\n");
}
}
// FIXME: Steer antenna to point to destination.
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, torState, L2_INFO_FRAME);
usedSlots = NUM_RESPONSE_SLOTS;
break;
#endif
default:
DBG_ERROR("UNKNOWN slot type [%d].\n", schedEntry->slotType);
return NUM_SLOTS_NONE;
return FAILURE;
}
#ifdef DATALINK_ENABLE_BERTEST
uint32_t pktLoss;
double errorRate, lossRate;
pktLoss = instPtr->serviceFsm.pktRecv - instPtr->serviceFsm.pktRecved;
lossRate = (instPtr->serviceFsm.pktRecv == 0) ? 0 : (double)pktLoss / (double)instPtr->serviceFsm.pktRecv;
pktLoss = instPtr->log.svcRecvCnt - instPtr->log.svcCorrectCnt;
lossRate = (instPtr->log.svcRecvCnt == 0) ? 0 : (double)pktLoss / (double)instPtr->serviceFsm.pktRecv;
errorRate = (instPtr->serviceFsm.bitTotal == 0) ? 0 : (double)instPtr->serviceFsm.bitError / (double)instPtr->serviceFsm.bitTotal;
DBG_PRINT("Packet Loss Rate -- Total: %lu, Loss: %lu.\n", instPtr->serviceFsm.pktRecv, pktLoss);
DBG_PRINT("Bit Error Rate -- Total:%lu, Error:%lu.\n", instPtr->serviceFsm.bitTotal, instPtr->serviceFsm.bitError);
#endif
return usedSlots;
return SUCCESS;
}
df_datalink_sp/src/networking/link_layer/time_sync.c
+40 -88
View File
@@ -2,6 +2,7 @@
/**
* Description: Time sync implementation file.
*
* Author: Author's name
* Date: 2021-12-04 16:32
* LastEditors: FanHao
* LastEditTime: 2022-07-10 20:43
@@ -17,34 +18,13 @@
/***************************** Include Files *********************************/
#include <string.h>
#include <stdlib.h>
#include "link_layer.h"
#include "../phy_layer/phy_layer.h"
#include "error.h"
#include "common.h"
#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 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 NORMAL_SLOT_CLK_NUM 960000.0 // 9600000= 5e-3*192e6
#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_2K_20MS 276336UL // 1.919ms
#define PHY_SYNCT2FEND_2K_100MS 11796336UL // 81.919ms
#define PHY_LOCAL_CNT_MAX 0x0000FFFFFFFFFFFFULL
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
/************************** Variable Definitions *****************************/
@@ -52,75 +32,47 @@
/*****************************************************************************/
/**
* @brief Apply local time adjustment by addition.
* @brief a slot alignment, base on physical layer information.
*
* @param instPtr.
* @param adjustment
* @param instPtr a pointer to linklayer layer instance
* @param frameNum
* @param slotNum
*
* @return
* @return SUCCESS Initialize successfully.
* FAILURE Initialization failed, allocate memory fail or initial
* bram controller fail.
*
******************************************************************************/
inline int32_t L2_TimeSync_LocalTimerAdd(L2_INST *instPtr, int64_t adjustment)
// TODO: Check
int32_t L2_TimeAlign(L2_INST *instPtr, uint32_t frameNum, uint32_t slotNum, bool relative)
{
// 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);
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Align slot, base on physical layer information.
*
* @param instPtr.
*
* @return
*
******************************************************************************/
int32_t L2_TimeSync_SlotAlign(L2_INST *instPtr)
{
int32_t Status;
int64_t slotAdj;
Status = L2_TimeSync_GetDeviatoion(instPtr, &slotAdj);
if (Status != SUCCESS)
L1_FRAME_TYPE lastRecvdFrame;
lastRecvdFrame = Phy_GetLastRecvFrameType(instPtr->config.phyInstPtr);
if (lastRecvdFrame == L1_NONE_FRAME)
{
DBG_ERROR("Failed to compute slot deviation.\n");
return Status;
}
return L2_TimeSync_LocalTimerAdd(instPtr, slotAdj);
}
/*****************************************************************************/
/**
* @brief Compute slot deviation of received frame.
*
* @param instPtr.
* @param devi
*
* @return
*
******************************************************************************/
int32_t L2_TimeSync_GetDeviatoion(L2_INST *instPtr, int64_t *devi)
{
// read nextSendTime from physical layer
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:
*devi = ((int64_t)PHY_SYNCT2FEND_2K_20MS) - slotMeasure;
break;
case L1_2KBPS_100MS_FRAME:
*devi = ((int64_t)PHY_SYNCT2FEND_2K_100MS) - slotMeasure;
break;
default:
DBG_ERROR("Unsupported frame type: %d.\n", Phy_GetLastDetectedFrameType(instPtr->config.phyInstRef));
return EIGNORE;
}
// read nextSendTime from physical layer
uint64_t currTime, expctTime;
int64_t timeDiff;
TIME_WORD timeStamp, slotTimerAdjmet;
expctTime = ((uint64_t)(frameNum * L2_SLOTS_PER_FRAME + slotNum)) * TIME_FRAC_MAX;
expctTime += Phy_GetTimeComp(lastRecvdFrame);
if (relative)
{
expctTime += (uint64_t)instPtr->netStartTime * L2_SLOTS_PER_FRAME * TIME_FRAC_MAX;
}
Phy_GetLastReceivedFrameSyncTimeStamp(instPtr->config.phyInstPtr, &timeStamp);
currTime = (uint64_t)timeStamp.frac_part;
currTime += ((uint64_t)timeStamp.int_part) * TIME_FRAC_MAX;
timeDiff = (int64_t)(expctTime - currTime);
slotTimerAdjmet.int_part = (uint32_t)(timeDiff / TIME_FRAC_MAX);
slotTimerAdjmet.frac_part = (uint32_t)(timeDiff % TIME_FRAC_MAX);
TimeCtrl_TimeAdjust(instPtr->config.tmcInstPtr, &slotTimerAdjmet, TM_ADJ_ADD);
return SUCCESS;
}
}
df_datalink_sp/src/networking/phy_layer/phy_antenna.c
+64 -68
View File
@@ -8,6 +8,7 @@
* LastEditTime: 2022-06-18 17:15
* Revisions: Rev 0.0
* History:
* [2024-01-20]: New branch for duifei.
* [2022-06-18]: Initial version.
*
* Copyright(c) 2022 Beijing Institute of Technology.
@@ -18,7 +19,6 @@
#include <stdint.h>
#include <math.h>
#include "error.h"
#include "../config//app_config.h"
#include "phy_layer.h"
#define ANT_PI 3.14159265358979323846
@@ -64,24 +64,19 @@ uint16_t hopPattern_2KBPS_20MS[L1_FRAME_2KBPS_20MS_HOPS] =
1500
};
// uint16_t hopPattern_2KBPS_100MS[L1_FRAME_2KBPS_100MS_HOPS] =
// {
// 0, 44, 88, 131, 175, 219, 263, 306,
// 350, 394, 438, 481, 525, 569, 613, 656,
// 700, 744, 788, 831, 875, 919, 963, 1006,
// 1050, 1094, 1138, 1181, 1225, 1269, 1313, 1356,
// 1400, 1444, 1488, 1531, 1575, 1619, 1663, 1706,
// 1750, 1794, 1838, 1881, 1925, 1969, 2013, 2056,
// 2100, 2144, 2188, 2231, 2275, 2319, 2363, 2406,
// 2450, 2494, 2538, 2581, 2625, 2669, 2713, 2756,
// 2800, 2844, 2888, 2931, 2975, 3019, 3063, 3106,
// 3150, 3194, 3238, 3281, 3325, 3369, 3413, 3456,
// 3500
// };
uint16_t hopPattern_2KBPS_100MS[L1_FRAME_2KBPS_100MS_HOPS] =
{
1500
0, 44, 88, 131, 175, 219, 263, 306,
350, 394, 438, 481, 525, 569, 613, 656,
700, 744, 788, 831, 875, 919, 963, 1006,
1050, 1094, 1138, 1181, 1225, 1269, 1313, 1356,
1400, 1444, 1488, 1531, 1575, 1619, 1663, 1706,
1750, 1794, 1838, 1881, 1925, 1969, 2013, 2056,
2100, 2144, 2188, 2231, 2275, 2319, 2363, 2406,
2450, 2494, 2538, 2581, 2625, 2669, 2713, 2756,
2800, 2844, 2888, 2931, 2975, 3019, 3063, 3106,
3150, 3194, 3238, 3281, 3325, 3369, 3413, 3456,
3500
};
uint16_t hopPattern_10KBPS_10MS[L1_FRAME_10KBPS_10MS_HOPS] =
@@ -95,6 +90,12 @@ uint16_t hopPattern_10KBPS_20MS[L1_FRAME_10KBPS_20MS_HOPS] =
1700, 1800, 1900
};
uint16_t hopPattern_40KBPS_10MS[L1_FRAME_40KBPS_10MS_HOPS] =
{
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400
};
uint16_t hopPattern_40KBPS_20MS[L1_FRAME_40KBPS_20MS_HOPS] =
{
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
@@ -102,17 +103,13 @@ uint16_t hopPattern_40KBPS_20MS[L1_FRAME_40KBPS_20MS_HOPS] =
2400, 2400, 2400
};
uint16_t hopPattern_40KBPS_10MS[L1_FRAME_40KBPS_10MS_HOPS] =
{
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400
};
uint16_t hopPattern_200KBPS_5MS[L1_FRAME_200KBPS_5MS_HOPS] =
{
500, 1600, 1700, 1800, 1900, 2000, 1500, 1600,
1700, 1800, 1900, 2000, 1500, 1600, 1700, 1800,
1900, 2000, 1500, 1500
};
uint16_t hopPattern_1M2BPS_5MS[L1_FRAME_1M2BPS_5MS_HOPS] =
{
500, 600, 700, 800, 900, 1000, 500, 600,
@@ -193,61 +190,60 @@ PHY_ANT_ANGLE antNrrwPattern[NUM_ANT_NRRW_PATTERN] =
/**
* @brief
*
* @param antConfigPtr
* @param antConfig
*
* @return
*
******************************************************************************/
int32_t Phy_AntConfigInit(PHY_ANT_COINFO *antConfigPtr)
void Phy_AntConfigInit(PHY_ANT_CONFIG *antConfig)
{
ASSERT(antConfigPtr != NULL);
ASSERT(antConfig != NULL);
antConfigPtr->ctrlWord = 0x00;
antConfigPtr->antMode = ATN_MODE_DEFAULT;
antConfig->ctrlWord = CTRL_WORD_DEFAULT;
antConfig->antMode = ATN_MODE_DEFAULT;
// FIXME: Dummy default value.
// off axis angle setting
antConfigPtr->oaxis[0] = 0;
antConfigPtr->oaxis[1] = 0;
antConfig->oaxis[0] = 0;
antConfig->oaxis[1] = 0;
// round axis angle setting
antConfigPtr->raxis[0] = 0;
antConfigPtr->raxis[1] = 0;
antConfig->raxis[0] = 0;
antConfig->raxis[1] = 0;
// ***********************
antConfigPtr->antFreq[0] = (uint8_t)(DEFAULT_CENTRTAL_FREQ);
antConfigPtr->antFreq[1] = (uint8_t)(DEFAULT_CENTRTAL_FREQ >> 8);
antConfig->antFreq[0] = (uint8_t)(DEFAULT_CENTRTAL_FREQ);
antConfig->antFreq[1] = (uint8_t)(DEFAULT_CENTRTAL_FREQ >> 8);
// FIXME: Dummy default value.
// trSelect
antConfigPtr->trSlect = 0;
antConfig->trSlect = 0;
// Antenna attenuation setting
antConfigPtr->antAtt = 20;
antConfig->antAtt = 20;
// phase setting
antConfigPtr->phase = 0;
antConfig->phase = 0;
// ***********************
antConfigPtr->rxtxMode = RXTXMODE_DEFAULT;
antConfigPtr->calibrMode = CALIBRMODE_NORMAL;
antConfigPtr->antReserved = 0x00;
antConfig->rxtxMode = RXTXMODE_DEFAULT;
antConfig->calibrMode = CALIBRMODE_NORMAL;
antConfig->antReserved = 0x00;
// FIXME: Dummy default value.
// RF mode control
antConfigPtr->rfCtrl = RF_CTRL_DEFAULT;
antConfig->rfCtrl = RF_CTRL_DEFAULT;
// RF frequency
antConfigPtr->rfFreq[0] = (uint8_t)(DEFAULT_CENTRTAL_FREQ);
antConfigPtr->rfFreq[1] = (uint8_t)(DEFAULT_CENTRTAL_FREQ >> 8);
antConfig->rfFreq[0] = (uint8_t)(DEFAULT_CENTRTAL_FREQ);
antConfig->rfFreq[1] = (uint8_t)(DEFAULT_CENTRTAL_FREQ >> 8);
// RF MGC control
antConfigPtr->rfTxMgc = 0;
antConfig->rfTxMgc = 0;
// RF channel 1&2 calibration mgc
antConfigPtr->rf12CalMgc = 0;
antConfig->rf12CalMgc = 0;
// RF channel 2&3 calibration mgc
antConfigPtr->rf34CalMgc = 0;
antConfig->rf34CalMgc = 0;
// Receive channel 1-4 MGC
antConfigPtr->rcvMgc = 0;
antConfigPtr->rfReserved0 = 0x00;
antConfigPtr->rfReserved1 = 0x00;
antConfig->rcvMgc = 0;
antConfig->rfReserved0 = 0x00;
antConfig->rfReserved1 = 0x00;
// ***********************
return SUCCESS;
}
int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo)
@@ -261,10 +257,10 @@ int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo)
else
{
instPtr->beamNo.next = beamNo;
instPtr->antConfigPtr.oaxis[1] = (uint8_t)(antNrrwPattern[beamNo].oaxis);
instPtr->antConfigPtr.oaxis[0] = (uint8_t)((antNrrwPattern[beamNo].oaxis) >> 8);
instPtr->antConfigPtr.raxis[1] = (uint8_t)(antNrrwPattern[beamNo].raxis);
instPtr->antConfigPtr.raxis[0] = (uint8_t)((antNrrwPattern[beamNo].raxis) >> 8);
instPtr->antConfig.oaxis[1] = (uint8_t)(antNrrwPattern[beamNo].oaxis);
instPtr->antConfig.oaxis[0] = (uint8_t)((antNrrwPattern[beamNo].oaxis) >> 8);
instPtr->antConfig.raxis[1] = (uint8_t)(antNrrwPattern[beamNo].raxis);
instPtr->antConfig.raxis[0] = (uint8_t)((antNrrwPattern[beamNo].raxis) >> 8);
return SUCCESS;
}
}
@@ -277,10 +273,10 @@ int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo)
else
{
instPtr->beamNo.next = beamNo;
instPtr->antConfigPtr.oaxis[1] = (uint8_t)(antSubnrrwPattern[beamNo].oaxis);
instPtr->antConfigPtr.oaxis[0] = (uint8_t)((antSubnrrwPattern[beamNo].oaxis) >> 8);
instPtr->antConfigPtr.raxis[1] = (uint8_t)(antSubnrrwPattern[beamNo].raxis);
instPtr->antConfigPtr.raxis[0] = (uint8_t)((antSubnrrwPattern[beamNo].raxis) >> 8);
instPtr->antConfig.oaxis[1] = (uint8_t)(antSubnrrwPattern[beamNo].oaxis);
instPtr->antConfig.oaxis[0] = (uint8_t)((antSubnrrwPattern[beamNo].oaxis) >> 8);
instPtr->antConfig.raxis[1] = (uint8_t)(antSubnrrwPattern[beamNo].raxis);
instPtr->antConfig.raxis[0] = (uint8_t)((antSubnrrwPattern[beamNo].raxis) >> 8);
return SUCCESS;
}
}
@@ -289,10 +285,10 @@ int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo)
// Beam number assign not support.
// Assign default angle (0,0)
instPtr->beamNo.next = 0;
instPtr->antConfigPtr.oaxis[1] = 0;
instPtr->antConfigPtr.oaxis[0] = 0;
instPtr->antConfigPtr.raxis[1] = 0;
instPtr->antConfigPtr.raxis[0] = 0;
instPtr->antConfig.oaxis[1] = 0;
instPtr->antConfig.oaxis[0] = 0;
instPtr->antConfig.raxis[1] = 0;
instPtr->antConfig.raxis[0] = 0;
DBG_WARNING("Using default beam number...\n");
return SUCCESS;
}
@@ -300,7 +296,7 @@ int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo)
int32_t Phy_SetBeamWidth(PHY_INST *instPtr, PHY_ANT_BEAM_WIDTH beamWidth)
{
uint8_t beamConfig = instPtr->antConfigPtr.antMode & (~ANT_MODE_BEAM_MASK);
uint8_t beamConfig = instPtr->antConfig.antMode & (~ANT_MODE_BEAM_MASK);
switch (beamWidth)
{
case PHY_ANT_BEAM_120:
@@ -320,13 +316,13 @@ int32_t Phy_SetBeamWidth(PHY_INST *instPtr, PHY_ANT_BEAM_WIDTH beamWidth)
return EPARAM;
break;
}
instPtr->antConfigPtr.antMode = beamConfig;
instPtr->antConfig.antMode = beamConfig;
instPtr->beamWd.next = beamWidth;
return SUCCESS;
}
// Common version radius double input
int32_t Phy_SetBeamAngle(PHY_INST *phyInstPtr, double offAx, double rndAx)
int32_t Phy_SetBeamAngle(PHY_INST *instPtr, double offAx, double rndAx)
{
uint16_t offAxDeg = 0;
uint16_t rndAxDeg = 0;
@@ -340,9 +336,9 @@ int32_t Phy_SetBeamAngle(PHY_INST *phyInstPtr, double offAx, double rndAx)
}
offAxDeg = offAxDeg * 10;
rndAxDeg = rndAxDeg * 10;
phyInstPtr->antConfigPtr.oaxis[1] = (uint8_t)offAxDeg;
phyInstPtr->antConfigPtr.oaxis[0] = (uint8_t)(offAxDeg >> 8);
phyInstPtr->antConfigPtr.raxis[1] = (uint8_t)rndAxDeg;
phyInstPtr->antConfigPtr.raxis[0] = (uint8_t)(rndAxDeg >> 8);
instPtr->antConfig.oaxis[1] = (uint8_t)offAxDeg;
instPtr->antConfig.oaxis[0] = (uint8_t)(offAxDeg >> 8);
instPtr->antConfig.raxis[1] = (uint8_t)rndAxDeg;
instPtr->antConfig.raxis[0] = (uint8_t)(rndAxDeg >> 8);
return SUCCESS;
}
df_datalink_sp/src/networking/phy_layer/phy_layer.c
+372 -251
View File
@@ -3,7 +3,7 @@
* Description: Implementation of physical control functions
*
* Date: 2021-11-16 00:08
* LastEditTime: 2023-04-23 17:48
* LastEditTime: 2024-01-20 17:48
* Revisions: Rev 0.2
* History:
* [2021-11-16]: Initial version.
@@ -12,7 +12,9 @@
* at specific time stamp.
* -Add 61/37 beam switch
* -Code simplification.
* [2023-04-24]: Porting for DUIFEI.
* [2023-04-24]: Feature add:
* - Read frame type from PL status register.
* [2024-01-20]: Porting for DUIFEI.
*
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Networking
@@ -22,81 +24,31 @@
/***************************** Include Files *********************************/
#include <string.h>
#include <stdlib.h>
#include "xbram.h"
#include "xadcps.h"
#include <math.h>
#include "phy_layer.h"
#include "util.h"
#include "error.h"
#include "../config/app_config.h"
#ifndef EMULATION
#include "xbram.h"
#include "xadcps.h"
#endif
/************************** Constant Definitions *****************************/
#define TOR_BUFF_MAX_SIZE 1024 // 8192bits
#define TMWORD_FRAC_MAX 500000
#define ANT_COINFO_LEN 32 // bytes
#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_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 int32_t Phy_SetSendData(PHY_INST *instPtr, L1_FRAME_TYPE frameType, uint8_t *dataBuff);
/**************************** Type Definitions *******************************/
/************************** Variable Definitions *****************************/
/*****************************************************************************/
/**
* @brief Get received data (base on PS Record), store it to data buffer.
*
* @param instPtr
* rcvBuffPtr
*
* @return SUCCESS
* FAILURE last frame type cannot be recognized.
*
******************************************************************************/
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)
{
return FAILURE;
}
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);
}
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Setup data will be sent in next slot.
@@ -107,26 +59,27 @@ 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 Phy_SetSendData(PHY_INST *instPtr, L1_FRAME_TYPE frameType, uint8_t *dataBuff)
{
ASSERT(instPtr != NULL);
ASSERT(dataBuff != NULL);
uint32_t frameLen, sendCmd;
frameLen = Phy_GetFrameLen(frameType);
if (frameLen == L1_FRAME_NONE_LEN)
{
return FAILURE;
}
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);
// 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;
PhyWrite(instPtr, SEND_DATA_CMD, sendCmd);
DBG_ERROR("Invalid frame length.\n");
return EPARAM;
}
instPtr->sendDataOffset = (L1_TXRXDATA_MAX_LEN >> 1) - instPtr->sendDataOffset;
memcpy((uint8_t *)(instPtr->sendDataAddr + instPtr->sendDataOffset), dataBuff, frameLen);
sendCmd = frameLen & SEND_DATA_CMD_LEN_MASK;
sendCmd |= (instPtr->sendDataOffset << SEND_DATA_CMD_OFFSET_SHIFT) & SEND_DATA_CMD_OFFSET_MASK;
// Physical layer start fetching data.
PhyWrite(instPtr, SEND_DATA_CMD, sendCmd);
return SUCCESS;
}
@@ -135,7 +88,7 @@ static int32_t SetSendData(PHY_INST *instPtr, L1_FRAME_TYPE frameType, uint8_t *
* @brief Check whether a frame was detected. (Do not check if the frame was
* deoced.)
*
* @param instPtr
* @param instPtr A pointer to physical layer control instance.
*
* @return true
* false
@@ -144,7 +97,7 @@ static int32_t SetSendData(PHY_INST *instPtr, L1_FRAME_TYPE frameType, uint8_t *
inline bool Phy_IsFrameDetected(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
if (instPtr->TOR.last == PHY_TOR_RX && instPtr->isAcq && instPtr->isFrameSync)
if (instPtr->isAcq && instPtr->isFrameSync)
{
return true;
}
@@ -159,7 +112,7 @@ inline bool Phy_IsFrameDetected(PHY_INST *instPtr)
* @brief Check whether a frame was received. (Do not check if the frame was
* crashed.)
*
* @param instPtr
* @param instPtr A pointer to physical layer control instance.
*
* @return true
* false
@@ -168,7 +121,7 @@ inline bool Phy_IsFrameDetected(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->isAcq && instPtr->isFrameSync && instPtr->isDecode)
{
return true;
}
@@ -192,58 +145,30 @@ inline bool Phy_IsFrameReceived(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)
{
return true;
}
else
{
return false;
}
return instPtr->isValid;
}
/*****************************************************************************/
/**
* @brief Get frame type of the frame in receive data buffer (Base on PS Record).
* @brief Get frame type of the frame in receive data buffer (Base on PL Record).
*
* @param instPtr A pointer to physical layer control instance.
*
* @return Layer2 frame type. Refer "frame_def.h"
*
******************************************************************************/
inline L1_FRAME_TYPE Phy_GetLastDetectedFrameType(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
if (Phy_IsFrameDetected(instPtr) == false)
{
return L1_NONE_FRAME;
}
else
{
return instPtr->frameType.last;
}
}
/*****************************************************************************/
/**
* @brief Get frame type of the frame in receive data buffer (Base on PS Record).
*
* @param instPtr A pointer to physical layer control instance.
*
* @return Layer2 frame type. Refer "frame_def.h"
*
******************************************************************************/
inline L1_FRAME_TYPE Phy_GetLastReceivedFrameType(PHY_INST *instPtr)
inline L1_FRAME_TYPE Phy_GetLastRecvFrameType(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
if (Phy_IsFrameReceived(instPtr) == false)
{
return L1_NONE_FRAME;
}
int32_t frame_type = (PhyRead(instPtr, PHY_MODE_CTRL) & PHY_MODE_CTRL_FRAME_MASK) >> PHY_MODE_CTRL_FRAME_SHIFT;
if (frame_type < L1_1M2BPS_5MS_FRAME)
return frame_type;
else
{
return instPtr->frameType.last;
}
return L1_NONE_FRAME;
}
/*****************************************************************************/
@@ -255,17 +180,14 @@ 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_GetLastSendFrameType(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
if (instPtr->TOR.last != PHY_TOR_TX)
{
return L1_NONE_FRAME;
}
else
{
return instPtr->frameType.last;
}
return instPtr->frameType.last;
}
/*****************************************************************************/
@@ -300,9 +222,7 @@ 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:
@@ -324,38 +244,38 @@ uint32_t Phy_GetFrameLen(L1_FRAME_TYPE frameType)
/*****************************************************************************/
/**
* @brief Update Physical Layer reception status.
* @brief Update physical layer status cache.
*
* @param instPtr
* @param instPtr A pointer to physical layer control instance.
*
* @return
* @return NONE.
*
******************************************************************************/
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);
/* 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;
// 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;
// FIXME: using default value, since physical layer may provide unexpected error value.
instPtr->isAcq = true;
instPtr->isFrameSync = true;
instPtr->isDecode = ((txrxSate & TXRX_STATE_DECODE_MASK) == TXRX_STATE_DECODE_SUCCESS) ? true : false;
if (Phy_IsFrameReceived(instPtr))
{
DBG_INFO("Physical layer finished reception procedure, retrieving received data...\n");
instPtr->isValid = true;
instPtr->isValid = ((txrxSate & TXRX_STATE_CRC_MASK) == TXRX_STATE_CRC_OK) ? true : false;
}
else
{
instPtr->isValid = false;
}
return;
instPtr->recvSlotTime.int_part = PhyRead(instPtr, TM_TARGET_H);
instPtr->recvSlotTime.frac_part = PhyRead(instPtr, TM_TARGET_L);
instPtr->recvFsTime.int_part = PhyRead(instPtr, FSTIME_H);
instPtr->recvFsTime.frac_part = PhyRead(instPtr, FSTIME_L);
}
/*****************************************************************************/
@@ -363,17 +283,18 @@ void Phy_UpdateState(PHY_INST *instPtr)
* @brief Initialize Physical Layer instance, allocate memory for buffers,
* apply a self test.
*
* @param instPtr
* phyConfig
* @param instPtr a pointer to physical instance
* @param phyConfig a pointer to configuration structure.
*
* @return
* @return SUCCESS
*
******************************************************************************/
int32_t Phy_InstInit(PHY_INST **instPtr, PHY_CONFIG *phyConfig)
{
ASSERT(instPtr != NULL);
ASSERT(phyConfig != NULL);
int32_t Status = SUCCESS;
int32_t Status;
*instPtr = (PHY_INST *)malloc(sizeof(PHY_INST));
if ((*instPtr) == NULL)
{
@@ -387,26 +308,27 @@ int32_t Phy_InstInit(PHY_INST **instPtr, PHY_CONFIG *phyConfig)
(*instPtr)->isDecode = false;
(*instPtr)->isValid = false;
(*instPtr)->isPresetHop = true;
(*instPtr)->recvFsTime.int_part = UINT32_MAX;
(*instPtr)->recvFsTime.frac_part = UINT32_MAX;
(*instPtr)->recvSlotTime.int_part = UINT32_MAX;
(*instPtr)->recvSlotTime.frac_part = UINT32_MAX;
(*instPtr)->antConfigOffset = 0;
(*instPtr)->sendOffset = 0;
(*instPtr)->sendDataOffset = 0;
Phy_AntConfigInit(&((*instPtr)->antConfig));
memset(&((*instPtr))->TOR, PHY_TOR_SUSPEND, sizeof(PHY_RCD));
memset(&((*instPtr)->frameType), L1_NONE_FRAME, sizeof(PHY_RCD));
memset(&((*instPtr)->antNo), (int32_t)INIT_BEAM_NO, sizeof(PHY_RCD));
memset(&((*instPtr)->beamNo), (int32_t)INIT_ANT_NO, sizeof(PHY_RCD));
memset(&((*instPtr)->beamWd), (int32_t)INIT_ANT_WD, sizeof(PHY_RCD));
memset(&((*instPtr)->antNo), (int32_t)L1_INIT_BEAM_NO, sizeof(PHY_RCD));
memset(&((*instPtr)->beamNo), (int32_t)L1_INIT_ANT_NO, sizeof(PHY_RCD));
memset(&((*instPtr)->beamWd), (int32_t)L1_INIT_ANT_WD, sizeof(PHY_RCD));
memset(&((*instPtr)->hwSate), 0x00, sizeof(HWSTATE));
Status = Phy_AntConfigInit(&((*instPtr)->antConfigPtr));
if (Status != SUCCESS)
{
goto init_error;
}
#ifndef EMULATION
XBram_Config *bramConfigPtr;
XBram antConfigBram;
XBram txDataBram;
XBram rxDataBram;
bramConfigPtr = XBram_LookupConfig(phyConfig->antBramId);
if (bramConfigPtr == NULL)
{
@@ -470,9 +392,29 @@ int32_t Phy_InstInit(PHY_INST **instPtr, PHY_CONFIG *phyConfig)
goto init_error;
}
(*instPtr)->txDataPtr = txDataBram.Config.MemBaseAddress;
(*instPtr)->rxDataPtr = rxDataBram.Config.MemBaseAddress;
(*instPtr)->antConfigDataPtr = antConfigBram.Config.MemBaseAddress;
(*instPtr)->antConfigAddr = antConfigBram.Config.MemBaseAddress;
(*instPtr)->recvDataAddr = rxDataBram.Config.MemBaseAddress;
(*instPtr)->sendDataAddr = txDataBram.Config.MemBaseAddress;
#else
(*instPtr)->antConfigAddr = (uintptr_t)calloc(L1_ANTCONFIG_MAX_LEN / sizeof(uint64_t), sizeof(uint64_t));
if ((*instPtr)->antConfigAddr == (uintptr_t)NULL)
{
Status = EDCONF;
goto init_error;
}
(*instPtr)->recvDataAddr = (uintptr_t)calloc(L1_TXRXDATA_MAX_LEN / sizeof(uint64_t), sizeof(uint64_t));
if ((*instPtr)->recvDataAddr == (uintptr_t)NULL)
{
Status = EDCONF;
goto init_error;
}
(*instPtr)->sendDataAddr = (uintptr_t)calloc(L1_TXRXDATA_MAX_LEN / sizeof(uint64_t), sizeof(uint64_t));
if ((*instPtr)->sendDataAddr == (uintptr_t)NULL)
{
Status = EDCONF;
goto init_error;
}
#endif
/* Initialize the XAdc driver. */
XAdcPs_Config *xadcConfigPtr;
@@ -497,10 +439,33 @@ int32_t Phy_InstInit(PHY_INST **instPtr, PHY_CONFIG *phyConfig)
return SUCCESS;
init_error:
free(*instPtr);
Phy_Destroy(*instPtr);
return Status;
}
/*****************************************************************************/
/**
* @brief Destroy physical layer inctance, free allocated memory.
*
* @param instPtr a pointer to physical instance
*
* @return NONE
*
******************************************************************************/
void Phy_Destroy(PHY_INST *instPtr)
{
if (instPtr == NULL)
{
return;
}
#ifdef EMULATION
free((void *)instPtr->antConfigAddr);
free((void *)instPtr->recvDataAddr);
free((void *)instPtr->sendDataAddr);
#endif
free(instPtr);
}
/*****************************************************************************/
/**
* @brief Start physical layer
@@ -546,17 +511,19 @@ void Phy_Halt(PHY_INST *instPtr)
/*****************************************************************************/
/**
* @brief Update Physical Layer mode.
* @brief Set physical layer working mode.
*
* @param instPtr
* modeCod
* @param instPtr a pointer to physical instance
* @param modeCod intened mode code
*
* @return
* @return SUCCESS
* other
*
******************************************************************************/
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)
{
@@ -584,23 +551,24 @@ int32_t Phy_SetMode(PHY_INST *instPtr, PHY_MODE modeCod)
/**
* @brief Setup next slot operation.
*
* @param instPtr
* txrxMode
* l2FrameType
* @param instPtr a pointer to physical instance
* @param txrxMode tor type of next slot
* @param frameType frame type of next slot
*
* @return
* @return SUCCESS
* other
*
******************************************************************************/
int32_t Phy_SetupNextSlot(PHY_INST *instPtr, PHY_TOR_TYPE txrxMode,
L1_FRAME_TYPE frameType, uint8_t *buff)
L1_FRAME_TYPE frameType, uint8_t *dataBuff)
{
ASSERT(instPtr != NULL);
int32_t Status = SUCCESS;
uint32_t phyModeCtrl, antConfigBramAddr, hopNum;
int32_t Status;
uint32_t phyModeCtrl, hopNum;
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;
instPtr->antConfigOffset = (L1_ANTCONFIG_MAX_LEN >> 1) - instPtr->antConfigOffset;
uint32_t antConfigAddr = instPtr->antConfigAddr + instPtr->antConfigOffset;
instPtr->TOR.next = txrxMode;
instPtr->frameType.next = frameType;
@@ -625,84 +593,98 @@ int32_t Phy_SetupNextSlot(PHY_INST *instPtr, PHY_TOR_TYPE txrxMode,
}
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 = hopPattern_2KBPS_100MS;
break;
case L1_2KBPS_100MS_FRAME:
phyModeCtrl |= PHY_MODE_CTRL_FRAME_2KBPS_100MS;
hopNum = L1_FRAME_2KBPS_100MS_HOPS;
hopPattern = hopPattern_2KBPS_100MS;
break;
case L1_10KBPS_10MS_FRAME:
phyModeCtrl |= PHY_MODE_CTRL_FRAME_10KBPS_10MS;
hopNum = L1_FRAME_10KBPS_10MS_HOPS;
hopPattern = hopPattern_10KBPS_10MS;
break;
case L1_10KBPS_20MS_FRAME:
phyModeCtrl |= PHY_MODE_CTRL_FRAME_10KBPS_20MS;
hopNum = L1_FRAME_10KBPS_20MS_HOPS;
hopPattern = hopPattern_10KBPS_20MS;
break;
case L1_40KBPS_20MS_FRAME:
phyModeCtrl |= PHY_MODE_CTRL_FRAME_200KBPS_5MS;
hopNum = L1_FRAME_200KBPS_5MS_HOPS;
hopPattern = hopPattern_200KBPS_5MS;
break;
case L1_40KBPS_10MS_FRAME:
phyModeCtrl |= PHY_MODE_CTRL_FRAME_40KBPS_10MS;
hopNum = L1_FRAME_40KBPS_10MS_HOPS;
hopPattern = hopPattern_40KBPS_10MS;
break;
case L1_200KBPS_5MS_FRAME:
phyModeCtrl |= PHY_MODE_CTRL_FRAME_200KBPS_5MS;
hopNum = L1_FRAME_200KBPS_5MS_HOPS;
hopPattern = hopPattern_200KBPS_5MS;
break;
case L1_1M2BPS_5MS_FRAME:
phyModeCtrl |= PHY_MODE_CTRL_FRAME_1M2BPS_5MS;
hopNum = L1_FRAME_1M2BPS_5MS_HOPS;
hopPattern = hopPattern_1M2BPS_5MS;
break;
default:
return EPARAM;
break;
}
switch (frameType)
{
case L1_2KBPS_20MS_FRAME:
phyModeCtrl |= PHY_MODE_CTRL_FRAME_2KBPS_20MS;
hopNum = L1_FRAME_2KBPS_20MS_HOPS;
hopPattern = hopPattern_10KBPS_10MS;
break;
case L1_2KBPS_100MS_FRAME:
phyModeCtrl |= PHY_MODE_CTRL_FRAME_2KBPS_100MS;
hopNum = L1_FRAME_2KBPS_100MS_HOPS;
hopPattern = hopPattern_2KBPS_100MS;
break;
case L1_10KBPS_10MS_FRAME:
phyModeCtrl |= PHY_MODE_CTRL_FRAME_10KBPS_10MS;
hopNum = L1_FRAME_10KBPS_10MS_HOPS;
hopPattern = hopPattern_10KBPS_10MS;
break;
case L1_10KBPS_20MS_FRAME:
phyModeCtrl |= PHY_MODE_CTRL_FRAME_10KBPS_20MS;
hopNum = L1_FRAME_10KBPS_20MS_HOPS;
hopPattern = hopPattern_10KBPS_20MS;
break;
case L1_40KBPS_20MS_FRAME:
phyModeCtrl |= PHY_MODE_CTRL_FRAME_200KBPS_5MS;
hopNum = L1_FRAME_200KBPS_5MS_HOPS;
hopPattern = hopPattern_200KBPS_5MS;
break;
case L1_40KBPS_10MS_FRAME:
phyModeCtrl |= PHY_MODE_CTRL_FRAME_40KBPS_10MS;
hopNum = L1_FRAME_40KBPS_10MS_HOPS;
hopPattern = hopPattern_40KBPS_10MS;
break;
case L1_200KBPS_5MS_FRAME:
phyModeCtrl |= PHY_MODE_CTRL_FRAME_200KBPS_5MS;
hopNum = L1_FRAME_200KBPS_5MS_HOPS;
hopPattern = hopPattern_200KBPS_5MS;
break;
case L1_1M2BPS_5MS_FRAME:
phyModeCtrl |= PHY_MODE_CTRL_FRAME_1M2BPS_5MS;
hopNum = L1_FRAME_1M2BPS_5MS_HOPS;
hopPattern = hopPattern_1M2BPS_5MS;
break;
default:
return EPARAM;
break;
}
}
PhyWrite(instPtr, PHY_MODE_CTRL, phyModeCtrl);
// TODO: Set antenna stand by, when suspend.
const uint8_t *dataSrcPtr = (uint8_t *)(&(instPtr->antConfigPtr));
const uint8_t *dataSrcPtr = (uint8_t *)(&(instPtr->antConfig));
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));
instPtr->antConfig.antFreq[1] = (uint8_t)(hopPattern[i] & 0x00FF);
instPtr->antConfig.antFreq[0] = (uint8_t)((hopPattern[i] >> 8) & 0x00FF);
instPtr->antConfig.rfFreq[1] = (uint8_t)(hopPattern[i] & 0x00FF);
instPtr->antConfig.rfFreq[0] = (uint8_t)((hopPattern[i] >> 8) & 0x00FF);
memcpy((uint8_t *)(antConfigAddr + i *ANT_COINFO_LEN),
dataSrcPtr, sizeof(PHY_ANT_CONFIG));
}
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);
uint32_t antCtrl = (instPtr->antConfigOffset & ANT_CONFIG_OFFSET_MASK)
| ((hopNum << ANT_CONFIG_NUM_SHIFT) & ANT_CONFIG_NUM_MASK)
| ((ANT_COINFO_LEN << ANT_CONFIG_LEN_SHIFT) & ANT_CONFIG_LEN_MASK);
/*
memcpy((uint8_t*)antConfigDataAddr, hopPattern, hopNum*sizeof(uint16_t));
uint32_t antCtrl = (instPtr->antCoinfigOffset & ANT_CONFIG_OFFSET_MASK)
| ((hopNum << ANT_CONFIG_NUM_SHIFT) & ANT_CONFIG_NUM_MASK)
| ((sizeof(uint16_t) << ANT_CONFIG_LEN_SHIFT) & ANT_CONFIG_LEN_MASK);
*/
PhyWrite(instPtr, ANT_CONFIG, antCtrl);
// FIXME: For DUIFEI network setup. Should only set send data in send slots.
if (txrxMode == PHY_TOR_TX)
{
Status = SetSendData(instPtr, frameType, buff);
Status = Phy_SetSendData(instPtr, frameType, dataBuff);
if (Status != SUCCESS)
{
DBG_ERROR("Failed to apply send data.\n");
return Status;
}
}
// Trigger physical configuration.
PhyWrite(instPtr, SLOT_CONFIG_CMD, SLOT_CONFIG_CMD_VAULE); // Phy start fetching data.
// Indicate physical layer that all the configurations were done.
PhyWrite(instPtr, SLOT_CMD, SLOT_CMD_KEY);
Phy_RcdShift(&(instPtr->TOR), txrxMode);
Phy_RcdShift(&(instPtr->frameType), frameType);
// TODO:
Phy_RcdShift(&(instPtr->antNo), L1_INIT_ANT_NO);
Phy_RcdShift(&(instPtr->beamNo), L1_INIT_BEAM_NO);
Phy_RcdShift(&(instPtr->beamWd), L1_INIT_ANT_WD);
return SUCCESS;
}
@@ -744,11 +726,12 @@ uint32_t Phy_GetHopLength(L1_FRAME_TYPE frameType)
/*****************************************************************************/
/**
* @brief Get the time from frame sync to slot end of different type.
* @brief Get the corresponding time compensation value (time interval from
* frame start to frame sync)
*
* @param frameType
* @param frameType a L1 frame type enumeration.
*
* @return 32bit unsigned integer (clock periods)
* @return 32bit unsigned integer (10ns)
*
******************************************************************************/
uint32_t Phy_GetTimeComp(L1_FRAME_TYPE frameType)
@@ -778,25 +761,44 @@ uint32_t Phy_GetTimeComp(L1_FRAME_TYPE frameType)
/*****************************************************************************/
/**
* @brief Get latest frequency deviation estimation.
* @brief Get coarse frequency estimation of last valid frame.
*
* @param instPtr
* @param instPtr a pointer to physical instance
*
* @return 32bit signed integer (Hz)
* @return frequency estimation
*
******************************************************************************/
int32_t Phy_GetFreqEst(PHY_INST *instPtr)
double Phy_GetCoarseFreqEst(PHY_INST *instPtr)
{
uint32_t temp;
int32_t acqCh, fsCh;
ASSERT(instPtr != NULL);
uint32_t regValue;
uint8_t acqFreqIdx;
int8_t syncFreqIdx;
L1_FRAME_TYPE frameType;
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);
frameType = Phy_GetLastRecvFrameType(instPtr);
if (frameType > L1_1M2BPS_5MS_FRAME || frameType == L1_NONE_FRAME)
{
return INFINITY; // FIXME:
}
regValue = PhyRead(instPtr, COASE_FREQEST);
acqFreqIdx = (regValue & COASE_FREQEST_ACQ_MASK) >> COASE_FREQEST_ACQ_SHIFT;
syncFreqIdx = (regValue & COASE_FREQEST_SYNC_MASK) >> COASE_FREQEST_SYNC_SHIFT;
if (frameType >= L1_200KBPS_5MS_FRAME)
{
// 1.5M Rc
freqEst = (double)(80 - acqFreqIdx) * (31.25e3 / 4);
freqEst += (double)syncFreqIdx * 1.5e6 / 64 / 128;
}
else
{
// 12M Rc
freqEst = (double)(8 - acqFreqIdx) * (250e3 / 4);
freqEst += (double)syncFreqIdx * 12e6 / 64 / 128;
}
return freqEst;
}
/*****************************************************************************/
@@ -833,21 +835,22 @@ 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, const TIME_WORD *tmWord, PHY_TIME_WORD_TYPE tmType)
{
ASSERT(instPtr != NULL);
if (tmWord->frac_part >= PHY_TIME_WORD_L_MAX)
{
return EPARAM;
}
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_L, tmWord->frac_part);
PhyWrite(instPtr, POLLTIME_H, tmWord->int_part);
break;
case PHY_TIME_WORD_LOCAL_ADJ:
PhyWrite(instPtr, LOCALTM_ADJ_L, (uint32_t)tmWord);
PhyWrite(instPtr, LOCALTM_ADJ_H, (uint32_t)(tmWord >> 32));
break;
case PHY_TIME_WORD_STEP:
PhyWrite(instPtr, CNT_STEP_OPT, (uint32_t)tmWord);
case PHY_TIME_WORD_LISTEN_TIMEOUT:
PhyWrite(instPtr, LISTENTIMEOUT_L, tmWord->frac_part);
PhyWrite(instPtr, LISTENTIMEOUT_H, tmWord->int_part);
break;
default:
return EPARAM;
@@ -855,6 +858,124 @@ int32_t Phy_SetTimeWord(PHY_INST *instPtr, uint64_t tmWord, PHY_TIME_WORD_TYPE t
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Get the frame sync time of last frame.
*
* @param instPtr a pointer to physical instance
* @param frameSycT a pointer to the frame sync time data buffer.
*
* @return NONE
*
******************************************************************************/
void Phy_GetFrameSyncTimeStamp(PHY_INST *instPtr, TIME_WORD *frameSyncT)
{
ASSERT(instPtr != NULL);
ASSERT(frameSyncT != NULL);
frameSyncT->int_part = PhyRead(instPtr, FSTIME_H);
frameSyncT->frac_part = PhyRead(instPtr, FSTIME_L);
}
/*****************************************************************************/
/**
* @brief Get the frame sync time of last received frame.
*
* @param instPtr a pointer to physical instance
* @param frameSycT a pointer to the frame sync time data buffer.
*
* @return NONE
*
******************************************************************************/
void Phy_GetLastReceivedFrameSyncTimeStamp(PHY_INST *instPtr, TIME_WORD *frameSyncT)
{
ASSERT(instPtr != NULL);
ASSERT(frameSyncT != NULL);
memcpy(frameSyncT, &(instPtr->recvFsTime), sizeof(TIME_WORD));
}
/*****************************************************************************/
/**
* @brief Get the slot start time of last received frame.
*
* @param instPtr a pointer to physical instance
* @param slotStart a pointer to the time data buffer.
*
* @return NONE
*
******************************************************************************/
void Phy_GetSlotStartTimeStamp(PHY_INST *instPtr, TIME_WORD *slotStart)
{
ASSERT(instPtr != NULL);
ASSERT(slotStart != NULL);
slotStart->int_part = PhyRead(instPtr, TM_TARGET_H);
slotStart->frac_part = PhyRead(instPtr, TM_TARGET_L);
}
/*****************************************************************************/
/**
* @brief Get the slot start time of last frame.
*
* @param instPtr a pointer to physical instance
* @param slotStart a pointer to the time data buffer.
*
* @return NONE
*
******************************************************************************/
void Phy_GetLastReceivedSlotStartTimeStamp(PHY_INST *instPtr, TIME_WORD *slotStart)
{
ASSERT(instPtr != NULL);
ASSERT(slotStart != NULL);
memcpy(slotStart, &(instPtr->recvSlotTime), sizeof(TIME_WORD));
}
/*****************************************************************************/
/**
* @brief Copy ans store the received data, if received data valid.
*
* @param instPtr a pointer to physical layer control instance.
*
* @return NONE
*
******************************************************************************/
void Phy_PeekRecvData(PHY_INST *instPtr, uint8_t *dataBuff)
{
ASSERT(instPtr != NULL);
ASSERT(dataBuff != NULL);
if (instPtr->isValid == false)
{
return;
}
uint32_t frameLen, offset;
frameLen = (PhyRead(instPtr, RECVDATA_STATE) & RECVDATA_STATE_LEN_MASK) >> RECVDATA_STATE_LEN_SHIFT;
offset = (PhyRead(instPtr, RECVDATA_STATE) & RECVDATA_STATE_OFFSET_MASK) >> RECVDATA_STATE_OFFSET_SHIFT;
memcpy(dataBuff, (uint8_t *)(instPtr->recvDataAddr + offset), frameLen);
}
/*****************************************************************************/
/**
* @brief Get the received data (if valid), store it to data buffer.
* and clear data valid flag.
*
* @param instPtr a pointer to physical layer control instance.
*
* @return NONE
*
******************************************************************************/
void Phy_RetrieveRecvData(PHY_INST *instPtr, uint8_t *dataBuff)
{
ASSERT(instPtr != NULL);
ASSERT(dataBuff != NULL);
if (instPtr->isValid == false)
{
return;
}
Phy_PeekRecvData(instPtr, dataBuff);
instPtr->isValid = false;
}
/*****************************************************************************/
/**
* @brief Setup a specific timer word register in physical controller.
df_datalink_sp/src/networking/phy_layer/phy_layer.h
+72 -35
View File
@@ -3,7 +3,7 @@
* Description: Physical layer control definitions and function prototypes.
*
* Date: 2021-12-04 16:32
* LastEditTime: 2023-04-24 17:41
* LastEditTime: 2024-01-20 17:41
* Revisions: Rev 0.1
* History:
* [2021-12-08]: Initial version.
@@ -12,7 +12,9 @@
* at specific time stamp.
* -Add 61/37 beam switch
* -Code simplification.
* [2023-04-24]: Porting for DUIFEI.
* [2023-04-24]: Feature add:
* - Read frame type from PL status register.
* [2024-01-20]: New branch for DUIFEI.
*
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Networking
@@ -29,7 +31,17 @@
#include "xadcps.h"
#include "phy_reg.h"
#include "../../system_monitor.h"
#include "../../time_ctrl/time_ctrl.h"
/************************** Constant Definitions *****************************/
#define L1_TMWORD_FRAC_MAX 500000
#define L1_ANTCONFIG_MAX_LEN 8192
#define L1_TXRXDATA_MAX_LEN 2048
#define L1_NUM_ANT_SUBNRRW_PATTERN 37
#define L1_NUM_ANT_NRRW_PATTERN 61
#define L1_INIT_BEAM_NO 0
#define L1_INIT_ANT_WD PHY_ANT_BEAM_120
#define L1_INIT_ANT_NO 0
#define L1_FRAME_NONE_HOPS UINT32_MAX
#define L1_FRAME_MAX_HOPS 81
#define L1_FRAME_2KBPS_20MS_HOPS 1
@@ -41,10 +53,29 @@
#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_FRAME_NONE_LEN UINT32_MAX // byte
#define L1_FRAME_LEN_MAX (8192 / 8) // byte
#define L1_FRAME_2KBPS_20MS_LEN 1 // byte
#define L1_FRAME_2KBPS_100MS_LEN 312 / 8 // byte
#define L1_FRAME_10KBPS_10MS_LEN 1 // byte
#define L1_FRAME_10KBPS_20MS_LEN (232 / 8) // byte
#define L1_FRAME_40KBPS_20MS_LEN (2152 / 8) // byte
#define L1_FRAME_40KBPS_10MS_LEN (712 / 8) // byte
#define L1_FRAME_200KBPS_5MS_LEN (2040 / 8) // byte
#define L1_FRAME_1M2BPS_5MS_LEN (8176 / 8) // byte
#define ANT_COINFO_LENGTH 32 // bytes
#define L1_FRAME_NONE_COMP UINT32_MAX // *10ns(Timer_L)
#define L1_FRAME_2KBPS_20MS_COMP 1841158 // *10ns(Timer_L)
#define L1_FRAME_2KBPS_100MS_COMP 1841158 // *10ns(Timer_L)
#define L1_FRAME_10KBPS_10MS_COMP 850950 // *10ns(Timer_L)
#define L1_FRAME_10KBPS_20MS_COMP 850950 // *10ns(Timer_L)
#define L1_FRAME_40KBPS_20MS_COMP 110667 // *10ns(Timer_L)
#define L1_FRAME_40KBPS_10MS_COMP 110667 // *10ns(Timer_L)
#define L1_FRAME_200KBPS_5MS_COMP 22493 // *10ns(Timer_L)
#define L1_FRAME_1M2BPS_5MS_COMP 22493 // *10ns(Timer_L)
#define PHY_TIME_WORD_H_MAX UINT32_MAX
#define PHY_TIME_WORD_L_MAX 0x0007A120 // 5ms/10ns
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
typedef enum
@@ -113,7 +144,7 @@ typedef struct
/* reserved for further use */
uint8_t rfReserved0;
uint8_t rfReserved1;
} PHY_ANT_COINFO;
} PHY_ANT_CONFIG;
typedef struct
{
@@ -129,9 +160,7 @@ typedef enum
{
PHY_TIME_WORD_NONE = 0,
PHY_TIME_WORD_POLL = 1,
PHY_TIME_WORD_LISTEN_TIMEOUT = 2,
PHY_TIME_WORD_LOCAL_ADJ = 3,
PHY_TIME_WORD_STEP = 4,
PHY_TIME_WORD_LISTEN_TIMEOUT = 2
} PHY_TIME_WORD_TYPE;
typedef enum
@@ -177,24 +206,26 @@ typedef struct
typedef struct
{
PHY_CONFIG phyCofig; // Device address configs
bool isPhyEn; // if physical layer running
bool isAcq; // if last reception acquisition success
bool isFrameSync; // if last reception frame sync success
bool isDecode; // if last reception decode success
bool isValid; // if last reception frame data valid (crc check pass)
bool isPresetHop; // if 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;
uintptr_t antConfigDataPtr;
PHY_CONFIG phyCofig; // Device address configs
uintptr_t recvDataAddr;
uintptr_t sendDataAddr;
uintptr_t antConfigAddr;
bool isPhyEn; // if physical layer running
bool isAcq; // if last reception acquisition success
bool isFrameSync; // if last reception frame sync success
bool isDecode; // if last reception decode success
bool isValid; // if last reception frame data valid (crc check pass)
bool isPresetHop; // if using preset frequency hop
TIME_WORD recvSlotTime; // last slot start time stamp
TIME_WORD recvFsTime; // last frame sync time stamp
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 sendDataOffset; // antenna configuration bram offset (to implement ping-pong data flow)
uint32_t antConfigOffset; // antenna configuration bram offset (to implement ping-pong data flow)
PHY_ANT_CONFIG antConfig; // antenna configuration pointer
XAdcPs hwXadc;
HWSTATE hwSate;
} PHY_INST;
@@ -216,6 +247,8 @@ extern PHY_ANT_ANGLE antNrrwPattern[L1_NUM_ANT_NRRW_PATTERN];
/************************** Function Prototypes ******************************/
int32_t Phy_InstInit(PHY_INST **instPtr, PHY_CONFIG *phyConfig);
void Phy_Destroy(PHY_INST *instPtr);
void Phy_UpdateState(PHY_INST *instPtr);
void Phy_UpdateHwSate(PHY_INST *instPtr);
@@ -223,27 +256,31 @@ 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 *dataBuff);
void Phy_PeekRecvData(PHY_INST *instPtr, uint8_t *dataBuff);
void Phy_RetrieveRecvData(PHY_INST *instPtr, uint8_t *dataBuff);
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_GetLastReceivedFrameType(PHY_INST *instPtr);
L1_FRAME_TYPE Phy_GetLastSentFrameType(PHY_INST *instPtr);
L1_FRAME_TYPE Phy_GetLastRecvFrameType(PHY_INST *instPtr);
L1_FRAME_TYPE Phy_GetLastSendFrameType(PHY_INST *instPtr);
int32_t Phy_SetHopPattern(void *patternBuff, uint32_t patternLen);
uint32_t Phy_GetHopLength(L1_FRAME_TYPE frameType);
uint32_t Phy_GetTimeComp(L1_FRAME_TYPE frameType);
int32_t Phy_GetFreqEst(PHY_INST *instPtr);
int32_t Phy_SetTimeWord(PHY_INST *instPtr, uint64_t tmWord, PHY_TIME_WORD_TYPE tmType);
int32_t Phy_AntConfigInit(PHY_ANT_COINFO *antCoinfoPtr);
void Phy_AntConfigInit(PHY_ANT_CONFIG *antConfig);
int32_t Phy_SetBeamWidth(PHY_INST *instPtr, PHY_ANT_BEAM_WIDTH beamWidth);
int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo);
int32_t Phy_SetBeamAngle(PHY_INST *phyInstPtr, double offAx, double rndAx);
int32_t Phy_SetTimeWord(PHY_INST *instPtr, const TIME_WORD *tmWord, PHY_TIME_WORD_TYPE tmType);
double Phy_GetCoarseFreqEst(PHY_INST *instPtr);
void Phy_GetFrameSyncTimeStamp(PHY_INST *instPtr, TIME_WORD *frameSyncT);
void Phy_GetLastReceivedFrameSyncTimeStamp(PHY_INST *instPtr, TIME_WORD *frameSyncT);
void Phy_GetSlotStartTimeStamp(PHY_INST *instPtr, TIME_WORD *slotStart);
void Phy_GetLastReceivedSlotStartTimeStamp(PHY_INST *instPtr, TIME_WORD *slotStart);
#endif /* PHY_CTRL_H_ */
df_datalink_sp/src/networking/phy_layer/phy_reg.h
+99 -105
View File
@@ -19,21 +19,17 @@
*
******************************************************************************/
#ifndef PHY_CTRLREG_H_
#define PHY_CTRLREG_H_
#ifndef PHY_REG_H_
#define PHY_REG_H_
/***************************** Include Files *********************************/
#include "util.h"
/***************** Macros (Inline Functions) Definitions *********************/
// Read Physical Layer register (0x00000000~0x000000FF)
#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)
/************************** Constant Definitions *****************************/
/************************* Ps2Pl Regs PS Write Only ***************************/
/**
* Register: PHY_MAIN_CTRL
*/
@@ -57,11 +53,14 @@
*/
#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_TOR_WIDTH 8
#define PHY_MODE_CTRL_TOR_MASK 0x000000FFU
#define PHY_MODE_CTRL_FRAME_SHIFT 8
#define PHY_MODE_CTRL_FRAME_WIDTH 8
#define PHY_MODE_CTRL_FRAME_MASK 0x00FF0000U
#define PHY_MODE_CTRL_FRAME_MASK 0x0000FF00U
#define PHY_MODE_CTRL_CRC_SHIFT 8
#define PHY_MODE_CTRL_CRC_WIDTH 16
#define PHY_MODE_CTRL_CRC_MASK 0x00FF0000U
#define PHY_MODE_CTRL_TOR_SUSPEND 0x00000000U
#define PHY_MODE_CTRL_TOR_TX 0x00000001U
@@ -77,6 +76,8 @@
#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_CRC_EN 0x00010000U
#define PHY_MODE_CTRL_CRC_DSBL 0x00000000U
/**
* Register: POLLTIME_L
*/
@@ -94,51 +95,43 @@
#define POLLTIME_H_MASK 0xFFFFFFFFU
/**
* Register: TM_ADJ_MODE
* Register: TIMEOUT_L
*/
#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 LISTENTIMEOUT_L 0x00000010U
#define LISTENTIMEOUT_L_SHIFT 0
#define LISTENTIMEOUT_L_WIDTH 32
#define LISTENTIMEOUT_L_MASK 0xFFFFFFFFU
/**
* Register: LOCALTM_ADJ_L
* Register: TIMEOUT_H
*/
#define LOCALTM_ADJ_L 0x00000014U
#define LOCALTM_ADJ_L_SHIFT 0
#define LOCALTM_ADJ_L_WIDTH 32
#define LOCALTM_ADJ_L_MASK 0xFFFFFFFFU
#define LISTENTIMEOUT_H 0x00000014U
#define LISTENTIMEOUT_H_SHIFT 0
#define LISTENTIMEOUT_H_WIDTH 32
#define LISTENTIMEOUT_H_MASK 0xFFFFFFFFU
/**
* Register: LOCALTM_ADJ_H
* Register: ANT_CTRL
*/
#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 ANT_CONFIG 0x00000018U
#define ANT_CONFIG_OFFSET_SHIFT 0
#define ANT_CONFIG_OFFSET_WIDTH 16
#define ANT_CONFIG_OFFSET_MASK 0x0000FFFFU
#define ANT_CONFIG_LEN_SHIFT 16
#define ANT_CONFIG_LEN_WIDTH 8
#define ANT_CONFIG_LEN_MASK 0x00FF0000U
#define ANT_CONFIG_NUM_SHIFT 24
#define ANT_CONFIG_NUM_WIDTH 8
#define ANT_CONFIG_NUM_MASK 0xFF000000U
/**
* Register: SEND_DATA_CMD
*/
#define SEND_DATA_CMD 0x00000020U
#define SEND_DATA_CMD 0x0000001CU
#define SEND_DATA_CMD_SHIFT 0
#define SEND_DATA_CMD_WIDTH 32
#define SEND_DATA_CMD_MASK 0xFFFFFFFFU
#define SEND_DATA_CMD_LEN_SHIFT 0
#define SEND_DATA_CMD_LEN_WIDTH 16
#define SEND_DATA_CMD_LEN_MASK 0x0000FFFFU
@@ -146,34 +139,19 @@
#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
/**
* 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_CMD 0x00000020U
#define SLOT_CMD_SHIFT 0
#define SLOT_CMD_WIDTH 32
#define SLOT_CMD_MASK 0xFFFFFFFFU
#define SLOT_CMD_KEY 0x434D442EU
/************************* Ps2Pl Regs PS Read Only ***************************/
/**
* Register: TXRX_STATE
*/
#define TXRX_STATE 0x00000100U
#define TXRX_STATE 0x00000024U
#define TXRX_STATE_ACQ_SHIFT 0
#define TXRX_STATE_ACQ_WIDTH 8
#define TXRX_STATE_ACQ_MASK 0x000000FFU
@@ -192,21 +170,28 @@
#define TXRX_STATE_DECODE_SUCCESS 0x00010000U
#define TXRX_STATE_DECODE_FAIL 0x00020000U
#define TXRX_STATE_CRC_SHIFT 24
#define TXRX_STATE_CRC_WIDTH 8
#define TXRX_STATE_CRC_MASK 0xFF000000U
#define TXRX_STATE_CRC_OK 0x01000000U
#define TXRX_STATE_CRC_ERROR 0x02000000U
/**
* Register: RECV_SATE
* Register: RECVDATA_STATE
*/
#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 RECVDATA_STATE 0x00000028U
#define RECVDATA_STATE_LEN_SHIFT 0
#define RECVDATA_STATE_LEN_WIDTH 16
#define RECVDATA_STATE_LEN_MASK 0x0000FFFFU
#define RECVDATA_STATE_OFFSET_SHIFT 16
#define RECVDATA_STATE_OFFSET_WIDTH 16
#define RECVDATA_STATE_OFFSET_MASK 0xFFFF0000U
/**
* Register: FSTIME_L
*/
#define FSTIME_L 0x00000108U
#define FSTIME_L 0x0000002CU
#define FSTIME_L_SHIFT 0
#define FSTIME_L_WIDTH 32
#define FSTIME_L_MASK 0xFFFFFFFFU
@@ -214,7 +199,7 @@
/**
* Register: FSTIME_INT1
*/
#define FSTIME_H 0x0000010CU
#define FSTIME_H 0x00000030U
#define FSTIME_H_SHIFT 0
#define FSTIME_H_WIDTH 32
#define FSTIME_H_MASK 0xFFFFFFFFU
@@ -222,7 +207,7 @@
/**
* Register: TM_TARGET_L
*/
#define TM_TARGET_L 0x00000110U
#define TM_TARGET_L 0x00000064U
#define TM_TARGET_L_SHIFT 0
#define TM_TARGET_L_WIDTH 32
#define TM_TARGET_L_MASK 0xFFFFFFFFU
@@ -230,44 +215,53 @@
/**
* Register: TM_TARGET_H
*/
#define TM_TARGET_H 0x00000114U
#define TM_TARGET_H 0x00000038U
#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
/**
* 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
/**
* Register: COASEFREQEST
*/
#define COASE_FREQEST 0x00000120U
#define COASE_FREQEST_SHIFT 0
#define COASE_FREQEST_WIDTH 32
#define COASE_FREQEST_MASK 0xFFFFFFFFU
#define COASE_FREQEST 0x0000003CU
#define COASE_FREQEST_ACQ_SHIFT 0
#define COASE_FREQEST_ACQ_WIDTH 8
#define COASE_FREQEST_ACQ_MASK 0x000000FFU
#define COASE_FREQEST_SYNC_SHIFT 8
#define COASE_FREQEST_SYNC_WIDTH 8
#define COASE_FREQEST_SYNC_MASK 0x000FF00U
/**
* Register: INTR_TYPE
* Register: FINE_FREQEST
*/
#define INTR_TYPE 0x00000124U
#define FINE_FREQEST 0x00000040U
#define FINE_FREQEST_SHIFT 0
#define FINE_FREQEST_WIDTH 32
#define FINE_FREQEST_MASK 0xFFFFFFFFU
/**
* Register: BEAM_SELECT
*/
#define BEAM_SELECT_INDEX 0x00000044U
#define BEAM_SELECT_INDEX_SHIFT 0
#define BEAM_SELECT_INDEX_WIDTH 32
#define BEAM_SELECT_INDEX_MASK 0xFFFFFFFFU
/**
* Register: BEAM_AXIS
*/
#define BEAM_AXIS 0x00000044U
#define BEAM_AXIS_EL_SHIFT 0
#define BEAM_AXIS_EL_WIDTH 16
#define BEAM_AXIS_EL_MASK 0x0000FFFFU
#define BEAM_AXIS_AZ_SHIFT 16
#define BEAM_AXIS_AZ_WIDTH 16
#define BEAM_AXIS_AZ_MASK 0xFFFF0000U
/**
* Register: ANT_STATUS_0
*/
#define ANT_STATUS 0x00000128U
#define ANT_STATUS 0x00000100U
#define ANT_STATUS_CODE_SHIFT 0
#define ANT_STATUS_CODE_WIDTH 8
#define ANT_STATUS_CODE_MASK 0x000000FFU
@@ -284,7 +278,7 @@
/**
* Register: ANT_FRAME_CNT
*/
#define ANT_FRAME_CNT 0x0000012CU
#define ANT_FRAME_CNT 0x00000104U
#define ANT_FRAME_CNT_SHIFT 0
#define ANT_FRAME_CNT_WIDTH 32
#define ANT_FRAME_CNT_MASK 0xFFFFFFFFU
@@ -292,7 +286,7 @@
/**
* Register: RF_STATUS
*/
#define RF_STATUS 0x00000130U
#define RF_STATUS 0x00000108U
#define RF_STATUS_TEMP_SHIFT 0
#define RF_STATUS_TEMP_WIDTH 8
#define RF_STATUS_TEMP_MASK 0x000000FFU
@@ -309,7 +303,7 @@
/**
* Register: RF_MGC
*/
#define RF_MGC 0x00000134U
#define RF_MGC 0x0000010CU
#define RF_MGC_CH1_SHIFT 0
#define RF_MGC_CH1_WIDTH 4
#define RF_MGC_CH1_MASK 0x0000000FU
@@ -326,9 +320,9 @@
/**
* Register: RF_FRAME_CNT
*/
#define RF_FRAME_CNT 0x00000138U
#define RF_FRAME_CNT 0x00000110U
#define RF_FRAME_CNT_SHIFT 0
#define RF_FRAME_CNT_WIDTH 32
#define RF_FRAME_CNT_MASK 0xFFFFFFFFU
#endif /* PHY_CTRLREG_H_ */
#endif /* PHY_REG_H_ */
df_datalink_sp/src/system_monitor.c
-19
View File
@@ -372,12 +372,6 @@ void SysMonitor_GetSendSlotLogRaw(uint8_t *buff)
buff += sizeof(logEntry->slotNo);
memcpy(buff, &(logEntry->frameType), sizeof(logEntry->frameType));
buff += sizeof(logEntry->frameType);
memcpy(buff, &(logEntry->srcId), sizeof(logEntry->srcId));
buff += sizeof(logEntry->srcId);
memcpy(buff, &(logEntry->dstId), sizeof(logEntry->dstId));
buff += sizeof(logEntry->dstId);
memcpy(buff, &(logEntry->baseInfo), sizeof(logEntry->baseInfo));
buff += sizeof(logEntry->baseInfo);
memcpy(buff, &(logEntry->payload), sizeof(logEntry->payload));
buff += sizeof(logEntry->payload);
memcpy(buff, &(logEntry->tmStamp), sizeof(logEntry->tmStamp));
@@ -442,12 +436,6 @@ void SysMonitor_GetRecvSlotLogRaw(uint8_t *buff)
buff += sizeof(logEntry->slotNo);
memcpy(buff, &(logEntry->frameType), sizeof(logEntry->frameType));
buff += sizeof(logEntry->frameType);
memcpy(buff, &(logEntry->srcId), sizeof(logEntry->srcId));
buff += sizeof(logEntry->srcId);
memcpy(buff, &(logEntry->dstId), sizeof(logEntry->dstId));
buff += sizeof(logEntry->dstId);
memcpy(buff, &(logEntry->baseInfo), sizeof(logEntry->baseInfo));
buff += sizeof(logEntry->baseInfo);
memcpy(buff, &(logEntry->payload), sizeof(logEntry->payload));
buff += sizeof(logEntry->payload);
memcpy(buff, &(logEntry->tmStamp), sizeof(logEntry->tmStamp));
@@ -520,13 +508,6 @@ void SysMonitor_GetProtocolLogRaw(uint8_t *buff)
buff += sizeof(logEntry->stage);
memcpy(buff, &(logEntry->recvCnt), sizeof(logEntry->recvCnt));
buff += sizeof(logEntry->recvCnt);
memcpy(buff, &(logEntry->recvFineCnt), sizeof(logEntry->recvFineCnt));
buff += sizeof(logEntry->recvFineCnt);
memcpy(buff, &(logEntry->nodeCnt), sizeof(logEntry->nodeCnt));
buff += sizeof(logEntry->nodeCnt);
memcpy(buff, &(logEntry->myId), sizeof(logEntry->myId));
buff += sizeof(logEntry->myId);
memcpy(buff, &(logEntry->dstId), sizeof(logEntry->dstId));
sysMonitorInst->protocolLogCtrl.rBusy = buffIndex_old;
}
df_datalink_sp/src/system_monitor.h
+3 -9
View File
@@ -63,11 +63,8 @@ typedef struct
uint32_t tuNo;
uint8_t slotNo;
uint8_t frameType;
uint8_t srcId;
uint8_t dstId;
uint8_t baseInfo[18];
// Payload
uint8_t payload[46];
uint8_t payload[256];
// Time sync info
uint64_t tmStamp;
uint32_t tmDevi;
@@ -82,16 +79,13 @@ typedef struct
// -- Physical layer status
uint8_t phySt;
// -- Link layer status
uint32_t tmLocal;
uint64_t tmLocal;
uint16_t beamNo;
uint8_t beamWidth;
// -- Network layer status
uint8_t stage;
uint32_t recvCnt;
uint32_t recvFineCnt;
uint8_t nodeCnt;
uint8_t myId;
uint8_t dstId;
uint32_t recvCorrect;
} PRTCLLOG;
typedef struct
df_datalink_sp/src/time_ctrl/time_ctrl.c
+276
View File
@@ -0,0 +1,276 @@
/*****************************************************************************/
/**
* Description: Time sync implementation file.
*
* Author: Author's name
* Date: 2023-12-20 16:32
* LastEditors: FanHao
* LastEditTime: 2023-12-22 20:43
* Revisions: Rev 0.0
* History:
* [2023-12-20]: Initial version.
*
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Networking
*
******************************************************************************/
/***************************** Include Files *********************************/
#include <string.h>
#include "time_ctrl.h"
#include "error.h"
#include "common.h"
#include "util.h"
/************************** Constant Definitions *****************************/
#define VERSION_REG 0x00000004
#define TIME_INT_REG 0x00000008
#define TIME_FRAC_REG 0x0000000C
#define TIME_RESET 0x00000010
#define PRESET_HIGH 0x00000014
#define PRESET_LOW 0x00000018
#define TIME_ADJ_INT 0x0000001C
#define TIME_ADJ_FRAC 0x00000020
#define TIME_ADJ_MODE 0x00000024
#define TIME_ADJ_TRIG 0x00000028
#define CNTSTEP_OPT 0x0000002c
#define TM_RESET_CODE 0x5253542E
/***************** Macros (Inline Functions) Definitions *********************/
#define TMC_Read(instPtr, addr) IO_In32(instPtr->config.baseAddr + addr)
#define TMC_Write(instPtr, addr, value) IO_Out32(instPtr->config.baseAddr + addr, value)
/*****************************************************************************/
/**
* @brief Initialize Time-Controller Process
*
* @param instPtr a second rank pointer to time controller instance
* @param config time controller configurations
*
* @return SUCCESS Initialize successfully.
* FAILURE Initialization failed
*
******************************************************************************/
int32_t TimeCtrl_Init(TIMECTRL_INST **instPtr, TIMECTRL_CONFIG *config)
{
ASSERT(config != NULL);
ASSERT(instPtr != NULL);
(*instPtr) = malloc(sizeof(TIMECTRL_INST));
memcpy(&((*instPtr)->config), config, sizeof(TIMECTRL_CONFIG));
(*instPtr)->modelVer = TMC_Read((*instPtr), VERSION_REG);
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Completely destroy time control instance.
*
* @param instPtr a pointer to time controller instance
*
* @return NONE
*
******************************************************************************/
void TimeCtrl_Destroy(TIMECTRL_INST *instPtr)
{
free(instPtr);
}
/*****************************************************************************/
/**
* @brief Read time controller version register.
*
* @param instPtr a pointer to time controller instance
*
* @return timer controller version.
*
******************************************************************************/
uint32_t TimeCtrl_GetVersion(TIMECTRL_INST *instPtr)
{
ASSERT(instPtr != NULL);
return TMC_Read(instPtr, VERSION_REG);
}
/*****************************************************************************/
/**
* @brief Apply a reset process to time controller
*
* @param instPtr a pointer to time controller instance
*
* @return SUCCESS Initialize successfully.
* FAILURE Initialization failed
*
******************************************************************************/
void TimeCtrl_Reset(TIMECTRL_INST *instPtr)
{
ASSERT(instPtr != NULL);
TMC_Write(instPtr, CNTSTEP_OPT, TM_RESET_CODE);
}
/*****************************************************************************/
/**
* @brief Assert time controller reset high
*
* @param instPtr a pointer to time controller instance
*
* @return NONE
*
******************************************************************************/
void TimeCtrl_ResetHigh(TIMECTRL_INST *instPtr)
{
ASSERT(instPtr != NULL);
// TODO:
}
/*****************************************************************************/
/**
* @brief Assert time controller reset low
*
* @param instPtr a pointer to time controller instance
*
* @return NONE
*
******************************************************************************/
void TimeCtrl_ResetLow(TIMECTRL_INST *instPtr)
{
ASSERT(instPtr != NULL);
// TODO:
}
/*****************************************************************************/
/**
* @brief Get current time integer value.
*
* @param instPtr a pointer to time controller instance
*
* @return integer value
*
******************************************************************************/
inline TIME_INT TimeCtrl_GetIntCnt(TIMECTRL_INST *instPtr)
{
ASSERT(instPtr != NULL);
return TMC_Read(instPtr, TIME_INT_REG);
}
/*****************************************************************************/
/**
* @brief Get current time fractional value.
*
* @param instPtr a pointer to time controller instance
*
* @return fractional value
*
******************************************************************************/
inline TIME_FRAC TimeCtrl_GetFracCnt(TIMECTRL_INST *instPtr)
{
ASSERT(instPtr != NULL);
return TMC_Read(instPtr, TIME_FRAC_REG);
}
/*****************************************************************************/
/**
* @brief Get current time value.
*
* @param instPtr a pointer to time controller instance
* @param tmWord a pointer to time data buffer
*
* @return NONE
*
******************************************************************************/
void TimeCtrl_GetCurrentTime(TIMECTRL_INST *instPtr, TIME_WORD *tmWord)
{
ASSERT(instPtr != NULL);
ASSERT(tmWord != NULL);
tmWord->frac_part = TMC_Read(instPtr, TIME_FRAC_REG);
tmWord->int_part = TMC_Read(instPtr, TIME_INT_REG);
}
/*****************************************************************************/
/**
* @brief Get current optimal counter step adjustment value.
*
* @param instPtr a pointer to time controller instance
* @param config a pointer to time data buffer
*
* @return NONE.
*
******************************************************************************/
void TimeCtrl_GetCurrentStep(TIMECTRL_INST *instPtr, TIME_FRAC *tmFrac)
{
ASSERT(instPtr != NULL);
ASSERT(tmFrac != NULL);
*tmFrac = TMC_Read(instPtr, CNTSTEP_OPT);
}
/*****************************************************************************/
/**
* @brief Set a new optimal adjustment value to the counter step.
*
* @param instPtr a pointer to time controller instance
* @param tmWord a pointer to the time data
* @param type adjustment type
*
* @return NONE.
*
******************************************************************************/
void TimeCtrl_TimeAdjust(TIMECTRL_INST *instPtr, const TIME_WORD *tmWord, TIMECTRL_ADJ_TYPE type)
{
ASSERT(instPtr != NULL);
ASSERT(tmWord != NULL);
if (type < TM_ADJ_ADD || TM_ADJ_ADD > TM_ADJ_SET)
{
DBG_ERROR("Unsupported adjustment type: %d", type);
return;
}
if (((int32_t)tmWord->frac_part) > TIME_FRAC_MAX || ((int32_t)tmWord->frac_part) < -TIME_FRAC_MAX)
{
DBG_WARNING("Invalid fractional value %d (should between %d~%d)\n",
tmWord->frac_part, -TIME_FRAC_MAX, TIME_FRAC_MAX);
return;
}
TMC_Write(instPtr, TIME_ADJ_FRAC, tmWord->frac_part);
TMC_Write(instPtr, TIME_ADJ_INT, tmWord->int_part);
TMC_Write(instPtr, TIME_ADJ_MODE, type);
TMC_Write(instPtr, TIME_ADJ_TRIG, type);
}
/*****************************************************************************/
/**
* @brief Set a new optimal adjustment value to the counter step.
*
* @param instPtr a pointer to time controller instance
* @param tmFrac a pointer to the time data
*
* @return NONE.
*
******************************************************************************/
void TimeCtrl_SetOptimizeStep(TIMECTRL_INST *instPtr, TIME_FRAC *tmFrac)
{
ASSERT(instPtr != NULL);
ASSERT(tmFrac != NULL);
TMC_Write(instPtr, CNTSTEP_OPT, *tmFrac);
}
/*****************************************************************************/
/**
* @brief Calculate b - a.
*
* @param a time stamp A
* @param b time stamp B
* @param gap t round
*
* @return NONE
*
******************************************************************************/
void TimeCtrl_CalculateTimeGap(const TIME_WORD *a, const TIME_WORD *b, TIME_WORD *gap)
{
ASSERT(a != NULL);
ASSERT(b != NULL);
ASSERT(gap != NULL);
uint64_t timeDiff;
timeDiff = (uint64_t)b->frac_part;
timeDiff += ((uint64_t)b->int_part) * TIME_FRAC_MAX;
timeDiff -= ((uint64_t)a->int_part) * TIME_FRAC_MAX;
timeDiff -= (uint64_t)a->frac_part;
gap->frac_part = (TIME_FRAC)(timeDiff % TIME_FRAC_MAX);
gap->frac_part = (TIME_INT)(timeDiff / TIME_FRAC_MAX);
}
df_datalink_sp/src/time_ctrl/time_ctrl.h
+74
View File
@@ -0,0 +1,74 @@
/*****************************************************************************/
/**
* Description: A simplified version of timesync for datalink 814 (Dijian)
*
* Date: 2023-12-20-04 16:32
* LastEditors: Fan Hao
* LastEditTime: 2023-12-20 16:32
* Revisions: Rev 0.0
* History:
* [2023-12-20]: Initial version.
*
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Networking
*
******************************************************************************/
#ifndef TIME_CTRL_H_
#define TIME_CTRL_H_
/***************************** Include Files *********************************/
#include <stdint.h>
/************************** Constant Definitions *****************************/
#define TIME_FRAC_MAX 500000 // how many 10ns in a 5ms
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
typedef enum
{
TM_ADJ_NONE = 0,
TM_ADJ_ADD = 1,
TM_ADJ_SET = 2
} TIMECTRL_ADJ_TYPE;
typedef uint32_t TIME_FRAC;
typedef uint32_t TIME_INT;
typedef struct
{
/* 5ms each LSB */
TIME_INT int_part;
/* 10ns each LSB */
TIME_FRAC frac_part;
} TIME_WORD;
typedef struct
{
uintptr_t baseAddr;
uint32_t reset; // time for reset (unit: us)
} TIMECTRL_CONFIG;
typedef struct
{
TIMECTRL_CONFIG config;
uint32_t modelVer;
} TIMECTRL_INST;
/************************** Function Prototypes ******************************/
int32_t TimeCtrl_Init(TIMECTRL_INST **instPtr, TIMECTRL_CONFIG *config);
uint32_t TimeCtrl_GetVersion(TIMECTRL_INST *instPtr);
void TimeCtrl_Destroy(TIMECTRL_INST *instPtr);
void TimeCtrl_Reset(TIMECTRL_INST *instPtr);
void TimeCtrl_ResetLow(TIMECTRL_INST *instPtr);
void TimeCtrl_ResetHigh(TIMECTRL_INST *instPtr);
TIME_INT TimeCtrl_GetIntCnt(TIMECTRL_INST *instPtr);
TIME_FRAC TimeCtrl_GetFracCnt(TIMECTRL_INST *instPtr);
void TimeCtrl_GetCurrentTime(TIMECTRL_INST *instPtr, TIME_WORD *tmWord);
void TimeCtrl_GetCurrentStep(TIMECTRL_INST *instPtr, TIME_FRAC *tmFrac);
void TimeCtrl_SetOptimizeStep(TIMECTRL_INST *instPtr, TIME_FRAC *tmFrac);
void TimeCtrl_TimeAdjust(TIMECTRL_INST *instPtr, const TIME_WORD *tmWord, TIMECTRL_ADJ_TYPE type);
void TimeCtrl_CalculateTimeGap(const TIME_WORD *a, const TIME_WORD *b, TIME_WORD *gap);
#endif /* TIME_SYNC_H_ */