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

formatted code.

Browse Source
This commit is contained in:
Gatsby Van
2023-12-26 22:04:40 +08:00
parent 5cb5ffff59
commit 83e43f791c
42 changed files with 4688 additions and 3313 deletions
Download Patch File Download Diff File Expand all files Collapse all files
datalink814_ge/src/linklayer/net_mgmt.c
+254 -191
View File
@@ -1,18 +1,18 @@
/*****************************************************************************/
/**
* Description: Implementations of 814 linklayer management net.
*
*
* Date: 2021-11-18 17:27
* LastEditors: Fan Hao
* LastEditTime: 2022-01-16 15:52
* Revisions: Rev 0.1
* History:
* History:
* [2022-07-16]: New branch for DiJian.
* [2021-11-18]: Initial version.
*
*
* Copyright(c) 2022 Beijing Institute of Technology.
* Lab of Communication and Networking
*
*
******************************************************************************/
/***************************** Include Files *********************************/
@@ -25,19 +25,20 @@
#include "../common.h"
#include "node_list.h"
/************************** Constant Definitions *****************************/
#define SQUENCE_MGMT_SLOTS 0
#define INTER_MGMT_SLOTS 2
#define INTER_TSYNC_SLOTS 2
#define INTRA_MGMT_SLOTS 1
#define INTRA_TSYNC_SLOTS 1
#define NONE_MGMT_SLOTS -1
#define CONX_LOST_TH 10 // 10 count of mgmt msg no response to delete
#define TIME2QUIT 80 // time to send quit message
#define SQUENCE_MGMT_SLOTS 0
#define INTER_MGMT_SLOTS 2
#define INTER_TSYNC_SLOTS 2
#define INTRA_MGMT_SLOTS 1
#define INTRA_TSYNC_SLOTS 1
#define NONE_MGMT_SLOTS -1
#define CONX_LOST_TH 10 // 10 count of mgmt msg no response to delete
#define TIME2QUIT 80 // time to send quit message
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
typedef enum {
typedef enum
{
MGMTPKT_NONE = 0,
MGMTPKT_INTER_MGMT = 1,
MGMTPKT_INTRA_MGMT = 2,
@@ -49,8 +50,8 @@ typedef enum {
/************************** Variable Definitions *****************************/
/************************** Function Prototypes ******************************/
static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE msgType);
static int32_t MgmtPkt_Packing(L2_INST* instPtr, MGMTPKT_TYPE pktType);
static int32_t MgmtMsg_Packing(L2_INST *instPtr, BIT_ARRAY *bitarr, L2_MSG_TYPE msgType);
static int32_t MgmtPkt_Packing(L2_INST *instPtr, MGMTPKT_TYPE pktType);
/*****************************************************************************/
/**
* @brief Packing management message to a bit array.
@@ -63,25 +64,32 @@ static int32_t MgmtPkt_Packing(L2_INST* instPtr, MGMTPKT_TYPE pktType);
* SUCCESS
*
******************************************************************************/
static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE msgType) {
static int32_t MgmtMsg_Packing(L2_INST *instPtr, BIT_ARRAY *bitarr, L2_MSG_TYPE msgType)
{
ASSERT(instPtr != NULL);
ASSERT(bitarr != NULL);
uint64_t dataChunk;
L2_RTT_FSM* rttFsmPtr = L2_GetRttFsm(instPtr);
if (msgType < L2_MSG_TK_INIT || msgType > L2_MSG_BCU_INIT) {
L2_RTT_FSM *rttFsmPtr = L2_GetRttFsm(instPtr);
if (msgType < L2_MSG_TK_INIT || msgType > L2_MSG_BCU_INIT)
{
return EPARAM;
}
if(msgType >= L2_MSG_TK_INIT && msgType <= L2_MSG_MADJ_EXTD) {
if (bitarr->num_of_bits < MGMT_MSG_LEN) {
if (msgType >= L2_MSG_TK_INIT && msgType <= L2_MSG_MADJ_EXTD)
{
if (bitarr->num_of_bits < MGMT_MSG_LEN)
{
return EPARAM;
}
}
if(msgType >= L2_MSG_RTT_REQ_INIT && msgType <= L2_MSG_RTT_RSP_CONT) {
if (bitarr->num_of_bits < RTT_MSG_LEN) {
if (msgType >= L2_MSG_RTT_REQ_INIT && msgType <= L2_MSG_RTT_RSP_CONT)
{
if (bitarr->num_of_bits < RTT_MSG_LEN)
{
return EPARAM;
}
}
switch (msgType) {
switch (msgType)
{
case L2_MSG_TK_INIT:
// Series tag
dataChunk = 0;
@@ -104,8 +112,10 @@ static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
FrameInsertChunk(bitarr, TK_MSG_INI_OFFSET, TK_MSG_INI_LEN, dataChunk);
// Next net stage
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD) {
switch (instPtr->nextState) {
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
{
switch (instPtr->nextState)
{
case L2_INIT_STATE:
dataChunk = NET_INI_STAGE;
break;
@@ -159,7 +169,7 @@ static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
// Upper speed
dataChunk = 0xDDDD;
FrameInsertChunk(bitarr, TK_MSG_CONT_1_UPPER_SPEED_OFFSET,
TK_MSG_CONT_1_UPPER_SPEED_LEN, dataChunk);
TK_MSG_CONT_1_UPPER_SPEED_LEN, dataChunk);
// Height
dataChunk = 0xEEEEEE;
@@ -226,7 +236,7 @@ static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
FrameInsertChunk(bitarr, TA_MSG_MSG_STAG_OFFSET, TA_MSG_MSG_STAG_LEN, dataChunk);
// Source node id
dataChunk = (L2_GetNodeId(instPtr) & 0x1F) | (((uint16_t) (L2_GetClusterId(instPtr) & 0x1F)) << 5);
dataChunk = (L2_GetNodeId(instPtr) & 0x1F) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
FrameInsertChunk(bitarr, TA_MSG_SRC_ADDR_OFFSET, TA_MSG_SRC_ADDR_LEN, dataChunk);
// Height
@@ -299,7 +309,7 @@ static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
FrameInsertChunk(bitarr, RTT_REQ_SRC_ADDR_OFFSET, RTT_REQ_SRC_ADDR_LEN, dataChunk);
// destination node address.
dataChunk = (0x02) | (((uint16_t) (L2_GetClusterId(instPtr) & 0x1F)) << 5);
dataChunk = (0x02) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
FrameInsertChunk(bitarr, RTT_REQ_DST_ADDR_OFFSET, RTT_REQ_DST_ADDR_LEN, dataChunk);
break;
@@ -335,7 +345,7 @@ static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
// message identification.
dataChunk = 29;
FrameInsertChunk(bitarr, RTT_RSP_MSG_TAG_OFFSET,
RTT_RSP_MSG_TAG_LEN, dataChunk);
RTT_RSP_MSG_TAG_LEN, dataChunk);
// message identification.
dataChunk = 2; // response
@@ -350,7 +360,7 @@ static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
FrameInsertChunk(bitarr, RTT_RSP_SRC_ADDR_OFFSET, RTT_RSP_SRC_ADDR_LEN, dataChunk);
// destination node address.
dataChunk = (0x01) | (((uint16_t) (L2_GetClusterId(instPtr) & 0x1F)) << 5);
dataChunk = (0x01) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
FrameInsertChunk(bitarr, RTT_RSP_DST_ADDR_OFFSET, RTT_RSP_DST_ADDR_LEN, dataChunk);
break;
@@ -404,13 +414,13 @@ static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
// dataChunk = (L2_GetNodeId(instPtr) & 0x1F) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
// FrameInsertChunk(bitarr, NQT_MSG_OBJ_OFFSET, NQT_MSG_OBJ_LEN, dataChunk);
// L2_NODE_STATE newState = L2_INIT_STATE;
// L2_EVENT stateEvent = {
// .event = L2_EV_MAIN_STATE_UPDATE,
// .msg = &newState
// L2_EVENT stateEvent = {
// .event = L2_EV_MAIN_STATE_UPDATE,
// .msg = &newState
// };
// AssertL2Event(instPtr, &stateEvent);
// L2_EVENT lostCxnEvent = {
// .event = L2_EV_LOST_CXN,
// L2_EVENT lostCxnEvent = {
// .event = L2_EV_LOST_CXN,
// .msg = NULL
// };
// AssertL2Event(instPtr, &lostCxnEvent);
@@ -430,7 +440,6 @@ static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Packing management message to a bit array.
@@ -442,64 +451,79 @@ static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
* error code
*
******************************************************************************/
static int32_t MgmtPkt_Packing(L2_INST* instPtr, MGMTPKT_TYPE pktType) {
static int32_t MgmtPkt_Packing(L2_INST *instPtr, MGMTPKT_TYPE pktType)
{
ASSERT(instPtr != NULL);
uint8_t* buff = instPtr->sendPktData;
uint8_t *buff = instPtr->sendPktData;
uint32_t filedSize = 0;
BIT_ARRAY* mgmtPkt = bit_array_create(L2_MSG_MAX);
if (mgmtPkt == NULL) {
BIT_ARRAY *mgmtPkt = bit_array_create(L2_MSG_MAX);
if (mgmtPkt == NULL)
{
DBG_ERROR("Failed to allocate memory for scanMsg.\n");
return FAILURE;
}
switch (pktType) {
switch (pktType)
{
case MGMTPKT_INTRA_MGMT:
for (int i = 0; i < ARRAY_SIZE(intraMgmtFrameMsgs); i++) {
if (MgmtMsg_Packing(instPtr, mgmtPkt, intraMgmtFrameMsgs[i]) == SUCCESS) {
memcpy((uint8_t*)buff + (size_t) (i * (MGMT_MSG_LEN / 8)),
mgmtPkt->words, (size_t) (MGMT_MSG_LEN / 8));
for (int i = 0; i < ARRAY_SIZE(intraMgmtFrameMsgs); i++)
{
if (MgmtMsg_Packing(instPtr, mgmtPkt, intraMgmtFrameMsgs[i]) == SUCCESS)
{
memcpy((uint8_t *)buff + (size_t)(i * (MGMT_MSG_LEN / 8)),
mgmtPkt->words, (size_t)(MGMT_MSG_LEN / 8));
}
else {
else
{
DBG_ERROR("Failed to packing intraMgmtFrameMsgs[%d].\n", i);
goto failed;
}
filedSize += MGMT_MSG_LEN;
if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX) {
if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX)
{
DBG_ERROR("Message is too long to be properly packed.\n");
goto failed;
}
}
break;
case MGMTPKT_INTRA_RTTREQ:
for(int i = 0; i < ARRAY_SIZE(rttReqFrameMsgs); i++) {
if (MgmtMsg_Packing(instPtr, mgmtPkt, rttReqFrameMsgs[i]) == SUCCESS) {
for (int i = 0; i < ARRAY_SIZE(rttReqFrameMsgs); i++)
{
if (MgmtMsg_Packing(instPtr, mgmtPkt, rttReqFrameMsgs[i]) == SUCCESS)
{
memcpy(buff + (size_t)(i * (MGMT_MSG_LEN / 8)),
mgmtPkt->words,
(size_t)(MGMT_MSG_LEN / 8));
mgmtPkt->words,
(size_t)(MGMT_MSG_LEN / 8));
}
else {
else
{
DBG_ERROR("Failed to packing rttReqFrameMsgs[%d].\n", i);
goto failed;
}
filedSize += MGMT_MSG_LEN;
if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX) {
if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX)
{
DBG_ERROR("Message is too long to be properly packed.\n");
goto failed;
}
}
break;
case MGMTPKT_INTRA_RTTRSP:
for (int i = 0; i < ARRAY_SIZE(rttRspFrameMsgs); i++) {
if (MgmtMsg_Packing(instPtr, mgmtPkt, rttRspFrameMsgs[i]) == SUCCESS) {
memcpy(buff + (size_t) (i * (MGMT_MSG_LEN / 8)),
mgmtPkt->words,
(size_t) (MGMT_MSG_LEN / 8));
for (int i = 0; i < ARRAY_SIZE(rttRspFrameMsgs); i++)
{
if (MgmtMsg_Packing(instPtr, mgmtPkt, rttRspFrameMsgs[i]) == SUCCESS)
{
memcpy(buff + (size_t)(i * (MGMT_MSG_LEN / 8)),
mgmtPkt->words,
(size_t)(MGMT_MSG_LEN / 8));
}
else {
else
{
DBG_ERROR("Failed to packing rttRspFrameMsgs[%d].\n", i);
goto failed;
}
filedSize += MGMT_MSG_LEN;
if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX) {
if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX)
{
DBG_ERROR("Message is too long to be properly packed.\n");
goto failed;
}
@@ -519,16 +543,18 @@ failed:
/*****************************************************************************/
/**
* @brief initialize the management FSM instance.
*
*
* @param mgmtFSM A second rank pointer to management FSM instance.
*
*
* @return SUCCESS Initialize successfully
* EALLOC Memory allocation error.
*
*
******************************************************************************/
int32_t MgmtNet_Init(L2_MGMT_FSM** mgmtFSM) {
(*mgmtFSM) = (L2_MGMT_FSM*) malloc(sizeof(L2_MGMT_FSM));
if (*mgmtFSM == NULL) {
int32_t MgmtNet_Init(L2_MGMT_FSM **mgmtFSM)
{
(*mgmtFSM) = (L2_MGMT_FSM *)malloc(sizeof(L2_MGMT_FSM));
if (*mgmtFSM == NULL)
{
return EALLOC;
}
MgmtNet_Reset(*mgmtFSM);
@@ -544,7 +570,8 @@ int32_t MgmtNet_Init(L2_MGMT_FSM** mgmtFSM) {
* @return NONE
*
******************************************************************************/
void MgmtNet_Reset(L2_MGMT_FSM* mgmtFSM) {
void MgmtNet_Reset(L2_MGMT_FSM *mgmtFSM)
{
mgmtFSM->isBisarProc = false;
mgmtFSM->isPendSlotAlloc = false;
mgmtFSM->isPendTimeAlloc = false;
@@ -553,25 +580,24 @@ void MgmtNet_Reset(L2_MGMT_FSM* mgmtFSM) {
mgmtFSM->mgmtMsgLog.isValid = false;
mgmtFSM->mgmtMsgLog.quitCtrlWord = 0;
mgmtFSM->mgmtMsgLog.quitNodeId = 0;
mgmtFSM->mgmtMsgLog.msgSubTag = 0 ;
mgmtFSM->mgmtMsgLog.msgSubTag = 0;
mgmtFSM->mgmtMsgLog.msgTag = 0;
mgmtFSM->mgmtMsgLog.wordTag = 0;
mgmtFSM->mgmtMsgLog.srcNodeId = 0;
// Position value
mgmtFSM->mgmtMsgLog.EastSpeed = 0 ;
mgmtFSM->mgmtMsgLog.NorthSpeed = 0 ;
mgmtFSM->mgmtMsgLog.UpperSpeed = 0 ;
mgmtFSM->mgmtMsgLog.Height = 0 ;
mgmtFSM->mgmtMsgLog.Longitude = 0 ;
mgmtFSM->mgmtMsgLog.Latitude = 0 ;
mgmtFSM->mgmtMsgLog.EastSpeed = 0;
mgmtFSM->mgmtMsgLog.NorthSpeed = 0;
mgmtFSM->mgmtMsgLog.UpperSpeed = 0;
mgmtFSM->mgmtMsgLog.Height = 0;
mgmtFSM->mgmtMsgLog.Longitude = 0;
mgmtFSM->mgmtMsgLog.Latitude = 0;
// clear buffer.
memset(&(mgmtFSM->rttMsgRspLog), 0x00, sizeof(L2_RTT_MSG_RECORD));
memset(&(mgmtFSM->rttMsgReqLog), 0x00, sizeof(L2_RTT_MSG_RECORD));
}
/*****************************************************************************/
/**
* @brief Destroy management net FSM, free all allocated memory.
@@ -581,7 +607,8 @@ void MgmtNet_Reset(L2_MGMT_FSM* mgmtFSM) {
* @return NONE
*
******************************************************************************/
void MgmtNet_Destroy(L2_MGMT_FSM* mgmtFSM){
void MgmtNet_Destroy(L2_MGMT_FSM *mgmtFSM)
{
free(mgmtFSM);
}
@@ -589,14 +616,15 @@ void MgmtNet_Destroy(L2_MGMT_FSM* mgmtFSM){
/**
* @brief Handles management net packet, apply time sync and other management
* net functions.
*
*
* @param instPtr a pointer to l2 instance.
*
*
* @return SUCCESS Packet was successfully handled.
* Other Packed was dumped(not addressed to me, unrecognized frame type)
*
*
******************************************************************************/
int32_t MgmtNet_PktHandler(L2_INST* instPtr) {
int32_t MgmtNet_PktHandler(L2_INST *instPtr)
{
ASSERT(instPtr != NULL);
int32_t Status;
uint32_t filedSize = 0;
@@ -607,85 +635,93 @@ int32_t MgmtNet_PktHandler(L2_INST* instPtr) {
DBG_INFO("Management net handling packet...\n");
BIT_ARRAY* mgmtMsg = bit_array_create(L2_MSG_MAX);
if (mgmtMsg == NULL) {
BIT_ARRAY *mgmtMsg = bit_array_create(L2_MSG_MAX);
if (mgmtMsg == NULL)
{
DBG_INFO("Failed to start handling process...\n");
return EALLOC;
}
switch (L2_GetLastFrameType(instPtr)) {
switch (L2_GetLastFrameType(instPtr))
{
case L2_INTER_MGMT_FRAME:
case L2_INTRA_MGMT_FRAME: {
for (int i = 0; i < ARRAY_SIZE(intraMgmtFrameMsgs); i++) {
case L2_INTRA_MGMT_FRAME:
{
for (int i = 0; i < ARRAY_SIZE(intraMgmtFrameMsgs); i++)
{
memcpy(mgmtMsg->words,
instPtr->recvPktData + (size_t) (i * (MGMT_MSG_LEN / 8)),
(size_t) (MGMT_MSG_LEN / 8));
switch (intraMgmtFrameMsgs[i]) {
instPtr->recvPktData + (size_t)(i * (MGMT_MSG_LEN / 8)),
(size_t)(MGMT_MSG_LEN / 8));
switch (intraMgmtFrameMsgs[i])
{
case L2_MSG_TK_INIT:
FrameGetChunk(mgmtMsg, TK_MSG_INI_NET_STATE_OFFSET, TK_MSG_INI_NET_STATE_LEN, &dataChunk);
mgmtMsgLog->nextNetState = (uint8_t) (dataChunk);
mgmtMsgLog->nextNetState = (uint8_t)(dataChunk);
FrameGetChunk(mgmtMsg, TK_MSG_INI_WTAG_OFFSET, TK_MSG_INI_WTAG_LEN, &dataChunk);
mgmtMsgLog->wordTag = (uint8_t) (dataChunk);
mgmtMsgLog->wordTag = (uint8_t)(dataChunk);
FrameGetChunk(mgmtMsg, TK_MSG_INI_TAG_OFFSET, TK_MSG_INI_TAG_LEN, &dataChunk);
mgmtMsgLog->msgTag = (uint8_t) (dataChunk);
mgmtMsgLog->msgTag = (uint8_t)(dataChunk);
FrameGetChunk(mgmtMsg, TK_MSG_INI_STAG_OFFSET, TK_MSG_INI_STAG_LEN, &dataChunk);
mgmtMsgLog->msgSubTag = (uint8_t) (dataChunk);
mgmtMsgLog->msgSubTag = (uint8_t)(dataChunk);
// Preventing the error response caused by unexpected bit error.
if ( mgmtMsgLog->wordTag == 3
&& mgmtMsgLog->msgTag == 28
&& mgmtMsgLog->msgSubTag == 1 ) {
if (mgmtMsgLog->wordTag == 3 && mgmtMsgLog->msgTag == 28 && mgmtMsgLog->msgSubTag == 1)
{
mgmtMsgLog->isValid = true;
}
else {
mgmtMsgLog->isValid = false ;
else
{
mgmtMsgLog->isValid = false;
}
break;
case L2_MSG_TK_EXTD :
case L2_MSG_TK_EXTD:
FrameGetChunk(mgmtMsg, TK_MSG_EXTEND_EAST_SPEED_OFFSET, TK_MSG_EXTEND_EAST_SPEED_LEN, &dataChunk);
mgmtMsgLog->EastSpeed = (uint16_t) (dataChunk);
mgmtMsgLog->EastSpeed = (uint16_t)(dataChunk);
break;
case L2_MSG_TK_CONT_1:
FrameGetChunk(mgmtMsg, TK_MSG_EXTEND_EAST_SPEED_OFFSET, TK_MSG_EXTEND_EAST_SPEED_LEN, &dataChunk);
mgmtMsgLog->EastSpeed = (uint16_t) (dataChunk);
mgmtMsgLog->EastSpeed = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, TK_MSG_CONT_1_NORTH_SPEED_OFFSET, TK_MSG_CONT_1_NORTH_SPEED_LEN, &dataChunk);
mgmtMsgLog->NorthSpeed = (uint16_t) (dataChunk);
mgmtMsgLog->NorthSpeed = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, TK_MSG_CONT_1_UPPER_SPEED_OFFSET, TK_MSG_CONT_1_UPPER_SPEED_LEN, &dataChunk);
mgmtMsgLog->UpperSpeed = (uint16_t) (dataChunk);
mgmtMsgLog->UpperSpeed = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, TK_MSG_CONT_1_HEIGHT_OFFSET, TK_MSG_CONT_1_HEIGHT_LEN, &dataChunk);
mgmtMsgLog->Height = (uint32_t) (dataChunk);
mgmtMsgLog->Height = (uint32_t)(dataChunk);
break;
case L2_MSG_TK_CONT_2:
FrameGetChunk(mgmtMsg, TK_MSG_CONT_2_LONGITUDE_OFFSET, TK_MSG_CONT_2_LONGITUDE_LEN, &dataChunk);
mgmtMsgLog->Longitude = (uint32_t) (dataChunk);
mgmtMsgLog->Longitude = (uint32_t)(dataChunk);
FrameGetChunk(mgmtMsg, TK_MSG_CONT_2_LATITUDE_OFFSET, TK_MSG_CONT_2_LATITUDE_LEN, &dataChunk);
mgmtMsgLog->Latitude = (uint32_t) (dataChunk);
mgmtMsgLog->Latitude = (uint32_t)(dataChunk);
break;
case L2_MSG_TK_CONT_3:
FrameGetChunk(mgmtMsg, TK_MSG_CONT_3_MEMBER_ADDR_OFFSET, TK_MSG_CONT_3_MEMBER_ADDR_LEN, &dataChunk);
mgmtMsgLog->srcNodeId = (uint8_t) (dataChunk & 0x1F);
mgmtMsgLog->srcNodeId = (uint8_t)(dataChunk & 0x1F);
break;
case L2_MSG_NQT_INIT:
FrameGetChunk(mgmtMsg, NQT_MSG_OBJ_OFFSET, NQT_MSG_OBJ_LEN, &dataChunk);
mgmtMsgLog->quitNodeId = (uint8_t) (dataChunk & 0x1F);
if (mgmtMsgLog->quitNodeId != L2_GetNodeId(instPtr)) {
mgmtMsgLog->quitNodeId = (uint8_t)(dataChunk & 0x1F);
if (mgmtMsgLog->quitNodeId != L2_GetNodeId(instPtr))
{
FrameGetChunk(mgmtMsg, NQT_MSG_CTRL_MUTE_OFFSET, NQT_MSG_CTRL_MUTE_LEN, &dataChunk);
mgmtMsgLog->quitCtrlWord = (uint8_t)(dataChunk & 0x7F);
if (mgmtMsgLog->quitCtrlWord == 0x01) {
if (mgmtMsgLog->quitCtrlWord == 0x01)
{
L2_DeleteNodeById(instPtr->nodeList, mgmtMsgLog->quitNodeId);
}
}
else {
else
{
DBG_WARNING("Ignore node quit request(ID not mine), target node id is [%d]\n", mgmtMsgLog->quitNodeId);
}
break;
@@ -695,16 +731,19 @@ int32_t MgmtNet_PktHandler(L2_INST* instPtr) {
goto failed;
}
filedSize += MGMT_MSG_LEN;
if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX) {
if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX)
{
DBG_ERROR("Message decapsulate exceeds frame length.\n");
Status = EPARAM;
goto failed;
}
}
// Member Node update state with received message
if(L2_GetNodeId(instPtr) != L2_NODE_HEAD && mgmtMsgLog->isValid == true ){
if (L2_GetNodeId(instPtr) != L2_NODE_HEAD && mgmtMsgLog->isValid == true)
{
L2_NODE_STATE newState = L2_NONE_STATE;
switch (mgmtMsgLog->nextNetState) {
switch (mgmtMsgLog->nextNetState)
{
case NET_INI_STAGE:
newState = L2_INIT_STATE;
break;
@@ -727,57 +766,61 @@ int32_t MgmtNet_PktHandler(L2_INST* instPtr) {
}
L2_EVENT stateUpEvent = {
.event = L2_EV_MAIN_STATE_UPDATE,
.msg = &newState
};
.msg = &newState};
AssertL2Event(instPtr, &stateUpEvent);
}
L2_RstNodeTTL(instPtr->nodeList, mgmtMsgLog->srcNodeId);
// TODO: Check
if(mgmtMsgLog->srcNodeId == L2_GetHeadId(instPtr)){
if (mgmtMsgLog->srcNodeId == L2_GetHeadId(instPtr))
{
L2_TimeAlign(instPtr, L2_GetLastFrameNum(instPtr), L2_GetLastSlotNum(instPtr), true);
}
break;
}
case L2_INTER_TSYNC_FRAME:
case L2_INTRA_TSYNC_FRAME: {
if(L2_GetNodeType(instPtr) == L2_NODE_HEAD){
for (int i = 0; i < ARRAY_SIZE(rttReqFrameMsgs); i++) {
case L2_INTRA_TSYNC_FRAME:
{
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
{
for (int i = 0; i < ARRAY_SIZE(rttReqFrameMsgs); i++)
{
memcpy(mgmtMsg->words,
instPtr->recvPktData + (size_t) (i * (RTT_MSG_LEN / 8)),
(size_t) (RTT_MSG_LEN / 8));
switch (rttReqFrameMsgs[i]) {
instPtr->recvPktData + (size_t)(i * (RTT_MSG_LEN / 8)),
(size_t)(RTT_MSG_LEN / 8));
switch (rttReqFrameMsgs[i])
{
case L2_MSG_RTT_REQ_INIT:
FrameGetChunk(mgmtMsg, RTT_REQ_WTAG_OFFSET, RTT_REQ_WTAG_LEN, &dataChunk);
rttMsgReqLog->wordTag = (uint16_t) (dataChunk);
rttMsgReqLog->wordTag = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_REQ_MSG_TAG_OFFSET, RTT_REQ_MSG_TAG_LEN, &dataChunk);
rttMsgReqLog->msgTag = (uint16_t) (dataChunk);
rttMsgReqLog->msgTag = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_REQ_MSG_STAG_OFFSET, RTT_REQ_MSG_STAG_LEN, &dataChunk);
rttMsgReqLog->msgSubTag = (uint16_t) (dataChunk);
rttMsgReqLog->msgSubTag = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_REQ_MSG_OFFSET, RTT_REQ_MSG_LEN, &dataChunk);
rttMsgReqLog->msgLen = (uint16_t) (dataChunk);
rttMsgReqLog->msgLen = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_REQ_SRC_ADDR_OFFSET, RTT_REQ_SRC_ADDR_LEN, &dataChunk);
rttMsgReqLog->srcAddr = (uint16_t) (dataChunk);
rttMsgReqLog->srcAddr = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_REQ_DST_ADDR_OFFSET, RTT_REQ_DST_ADDR_LEN, &dataChunk);
rttMsgReqLog->dstAddr = (uint16_t) (dataChunk);
rttMsgReqLog->dstAddr = (uint16_t)(dataChunk);
break;
case L2_MSG_RTT_REQ_EXTD:
FrameGetChunk(mgmtMsg, RTT_REQ_EX_TTAGL_OFFSET, RTT_REQ_EX_TTAGL_LEN, &dataChunk);
rttMsgReqLog->timeWordL = (uint32_t) (dataChunk);
rttMsgReqLog->timeWordL = (uint32_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_REQ_EX_TTAGH_OFFSET, RTT_REQ_EX_TTAGH_LEN, &dataChunk);
rttMsgReqLog->timeWordH = (uint32_t) (dataChunk);
rttMsgReqLog->timeWordH = (uint32_t)(dataChunk);
break;
case L2_MSG_RTT_REQ_CONT:
FrameGetChunk(mgmtMsg, RTT_REQ_CONT_DOP_OFFSET, RTT_REQ_CONT_DOP_LEN, &dataChunk);
rttMsgReqLog->doppler = (int32_t) (dataChunk);
rttMsgReqLog->doppler = (int32_t)(dataChunk);
break;
default:
@@ -786,51 +829,55 @@ int32_t MgmtNet_PktHandler(L2_INST* instPtr) {
goto failed;
}
filedSize += RTT_MSG_LEN;
if (filedSize / 8 >= INTRA_RTT_FRAME_LENGTH_MAX) {
if (filedSize / 8 >= INTRA_RTT_FRAME_LENGTH_MAX)
{
DBG_ERROR("Message decapsulate exceeds frame length.\n");
Status = EPARAM;
goto failed;
}
}
}
else {
for (int i = 0; i < ARRAY_SIZE(rttRspFrameMsgs); i++) {
else
{
for (int i = 0; i < ARRAY_SIZE(rttRspFrameMsgs); i++)
{
memcpy(mgmtMsg->words,
instPtr->recvPktData + (size_t) (i * (RTT_MSG_LEN / 8)),
(size_t) (RTT_MSG_LEN / 8));
switch (rttRspFrameMsgs[i]) {
instPtr->recvPktData + (size_t)(i * (RTT_MSG_LEN / 8)),
(size_t)(RTT_MSG_LEN / 8));
switch (rttRspFrameMsgs[i])
{
case L2_MSG_RTT_RSP_INIT:
FrameGetChunk(mgmtMsg, RTT_RSP_WTAG_OFFSET, RTT_RSP_WTAG_LEN, &dataChunk);
rttMsgRspLog->wordTag = (uint16_t) (dataChunk);
rttMsgRspLog->wordTag = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_RSP_MSG_TAG_OFFSET, RTT_RSP_MSG_TAG_LEN, &dataChunk);
rttMsgRspLog->msgTag = (uint16_t) (dataChunk);
rttMsgRspLog->msgTag = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_RSP_MSG_STAG_OFFSET, RTT_RSP_MSG_STAG_LEN, &dataChunk);
rttMsgRspLog->msgSubTag = (uint16_t) (dataChunk);
rttMsgRspLog->msgSubTag = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_RSP_MSG_OFFSET, RTT_RSP_MSG_LEN, &dataChunk);
rttMsgRspLog->msgLen = (uint16_t) (dataChunk);
rttMsgRspLog->msgLen = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_RSP_SRC_ADDR_OFFSET, RTT_RSP_SRC_ADDR_LEN, &dataChunk);
rttMsgRspLog->srcAddr = (uint16_t) (dataChunk);
rttMsgRspLog->srcAddr = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_RSP_DST_ADDR_OFFSET, RTT_RSP_DST_ADDR_LEN, &dataChunk);
rttMsgRspLog->dstAddr = (uint16_t) (dataChunk);
rttMsgRspLog->dstAddr = (uint16_t)(dataChunk);
break;
case L2_MSG_RTT_RSP_EXTD:
FrameGetChunk(mgmtMsg, RTT_RSP_EX_TTAGL_OFFSET, RTT_RSP_EX_TTAGL_LEN, &dataChunk);
rttMsgRspLog->timeWordL = (uint32_t) (dataChunk);
rttMsgRspLog->timeWordL = (uint32_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_RSP_EX_TTAGH_OFFSET, RTT_RSP_EX_TTAGH_LEN, &dataChunk);
rttMsgRspLog->timeWordH = (uint32_t) (dataChunk);
rttMsgRspLog->timeWordH = (uint32_t)(dataChunk);
break;
case L2_MSG_RTT_RSP_CONT:
FrameGetChunk(mgmtMsg, RTT_RSP_CONT_DOP_OFFSET,
RTT_RSP_CONT_DOP_LEN, &dataChunk);
rttMsgRspLog->doppler = (int32_t) (dataChunk);
RTT_RSP_CONT_DOP_LEN, &dataChunk);
rttMsgRspLog->doppler = (int32_t)(dataChunk);
break;
default:
@@ -839,7 +886,8 @@ int32_t MgmtNet_PktHandler(L2_INST* instPtr) {
goto failed;
}
filedSize += RTT_MSG_LEN;
if (filedSize / 8 >= INTRA_RTT_FRAME_LENGTH_MAX) {
if (filedSize / 8 >= INTRA_RTT_FRAME_LENGTH_MAX)
{
DBG_ERROR("Message decapsulate exceeds frame length.\n");
Status = EPARAM;
goto failed;
@@ -849,7 +897,8 @@ int32_t MgmtNet_PktHandler(L2_INST* instPtr) {
L2_RstNodeTTL(instPtr->nodeList, rttMsgRspLog->srcAddr & 0x001F);
// TODO: Check
if((rttMsgRspLog->srcAddr&0x001F) == L2_GetHeadId(instPtr)){
if ((rttMsgRspLog->srcAddr & 0x001F) == L2_GetHeadId(instPtr))
{
L2_TimeAlign(instPtr, L2_GetLastFrameNum(instPtr), L2_GetLastSlotNum(instPtr), true);
}
break;
@@ -866,34 +915,35 @@ failed:
return Status;
}
/**
* @brief Helper function for check node TTL.
* @param entry
* @return trur/false
*/
static bool IsTTLValid(NODE_ENTRY* entry) {
static bool IsTTLValid(NODE_ENTRY *entry)
{
ASSERT(entry != NULL);
return (entry->ttlCnt <= CONX_LOST_TH);
}
/*****************************************************************************/
/**
* @brief Management net FSM routine.
*
* @param mgmtFSM
*
* @param mgmtFSM
* mainState
*
*
* @return Slot has be used.
*
*
******************************************************************************/
int32_t MgmtNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry){
int32_t MgmtNet_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
{
ASSERT(instPtr != NULL);
int32_t Status;
DBG_INFO("-------- MgmtNet Routine --------\n");
if(rtneEntry->slotSeq != 0){
if (rtneEntry->slotSeq != 0)
{
return SUCCESS;
}
@@ -902,27 +952,31 @@ int32_t MgmtNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry){
// Determine next slot TOR state.
L2_TOR_TYPE torState = L2_TOR_SUSPEND;
if ((rtneEntry->src0 == L2_GetNodeId(instPtr))
|| (rtneEntry->src1 == L2_GetNodeId(instPtr))) {
if ((rtneEntry->src0 == L2_GetNodeId(instPtr)) || (rtneEntry->src1 == L2_GetNodeId(instPtr)))
{
torState = L2_TOR_TX;
}
else if ((rtneEntry->dst0 == L2_GetNodeId(instPtr))
|| (rtneEntry->dst1 == L2_GetNodeId(instPtr))) {
else if ((rtneEntry->dst0 == L2_GetNodeId(instPtr)) || (rtneEntry->dst1 == L2_GetNodeId(instPtr)))
{
torState = L2_TOR_RX;
}
else {
else
{
torState = L2_TOR_SUSPEND;
}
// Update RTT FSM if needed.
if (rtneEntry->slotType == L2_SLOT_INTRA_TSYNC) {
if(torState == L2_TOR_TX) {
// Update RTT FSM if needed.
if (rtneEntry->slotType == L2_SLOT_INTRA_TSYNC)
{
if (torState == L2_TOR_TX)
{
RTT_FsmUpdate(instPtr, L2_TOR_TX);
}
else if (torState == L2_TOR_RX)
{
L2_SCHD_EVENT* rttRxEvent = L2_CreatNewSchdEvent(instPtr->schdEventPool, L2_EV_RTT_RECV_UPDATE, true);
if(rttRxEvent == NULL) {
L2_SCHD_EVENT *rttRxEvent = L2_CreatNewSchdEvent(instPtr->schdEventPool, L2_EV_RTT_RECV_UPDATE, true);
if (rttRxEvent == NULL)
{
DBG_ERROR("Failed to initialize rtt event.\n");
}
// Schedule rtt update after receive process end.
@@ -930,15 +984,18 @@ int32_t MgmtNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry){
}
}
switch (rtneEntry->slotType) {
switch (rtneEntry->slotType)
{
case L2_SLOT_INTER_MGMT:
// TODO:
// TODO:
L2_SetupNextSlot(instPtr, torState, L2_INTER_MGMT_FRAME);
break;
case L2_SLOT_INTRA_MGMT:
memset(instPtr->sendPktData, 0xAA, L2_PACKET_LEN_MAX);
if (torState == L2_TOR_TX) {
if(MgmtPkt_Packing(instPtr, MGMTPKT_INTRA_MGMT) != SUCCESS) {
if (torState == L2_TOR_TX)
{
if (MgmtPkt_Packing(instPtr, MGMTPKT_INTRA_MGMT) != SUCCESS)
{
Status = FAILURE;
goto routine_err;
}
@@ -946,20 +1003,25 @@ int32_t MgmtNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry){
L2_SetupNextSlot(instPtr, torState, L2_INTRA_MGMT_FRAME);
break;
case L2_SLOT_INTER_TSYNC:
// TODO:
// TODO:
L2_SetupNextSlot(instPtr, torState, L2_INTER_TSYNC_FRAME);
break;
case L2_SLOT_INTRA_TSYNC:
if (torState == L2_TOR_TX) {
if (torState == L2_TOR_TX)
{
memset(instPtr->sendPktData, 0xAA, L2_PACKET_LEN_MAX);
if(L2_GetNodeType(instPtr) == L2_NODE_HEAD){
if(MgmtPkt_Packing(instPtr, MGMTPKT_INTRA_RTTREQ) != SUCCESS) {
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
{
if (MgmtPkt_Packing(instPtr, MGMTPKT_INTRA_RTTREQ) != SUCCESS)
{
Status = FAILURE;
goto routine_err;
}
}
else {
if(MgmtPkt_Packing(instPtr, MGMTPKT_INTRA_RTTRSP) != SUCCESS) {
else
{
if (MgmtPkt_Packing(instPtr, MGMTPKT_INTRA_RTTRSP) != SUCCESS)
{
Status = FAILURE;
goto routine_err;
}
@@ -975,35 +1037,36 @@ int32_t MgmtNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry){
/*************************************************/
// Update link layer state...
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD) {
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
{
L2_NODE_STATE newState = L2_NONE_STATE;
L2_EVENT stateEvent = {
.event = L2_EV_MAIN_STATE_UPDATE,
.msg = &newState
};
if (L2_GetNodeListLen(instPtr->nodeList) < instPtr->config.netScale
&& L2_GetMainState(instPtr) != L2_INTRA_SEEK_STATE) {
.msg = &newState};
if (L2_GetNodeListLen(instPtr->nodeList) < instPtr->config.netScale && L2_GetMainState(instPtr) != L2_INTRA_SEEK_STATE)
{
newState = L2_INTRA_SEEK_STATE;
AssertL2Event(instPtr, &stateEvent);
}
else if(L2_GetNodeListLen(instPtr->nodeList) >= instPtr->config.netScale &&
L2_GetMainState(instPtr) != L2_NORMAL_STATE){
else if (L2_GetNodeListLen(instPtr->nodeList) >= instPtr->config.netScale &&
L2_GetMainState(instPtr) != L2_NORMAL_STATE)
{
newState = L2_NORMAL_STATE;
AssertL2Event(instPtr, &stateEvent);
}
}
else{ // L2_NODE_MEMBER or BACKUP_HEAD
if(L2_isNodeInList(instPtr->nodeList, L2_GetHeadId(instPtr)) == false){
else
{ // L2_NODE_MEMBER or BACKUP_HEAD
if (L2_isNodeInList(instPtr->nodeList, L2_GetHeadId(instPtr)) == false)
{
L2_NODE_STATE newState = L2_INIT_STATE;
L2_EVENT stateEvent = {
.event = L2_EV_MAIN_STATE_UPDATE,
.msg = &newState
};
.msg = &newState};
AssertL2Event(instPtr, &stateEvent);
L2_EVENT lostCxnEvent = {
.event = L2_EV_LOST_CXN,
.msg = NULL
};
.msg = NULL};
AssertL2Event(instPtr, &lostCxnEvent);
DBG_WARNING("Lost connection with head node [%d].\n", L2_GetHeadId(instPtr));
}