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/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;
}
}