XiaQingMiao/datalink_nc_mode
1
0
Fork 0
forked from gatsby/datalink_repo
Code Pull Requests Activity

formatted.

Browse Source
This commit is contained in:
Gatsby Van
2023-12-27 14:59:41 +08:00
parent fa5ff861d5
commit 677b8b4af5
27 changed files with 3211 additions and 2274 deletions
Download Patch File Download Diff File Expand all files Collapse all files
df_datalink/src/networking/link_layer/framing.c
+32 -23
View File
@@ -1,18 +1,18 @@
/*****************************************************************************/
/**
* Description: Functions for framing operations.
*
*
* Date: 2022-07-16 14:17
* LastEditors: Fan Hao
* LastEditTime: 2022-07-16 17:11
* Revisions: Rev 0.0
* History:
* History:
* [2022-07-16]: Description of modification.
*
*
* Copyright(c) 2022 Beijing Institute of Technology.
* Lab of Communication and Networking
*
******************************************************************************/
*
******************************************************************************/
/***************************** Include Files *********************************/
#include <stdint.h>
@@ -24,7 +24,6 @@
/************************** Constant Definitions *****************************/
/*****************************************************************************/
/**
* @brief Generate crc of specified data.
@@ -38,18 +37,20 @@
* SUCCESS
*
******************************************************************************/
static int32_t L2_CrcGen(uint8_t* dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t* crc){
static int32_t L2_CrcGen(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t *crc)
{
ASSERT(dataBuff != NULL);
int32_t Status;
switch (crcType){
switch (crcType)
{
case L2_CRC16_1021:
Status = crc16_gen(dataBuff, dataLen, 0x1021, (uint16_t*)crc);
Status = crc16_gen(dataBuff, dataLen, 0x1021, (uint16_t *)crc);
break;
case L2_CRC16_8005:
Status = crc16_gen(dataBuff, dataLen, 0x8005, (uint16_t*)crc);
Status = crc16_gen(dataBuff, dataLen, 0x8005, (uint16_t *)crc);
break;
case L2_CRC8_31:
Status = crc8_gen(dataBuff, dataLen, 0x31, (uint8_t*)crc);
Status = crc8_gen(dataBuff, dataLen, 0x31, (uint8_t *)crc);
break;
default:
Status = EPARAM;
@@ -71,23 +72,26 @@ static int32_t L2_CrcGen(uint8_t* dataBuff, uint32_t dataLen, L2_CRC_TYPE crcTyp
* SUCCESS
*
******************************************************************************/
int32_t L2_CrcAppend(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType){
int32_t L2_CrcAppend(uint8_t *dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType)
{
ASSERT(dataBuff != NULL);
int32_t Status;
uint32_t crcVal;
uint8_t* crcPtr;
uint8_t *crcPtr;
crcPtr = dataBuff + crcPos;
Status = L2_CrcGen(dataBuff,dataLen, crcType, &crcVal);
if(Status != SUCCESS){
Status = L2_CrcGen(dataBuff, dataLen, crcType, &crcVal);
if (Status != SUCCESS)
{
return Status;
}
switch (crcType){
switch (crcType)
{
case L2_CRC16_1021:
*(crcPtr+1) = (uint8_t)(crcVal>>8);
*(crcPtr + 1) = (uint8_t)(crcVal >> 8);
*crcPtr = (uint8_t)(crcVal);
break;
case L2_CRC16_8005:
*(crcPtr+1) = (uint8_t)(crcVal>>8);
*(crcPtr + 1) = (uint8_t)(crcVal >> 8);
*crcPtr = (uint8_t)(crcVal);
break;
case L2_CRC8_31:
@@ -112,21 +116,26 @@ int32_t L2_CrcAppend(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CR
* SUCCESS
*
******************************************************************************/
bool L2_CrcCheck(uint8_t* dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType){
bool L2_CrcCheck(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType)
{
ASSERT(dataBuff != NULL);
int32_t Status;
uint32_t crcVal;
crcVal = 0;
Status = L2_CrcGen(dataBuff, dataLen, crcType, &crcVal);
if(Status == SUCCESS){
if(crcVal == 0){
if (Status == SUCCESS)
{
if (crcVal == 0)
{
return true;
}
else{
else
{
return false;
}
}
else {
else
{
return false;
}
}
df_datalink/src/networking/link_layer/link_layer.c
+297 -184
View File
File diff suppressed because it is too large Load Diff
df_datalink/src/networking/link_layer/link_layer.h
+45 -42
View File
@@ -1,17 +1,17 @@
/*****************************************************************************/
/**
* Description: Header file for df linklayer functions
*
*
* Date: 2021-11-13 13:33
* LastEditTime: 2022-07-16 10:12
* Revisions: Rev 0.1
* History:
* History:
* [2022-07-16]: New branch for DiJian.
* [2021-11-23]: Initial version.
*
*
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Networking
*
*
******************************************************************************/
#ifndef __LINK_LAYER_H_
@@ -26,50 +26,53 @@
#include "../config/app_config.h"
#include "node_list.h"
/************************** Constant Definitions *****************************/
#define L2_HEAD_NODE_ID 1
#define L2_MEMBER_NODE_ID 2
#define L2_HEAD_NODE_ID 1
#define L2_MEMBER_NODE_ID 2
/***************** Macros (Inline Functions) Definitions *********************/
//#define AssertL2Event(inst, envent) Linklayer_EventHandler(inst, envent)
// #define AssertL2Event(inst, event) Linklayer_EventHandler(inst, event)
/**************************** Type Definitions *******************************/
typedef struct{
L2_RTNE_TABLE_ENTRY** rtneTable;
typedef struct
{
L2_RTNE_TABLE_ENTRY **rtneTable;
uint32_t rtnePrd;
uint32_t rtneStep;
L2_RTNE_TABLE_ENTRY* last;
L2_RTNE_TABLE_ENTRY* curr;
L2_RTNE_TABLE_ENTRY* next;
}L2_RTNE_CONFIG;
L2_RTNE_TABLE_ENTRY *last;
L2_RTNE_TABLE_ENTRY *curr;
L2_RTNE_TABLE_ENTRY *next;
} L2_RTNE_CONFIG;
typedef struct {
typedef struct
{
L2_NODE_TYPE myType;
// bool isInterHead;
// uint8_t clustId;
// bool isInterHead;
// uint8_t clustId;
uint8_t myId;
uint8_t headId;
// uint8_t bkpHeadId;
// uint8_t bkpHeadId;
uint8_t netScale;
PHY_INST* phyInstRef; // pyhsical layer reference
PHY_INST *phyInstRef; // pyhsical layer reference
} L2_CONFIG;
typedef struct {
typedef struct
{
L2_CONFIG config;
bool dataAvail; // if there is a valid received frame data in rx buffer
bool cxn2Net; // if the node is connected to the network
L2_RCD frameNo; // stores the last current and next frame number.
L2_RCD slotNo; // stores the last current and next slot number.
L2_RCD frame; // stores the last current and next link layer frame type.
L2_RCD torState; // stores the last current and next link layer send/receive state.
L2_NODE_STATE mainState; // the main state of link layer state
L2_NODE_STATE nextState; // next state of link layer state, which will be applied to mainState every second.
bool dataAvail; // if there is a valid received frame data in rx buffer
bool cxn2Net; // if the node is connected to the network
L2_RCD frameNo; // stores the last current and next frame number.
L2_RCD slotNo; // stores the last current and next slot number.
L2_RCD frame; // stores the last current and next link layer frame type.
L2_RCD torState; // stores the last current and next link layer send/receive state.
L2_NODE_STATE mainState; // the main state of link layer state
L2_NODE_STATE nextState; // next state of link layer state, which will be applied to mainState every second.
L2_RTNE_CONFIG rtneConfig; // current routine table configuration and status.
L2_SCHD_EVENT* schdEventPool[L2_SCHD_EVENT_MAX];
L2_SCHD_EVENT *schdEventPool[L2_SCHD_EVENT_MAX];
uint32_t schdEventCnt;
SCAN_FSM scanFsm;
SVC_FSM serviceFsm;
NODE_LIST *nodeInfoList;
uint8_t* sendPktData;
uint8_t* recvPktData;
uint8_t *sendPktData;
uint8_t *recvPktData;
// for system log
SYSINFO sysInfo;
SLOTLOG txSlotLog;
@@ -83,32 +86,32 @@ typedef struct {
} L2_INST;
/************************** Variable Definitions *****************************/
extern L2_RTNE_TABLE_ENTRY* normalTable[L2_RTNE_PERIOD];
extern L2_RTNE_TABLE_ENTRY *normalTable[L2_RTNE_PERIOD];
/************************** Function Prototypes ******************************/
// Main
int32_t Linklayer_Init(L2_INST **instPtr, L2_CONFIG *config);
int32_t Linklayer_Proc(L2_INST *instPtr);
int32_t Linklayer_EventHandler(L2_INST *instPtr, L2_EVENT* l2Event);
int32_t Linklayer_EventHandler(L2_INST *instPtr, L2_EVENT *l2Event);
int32_t Linklayer_PktHandler(L2_INST *instPtr);
int32_t L2_SteerBeam2Node(L2_INST *instPtr, uint8_t nodeId);
int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE frameType);
// Event
L2_SCHD_EVENT* L2_CreatNewSchdEvent(L2_INST *instPtr, L2_EVENT_TYPE event);
bool L2_isEventScheduled(L2_SCHD_EVENT* schdEvent);
int32_t L2_CancelEvent(L2_SCHD_EVENT* schdEvent);
int32_t L2_SchdEventAppendMsg(L2_SCHD_EVENT* schdEvent, void* msg);
int32_t L2_ScheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo);
int32_t L2_ScheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots);
int32_t L2_RecheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo);
int32_t L2_RecheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots);
L2_SCHD_EVENT *L2_CreatNewSchdEvent(L2_INST *instPtr, L2_EVENT_TYPE event);
bool L2_isEventScheduled(L2_SCHD_EVENT *schdEvent);
int32_t L2_CancelEvent(L2_SCHD_EVENT *schdEvent);
int32_t L2_SchdEventAppendMsg(L2_SCHD_EVENT *schdEvent, void *msg);
int32_t L2_ScheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t frameNo, uint32_t slotNo);
int32_t L2_ScheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t numSlots);
int32_t L2_RecheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t frameNo, uint32_t slotNo);
int32_t L2_RecheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t numSlots);
// Framing
void L2_FrameStateUpdate(L2_INST *instPtr);
L2_FRAME_TYPE L2_GetLastRcvFrameType(L2_INST *instPtr);
L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame);
int32_t L2_CrcAppend(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType);
bool L2_CrcCheck(uint8_t* dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType);
int32_t L2_CrcAppend(uint8_t *dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType);
bool L2_CrcCheck(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType);
// Miscellaneous
L2_NODE_STATE L2_GetMainState(L2_INST *instPtr);
df_datalink/src/networking/link_layer/link_layer_types.h
+44 -31
View File
@@ -1,17 +1,17 @@
/*****************************************************************************/
/**
* Description: A brief description of this file
*
*
* Date: 2022-08-28 22:53
* LastEditTime: 2022-08-29 00:45
* Revisions: Rev 0.0
* History:
* History:
* [2022-08-29]: Initial version.
*
*
* Copyright(c) 2022 Beijing Institute of Technology.
* Lab of Communication and Networking
*
******************************************************************************/
*
******************************************************************************/
#ifndef __LINK_LAYER_DEF_H_
#define __LINK_LAYER_DEF_H_
@@ -36,16 +36,16 @@
#define L2_FRAMENO_NONE 0
// Common number of slots definition
#define L2_FRAME_NO_MAX 256
#define L2_SLOTS_PER_FRAME 200
#define L2_SLOTS_PER_SUBFRAME 20
#define L2_FRAME_NO_MAX 256
#define L2_SLOTS_PER_FRAME 200
#define L2_SLOTS_PER_SUBFRAME 20
#define NUM_SLOTS_NONE UINT32_MAX
#define NUM_SLOTS_100MS 20
#define NUM_SLOTS_20MS 4
#define NUM_SLOTS_10MS 2
#define NUM_SLOTS_5MS 1
#define NUM_SLOTS_SKIP 0
#define NUM_SLOTS_100MS 20
#define NUM_SLOTS_20MS 4
#define NUM_SLOTS_10MS 2
#define NUM_SLOTS_5MS 1
#define NUM_SLOTS_SKIP 0
// Common schedule event definition
#define L2_SCHD_EVENT_MAX 16
@@ -54,13 +54,15 @@
#define AssertL2Event(inst, envent) Linklayer_EventHandler(inst, envent)
/**************************** Type Definitions *******************************/
typedef struct {
typedef struct
{
int32_t last;
int32_t curr;
int32_t next;
} L2_RCD;
typedef enum {
typedef enum
{
L2_TOR_SUSPEND = 0,
L2_TOR_TX = 1,
L2_TOR_RX = 2
@@ -68,7 +70,8 @@ typedef enum {
/** @name Prior information type definition
*/
typedef enum {
typedef enum
{
L2_NONE_TYPE = 0,
L2_HEAD_NODE = 1,
L2_MEMBER_NODE = 2,
@@ -77,17 +80,18 @@ typedef enum {
/** @name Node state type definition
*/
typedef enum {
NONE_STATE = 0,
INIT_STATE = 1,
typedef enum
{
NONE_STATE = 0,
INIT_STATE = 1,
NORMAL_STATE = 2
} L2_NODE_STATE;
/** @name Node information type definition
* The following types define the detail node info which needed by the link layer.
*/
typedef struct {
typedef struct
{
uint8_t nodeId;
uint8_t nodeType;
uint8_t beamNo;
@@ -99,7 +103,8 @@ typedef struct {
/** @name L2 event enum
* A enum of L2 event types.
*/
typedef enum {
typedef enum
{
L2_EV_NONE = 0,
L2_EV_SCAN_PROC_DONE = 1,
L2_EV_NET_SETUP_DONE = 2,
@@ -116,15 +121,17 @@ typedef enum {
/** @name L2 event
* A definition of L2 event instance.
*/
typedef struct {
typedef struct
{
L2_EVENT_TYPE event;
void* msg;
void *msg;
} L2_EVENT;
/** @name L2 event
* A definition of L2 event instance.
*/
typedef struct {
typedef struct
{
uint8_t eventId;
bool isSchd;
L2_EVENT l2Event;
@@ -135,7 +142,8 @@ typedef struct {
/** @name Slot types
* A definition of different slots.
*/
typedef enum {
typedef enum
{
L2_SLOT_NONE = 0,
L2_SLOT_IDLE = 1,
L2_SLOT_SCAN = 2,
@@ -146,7 +154,8 @@ typedef enum {
/** @name Frame types
* A definition of different frame types.
*/
typedef enum {
typedef enum
{
L2_NONE_FRAME = 0,
L2_SCAN_FRAME = 1,
L2_SVC_FRAME = 2,
@@ -156,7 +165,8 @@ typedef enum {
/** @name Frame types
* A definition of different frame types.
*/
typedef enum {
typedef enum
{
L2_CRC_NONE = 0,
L2_CRC16_1021 = 1,
L2_CRC16_8005 = 2,
@@ -166,7 +176,8 @@ typedef enum {
/** @name Schedule subtable entry
* A definition of Schedule subtable entry.
*/
typedef struct {
typedef struct
{
L2_SLOT_TYPE slotType : 8;
uint8_t slotSeq;
uint8_t src;
@@ -175,14 +186,16 @@ typedef struct {
/** @name Scan procedure FSM
*/
typedef struct {
typedef struct
{
uint8_t sacanCnt;
uint8_t beamNo;
} SCAN_FSM;
/** @name Service procedure FSM
*/
typedef struct {
typedef struct
{
uint32_t txSeqNum;
uint32_t rxSeqNum;
uint8_t rxLog[256];
df_datalink/src/networking/link_layer/msg_def.h
+18 -18
View File
@@ -26,10 +26,10 @@
// 1 slot number
// 26 payload
// 2 crc
#define L2_MSG_FRAME_NO 0
#define L2_MSG_SLOT_NO 1
#define L2_MSG_PAYLOAD 2
#define L2_MSG_CRC 28
#define L2_MSG_FRAME_NO 0
#define L2_MSG_SLOT_NO 1
#define L2_MSG_PAYLOAD 2
#define L2_MSG_CRC 28
// service frame structure:
// [bytes] [name]
@@ -38,13 +38,13 @@
// 3 time sync
// 23 payload
// 2 crc
#define L2_MSG_SVC_TMSYNC 2
#define L2_MSG_SVC_TMSYNC_LEN 3
#define L2_MSG_SVC_TMSYNC_MAX (8388607) //0x007FFFFF
#define L2_MSG_SVC_TMSYNC_MIN (-8388608) //0x00800000
#define L2_MSG_SVC_SEQ_ID 5
#define L2_MSG_SVC_PAYLOAD 6
#define L2_MSG_SVC_PAYLOAD_LEN 22
#define L2_MSG_SVC_TMSYNC 2
#define L2_MSG_SVC_TMSYNC_LEN 3
#define L2_MSG_SVC_TMSYNC_MAX (8388607) // 0x007FFFFF
#define L2_MSG_SVC_TMSYNC_MIN (-8388608) // 0x00800000
#define L2_MSG_SVC_SEQ_ID 5
#define L2_MSG_SVC_PAYLOAD 6
#define L2_MSG_SVC_PAYLOAD_LEN 22
// response frame structure:
// [bytes] [name]
@@ -58,11 +58,11 @@
// 4 speed z
// 2 dummy
// 2 crc
#define L2_MSG_INFO_POSX 2
#define L2_MSG_INFO_POSY 6
#define L2_MSG_INFO_POSZ 10
#define L2_MSG_INFO_SPEEDX 14
#define L2_MSG_INFO_SPEEDY 18
#define L2_MSG_INFO_SPEEDZ 22
#define L2_MSG_INFO_DUMMY 26
#define L2_MSG_INFO_POSX 2
#define L2_MSG_INFO_POSY 6
#define L2_MSG_INFO_POSZ 10
#define L2_MSG_INFO_SPEEDX 14
#define L2_MSG_INFO_SPEEDY 18
#define L2_MSG_INFO_SPEEDZ 22
#define L2_MSG_INFO_DUMMY 26
#endif //__L2_MSG_DEF_H_
df_datalink/src/networking/link_layer/node_list.c
+119 -69
View File
@@ -23,7 +23,8 @@
/************************** Constant Definitions *****************************/
/***************** Macros (Inline Functions) Definitions *********************/
inline int32_t GetNodeNum(NODE_LIST *list) {
inline int32_t GetNodeNum(NODE_LIST *list)
{
ASSERT(list != NULL);
return list->nodeNum;
}
@@ -43,14 +44,17 @@ inline int32_t GetNodeNum(NODE_LIST *list) {
* ENOMEM - not enough memory.
*
******************************************************************************/
int32_t NodeList_Init(NODE_LIST **list) {
int32_t NodeList_Init(NODE_LIST **list)
{
NODE_ENTRY *entry;
(*list) = malloc(sizeof(NODE_LIST));
if (*list == NULL) {
if (*list == NULL)
{
return ENOMEM;
}
entry = malloc(sizeof(NODE_ENTRY));
if(entry == NULL) {
if (entry == NULL)
{
free(*list);
return ENOMEM;
}
@@ -62,11 +66,13 @@ int32_t NodeList_Init(NODE_LIST **list) {
return SUCCESS;
}
void NodeList_Clear(NODE_LIST *list){
void NodeList_Clear(NODE_LIST *list)
{
ASSERT(list != NULL);
NODE_ENTRY *nodeEntry, *deletEntry;
nodeEntry = list->head->nextNode;
while (nodeEntry != list->head) {
while (nodeEntry != list->head)
{
deletEntry = nodeEntry;
nodeEntry = nodeEntry->nextNode;
deletEntry->preNode->nextNode = deletEntry->nextNode;
@@ -77,8 +83,10 @@ void NodeList_Clear(NODE_LIST *list){
}
}
void NodeList_Destroy(NODE_LIST *list){
if(list == NULL){
void NodeList_Destroy(NODE_LIST *list)
{
if (list == NULL)
{
return;
}
NodeList_Clear(list);
@@ -97,21 +105,27 @@ void NodeList_Destroy(NODE_LIST *list){
* NULL if node dose not exist.
*
******************************************************************************/
NODE_ENTRY* GetNode(NODE_LIST *list, uint32_t nodeId) {
NODE_ENTRY *GetNode(NODE_LIST *list, uint32_t nodeId)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = list->head->nextNode;
while (nodeEntry != list->head) {
if (nodeEntry->nodeInfo.nodeId == nodeId) {
NODE_ENTRY *nodeEntry = list->head->nextNode;
while (nodeEntry != list->head)
{
if (nodeEntry->nodeInfo.nodeId == nodeId)
{
break;
}
else {
else
{
nodeEntry = nodeEntry->nextNode;
}
}
if (nodeEntry == list->head) {
if (nodeEntry == list->head)
{
return NULL;
}
else {
else
{
return nodeEntry;
}
}
@@ -127,7 +141,8 @@ NODE_ENTRY* GetNode(NODE_LIST *list, uint32_t nodeId) {
* false node not in the list
*
******************************************************************************/
inline bool IsNodeInList(NODE_LIST *list, uint8_t nodeId) {
inline bool IsNodeInList(NODE_LIST *list, uint8_t nodeId)
{
ASSERT(list != NULL);
return (GetNode(list, nodeId) != NULL);
}
@@ -147,17 +162,19 @@ inline bool IsNodeInList(NODE_LIST *list, uint8_t nodeId) {
int32_t AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType)
{
ASSERT(list != NULL);
if(IsNodeInList(list, nodeId)){
if (IsNodeInList(list, nodeId))
{
DBG_WARNING("Node [%d] already exist.\n", nodeId);
return FAILURE;
}
DBG_INFO("Link layer appending node [%d] to node list.\n", nodeId);
NODE_ENTRY* nodeEntry = (NODE_ENTRY*) malloc(sizeof(NODE_ENTRY));
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = (NODE_ENTRY *)malloc(sizeof(NODE_ENTRY));
if (nodeEntry == NULL)
{
return ENOMEM;
}
nodeEntry->nodeInfo.nodeId = nodeId;
nodeEntry->nodeInfo.nodeType = (uint8_t) nodeType;
nodeEntry->nodeInfo.nodeType = (uint8_t)nodeType;
nodeEntry->nodeInfo.pos.x = 0;
nodeEntry->nodeInfo.pos.y = 0;
nodeEntry->nodeInfo.pos.z = 0;
@@ -186,10 +203,12 @@ int32_t AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType)
* Other Node not exist.
*
******************************************************************************/
int32_t DeletNode(NODE_LIST *list, uint8_t nodeId) {
int32_t DeletNode(NODE_LIST *list, uint8_t nodeId)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if(nodeEntry == NULL){
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
DBG_WARNING("Node [%d] in not in node list, skip deleting process.\n", nodeId);
return SUCCESS;
}
@@ -201,7 +220,6 @@ int32_t DeletNode(NODE_LIST *list, uint8_t nodeId) {
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Get the distance between me and the node.
@@ -215,13 +233,16 @@ int32_t DeletNode(NODE_LIST *list, uint8_t nodeId) {
*
******************************************************************************/
int32_t GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double* dst) {
int32_t GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double *dst)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
*dst = Coord_Distance(myCoord, nodeEntry->nodeInfo.pos);
return SUCCESS;
}
@@ -239,19 +260,21 @@ int32_t GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double* dst)
* FAILURE node not exist.
*
******************************************************************************/
int32_t UpdateNodePos(NODE_LIST *list, uint8_t nodeId, const COORD *newPos) {
int32_t UpdateNodePos(NODE_LIST *list, uint8_t nodeId, const COORD *newPos)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
memcpy(&(nodeEntry->nodeInfo.pos), newPos, sizeof(COORD));
}
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Get node positions information.
@@ -264,13 +287,16 @@ int32_t UpdateNodePos(NODE_LIST *list, uint8_t nodeId, const COORD *newPos) {
* FAILURE node not exist.
*
******************************************************************************/
int32_t GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *pos) {
int32_t GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *pos)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
memcpy(pos, &(nodeEntry->nodeInfo.pos), sizeof(COORD));
}
return SUCCESS;
@@ -288,19 +314,21 @@ int32_t GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *pos) {
* FAILURE node not exist.
*
******************************************************************************/
int32_t UpdateNodeSpeed(NODE_LIST *list, uint8_t nodeId, const COORD *newSpeed) {
int32_t UpdateNodeSpeed(NODE_LIST *list, uint8_t nodeId, const COORD *newSpeed)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
memcpy(&(nodeEntry->nodeInfo.speed), newSpeed, sizeof(COORD));
}
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Get node speed information.
@@ -313,13 +341,16 @@ int32_t UpdateNodeSpeed(NODE_LIST *list, uint8_t nodeId, const COORD *newSpeed)
* FAILURE node not exist.
*
******************************************************************************/
int32_t GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed) {
int32_t GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
memcpy(speed, &(nodeEntry->nodeInfo.speed), sizeof(COORD));
}
return SUCCESS;
@@ -336,13 +367,16 @@ int32_t GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed) {
* FAILURE Node not exist.
*
******************************************************************************/
int32_t UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi) {
int32_t UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
nodeEntry->timeDevi = devi;
}
return SUCCESS;
@@ -359,13 +393,16 @@ int32_t UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi) {
* @return NONE
*
******************************************************************************/
int32_t IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc) {
int32_t IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
nodeEntry->ttlCnt += inc;
}
return SUCCESS;
@@ -382,13 +419,16 @@ int32_t IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc) {
* FAILURE Node not exist.
*
******************************************************************************/
int32_t RstNodeTTL(NODE_LIST *list, uint8_t nodeId) {
int32_t RstNodeTTL(NODE_LIST *list, uint8_t nodeId)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
nodeEntry->ttlCnt = 0;
}
return SUCCESS;
@@ -405,13 +445,16 @@ int32_t RstNodeTTL(NODE_LIST *list, uint8_t nodeId) {
* @return NONE
*
******************************************************************************/
int32_t GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl) {
int32_t GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = GetNode(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
*ttl = nodeEntry->ttlCnt;
}
return SUCCESS;
@@ -430,13 +473,16 @@ int32_t GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl) {
* Other Error occurred.
*
******************************************************************************/
int32_t IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)(NODE_ENTRY *, const void *)) {
int32_t IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)(NODE_ENTRY *, const void *))
{
ASSERT(list != NULL);
int32_t Status = SUCCESS;
NODE_ENTRY *nodeEntry = list->head->nextNode;
while (nodeEntry != list->head) {
while (nodeEntry != list->head)
{
Status = operation(nodeEntry, param);
if(Status != SUCCESS){
if (Status != SUCCESS)
{
break;
}
nodeEntry = nodeEntry->nextNode;
@@ -455,12 +501,15 @@ int32_t IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)(NO
* @return SUCCESS
*
******************************************************************************/
int32_t IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *)) {
int32_t IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *))
{
ASSERT(list != NULL);
NODE_ENTRY *nodeEntry, *deletEntry;
nodeEntry = list->head->nextNode;
while (nodeEntry != list->head) {
if (condition(nodeEntry) == false) {
while (nodeEntry != list->head)
{
if (condition(nodeEntry) == false)
{
deletEntry = nodeEntry;
nodeEntry = nodeEntry->nextNode;
deletEntry->preNode->nextNode = deletEntry->nextNode;
@@ -469,7 +518,8 @@ int32_t IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *)) {
free(deletEntry);
list->nodeNum--;
}
else {
else
{
nodeEntry = nodeEntry->nextNode;
}
}
df_datalink/src/networking/link_layer/node_list.h
+6 -4
View File
@@ -26,7 +26,8 @@
/************************** Constant Definitions *****************************/
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
typedef struct NODE_LIST_ENTRY {
typedef struct NODE_LIST_ENTRY
{
L2_NODE_INFO nodeInfo;
uint32_t ttlCnt;
int32_t timeDevi;
@@ -34,7 +35,8 @@ typedef struct NODE_LIST_ENTRY {
struct NODE_LIST_ENTRY *preNode;
} NODE_ENTRY;
typedef struct {
typedef struct
{
uint32_t nodeNum;
NODE_ENTRY *head;
} NODE_LIST;
@@ -53,12 +55,12 @@ int32_t GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *newPos);
int32_t UpdateNodeSpeed(NODE_LIST *list, uint8_t nodeId, const COORD *newSpeed);
int32_t GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed);
int32_t UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi);
int32_t GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double* dst);
int32_t GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double *dst);
int32_t GetNodeNum(NODE_LIST *list);
int32_t RstNodeTTL(NODE_LIST *list, uint8_t nodeId);
int32_t IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc);
int32_t GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl);
bool IsNodeInList(NODE_LIST *list, uint8_t nodeId);
int32_t IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)(NODE_ENTRY*, const void*));
int32_t IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)(NODE_ENTRY *, const void *));
int32_t IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *));
#endif /* __L2_LOCAL_RECORD_H_ */
df_datalink/src/networking/link_layer/routine_table.c
+843 -451
View File
File diff suppressed because it is too large Load Diff
df_datalink/src/networking/link_layer/scan.c
+108 -72
View File
@@ -45,45 +45,52 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type);
* SUCCESS
*
******************************************************************************/
static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type)
{
ASSERT(instPtr != NULL);
int32_t Status;
uint8_t *buff;
uint32_t packLen;
switch (type) {
case L2_INFO_FRAME:{
packLen = RSP_PACK_LEN;
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
buff = malloc(packLen);
if (buff == NULL) {
DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM;
}
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
memcpy(buff+L2_MSG_INFO_POSX, &(instPtr->sysInfo.posX), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSY, &(instPtr->sysInfo.posY), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSZ, &(instPtr->sysInfo.posZ), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDX, &(instPtr->sysInfo.vX), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDY, &(instPtr->sysInfo.vY), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDZ, &(instPtr->sysInfo.vZ), sizeof(float));
switch (type)
{
case L2_INFO_FRAME:
{
packLen = RSP_PACK_LEN;
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
buff = malloc(packLen);
if (buff == NULL)
{
DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM;
}
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
memcpy(buff + L2_MSG_INFO_POSX, &(instPtr->sysInfo.posX), sizeof(float));
memcpy(buff + L2_MSG_INFO_POSY, &(instPtr->sysInfo.posY), sizeof(float));
memcpy(buff + L2_MSG_INFO_POSZ, &(instPtr->sysInfo.posZ), sizeof(float));
memcpy(buff + L2_MSG_INFO_SPEEDX, &(instPtr->sysInfo.vX), sizeof(float));
memcpy(buff + L2_MSG_INFO_SPEEDY, &(instPtr->sysInfo.vY), sizeof(float));
memcpy(buff + L2_MSG_INFO_SPEEDZ, &(instPtr->sysInfo.vZ), sizeof(float));
Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
if (Status != SUCCESS) {
DBG_ERROR("Fail to append CRC, error code: %ld.\n", Status);
return Status;
}
if(CircQueuePush(instPtr->txQueue, buff) == false){
DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE;
}
break;
Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
if (Status != SUCCESS)
{
DBG_ERROR("Fail to append CRC, error code: %ld.\n", Status);
return Status;
}
default :{
DBG_ERROR("Unexpected frame type: %d.\n", type);
return EPARAM;
break;
if (CircQueuePush(instPtr->txQueue, buff) == false)
{
DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE;
}
break;
}
default:
{
DBG_ERROR("Unexpected frame type: %d.\n", type);
return EPARAM;
break;
}
}
return SUCCESS;
}
@@ -97,7 +104,8 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
* @return
*
******************************************************************************/
int32_t L2_Scan_Init(L2_INST *instPtr) {
int32_t L2_Scan_Init(L2_INST *instPtr)
{
ASSERT(instPtr != NULL);
instPtr->scanFsm.sacanCnt = 0;
instPtr->scanFsm.beamNo = 0;
@@ -113,25 +121,32 @@ int32_t L2_Scan_Init(L2_INST *instPtr) {
* @return
*
******************************************************************************/
int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) {
int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
{
ASSERT(instPtr != NULL);
ASSERT(msg != NULL);
int32_t Status;
switch (instPtr->frame.last) {
case L2_SCAN_FRAME: {
if (instPtr->config.myType == L2_HEAD_NODE) {
switch (instPtr->frame.last)
{
case L2_SCAN_FRAME:
{
if (instPtr->config.myType == L2_HEAD_NODE)
{
DBG_ERROR("Unexpected call from head node, Only None-Head Node can handle L2_SVC_FRAME.\n");
Status = EUNEXP;
break;
}
Status = AppendNode(instPtr->nodeInfoList, L2_HEAD_NODE_ID, L2_HEAD_NODE);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
}
break;
}
case L2_INFO_FRAME: {
if (instPtr->config.myType != L2_HEAD_NODE) {
case L2_INFO_FRAME:
{
if (instPtr->config.myType != L2_HEAD_NODE)
{
DBG_ERROR("Unexpected call from member node, Only Head Node can handle L2_SVC_FRAME.\n");
Status = EUNEXP;
break;
@@ -139,47 +154,55 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) {
COORD coordTemp;
float tmpVal;
Status = AppendNode(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, L2_MEMBER_NODE);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
break;
}
memcpy(&tmpVal, msg+L2_MSG_INFO_POSX, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_POSX, sizeof(float));
coordTemp.x = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_POSY, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_POSY, sizeof(float));
coordTemp.y = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_POSZ, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_POSZ, sizeof(float));
coordTemp.z = tmpVal;
Status = UpdateNodePos(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("Failed to update node %d position, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
}
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDX, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDX, sizeof(float));
coordTemp.x = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDY, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDY, sizeof(float));
coordTemp.y = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDZ, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDZ, sizeof(float));
coordTemp.z = tmpVal;
Status = UpdateNodeSpeed(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("Failed to update node %d speed, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
}
// Update Time deviation
int64_t devi, deviTrunc;
Status = L2_TimeSync_GetDeviatoion(instPtr, &devi);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("Failed to compute slot deviation.\n");
UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, 0);
}
else {
if(devi > INT32_MAX || devi < INT32_MIN) {
else
{
if (devi > INT32_MAX || devi < INT32_MIN)
{
DBG_ERROR("Illegal deviation value: %lld.\n", devi);
deviTrunc = 0;
}
else if(devi > L2_MSG_SVC_TMSYNC_MAX || devi < L2_MSG_SVC_TMSYNC_MIN) {
else if (devi > L2_MSG_SVC_TMSYNC_MAX || devi < L2_MSG_SVC_TMSYNC_MIN)
{
DBG_WARNING("Deviation value: %lld is out of adjustable range.\n", devi);
deviTrunc = (int32_t)devi;
}
else {
else
{
deviTrunc = (int32_t)devi;
}
UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc);
@@ -193,7 +216,8 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) {
}
// Net setup finished.
if(L2_GetMainState(instPtr) == INIT_STATE && Status == SUCCESS){
if (L2_GetMainState(instPtr) == INIT_STATE && Status == SUCCESS)
{
L2_EVENT l2Event;
L2_RTNE_CONFIG newRtneConfig;
@@ -204,7 +228,7 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) {
// Assert Routine Table Update...
newRtneConfig.rtneTable = normalTable;
newRtneConfig.rtnePrd = ARRAY_SIZE(normalTable)*L2_SLOTS_PER_FRAME;
newRtneConfig.rtnePrd = ARRAY_SIZE(normalTable) * L2_SLOTS_PER_FRAME;
newRtneConfig.rtneStep = 0;
l2Event.event = L2_ROUTINE_UPDATE;
l2Event.msg = &newRtneConfig;
@@ -222,43 +246,50 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) {
* @return used slot (5ms slot).
*
******************************************************************************/
int32_t L2_Scan_Proc(L2_INST *instPtr) {
int32_t L2_Scan_Proc(L2_INST *instPtr)
{
int32_t Status;
/* instPtr->nextState == NORMAL_STATE indicates network set up procedure was finished.
* Skip 5ms for nodes for node configuration update.
*/
DBG_INFO("-------- Scan Process --------\n");
if (instPtr->nextState == NORMAL_STATE) {
if (instPtr->nextState == NORMAL_STATE)
{
DBG_INFO("NETSET DONE, halt 20ms before normal routine.\n");
if(instPtr->config.myType != L2_HEAD_NODE) {
if (instPtr->config.myType != L2_HEAD_NODE)
{
Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_120);
Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_SCAN_FRAME);
}
else {
else
{
DBG_ERROR("Illegal call from Head Node.\n");
}
return NUM_SLOTS_SKIP;
}
/* Head node implement a periodical scan pattern
/* 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
* Increase beam number after a Tx-Rx procedure pair, reset beam number when
* it reaches the boundary.
*/
if (instPtr->config.myType == L2_HEAD_NODE) {
if (instPtr->config.myType == L2_HEAD_NODE)
{
// Apply a scan procedure
if (instPtr->scanFsm.sacanCnt % 2 == 0){
if (instPtr->scanFsm.sacanCnt % 2 == 0)
{
// Antenna configure
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetBeamNo(instPtr->config.phyInstRef, instPtr->scanFsm.beamNo);
Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(instPtr, L2_TOR_TX, L2_SCAN_FRAME);
}
else {
else
{
// Antenna configure
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetBeamNo(instPtr->config.phyInstRef, instPtr->scanFsm.beamNo);
@@ -270,23 +301,27 @@ int32_t L2_Scan_Proc(L2_INST *instPtr) {
}
/* 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
* 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 {
else
{
uint8_t *sendData;
// Pack node information.
Status = ScanMsg_Packing(instPtr, L2_INFO_FRAME);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_INFO("Failed to pack response.\n");
}
if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true) {
if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true)
{
memcpy(instPtr->sendPktData, sendData, RSP_PACK_LEN);
free(sendData);
}
else {
else
{
DBG_INFO("Failed to pop tx Queue.\n");
}
// Antenna configure
@@ -297,7 +332,7 @@ int32_t L2_Scan_Proc(L2_INST *instPtr) {
}
// Increase scan counter (for debug purpose).
instPtr->scanFsm.sacanCnt ++;
instPtr->scanFsm.sacanCnt++;
return NUM_SLOTS_SKIP;
}
@@ -311,7 +346,8 @@ int32_t L2_Scan_Proc(L2_INST *instPtr) {
* @return used slot (5ms slot).
*
******************************************************************************/
int32_t L2_Scan_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry) {
int32_t L2_Scan_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry)
{
DBG_INFO("-------- Scan Routine --------\n");
ASSERT(false); // Not support.
return 0;
df_datalink/src/networking/link_layer/service.c
+220 -160
View File
@@ -25,10 +25,10 @@
#include "../config/app_config.h"
/************************** Constant Definitions *****************************/
#define NUM_SERVICE_SLOTS 20
#define NUM_RESPONSE_SLOTS 20
#define NUM_NONE_SERVICE_SLOTS 0
#define NUM_SERVICE_PRD_SLOTS 60
#define NUM_SERVICE_SLOTS 20
#define NUM_RESPONSE_SLOTS 20
#define NUM_NONE_SERVICE_SLOTS 0
#define NUM_SERVICE_PRD_SLOTS 60
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
@@ -42,8 +42,7 @@ static uint8_t berTestData[SVC_PACK_LEN] = {
0x27, 0x4E, 0xFB, 0x39, 0x11, 0xE7, 0x12, 0xDA,
0xDF, 0xD2, 0x27, 0x8F, 0xC9, 0x42, 0xCD, 0x0C,
0x76, 0x1C, 0x81, 0x87, 0x69, 0xC9, 0x60, 0xFD,
0xC4, 0x27, 0xA5, 0x09, 0xE5, 0x71
};
0xC4, 0x27, 0xA5, 0x09, 0xE5, 0x71};
#endif
/*****************************************************************************/
@@ -58,116 +57,134 @@ static uint8_t berTestData[SVC_PACK_LEN] = {
* SUCCESS
*
******************************************************************************/
static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type)
{
#ifndef DATALINK_ENABLE_BERTEST
ASSERT(instPtr != NULL);
int32_t Status;
uint8_t *buff;
uint32_t packLen;
switch (type) {
case L2_SVC_FRAME:{
const NODE_ENTRY *nodeEntry;
int32_t timeDevi;
packLen = SVC_PACK_LEN;
switch (type)
{
case L2_SVC_FRAME:
{
const NODE_ENTRY *nodeEntry;
int32_t timeDevi;
packLen = SVC_PACK_LEN;
buff = malloc(packLen);
if (buff == NULL) {
DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM;
}
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
// TODO: Check Pack TimeSync info, and better approch?.
nodeEntry = GetNode(instPtr->nodeInfoList, L2_MEMBER_NODE_ID);
if(nodeEntry != NULL) {
// check if deviation info outdated.
if(nodeEntry->ttlCnt <= NUM_SERVICE_PRD_SLOTS) {
// Member node should adjust in the opposite way.
timeDevi = -nodeEntry->timeDevi;
}
else {
DBG_WARNING("Time deviation is outdated, omitted it.\n");
}
}
else {
DBG_ERROR("Fail to get time deviation...\n");
timeDevi = 0;
}
if(timeDevi > L2_MSG_SVC_TMSYNC_MAX || timeDevi < L2_MSG_SVC_TMSYNC_MIN) {
do {
timeDevi /= 2;
} while(timeDevi > L2_MSG_SVC_TMSYNC_MAX || timeDevi < L2_MSG_SVC_TMSYNC_MIN);
DBG_WARNING("Deviation is out of adjustable range, truncate it to %d.\n", timeDevi);
}
memcpy(buff+L2_MSG_SVC_TMSYNC, &timeDevi, L2_MSG_SVC_TMSYNC_LEN);
// Pack Seq ID
buff[L2_MSG_SVC_SEQ_ID] = instPtr->serviceFsm.txSeqNum;
// Pack payload
memcpy(buff+L2_MSG_SVC_PAYLOAD,
instPtr->serviceFsm.txLog + L2_MSG_SVC_PAYLOAD_LEN*instPtr->serviceFsm.txSeqNum,
L2_MSG_SVC_PAYLOAD_LEN);
if (instPtr->config.myType == L2_HEAD_NODE) {
// if last packet reset counting
instPtr->serviceFsm.txSeqNum = (instPtr->serviceFsm.txSeqNum
>= (PAYLOAD_FLJ-1)/L2_MSG_SVC_PAYLOAD+1)
? 0 : instPtr->serviceFsm.txSeqNum + 1;
}
else{
// if last packet reset counting
instPtr->serviceFsm.txSeqNum = (instPtr->serviceFsm.txSeqNum
>= (PAYLOAD_LJ-1)/L2_MSG_SVC_PAYLOAD+1)
? 0 : instPtr->serviceFsm.txSeqNum + 1;
}
Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
if (Status != SUCCESS) {
DBG_ERROR("Fail to append CRC...\n");
return Status;
}
if(CircQueuePush(instPtr->txQueue, buff) == false){
DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE;
}
break;
buff = malloc(packLen);
if (buff == NULL)
{
DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM;
}
case L2_INFO_FRAME:{
packLen = RSP_PACK_LEN;
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
buff = malloc(packLen);
if (buff == NULL) {
DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM;
}
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
memcpy(buff+L2_MSG_INFO_POSX, &(instPtr->sysInfo.posX), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSY, &(instPtr->sysInfo.posY), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSZ, &(instPtr->sysInfo.posZ), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDX, &(instPtr->sysInfo.vX), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDY, &(instPtr->sysInfo.vY), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDZ, &(instPtr->sysInfo.vZ), sizeof(float));
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
if (Status != SUCCESS) {
DBG_ERROR("Fail to append CRC...\n");
return Status;
// TODO: Check Pack TimeSync info, and better approch?.
nodeEntry = GetNode(instPtr->nodeInfoList, L2_MEMBER_NODE_ID);
if (nodeEntry != NULL)
{
// check if deviation info outdated.
if (nodeEntry->ttlCnt <= NUM_SERVICE_PRD_SLOTS)
{
// Member node should adjust in the opposite way.
timeDevi = -nodeEntry->timeDevi;
}
if(CircQueuePush(instPtr->txQueue, buff) == false){
DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE;
else
{
DBG_WARNING("Time deviation is outdated, omitted it.\n");
}
break;
}
default :{
DBG_ERROR("Unexpected frame type: %d.\n", type);
return EPARAM;
break;
else
{
DBG_ERROR("Fail to get time deviation...\n");
timeDevi = 0;
}
if (timeDevi > L2_MSG_SVC_TMSYNC_MAX || timeDevi < L2_MSG_SVC_TMSYNC_MIN)
{
do
{
timeDevi /= 2;
} while (timeDevi > L2_MSG_SVC_TMSYNC_MAX || timeDevi < L2_MSG_SVC_TMSYNC_MIN);
DBG_WARNING("Deviation is out of adjustable range, truncate it to %d.\n", timeDevi);
}
memcpy(buff + L2_MSG_SVC_TMSYNC, &timeDevi, L2_MSG_SVC_TMSYNC_LEN);
// Pack Seq ID
buff[L2_MSG_SVC_SEQ_ID] = instPtr->serviceFsm.txSeqNum;
// Pack payload
memcpy(buff + L2_MSG_SVC_PAYLOAD,
instPtr->serviceFsm.txLog + L2_MSG_SVC_PAYLOAD_LEN * instPtr->serviceFsm.txSeqNum,
L2_MSG_SVC_PAYLOAD_LEN);
if (instPtr->config.myType == L2_HEAD_NODE)
{
// if last packet reset counting
instPtr->serviceFsm.txSeqNum = (instPtr->serviceFsm.txSeqNum >= (PAYLOAD_FLJ - 1) / L2_MSG_SVC_PAYLOAD + 1)
? 0
: instPtr->serviceFsm.txSeqNum + 1;
}
else
{
// if last packet reset counting
instPtr->serviceFsm.txSeqNum = (instPtr->serviceFsm.txSeqNum >= (PAYLOAD_LJ - 1) / L2_MSG_SVC_PAYLOAD + 1)
? 0
: instPtr->serviceFsm.txSeqNum + 1;
}
Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
if (Status != SUCCESS)
{
DBG_ERROR("Fail to append CRC...\n");
return Status;
}
if (CircQueuePush(instPtr->txQueue, buff) == false)
{
DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE;
}
break;
}
case L2_INFO_FRAME:
{
packLen = RSP_PACK_LEN;
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
buff = malloc(packLen);
if (buff == NULL)
{
DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM;
}
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
memcpy(buff + L2_MSG_INFO_POSX, &(instPtr->sysInfo.posX), sizeof(float));
memcpy(buff + L2_MSG_INFO_POSY, &(instPtr->sysInfo.posY), sizeof(float));
memcpy(buff + L2_MSG_INFO_POSZ, &(instPtr->sysInfo.posZ), sizeof(float));
memcpy(buff + L2_MSG_INFO_SPEEDX, &(instPtr->sysInfo.vX), sizeof(float));
memcpy(buff + L2_MSG_INFO_SPEEDY, &(instPtr->sysInfo.vY), sizeof(float));
memcpy(buff + L2_MSG_INFO_SPEEDZ, &(instPtr->sysInfo.vZ), sizeof(float));
Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
if (Status != SUCCESS)
{
DBG_ERROR("Fail to append CRC...\n");
return Status;
}
if (CircQueuePush(instPtr->txQueue, buff) == false)
{
DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE;
}
break;
}
default:
{
DBG_ERROR("Unexpected frame type: %d.\n", type);
return EPARAM;
break;
}
}
return SUCCESS;
#else
@@ -176,7 +193,6 @@ static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
#endif
}
/*****************************************************************************/
/**
* @brief Initialize scan FSM.
@@ -186,7 +202,8 @@ static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
* @return
*
******************************************************************************/
int32_t L2_Service_Init(L2_INST *instPtr) {
int32_t L2_Service_Init(L2_INST *instPtr)
{
ASSERT(instPtr != NULL);
instPtr->serviceFsm.txSeqNum = 0;
instPtr->serviceFsm.rxSeqNum = 0;
@@ -211,13 +228,17 @@ int32_t L2_Service_Init(L2_INST *instPtr) {
* other error occurred.
*
******************************************************************************/
int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) {
int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg)
{
ASSERT(instPtr != NULL);
ASSERT(msg != NULL);
int32_t Status;
switch (instPtr->frame.last) {
case L2_SVC_FRAME: {
if (instPtr->config.myType == L2_HEAD_NODE) {
switch (instPtr->frame.last)
{
case L2_SVC_FRAME:
{
if (instPtr->config.myType == L2_HEAD_NODE)
{
DBG_ERROR("Unexpected call, Only None-Head Node can handle L2_SVC_FRAME.\n");
Status = EUNEXP;
break;
@@ -241,57 +262,72 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) {
timeAdj |= (timeAdj & 0x0000000000800000) ? 0xFFFFFFFFFF000000 : 0x0000000000000000;
// FIXME: better approach?
// Implement a time sync procedure, first time received a time deviation in a service period.
if (seqNum % NUM_SERVICE_PRD_SLOTS == 0 || instPtr->serviceFsm.seqNum != seqNum) {
if (seqNum % NUM_SERVICE_PRD_SLOTS == 0 || instPtr->serviceFsm.seqNum != seqNum)
{
AssertL2Event(instPtr, &l2Event);
}
#endif
#ifndef DATALINK_ENABLE_BERTEST
memcpy(instPtr->serviceFsm.rxLog + L2_MSG_SVC_PAYLOAD_LEN*seqNum, msg + L2_MSG_SVC_PAYLOAD, L2_MSG_SVC_PAYLOAD_LEN);
if (instPtr->config.myType == L2_HEAD_NODE) {
memcpy(instPtr->serviceFsm.rxLog + L2_MSG_SVC_PAYLOAD_LEN * seqNum, msg + L2_MSG_SVC_PAYLOAD, L2_MSG_SVC_PAYLOAD_LEN);
if (instPtr->config.myType == L2_HEAD_NODE)
{
// if last packet reset counting
if(seqNum >= (PAYLOAD_FLJ-1)/L2_MSG_SVC_PAYLOAD+1) {
if(seqNum == instPtr->serviceFsm.rxSeqNum) {
if (seqNum >= (PAYLOAD_FLJ - 1) / L2_MSG_SVC_PAYLOAD + 1)
{
if (seqNum == instPtr->serviceFsm.rxSeqNum)
{
SysMonitor_UpdateRecvPayloadLJRaw(instPtr->serviceFsm.rxLog);
}
instPtr->serviceFsm.rxSeqNum = 0;
}
else {
if(instPtr->serviceFsm.rxSeqNum == seqNum){
else
{
if (instPtr->serviceFsm.rxSeqNum == seqNum)
{
instPtr->serviceFsm.rxSeqNum++;
}
}
}
else{
else
{
// if last packet reset counting
if(seqNum >= (PAYLOAD_LJ-1)/L2_MSG_SVC_PAYLOAD+1) {
if(seqNum == instPtr->serviceFsm.rxSeqNum) {
if (seqNum >= (PAYLOAD_LJ - 1) / L2_MSG_SVC_PAYLOAD + 1)
{
if (seqNum == instPtr->serviceFsm.rxSeqNum)
{
SysMonitor_UpdateRecvPayloadFLJRaw(instPtr->serviceFsm.rxLog);
}
instPtr->serviceFsm.rxSeqNum = 0;
}
else {
if(instPtr->serviceFsm.rxSeqNum == seqNum){
else
{
if (instPtr->serviceFsm.rxSeqNum == seqNum)
{
instPtr->serviceFsm.rxSeqNum++;
}
}
}
#else
for(int i = 0; i < SVC_PACK_LEN; i++) {
instPtr->serviceFsm.bitError += count_ones(msg[i]^berTestData[i]);
for (int i = 0; i < SVC_PACK_LEN; i++)
{
instPtr->serviceFsm.bitError += count_ones(msg[i] ^ berTestData[i]);
}
instPtr->serviceFsm.bitTotal += SVC_PACK_LEN*8;
instPtr->serviceFsm.bitTotal += SVC_PACK_LEN * 8;
#endif
instPtr->serviceFsm.pktRecved++;
// Reset Node TTL
Status = RstNodeTTL(instPtr->nodeInfoList, L2_HEAD_NODE_ID);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("Failed to reset node %d TTL, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
}
break;
}
case L2_INFO_FRAME: {
if (instPtr->config.myType != L2_HEAD_NODE) {
case L2_INFO_FRAME:
{
if (instPtr->config.myType != L2_HEAD_NODE)
{
DBG_ERROR("Unexpected call, Only Head Node can handle L2_SVC_FRAME.\n");
Status = EUNEXP;
break;
@@ -299,24 +335,26 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) {
#ifndef DATALINK_ENABLE_BERTEST
COORD coordTemp;
float tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_POSX, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_POSX, sizeof(float));
coordTemp.x = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_POSY, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_POSY, sizeof(float));
coordTemp.y = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_POSZ, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_POSZ, sizeof(float));
coordTemp.z = tmpVal;
Status = UpdateNodePos(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("Failed to update node %d position, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
}
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDX, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDX, sizeof(float));
coordTemp.x = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDY, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDY, sizeof(float));
coordTemp.y = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDZ, sizeof(float));
memcpy(&tmpVal, msg + L2_MSG_INFO_SPEEDZ, sizeof(float));
coordTemp.z = tmpVal;
Status = UpdateNodeSpeed(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("Failed to update node %d speed, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
}
@@ -324,38 +362,46 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) {
// Update Time deviation
int64_t devi, deviTrunc;
Status = L2_TimeSync_GetDeviatoion(instPtr, &devi);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("Failed to compute slot deviation.\n");
deviTrunc = 0;
}
else {
if(devi > INT32_MAX || devi < INT32_MIN) {
else
{
if (devi > INT32_MAX || devi < INT32_MIN)
{
DBG_WARNING("Illegal deviation value: %lld, omit it.\n", devi);
deviTrunc = 0;
}
else if(devi > L2_MSG_SVC_TMSYNC_MAX || devi < L2_MSG_SVC_TMSYNC_MIN) {
else if (devi > L2_MSG_SVC_TMSYNC_MAX || devi < L2_MSG_SVC_TMSYNC_MIN)
{
DBG_WARNING("Deviation value: %lld is out of adjustable range.\n", devi);
deviTrunc = (int32_t)devi;
}
else {
else
{
deviTrunc = (int32_t)devi;
}
UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc);
}
Status = UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("Failed to update node %d time deviation, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
}
#else
for(int i = 0; i < SVC_PACK_LEN; i++) {
instPtr->serviceFsm.bitError += count_ones(msg[i]^berTestData[i]);
for (int i = 0; i < SVC_PACK_LEN; i++)
{
instPtr->serviceFsm.bitError += count_ones(msg[i] ^ berTestData[i]);
}
instPtr->serviceFsm.bitTotal += SVC_PACK_LEN*8;
instPtr->serviceFsm.bitTotal += SVC_PACK_LEN * 8;
#endif
instPtr->serviceFsm.pktRecved++;
// Reset Node TTL
Status = RstNodeTTL(instPtr->nodeInfoList, L2_MEMBER_NODE_ID);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("Failed to reset node %d ttl, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
}
break;
@@ -377,42 +423,53 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) {
* @return Slot has been used.
*
******************************************************************************/
int32_t L2_Service_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry) {
int32_t L2_Service_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry)
{
ASSERT(instPtr != NULL);
DBG_INFO("-------- ServiceNet Routine --------\n");
uint8_t *sendData;
int32_t usedSlots;
L2_TOR_TYPE torState;
if(schedEntry->src == instPtr->config.myId){
if (schedEntry->src == instPtr->config.myId)
{
torState = L2_TOR_TX;
}
else if(schedEntry->dst == instPtr->config.myId){
else if (schedEntry->dst == instPtr->config.myId)
{
torState = L2_TOR_RX;
instPtr->serviceFsm.pktRecv++;
}
else{
else
{
torState = L2_TOR_SUSPEND;
}
switch(schedEntry->slotType){
switch (schedEntry->slotType)
{
case L2_SLOT_SERVICE:
if(torState == L2_TOR_TX){
if (torState == L2_TOR_TX)
{
#ifndef DATALINK_ENABLE_BERTEST
if (instPtr->serviceFsm.txSeqNum == 0) {
if(instPtr->config.myType == L2_HEAD_NODE){
if (instPtr->serviceFsm.txSeqNum == 0)
{
if (instPtr->config.myType == L2_HEAD_NODE)
{
SysMonitor_GetSendPayloadFLJRaw(instPtr->serviceFsm.txLog);
}
else {
else
{
SysMonitor_GetSendPayloadLJRaw(instPtr->serviceFsm.txLog);
}
}
// Packing service data
Service_Packing(instPtr, L2_SVC_FRAME);
if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true) {
if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true)
{
memcpy(instPtr->sendPktData, sendData, SVC_PACK_LEN);
free(sendData);
}
else {
else
{
DBG_ERROR("Failed to pop tx queue.\n");
}
#else
@@ -428,14 +485,17 @@ int32_t L2_Service_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry) {
break;
#ifndef DATALINK_ENABLE_BERTEST
case L2_SLOT_ADVERTISE:
if(torState == L2_TOR_TX){
if (torState == L2_TOR_TX)
{
// Packing response data.
Service_Packing(instPtr, L2_INFO_FRAME);
if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true) {
if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true)
{
memcpy(instPtr->sendPktData, sendData, RSP_PACK_LEN);
free(sendData);
}
else {
else
{
DBG_ERROR("Failed to pop tx queue.\n");
}
}
df_datalink/src/networking/link_layer/time_sync.c
+27 -24
View File
@@ -1,18 +1,18 @@
/*****************************************************************************/
/**
* Description: Time sync implementation file.
*
*
* Date: 2021-12-04 16:32
* LastEditors: FanHao
* LastEditTime: 2022-07-10 20:43
* Revisions: Rev 0.0
* History:
* History:
* [2022-07-10]: New branch for Dijian.
* [2021-12-04]: Initial version.
*
*
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Networking
*
*
******************************************************************************/
/***************************** Include Files *********************************/
@@ -23,25 +23,25 @@
#include "link_layer.h"
#include "../phy_layer/phy_reg.h"
/************************** Constant Definitions *****************************/
#define MAX_FREQ_EST_NUM 128 // 32bit word
#define SVC_FREQ_EST_NUM 512 // byte
#define MGMT_FREQ_EST_NUM 512 // byte
#define MAX_FREQ_EST_NUM 128 // 32bit word
#define SVC_FREQ_EST_NUM 512 // byte
#define MGMT_FREQ_EST_NUM 512 // byte
#define BISAR_FREQ_EST_NUM 480 // byte
#define SYS_CLK_FREQ 192000000.0 // Hz
#define SIG_CLK_FREQ 245760000.0 // Hz
#define CXR_FREQ 3800000000.0 // Hz
#define SYS_CLK_FREQ 192000000.0 // Hz
#define SIG_CLK_FREQ 245760000.0 // Hz
#define CXR_FREQ 3800000000.0 // Hz
#define NORMAL_SLOT_CLK_NUM 960000.0 // 9600000= 5e-3*192e6
#define COUNTER_10NS_MAX 500000 // how many 10ns in a 5ms
#define COUNTER_10NS_MAX 500000 // how many 10ns in a 5ms
#define LIGHT_SPPED_AIR 299702547.0 // m/s
#define LIGHT_SPPED_VAC 299792458.0 // m/s
//#define PHY_SYNCT2FEND_200K 904052UL
//#define PHY_SYNCT2FEND_200K 924232UL
// #define PHY_SYNCT2FEND_200K 904052UL
// #define PHY_SYNCT2FEND_200K 924232UL
#define PHY_SYNCT2FEND_2K_20MS 276336UL // 1.919ms
#define PHY_SYNCT2FEND_2K_20MS 276336UL // 1.919ms
#define PHY_SYNCT2FEND_2K_100MS 11796336UL // 81.919ms
#define PHY_LOCAL_CNT_MAX 0x0000FFFFFFFFFFFFULL
@@ -60,7 +60,8 @@
* @return
*
******************************************************************************/
inline int32_t L2_TimeSync_LocalTimerAdd(L2_INST *instPtr, int64_t adjustment) {
inline int32_t L2_TimeSync_LocalTimerAdd(L2_INST *instPtr, int64_t adjustment)
{
// read nextSendTime from physical layer
Phy_SetTimeWord(instPtr->config.phyInstRef, adjustment, PHY_TIME_WORD_LOCAL_ADJ);
PhyWrite(instPtr->config.phyInstRef, TM_ADJ_MODE, TM_ADJ_MODE_ADD);
@@ -76,11 +77,13 @@ inline int32_t L2_TimeSync_LocalTimerAdd(L2_INST *instPtr, int64_t adjustment) {
* @return
*
******************************************************************************/
int32_t L2_TimeSync_SlotAlign(L2_INST *instPtr) {
int32_t L2_TimeSync_SlotAlign(L2_INST *instPtr)
{
int32_t Status;
int64_t slotAdj;
int64_t slotAdj;
Status = L2_TimeSync_GetDeviatoion(instPtr, &slotAdj);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("Failed to compute slot deviation.\n");
return Status;
}
@@ -97,17 +100,19 @@ int32_t L2_TimeSync_SlotAlign(L2_INST *instPtr) {
* @return
*
******************************************************************************/
int32_t L2_TimeSync_GetDeviatoion(L2_INST *instPtr, int64_t *devi) {
int32_t L2_TimeSync_GetDeviatoion(L2_INST *instPtr, int64_t *devi)
{
// read nextSendTime from physical layer
int64_t currentTarget, lastFrameSync, slotMeasure;
int64_t currentTarget, lastFrameSync, slotMeasure;
currentTarget = PhyRead(instPtr->config.phyInstRef, TM_TARGET_L);
currentTarget |= ((int64_t)PhyRead(instPtr->config.phyInstRef, TM_TARGET_H)) << 32;
lastFrameSync = PhyRead(instPtr->config.phyInstRef, FSTIME_L);
lastFrameSync |= ((int64_t)PhyRead(instPtr->config.phyInstRef, FSTIME_H)) << 32;
slotMeasure = (currentTarget > lastFrameSync) ? (currentTarget - lastFrameSync)
: (currentTarget - lastFrameSync + PHY_LOCAL_CNT_MAX);
switch (Phy_GetLastDetectedFrameType(instPtr->config.phyInstRef)) {
case L1_2KBPS_20MS_FRAME :
switch (Phy_GetLastDetectedFrameType(instPtr->config.phyInstRef))
{
case L1_2KBPS_20MS_FRAME:
*devi = ((int64_t)PHY_SYNCT2FEND_2K_20MS) - slotMeasure;
break;
case L1_2KBPS_100MS_FRAME:
@@ -119,5 +124,3 @@ int32_t L2_TimeSync_GetDeviatoion(L2_INST *instPtr, int64_t *devi) {
}
return SUCCESS;
}