1077 lines
32 KiB
C
1077 lines
32 KiB
C
/*****************************************************************************/
|
|
/**
|
|
* 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:
|
|
* [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 *********************************/
|
|
#include <string.h>
|
|
#include "linklayer814.h"
|
|
#include "linklayer814_types.h"
|
|
#include "msg_def.h"
|
|
#include "rtt.h"
|
|
#include "../error.h"
|
|
#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
|
|
|
|
/***************** Macros (Inline Functions) Definitions *********************/
|
|
|
|
/**************************** Type Definitions *******************************/
|
|
typedef enum
|
|
{
|
|
MGMTPKT_NONE = 0,
|
|
MGMTPKT_INTER_MGMT = 1,
|
|
MGMTPKT_INTRA_MGMT = 2,
|
|
MGMTPKT_INTER_RTTREQ = 3,
|
|
MGMTPKT_INTER_RTTRSP = 4,
|
|
MGMTPKT_INTRA_RTTREQ = 5,
|
|
MGMTPKT_INTRA_RTTRSP = 6
|
|
} MGMTPKT_TYPE;
|
|
/************************** 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);
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Packing management message to a bit array.
|
|
*
|
|
* @param instPtr a pointer to l2 instance.
|
|
* @param bitarr pointer to bit array buffer.
|
|
* @param msgType message type.
|
|
*
|
|
* @return EPARAM
|
|
* SUCCESS
|
|
*
|
|
******************************************************************************/
|
|
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)
|
|
{
|
|
return EPARAM;
|
|
}
|
|
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)
|
|
{
|
|
return EPARAM;
|
|
}
|
|
}
|
|
switch (msgType)
|
|
{
|
|
case L2_MSG_TK_INIT:
|
|
// Series tag
|
|
dataChunk = 0;
|
|
FrameInsertChunk(bitarr, TK_MSG_INI_SER_TAG_OFFSET, TK_MSG_INI_SER_TAG_LEN, dataChunk);
|
|
|
|
// word tag
|
|
dataChunk = 3;
|
|
FrameInsertChunk(bitarr, TK_MSG_INI_WTAG_OFFSET, TK_MSG_INI_WTAG_LEN, dataChunk);
|
|
|
|
// message tag
|
|
dataChunk = 28;
|
|
FrameInsertChunk(bitarr, TK_MSG_INI_TAG_OFFSET, TK_MSG_INI_TAG_LEN, dataChunk);
|
|
|
|
// message sub tag
|
|
dataChunk = 1;
|
|
FrameInsertChunk(bitarr, TK_MSG_INI_STAG_OFFSET, TK_MSG_INI_STAG_LEN, dataChunk);
|
|
|
|
// message length tag
|
|
dataChunk = 4;
|
|
FrameInsertChunk(bitarr, TK_MSG_INI_OFFSET, TK_MSG_INI_LEN, dataChunk);
|
|
|
|
// Next net stage
|
|
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
|
|
{
|
|
switch (instPtr->nextState)
|
|
{
|
|
case L2_INIT_STATE:
|
|
dataChunk = NET_INI_STAGE;
|
|
break;
|
|
case L2_NORMAL_STATE:
|
|
dataChunk = NET_NORMAL_STAGE;
|
|
break;
|
|
case L2_INTRA_SEEK_STATE:
|
|
dataChunk = NET_INTRA_SEEK_STAGE;
|
|
break;
|
|
case L2_INTER_SEEK_STATE:
|
|
dataChunk = NET_INTER_SEEK_STAGE;
|
|
break;
|
|
case L2_SEEK_STATE:
|
|
dataChunk = NET_SEEK_STAGE;
|
|
break;
|
|
default:
|
|
dataChunk = NET_ERROR_STAGE;
|
|
break;
|
|
}
|
|
FrameInsertChunk(bitarr, TK_MSG_INI_NET_STATE_OFFSET, TK_MSG_INI_NET_STATE_LEN, dataChunk);
|
|
}
|
|
break;
|
|
|
|
case L2_MSG_TK_EXTD:
|
|
// Word tag
|
|
dataChunk = 1;
|
|
FrameInsertChunk(bitarr, TK_MSG_EXTEND_WTAG_OFFSET, TK_MSG_EXTEND_WTAG_LEN, dataChunk);
|
|
|
|
// Time tag
|
|
dataChunk = 0xAAAAAAAAAAAA;
|
|
FrameInsertChunk(bitarr, TK_MSG_EXTEND_TIME_TAG_OFFSET, TK_MSG_EXTEND_TIME_TAG_LEN, dataChunk);
|
|
|
|
// East speed
|
|
dataChunk = 0xBBBB;
|
|
FrameInsertChunk(bitarr, TK_MSG_EXTEND_EAST_SPEED_OFFSET, TK_MSG_EXTEND_EAST_SPEED_LEN, dataChunk);
|
|
break;
|
|
|
|
case L2_MSG_TK_CONT_1:
|
|
// Word tag
|
|
dataChunk = 2;
|
|
FrameInsertChunk(bitarr, TK_MSG_CONT_1_WTAG_OFFSET, TK_MSG_CONT_1_WTAG_LEN, dataChunk);
|
|
|
|
// Continue word identification.
|
|
dataChunk = 1;
|
|
FrameInsertChunk(bitarr, TK_MSG_CONT_1_CONTID_OFFSET, TK_MSG_CONT_1_CONTID_LEN, dataChunk);
|
|
|
|
// North speed
|
|
dataChunk = 0xCCCC;
|
|
FrameInsertChunk(bitarr, TK_MSG_CONT_1_NORTH_SPEED_OFFSET, TK_MSG_CONT_1_NORTH_SPEED_LEN, dataChunk);
|
|
|
|
// Upper speed
|
|
dataChunk = 0xDDDD;
|
|
FrameInsertChunk(bitarr, TK_MSG_CONT_1_UPPER_SPEED_OFFSET,
|
|
TK_MSG_CONT_1_UPPER_SPEED_LEN, dataChunk);
|
|
|
|
// Height
|
|
dataChunk = 0xEEEEEE;
|
|
FrameInsertChunk(bitarr, TK_MSG_CONT_1_HEIGHT_OFFSET, TK_MSG_CONT_1_HEIGHT_LEN, dataChunk);
|
|
break;
|
|
|
|
case L2_MSG_TK_CONT_2:
|
|
// Word tag
|
|
dataChunk = 2;
|
|
FrameInsertChunk(bitarr, TK_MSG_CONT_2_WTAG_OFFSET, TK_MSG_CONT_2_WTAG_LEN, dataChunk);
|
|
|
|
// Continue word identification.
|
|
dataChunk = 2;
|
|
FrameInsertChunk(bitarr, TK_MSG_CONT_2_CONTID_OFFSET, TK_MSG_CONT_2_CONTID_LEN, dataChunk);
|
|
|
|
// Longitude
|
|
dataChunk = 0xAAAAAAAA;
|
|
FrameInsertChunk(bitarr, TK_MSG_CONT_2_LONGITUDE_OFFSET, TK_MSG_CONT_2_LONGITUDE_LEN, dataChunk);
|
|
|
|
// Latitude
|
|
dataChunk = 0xBBBBBBBB;
|
|
FrameInsertChunk(bitarr, TK_MSG_CONT_2_LATITUDE_OFFSET, TK_MSG_CONT_2_LATITUDE_LEN, dataChunk);
|
|
break;
|
|
|
|
case L2_MSG_TK_CONT_3:
|
|
|
|
// Word tag
|
|
dataChunk = 2;
|
|
FrameInsertChunk(bitarr, TK_MSG_CONT_3_WTAG_OFFSET, TK_MSG_CONT_3_WTAG_LEN, dataChunk);
|
|
|
|
// Continue word identification.
|
|
dataChunk = 3;
|
|
FrameInsertChunk(bitarr, TK_MSG_CONT_3_CONTID_OFFSET, TK_MSG_CONT_3_CONTID_LEN, dataChunk);
|
|
|
|
// source node id
|
|
dataChunk = (L2_GetNodeId(instPtr) & 0x1F) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
|
|
FrameInsertChunk(bitarr, TK_MSG_CONT_3_MEMBER_ADDR_OFFSET, TK_MSG_CONT_3_MEMBER_ADDR_LEN, dataChunk);
|
|
|
|
// East speed
|
|
dataChunk = 0xBBBB;
|
|
FrameInsertChunk(bitarr, TK_MSG_CONT_3_EAST_ACC_OFFSET, TK_MSG_CONT_3_EAST_ACC_LEN, dataChunk);
|
|
|
|
// North speed
|
|
dataChunk = 0xCCCC;
|
|
FrameInsertChunk(bitarr, TK_MSG_CONT_3_NORTH_ACC_OFFSET, TK_MSG_CONT_3_NORTH_ACC_LEN, dataChunk);
|
|
|
|
// Upper speed
|
|
dataChunk = 0xDDDD;
|
|
FrameInsertChunk(bitarr, TK_MSG_CONT_3_UPPER_ACC_OFFSET, TK_MSG_CONT_3_UPPER_ACC_LEN, dataChunk);
|
|
break;
|
|
|
|
case L2_MSG_TA_INIT:
|
|
|
|
// Word tag
|
|
dataChunk = 3;
|
|
FrameInsertChunk(bitarr, TA_MSG_WTAG_OFFSET, TA_MSG_WTAG_LEN, dataChunk);
|
|
|
|
// Msg tag
|
|
dataChunk = 29;
|
|
FrameInsertChunk(bitarr, TA_MSG_MSG_TAG_OFFSET, TA_MSG_MSG_TAG_LEN, dataChunk);
|
|
|
|
// Sub msg tag
|
|
dataChunk = 0;
|
|
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);
|
|
FrameInsertChunk(bitarr, TA_MSG_SRC_ADDR_OFFSET, TA_MSG_SRC_ADDR_LEN, dataChunk);
|
|
|
|
// Height
|
|
dataChunk = 0xEEEEEE;
|
|
FrameInsertChunk(bitarr, TA_MSG_HEIGHT_OFFSET, TA_MSG_HEIGHT_LEN, dataChunk);
|
|
break;
|
|
|
|
case L2_MSG_TA_EXTD:
|
|
|
|
// Longitude
|
|
dataChunk = 0xAAAAAAAA;
|
|
FrameInsertChunk(bitarr, TA_MSG_EX_LONGITUDE_OFFSET, TA_MSG_EX_LONGITUDE_LEN, dataChunk);
|
|
|
|
// Latitude
|
|
dataChunk = 0xBBBBBBBB;
|
|
FrameInsertChunk(bitarr, TA_MSG_EX_LATITUDE_OFFSET, TA_MSG_EX_LATITUDE_LEN, dataChunk);
|
|
|
|
break;
|
|
case L2_MSG_TA_CONT_1:
|
|
|
|
// Time tag
|
|
dataChunk = 0xCCCCCCCCCCCC;
|
|
FrameInsertChunk(bitarr, TA_MSG_CONT_1_TIME_TAG_OFFSET, TA_MSG_CONT_1_TIME_TAG_LEN, dataChunk);
|
|
|
|
break;
|
|
case L2_MSG_TA_CONT_2:
|
|
|
|
// East speed
|
|
dataChunk = 0xBBBB;
|
|
FrameInsertChunk(bitarr, TA_MSG_CONT_2_EAST_OFFSET, TA_MSG_CONT_2_EAST_LEN, dataChunk);
|
|
|
|
// North speed
|
|
dataChunk = 0xCCCC;
|
|
FrameInsertChunk(bitarr, TA_MSG_CONT_2_NORTH_OFFSET, TA_MSG_CONT_2_NORTH_LEN, dataChunk);
|
|
|
|
// Upper speed
|
|
dataChunk = 0xDDDD;
|
|
FrameInsertChunk(bitarr, TA_MSG_CONT_2_UPPER_OFFSET, TA_MSG_CONT_2_UPPER_LEN, dataChunk);
|
|
break;
|
|
|
|
case L2_MSG_MADJ_INIT:
|
|
break;
|
|
|
|
case L2_MSG_MADJ_EXTD:
|
|
break;
|
|
|
|
case L2_MSG_RTT_REQ_INIT:
|
|
// Series tag
|
|
dataChunk = 0;
|
|
FrameInsertChunk(bitarr, RTT_REQ_SER_TAG_OFFSET, RTT_REQ_SER_TAG_LEN, dataChunk);
|
|
|
|
// Word tag
|
|
dataChunk = 3;
|
|
FrameInsertChunk(bitarr, RTT_REQ_WTAG_OFFSET, RTT_REQ_WTAG_LEN, dataChunk);
|
|
|
|
// message identification.
|
|
dataChunk = 29;
|
|
FrameInsertChunk(bitarr, RTT_REQ_MSG_TAG_OFFSET, RTT_REQ_MSG_TAG_LEN, dataChunk);
|
|
|
|
// message identification.
|
|
dataChunk = 1; // request
|
|
FrameInsertChunk(bitarr, RTT_REQ_MSG_STAG_OFFSET, RTT_REQ_MSG_STAG_LEN, dataChunk);
|
|
|
|
// message length.
|
|
dataChunk = 2;
|
|
FrameInsertChunk(bitarr, RTT_REQ_MSG_OFFSET, RTT_REQ_MSG_LEN, dataChunk);
|
|
|
|
// source node address.
|
|
dataChunk = (L2_GetNodeId(instPtr) & 0x1F) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
|
|
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);
|
|
FrameInsertChunk(bitarr, RTT_REQ_DST_ADDR_OFFSET, RTT_REQ_DST_ADDR_LEN, dataChunk);
|
|
break;
|
|
|
|
case L2_MSG_RTT_REQ_EXTD:
|
|
// Word tag
|
|
dataChunk = 1;
|
|
FrameInsertChunk(bitarr, RTT_REQ_EX_WTAG_OFFSET, RTT_REQ_EX_WTAG_LEN, dataChunk);
|
|
|
|
// Time tag Low.
|
|
dataChunk = rttFsmPtr->timeStamp.frac_part;
|
|
FrameInsertChunk(bitarr, RTT_REQ_EX_TTAGL_OFFSET, RTT_REQ_EX_TTAGL_LEN, dataChunk);
|
|
|
|
// Time tag High.
|
|
dataChunk = rttFsmPtr->timeStamp.int_part;
|
|
FrameInsertChunk(bitarr, RTT_REQ_EX_TTAGH_OFFSET, RTT_REQ_EX_TTAGH_LEN, dataChunk);
|
|
break;
|
|
|
|
case L2_MSG_RTT_REQ_CONT:
|
|
// Doppler
|
|
dataChunk = rttFsmPtr->doppler;
|
|
FrameInsertChunk(bitarr, RTT_REQ_CONT_DOP_OFFSET, RTT_REQ_CONT_DOP_LEN, dataChunk);
|
|
break;
|
|
|
|
case L2_MSG_RTT_RSP_INIT:
|
|
// Series tag
|
|
dataChunk = 0;
|
|
FrameInsertChunk(bitarr, RTT_RSP_SER_TAG_OFFSET, RTT_RSP_SER_TAG_LEN, dataChunk);
|
|
|
|
// Word tag
|
|
dataChunk = 3;
|
|
FrameInsertChunk(bitarr, RTT_RSP_WTAG_OFFSET, RTT_RSP_WTAG_LEN, dataChunk);
|
|
|
|
// message identification.
|
|
dataChunk = 29;
|
|
FrameInsertChunk(bitarr, RTT_RSP_MSG_TAG_OFFSET,
|
|
RTT_RSP_MSG_TAG_LEN, dataChunk);
|
|
|
|
// message identification.
|
|
dataChunk = 2; // response
|
|
FrameInsertChunk(bitarr, RTT_RSP_MSG_STAG_OFFSET, RTT_RSP_MSG_STAG_LEN, dataChunk);
|
|
|
|
// message length.
|
|
dataChunk = 2;
|
|
FrameInsertChunk(bitarr, RTT_RSP_MSG_OFFSET, RTT_RSP_MSG_LEN, dataChunk);
|
|
|
|
// source node address.
|
|
dataChunk = (L2_GetNodeId(instPtr) & 0x1F) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
|
|
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);
|
|
FrameInsertChunk(bitarr, RTT_RSP_DST_ADDR_OFFSET, RTT_RSP_DST_ADDR_LEN, dataChunk);
|
|
break;
|
|
|
|
case L2_MSG_RTT_RSP_EXTD:
|
|
// Word tag
|
|
dataChunk = 1;
|
|
FrameInsertChunk(bitarr, RTT_RSP_EX_WTAG_OFFSET, RTT_RSP_EX_WTAG_LEN, dataChunk);
|
|
|
|
// Time tag Low.
|
|
dataChunk = rttFsmPtr->timeStamp.frac_part;
|
|
FrameInsertChunk(bitarr, RTT_RSP_EX_TTAGL_OFFSET, RTT_RSP_EX_TTAGL_LEN, dataChunk);
|
|
|
|
// Time tag High.
|
|
dataChunk = rttFsmPtr->timeStamp.int_part;
|
|
FrameInsertChunk(bitarr, RTT_RSP_EX_TTAGH_OFFSET, RTT_RSP_EX_TTAGH_LEN, dataChunk);
|
|
break;
|
|
|
|
case L2_MSG_RTT_RSP_CONT:
|
|
// Doppler
|
|
dataChunk = rttFsmPtr->doppler;
|
|
FrameInsertChunk(bitarr, RTT_RSP_CONT_DOP_OFFSET, RTT_RSP_CONT_DOP_LEN, dataChunk);
|
|
break;
|
|
|
|
case L2_MSG_NQT_INIT:
|
|
// // word tag
|
|
// dataChunk = 3;
|
|
// FrameInsertChunk(bitarr, NQT_MSG_WTAG_OFFSET,
|
|
// NQT_MSG_WTAG_LEN, dataChunk);
|
|
|
|
// // msg tag
|
|
// dataChunk = 30;
|
|
// FrameInsertChunk(bitarr, NQT_MSG_MSG_TAG_OFFSET,
|
|
// NQT_MSG_MSG_TAG_LEN, dataChunk);
|
|
|
|
// // msg sub tag
|
|
// dataChunk = 1;
|
|
// FrameInsertChunk(bitarr, NQT_MSG_MSG_STAG_OFFSET,
|
|
// NQT_MSG_MSG_STAG_LEN, dataChunk);
|
|
|
|
// // msg length
|
|
// dataChunk = 0;
|
|
// FrameInsertChunk(bitarr, NQT_MSG_MSG_OFFSET,
|
|
// NQT_MSG_MSG_LEN, dataChunk);
|
|
|
|
// // Ctrol Word
|
|
// dataChunk = 1;
|
|
// FrameInsertChunk(bitarr, NQT_MSG_CTRL_MUTE_OFFSET,
|
|
// NQT_MSG_CTRL_MUTE_LEN, dataChunk);
|
|
|
|
// // Quit object
|
|
// 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
|
|
// };
|
|
// AssertL2Event(instPtr, &stateEvent);
|
|
// L2_EVENT lostCxnEvent = {
|
|
// .event = L2_EV_LOST_CXN,
|
|
// .msg = NULL
|
|
// };
|
|
// AssertL2Event(instPtr, &lostCxnEvent);
|
|
break;
|
|
|
|
case L2_MSG_CMG_INIT:
|
|
break;
|
|
|
|
case L2_MSG_BCU_INIT:
|
|
break;
|
|
|
|
default:
|
|
DBG_ERROR("Unsupported message type: %d\n", msgType);
|
|
return EPARAM;
|
|
break;
|
|
}
|
|
return SUCCESS;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Packing management message to a bit array.
|
|
*
|
|
* @param instPtr a pointer to l2 instance.
|
|
* @param pktType packet type.
|
|
*
|
|
* @return SUCCESS
|
|
* error code
|
|
*
|
|
******************************************************************************/
|
|
static int32_t MgmtPkt_Packing(L2_INST *instPtr, MGMTPKT_TYPE pktType)
|
|
{
|
|
ASSERT(instPtr != NULL);
|
|
uint8_t *buff = instPtr->sendPktData;
|
|
uint32_t filedSize = 0;
|
|
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)
|
|
{
|
|
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));
|
|
}
|
|
else
|
|
{
|
|
DBG_ERROR("Failed to packing intraMgmtFrameMsgs[%d].\n", i);
|
|
goto failed;
|
|
}
|
|
filedSize += MGMT_MSG_LEN;
|
|
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)
|
|
{
|
|
memcpy(buff + (size_t)(i * (MGMT_MSG_LEN / 8)),
|
|
mgmtPkt->words,
|
|
(size_t)(MGMT_MSG_LEN / 8));
|
|
}
|
|
else
|
|
{
|
|
DBG_ERROR("Failed to packing rttReqFrameMsgs[%d].\n", i);
|
|
goto failed;
|
|
}
|
|
filedSize += MGMT_MSG_LEN;
|
|
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));
|
|
}
|
|
else
|
|
{
|
|
DBG_ERROR("Failed to packing rttRspFrameMsgs[%d].\n", i);
|
|
goto failed;
|
|
}
|
|
filedSize += MGMT_MSG_LEN;
|
|
if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX)
|
|
{
|
|
DBG_ERROR("Message is too long to be properly packed.\n");
|
|
goto failed;
|
|
}
|
|
}
|
|
break;
|
|
default:
|
|
DBG_ERROR("Scan Packet type [%d] is not surpported yet.\n", pktType);
|
|
goto failed;
|
|
}
|
|
bit_array_free(mgmtPkt);
|
|
return SUCCESS;
|
|
failed:
|
|
bit_array_free(mgmtPkt);
|
|
return FAILURE;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @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 L2_Mgmt_Init(L2_MGMT_FSM **mgmtFSM)
|
|
{
|
|
(*mgmtFSM) = (L2_MGMT_FSM *)malloc(sizeof(L2_MGMT_FSM));
|
|
if (*mgmtFSM == NULL)
|
|
{
|
|
return EALLOC;
|
|
}
|
|
L2_Mgmt_Reset(*mgmtFSM);
|
|
return SUCCESS;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Reset management net FSM.
|
|
*
|
|
* @param mgmtFSM a pointer to l2 management net FSM instance.
|
|
*
|
|
* @return NONE
|
|
*
|
|
******************************************************************************/
|
|
void L2_Mgmt_Reset(L2_MGMT_FSM *mgmtFSM)
|
|
{
|
|
mgmtFSM->isBisarProc = false;
|
|
mgmtFSM->isPendSlotAlloc = false;
|
|
mgmtFSM->isPendTimeAlloc = false;
|
|
|
|
mgmtFSM->mgmtMsgLog.nextNetState = L2_NONE_STATE;
|
|
mgmtFSM->mgmtMsgLog.isValid = false;
|
|
mgmtFSM->mgmtMsgLog.quitCtrlWord = 0;
|
|
mgmtFSM->mgmtMsgLog.quitNodeId = 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;
|
|
|
|
// 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.
|
|
*
|
|
* @param mgmtFSM a pointer to l2 management net FSM instance.
|
|
*
|
|
* @return NONE
|
|
*
|
|
******************************************************************************/
|
|
void L2_Mgmt_Destroy(L2_MGMT_FSM *mgmtFSM)
|
|
{
|
|
free(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 L2_Mgmt_PktHandler(L2_INST *instPtr)
|
|
{
|
|
ASSERT(instPtr != NULL);
|
|
int32_t Status;
|
|
uint32_t filedSize = 0;
|
|
uint64_t dataChunk = 0;
|
|
L2_MGMT_MSG_RECORD *mgmtMsgLog = &(instPtr->mgmtFSM->mgmtMsgLog);
|
|
L2_RTT_MSG_RECORD *rttMsgRspLog = &(instPtr->mgmtFSM->rttMsgRspLog);
|
|
L2_RTT_MSG_RECORD *rttMsgReqLog = &(instPtr->mgmtFSM->rttMsgReqLog);
|
|
|
|
DBG_INFO("Management net handling packet...\n");
|
|
|
|
BIT_ARRAY *mgmtMsg = bit_array_create(L2_MSG_MAX);
|
|
if (mgmtMsg == NULL)
|
|
{
|
|
DBG_INFO("Failed to start handling process...\n");
|
|
return EALLOC;
|
|
}
|
|
switch (L2_GetLastOptFrameType(instPtr))
|
|
{
|
|
case L2_INTER_MGMT_FRAME:
|
|
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])
|
|
{
|
|
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);
|
|
|
|
FrameGetChunk(mgmtMsg, TK_MSG_INI_WTAG_OFFSET, TK_MSG_INI_WTAG_LEN, &dataChunk);
|
|
mgmtMsgLog->wordTag = (uint8_t)(dataChunk);
|
|
|
|
FrameGetChunk(mgmtMsg, TK_MSG_INI_TAG_OFFSET, TK_MSG_INI_TAG_LEN, &dataChunk);
|
|
mgmtMsgLog->msgTag = (uint8_t)(dataChunk);
|
|
|
|
FrameGetChunk(mgmtMsg, TK_MSG_INI_STAG_OFFSET, TK_MSG_INI_STAG_LEN, &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)
|
|
{
|
|
mgmtMsgLog->isValid = true;
|
|
}
|
|
else
|
|
{
|
|
mgmtMsgLog->isValid = false;
|
|
}
|
|
break;
|
|
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);
|
|
|
|
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);
|
|
|
|
FrameGetChunk(mgmtMsg, TK_MSG_CONT_1_NORTH_SPEED_OFFSET, TK_MSG_CONT_1_NORTH_SPEED_LEN, &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);
|
|
|
|
FrameGetChunk(mgmtMsg, TK_MSG_CONT_1_HEIGHT_OFFSET, TK_MSG_CONT_1_HEIGHT_LEN, &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);
|
|
|
|
FrameGetChunk(mgmtMsg, TK_MSG_CONT_2_LATITUDE_OFFSET, TK_MSG_CONT_2_LATITUDE_LEN, &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);
|
|
|
|
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))
|
|
{
|
|
FrameGetChunk(mgmtMsg, NQT_MSG_CTRL_MUTE_OFFSET, NQT_MSG_CTRL_MUTE_LEN, &dataChunk);
|
|
mgmtMsgLog->quitCtrlWord = (uint8_t)(dataChunk & 0x7F);
|
|
if (mgmtMsgLog->quitCtrlWord == 0x01)
|
|
{
|
|
L2_DeleteNodeById(instPtr->nodeList, mgmtMsgLog->quitNodeId);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
DBG_WARNING("Ignore node quit request(ID not mine), target node id is [%d]\n", mgmtMsgLog->quitNodeId);
|
|
}
|
|
break;
|
|
default:
|
|
DBG_ERROR("Cannot decapsulate message type [%d] in L2_INTRA_MGMT_FRAME.\n", intraMgmtFrameMsgs[i]);
|
|
Status = EPARAM;
|
|
goto failed;
|
|
}
|
|
filedSize += MGMT_MSG_LEN;
|
|
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)
|
|
{
|
|
L2_NODE_STATE newState = L2_NONE_STATE;
|
|
switch (mgmtMsgLog->nextNetState)
|
|
{
|
|
case NET_INI_STAGE:
|
|
newState = L2_INIT_STATE;
|
|
break;
|
|
case NET_NORMAL_STAGE:
|
|
newState = L2_NORMAL_STATE;
|
|
break;
|
|
case NET_INTRA_SEEK_STAGE:
|
|
newState = L2_INTRA_SEEK_STATE;
|
|
break;
|
|
case NET_INTER_SEEK_STAGE:
|
|
newState = L2_INTER_SEEK_STATE;
|
|
break;
|
|
case NET_SEEK_STAGE:
|
|
newState = L2_SEEK_STATE;
|
|
break;
|
|
default:
|
|
DBG_WARNING("Unknown Head node state [%d].\n", mgmtMsgLog->nextNetState);
|
|
newState = L2_GetMainState(instPtr);
|
|
break;
|
|
}
|
|
L2_EVENT stateUpEvent = {
|
|
.event = L2_EV_MAIN_STATE_UPDATE,
|
|
.msg = &newState};
|
|
AssertL2Event(instPtr, &stateUpEvent);
|
|
}
|
|
|
|
L2_RstNodeTTL(instPtr->nodeList, mgmtMsgLog->srcNodeId);
|
|
// TODO: Check
|
|
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++)
|
|
{
|
|
memcpy(mgmtMsg->words,
|
|
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);
|
|
|
|
FrameGetChunk(mgmtMsg, RTT_REQ_MSG_TAG_OFFSET, RTT_REQ_MSG_TAG_LEN, &dataChunk);
|
|
rttMsgReqLog->msgTag = (uint16_t)(dataChunk);
|
|
|
|
FrameGetChunk(mgmtMsg, RTT_REQ_MSG_STAG_OFFSET, RTT_REQ_MSG_STAG_LEN, &dataChunk);
|
|
rttMsgReqLog->msgSubTag = (uint16_t)(dataChunk);
|
|
|
|
FrameGetChunk(mgmtMsg, RTT_REQ_MSG_OFFSET, RTT_REQ_MSG_LEN, &dataChunk);
|
|
rttMsgReqLog->msgLen = (uint16_t)(dataChunk);
|
|
|
|
FrameGetChunk(mgmtMsg, RTT_REQ_SRC_ADDR_OFFSET, RTT_REQ_SRC_ADDR_LEN, &dataChunk);
|
|
rttMsgReqLog->srcAddr = (uint16_t)(dataChunk);
|
|
|
|
FrameGetChunk(mgmtMsg, RTT_REQ_DST_ADDR_OFFSET, RTT_REQ_DST_ADDR_LEN, &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);
|
|
|
|
FrameGetChunk(mgmtMsg, RTT_REQ_EX_TTAGH_OFFSET, RTT_REQ_EX_TTAGH_LEN, &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);
|
|
|
|
break;
|
|
default:
|
|
DBG_ERROR("Cannot decapsulate message type [%d] in L2_INTRA_TSYNC_FRAME.\n", rttReqFrameMsgs[i]);
|
|
Status = EPARAM;
|
|
goto failed;
|
|
}
|
|
filedSize += RTT_MSG_LEN;
|
|
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++)
|
|
{
|
|
memcpy(mgmtMsg->words,
|
|
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);
|
|
|
|
FrameGetChunk(mgmtMsg, RTT_RSP_MSG_TAG_OFFSET, RTT_RSP_MSG_TAG_LEN, &dataChunk);
|
|
rttMsgRspLog->msgTag = (uint16_t)(dataChunk);
|
|
|
|
FrameGetChunk(mgmtMsg, RTT_RSP_MSG_STAG_OFFSET, RTT_RSP_MSG_STAG_LEN, &dataChunk);
|
|
rttMsgRspLog->msgSubTag = (uint16_t)(dataChunk);
|
|
|
|
FrameGetChunk(mgmtMsg, RTT_RSP_MSG_OFFSET, RTT_RSP_MSG_LEN, &dataChunk);
|
|
rttMsgRspLog->msgLen = (uint16_t)(dataChunk);
|
|
|
|
FrameGetChunk(mgmtMsg, RTT_RSP_SRC_ADDR_OFFSET, RTT_RSP_SRC_ADDR_LEN, &dataChunk);
|
|
rttMsgRspLog->srcAddr = (uint16_t)(dataChunk);
|
|
|
|
FrameGetChunk(mgmtMsg, RTT_RSP_DST_ADDR_OFFSET, RTT_RSP_DST_ADDR_LEN, &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);
|
|
|
|
FrameGetChunk(mgmtMsg, RTT_RSP_EX_TTAGH_OFFSET, RTT_RSP_EX_TTAGH_LEN, &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);
|
|
|
|
break;
|
|
default:
|
|
DBG_ERROR("Cannot decapsulate message type [%d] in L2_INTRA_TSYNC_FRAME.\n", rttRspFrameMsgs[i]);
|
|
Status = EPARAM;
|
|
goto failed;
|
|
}
|
|
filedSize += RTT_MSG_LEN;
|
|
if (filedSize / 8 >= INTRA_RTT_FRAME_LENGTH_MAX)
|
|
{
|
|
DBG_ERROR("Message decapsulate exceeds frame length.\n");
|
|
Status = EPARAM;
|
|
goto failed;
|
|
}
|
|
}
|
|
}
|
|
|
|
L2_RstNodeTTL(instPtr->nodeList, rttMsgRspLog->srcAddr & 0x001F);
|
|
// TODO: Check
|
|
if ((rttMsgRspLog->srcAddr & 0x001F) == L2_GetHeadId(instPtr))
|
|
{
|
|
L2_TimeAlign(instPtr, L2_GetLastFrameNum(instPtr), L2_GetLastSlotNum(instPtr), true);
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
DBG_ERROR("Cannot handle frame type [%d] properly.\n", L2_GetLastFrameType(instPtr));
|
|
Status = EPARAM;
|
|
goto failed;
|
|
}
|
|
bit_array_free(mgmtMsg);
|
|
return SUCCESS;
|
|
failed:
|
|
bit_array_free(mgmtMsg);
|
|
return Status;
|
|
}
|
|
|
|
/**
|
|
* @brief Helper function for check node TTL.
|
|
* @param entry
|
|
* @return trur/false
|
|
*/
|
|
static bool IsTTLValid(NODE_ENTRY *entry)
|
|
{
|
|
ASSERT(entry != NULL);
|
|
return (entry->ttlCnt <= CONX_LOST_TH);
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Management net FSM routine.
|
|
*
|
|
* @param mgmtFSM
|
|
* mainState
|
|
*
|
|
* @return Slot has be used.
|
|
*
|
|
******************************************************************************/
|
|
int32_t L2_Mgmt_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)
|
|
{
|
|
return SUCCESS;
|
|
}
|
|
|
|
// Check if there is any node exceeds the ttl threshold.
|
|
L2_IterDeleteNode(instPtr->nodeList, IsTTLValid);
|
|
|
|
// Determine next slot TOR state.
|
|
L2_TOR_TYPE torState = L2_TOR_SUSPEND;
|
|
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)))
|
|
{
|
|
torState = L2_TOR_RX;
|
|
}
|
|
else
|
|
{
|
|
torState = L2_TOR_SUSPEND;
|
|
}
|
|
|
|
// 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)
|
|
{
|
|
DBG_ERROR("Failed to initialize rtt event.\n");
|
|
}
|
|
// Schedule rtt update after receive process end.
|
|
Status = L2_ScheduleEventAfter(instPtr, rttRxEvent, INTRA_TSYNC_SLOTS + 1);
|
|
}
|
|
}
|
|
|
|
switch (rtneEntry->slotType)
|
|
{
|
|
case L2_SLOT_INTER_MGMT:
|
|
// 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)
|
|
{
|
|
Status = FAILURE;
|
|
goto routine_err;
|
|
}
|
|
}
|
|
L2_SetupNextSlot(instPtr, torState, L2_INTRA_MGMT_FRAME);
|
|
break;
|
|
case L2_SLOT_INTER_TSYNC:
|
|
// TODO:
|
|
L2_SetupNextSlot(instPtr, torState, L2_INTER_TSYNC_FRAME);
|
|
break;
|
|
case L2_SLOT_INTRA_TSYNC:
|
|
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)
|
|
{
|
|
Status = FAILURE;
|
|
goto routine_err;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (MgmtPkt_Packing(instPtr, MGMTPKT_INTRA_RTTRSP) != SUCCESS)
|
|
{
|
|
Status = FAILURE;
|
|
goto routine_err;
|
|
}
|
|
}
|
|
}
|
|
L2_SetupNextSlot(instPtr, torState, L2_INTRA_TSYNC_FRAME);
|
|
break;
|
|
default:
|
|
DBG_ERROR("UNKNOWN slot type [%d].\n", rtneEntry->slotType);
|
|
Status = FAILURE;
|
|
goto routine_err;
|
|
}
|
|
|
|
/*************************************************/
|
|
// Update link layer state...
|
|
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 - 1)
|
|
{
|
|
newState = L2_INTRA_SEEK_STATE;
|
|
AssertL2Event(instPtr, &stateEvent);
|
|
}
|
|
else
|
|
{
|
|
newState = L2_NORMAL_STATE;
|
|
AssertL2Event(instPtr, &stateEvent);
|
|
}
|
|
}
|
|
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};
|
|
AssertL2Event(instPtr, &stateEvent);
|
|
L2_EVENT lostCxnEvent = {
|
|
.event = L2_EV_LOST_CXN,
|
|
.msg = NULL};
|
|
AssertL2Event(instPtr, &lostCxnEvent);
|
|
DBG_WARNING("Lost connection with head node [%d].\n", L2_GetHeadId(instPtr));
|
|
}
|
|
}
|
|
return SUCCESS;
|
|
routine_err:
|
|
return Status;
|
|
}
|