1009 lines
33 KiB
C
1009 lines
33 KiB
C
/*****************************************************************************/
|
|
/**
|
|
* Description: Implementations of 814 linklayer scan net.
|
|
*
|
|
* Date: 2021-11-10 19:04
|
|
* LastEditors: Fan Hao
|
|
* LastEditTime: 2022-07-16 17:04
|
|
* Revisions: Rev 0.1
|
|
* History:
|
|
* [2022-07-16]: New branch for DiJian.
|
|
* [2021-11-10]: Initial version.
|
|
*
|
|
* Copyright(c) 2021 Beijing Institute of Technology.
|
|
* Lab of Communication and Networking
|
|
*
|
|
******************************************************************************/
|
|
|
|
/***************************** Include Files *********************************/
|
|
#include <string.h>
|
|
#include "linklayer814.h"
|
|
#include "linklayer814_types.h"
|
|
#include "node_list.h"
|
|
#include "msg_def.h"
|
|
#include "../error.h"
|
|
#include "../common.h"
|
|
#include "../bit_array/bit_array.h"
|
|
#include "../bit_array/bit_macros.h"
|
|
|
|
/************************** Constant Definitions *****************************/
|
|
#define INIT_BEAM_NO 0
|
|
#define BEAM_PER_ANT 37
|
|
#define INIT_ANT_NO 0
|
|
#define ANT_NUM 6
|
|
|
|
#define BM_LSTEN_TIMEOUT 40
|
|
|
|
#define SQUENCE_SCAN_SLOTS 0
|
|
|
|
#define INTRA_INIT_SCAN_SLOTS 2
|
|
#define INTRA_BM_ADVT_SLOTS 4
|
|
#define INTRA_WAY2ACQ_SLOTS 1
|
|
|
|
#define INTER_INIT_SCAN_SLOTS 4
|
|
#define INTER_BM_ADVT_SLOTS 20
|
|
#define INTER_WAY2ACQ_SLOTS 2
|
|
|
|
//#define INIT_SCAN_SLOT_MAX 37
|
|
//#define BM_ADVT_SLOT_MAX 2
|
|
#define WAY2_ACQ_SLOT_MAX 120
|
|
|
|
/***************** Macros (Inline Functions) Definitions *********************/
|
|
|
|
/**************************** Type Definitions *******************************/
|
|
typedef enum {
|
|
SCANPKT_NONE = 0,
|
|
SCANPKT_INTER_BM = 1,
|
|
SCANPKT_INTRA_BM = 2,
|
|
SCANPKT_INTER_2WACQ = 3,
|
|
SCANPKT_INTRA_2WACQ = 4
|
|
} SCANPKT_TYPE;
|
|
/************************** Variable Definitions *****************************/
|
|
|
|
/************************** Function Prototypes ******************************/
|
|
static void LockStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state);
|
|
static void BmrecvStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state);
|
|
static void W2ackStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state);
|
|
|
|
static void StageUpdate(L2_SCAN_FSM* scanFsmPtr, SCAN_STAGE newStage);
|
|
//static int32_t Check_W2AcqAck(NODE_ENTRY* nodeEntry);
|
|
static int32_t Scan_InitScanHandler(L2_INST* instPtr);
|
|
static int32_t Scan_BmAdvtHandler(L2_INST* instPtr);
|
|
static int32_t Scan_2WAcqHandler(L2_INST* instPtr);
|
|
static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE msgType);
|
|
static int32_t ScanPkt_Packing(L2_INST* instPtr, SCANPKT_TYPE pktType);
|
|
|
|
/**
|
|
* @brief Update Init-Scan lock state.
|
|
*/
|
|
static inline void LockStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state){
|
|
ASSERT(scanFsmPtr != NULL);
|
|
DBG_INFO("Update BeamLockState from [%d] to [%d].\n",
|
|
scanFsmPtr->isLocked, state);
|
|
scanFsmPtr->isLocked = state;
|
|
return;
|
|
}
|
|
|
|
/**
|
|
* @brief Update Base-info Advertisement state.
|
|
*/
|
|
static void BmrecvStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state){
|
|
ASSERT(scanFsmPtr != NULL);
|
|
DBG_INFO("Update BmRecvState from [%d] to [%d].\n",
|
|
scanFsmPtr->isBmRcved, state);
|
|
scanFsmPtr->isBmRcved = state;
|
|
return;
|
|
}
|
|
|
|
/**
|
|
* @brief Update 2-Way Acquisition ack state.
|
|
*/
|
|
static void W2ackStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state){
|
|
ASSERT(scanFsmPtr != NULL);
|
|
DBG_INFO("Update W2acqAckState from [%d] to [%d].\n",
|
|
scanFsmPtr->isAcked, state);
|
|
scanFsmPtr->isAcked = state;
|
|
return;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
|
|
/**
|
|
* @brief Update current Scan-Net stage
|
|
*
|
|
* @param scanFsmPtr A pointer to scan net fsm.
|
|
* newStage New state.
|
|
*
|
|
* @return NONE.
|
|
*
|
|
******************************************************************************/
|
|
static void StageUpdate(L2_SCAN_FSM* scanFsmPtr, SCAN_STAGE newStage) {
|
|
DBG_INFO("ScanNet current stage: %u -> ScanNet new stage: %u \n",
|
|
scanFsmPtr->scanStage, newStage);
|
|
scanFsmPtr->scanStage = newStage;
|
|
}
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Packing Scan message to a bit array.
|
|
*
|
|
* @param bitarr pointer to bit array buffer.
|
|
* msgType message type.
|
|
*
|
|
* @return EPARAM
|
|
* SUCCESS
|
|
*
|
|
******************************************************************************/
|
|
static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE msgType) {
|
|
ASSERT(bitarr != NULL);
|
|
int32_t Status = SUCCESS;
|
|
if (bitarr->num_of_bits < NS_MSG_LEN || msgType < L2_MSG_BM
|
|
|| msgType > L2_MSG_SCAN_CONT_5) {
|
|
return (EPARAM);
|
|
}
|
|
uint64_t dataChunk = 0;
|
|
memset(bitarr->words, 0xAA, (size_t)(bitarr->num_of_bits / 8));
|
|
switch (msgType) {
|
|
case L2_MSG_BM:
|
|
if (bitarr->num_of_bits < BM_MSG_LEN) {
|
|
return EPARAM;
|
|
}
|
|
/* message type all zero */
|
|
dataChunk = 0;
|
|
FrameInsertChunk(bitarr, BM_MSG_TYPE_OFFSET, BM_MSG_TYPE_LEN, dataChunk);
|
|
|
|
/* source node id */
|
|
dataChunk = (L2_GetNodeId(instPtr) & 0x1F) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
|
|
FrameInsertChunk(bitarr, BM_MSG_SRC_ADDR_OFFSET, BM_MSG_SRC_ADDR_LEN, dataChunk);
|
|
|
|
/* net management node id */
|
|
dataChunk = (L2_GetHeadId(instPtr) & 0x1F) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
|
|
FrameInsertChunk(bitarr, BM_MSG_NETMG_ADDR_OFFSET, BM_MSG_NETMG_ADDR_LEN, dataChunk);
|
|
|
|
/* time unit number */
|
|
dataChunk = L2_GetNextFrameNum(instPtr);
|
|
FrameInsertChunk(bitarr, BM_MSG_TUNIT_NUM_OFFSET, BM_MSG_TUNIT_NUM_LEN, dataChunk);
|
|
|
|
/* frame number */
|
|
dataChunk = L2_GetNextSlotNum(instPtr) / L2_SLOTS_PER_SUBFRAME;
|
|
FrameInsertChunk(bitarr, BM_MSG_TFRAME_NUM_OFFSET, BM_MSG_TFRAME_NUM_LEN, dataChunk);
|
|
|
|
/* mini slot number, 20 mini slots per frame. */
|
|
dataChunk = L2_GetNextSlotNum(instPtr) % L2_SLOTS_PER_SUBFRAME;
|
|
FrameInsertChunk(bitarr, BM_MSG_TSLOT_NUM_OFFSET, BM_MSG_TSLOT_NUM_LEN, dataChunk);
|
|
|
|
/* FIXME : Only for test */
|
|
/* longitude. */
|
|
dataChunk = 203;
|
|
FrameInsertChunk(bitarr, BM_MSG_LONGITUDE_OFFSET, BM_MSG_LONGITUDE_LEN, dataChunk);
|
|
|
|
/* latitude. */
|
|
dataChunk = 589;
|
|
FrameInsertChunk(bitarr, BM_MSG_LATITUDE_OFFSET, BM_MSG_LATITUDE_LEN, dataChunk);
|
|
|
|
/* Height. */
|
|
dataChunk = 589;
|
|
FrameInsertChunk(bitarr, BM_MSG_HEIGHT_OFFSET, BM_MSG_HEIGHT_LEN, dataChunk);
|
|
|
|
/* East. */
|
|
dataChunk = 19;
|
|
FrameInsertChunk(bitarr, BM_MSG_EAST_SPEED_OFFSET, BM_MSG_EAST_SPEED_LEN, dataChunk);
|
|
|
|
/* North. */
|
|
dataChunk = 30;
|
|
FrameInsertChunk(bitarr, BM_MSG_NORTH_SPEED_OFFSET, BM_MSG_NORTH_SPEED_LEN, dataChunk);
|
|
|
|
/* Upper. */
|
|
dataChunk = 69;
|
|
FrameInsertChunk(bitarr, BM_MSG_UPPER_SPEED_OFFSET, BM_MSG_UPPER_SPEED_LEN, dataChunk);
|
|
|
|
/* Append CRC */
|
|
L2_CrcAppend((uint8_t *) bitarr->words, BM_MSG_CRC_OFFSET / 8, BM_MSG_CRC_OFFSET / 8, L2_CRC8_31);
|
|
|
|
break;
|
|
case L2_MSG_SCAN_INIT:
|
|
if (bitarr->num_of_bits < NS_MSG_LEN) {
|
|
return EPARAM;
|
|
}
|
|
/* Sequence tag */
|
|
dataChunk = 0;
|
|
FrameInsertChunk(bitarr, NS_MSG_INI_SER_TAG_OFFSET, NS_MSG_INI_SER_TAG_LEN, dataChunk);
|
|
|
|
/* word tag */
|
|
dataChunk = 3;
|
|
FrameInsertChunk(bitarr, NS_MSG_INI_WTAG_OFFSET, NS_MSG_INI_WTAG_LEN, dataChunk);
|
|
|
|
/* message tag */
|
|
dataChunk = 28;
|
|
FrameInsertChunk(bitarr, NS_MSG_INI_MTAG_OFFSET, NS_MSG_INI_MTAG_LEN, dataChunk);
|
|
|
|
/* message sub tag */
|
|
dataChunk = 0;
|
|
FrameInsertChunk(bitarr, NS_MSG_INI_STAG_OFFSET, NS_MSG_INI_STAG_LEN, dataChunk);
|
|
|
|
/* message length tag */
|
|
dataChunk = 5;
|
|
FrameInsertChunk(bitarr, NS_MSG_INI_LEN_OFFSET, NS_MSG_INI_LEN_LEN, dataChunk);
|
|
|
|
/* message type */
|
|
dataChunk = (L2_GetNodeId(instPtr) == HEAD_NODE) ? 0 : 1;
|
|
FrameInsertChunk(bitarr, NS_MSG_INI_TYPE_OFFSET, NS_MSG_INI_TYPE_LEN, dataChunk);
|
|
|
|
/* source node id */
|
|
dataChunk = (L2_GetNodeId(instPtr) & 0x1F)
|
|
| (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
|
|
FrameInsertChunk(bitarr, NS_MSG_INI_SRC_ADDR_OFFSET, NS_MSG_INI_SRC_ADDR_LEN, dataChunk);
|
|
|
|
/* destination node id */
|
|
dataChunk = (L2_GetNodeId(instPtr) == HEAD_NODE) ?
|
|
(BROADCAST_ID | (BROADCAST_ID << 5))
|
|
: (L2_GetHeadId(instPtr) & 0x1F) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
|
|
FrameInsertChunk(bitarr, NS_MSG_INI_LEN_OFFSET, NS_MSG_INI_LEN_LEN, dataChunk);
|
|
if (L2_GetNodeType(instPtr) == HEAD_NODE) {
|
|
/* current net stage */
|
|
switch (instPtr->mainState) {
|
|
case INIT_STATE:
|
|
dataChunk = NET_INI_STAGE;
|
|
break;
|
|
case SEEK_STATE:
|
|
dataChunk = NET_INTER_SEEK_STAGE;
|
|
break;
|
|
case NORMAL_STATE:
|
|
dataChunk = NET_NORMAL_STAGE;
|
|
break;
|
|
default:
|
|
dataChunk = NET_ERROR_STAGE;
|
|
break;
|
|
}
|
|
FrameInsertChunk(bitarr, NS_MSG_INI_NET_STAGE_OFFSET, NS_MSG_INI_NET_STAGE_LEN, dataChunk);
|
|
|
|
// Begin Scan Time Word
|
|
dataChunk = instPtr->netStartTime;
|
|
FrameInsertChunk(bitarr, NS_MSG_INI_SCAN_TIME_OFFSET, NS_MSG_INI_SCAN_TIME_LEN, dataChunk);
|
|
}
|
|
break;
|
|
case L2_MSG_SCAN_CONT_1:
|
|
if (bitarr->num_of_bits < NS_MSG_LEN) {
|
|
return EPARAM;
|
|
}
|
|
/* word tag */
|
|
dataChunk = 2;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_1_WTAG_OFFSET, NS_MSG_CONT_1_WTAG_LEN, dataChunk);
|
|
|
|
/* Continue word identification. */
|
|
dataChunk = 1;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_1_CONTID_OFFSET, NS_MSG_CONT_1_CONTID_LEN, dataChunk);
|
|
|
|
/* Time tag */
|
|
dataChunk = 0x00FF00FF00FF;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_1_TIME_TAG_OFFSET, NS_MSG_CONT_1_TIME_TAG_LEN, dataChunk);
|
|
|
|
/* East speed */
|
|
dataChunk = 0xF0F0;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_1_EAST_SPEED_OFFSET, NS_MSG_CONT_1_EAST_SPEED_LEN, dataChunk);
|
|
|
|
break;
|
|
case L2_MSG_SCAN_CONT_2:
|
|
if (bitarr->num_of_bits < NS_MSG_LEN) {
|
|
return EPARAM;
|
|
}
|
|
/* word tag */
|
|
dataChunk = 2;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_2_WTAG_OFFSET, NS_MSG_CONT_2_WTAG_LEN, dataChunk);
|
|
|
|
/* Continue word identification. */
|
|
dataChunk = 2;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_2_CONTID_OFFSET, NS_MSG_CONT_2_CONTID_LEN, dataChunk);
|
|
|
|
/* North speed */
|
|
dataChunk = 0x0F0F;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_2_NORTH_SPEED_OFFSET, NS_MSG_CONT_2_NORTH_SPEED_LEN, dataChunk);
|
|
|
|
/* Upper speed */
|
|
dataChunk = 0x0FF0;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_2_UPPER_SPEED_OFFSET, NS_MSG_CONT_2_UPPER_SPEED_LEN, dataChunk);
|
|
|
|
/* Height */
|
|
dataChunk = 0x00FF00;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_2_HEIGHT_OFFSET, NS_MSG_CONT_2_HEIGHT_LEN, dataChunk);
|
|
|
|
break;
|
|
case L2_MSG_SCAN_CONT_3:
|
|
if (bitarr->num_of_bits < NS_MSG_LEN) {
|
|
return EPARAM;
|
|
}
|
|
/* word tag */
|
|
dataChunk = 2;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_3_WTAG_OFFSET, NS_MSG_CONT_3_WTAG_LEN, dataChunk);
|
|
|
|
/* Continue word identification. */
|
|
dataChunk = 3;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_3_CONTID_OFFSET, NS_MSG_CONT_3_CONTID_LEN, dataChunk);
|
|
|
|
/* Longitude */
|
|
dataChunk = 0x00FF00FF;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_3_LONGITUDE_OFFSET, NS_MSG_CONT_3_LONGITUDE_LEN, dataChunk);
|
|
|
|
/* Latitude */
|
|
dataChunk = 0xFF00FF00;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_3_LATITUDE_OFFSET, NS_MSG_CONT_3_LATITUDE_LEN, dataChunk);
|
|
|
|
break;
|
|
case L2_MSG_SCAN_CONT_4:
|
|
if (bitarr->num_of_bits < NS_MSG_LEN) {
|
|
return EPARAM;
|
|
}
|
|
/* word tag */
|
|
dataChunk = 2;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_4_WTAG_OFFSET, NS_MSG_CONT_4_WTAG_LEN, dataChunk);
|
|
|
|
/* Continue word identification. */
|
|
dataChunk = 4;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_4_CONTID_OFFSET, NS_MSG_CONT_4_CONTID_LEN, dataChunk);
|
|
|
|
/* Message key */
|
|
dataChunk = 0xF0F0F0F0F0F0;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_4_KEY_OFFSET, NS_MSG_CONT_4_KEY_LEN, dataChunk);
|
|
break;
|
|
case L2_MSG_SCAN_CONT_5:
|
|
if (bitarr->num_of_bits < NS_MSG_LEN) {
|
|
return EPARAM;
|
|
}
|
|
/* word tag */
|
|
dataChunk = 2;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_5_WTAG_OFFSET, NS_MSG_CONT_5_WTAG_LEN, dataChunk);
|
|
|
|
/* Continue word identification. */
|
|
dataChunk = 5;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_5_CONTID_OFFSET, NS_MSG_CONT_5_CONTID_LEN, dataChunk);
|
|
|
|
/* Message key */
|
|
dataChunk = 0x0F0F0F0F0F;
|
|
FrameInsertChunk(bitarr, NS_MSG_CONT_5_KEY_OFFSET, NS_MSG_CONT_5_KEY_LEN, dataChunk);
|
|
break;
|
|
default:
|
|
Status = EPARAM;
|
|
break;
|
|
}
|
|
return Status;
|
|
}
|
|
|
|
static int32_t ScanPkt_Packing(L2_INST* instPtr, SCANPKT_TYPE pktType){
|
|
uint32_t filedSize = 0;
|
|
uint8_t* buff = instPtr->sendPktData;
|
|
BIT_ARRAY* scanPkt = bit_array_create(L2_MSG_MAX);
|
|
if (scanPkt == NULL) {
|
|
DBG_ERROR("Failed to allocate memory for scanMsg.\n");
|
|
return NUM_SLOTS_NONE;
|
|
}
|
|
switch (pktType) {
|
|
case SCANPKT_INTRA_BM:
|
|
if (ScanMsg_Packing(instPtr, scanPkt, L2_MSG_BM) != SUCCESS) {
|
|
DBG_ERROR("Failed to packing L2_MSG_BM frame.\n");
|
|
goto failed;
|
|
}
|
|
memcpy(buff, scanPkt->words, (size_t)(BM_MSG_LEN / 8));
|
|
break;
|
|
case SCANPKT_INTRA_2WACQ:
|
|
for (int i = 0; i < ARRAY_SIZE(intraW2AcqFrameMsgs); i++) {
|
|
if (ScanMsg_Packing(instPtr, scanPkt, intraW2AcqFrameMsgs[i]) == SUCCESS) {
|
|
memcpy((uint8_t*)buff + (size_t)(i * (NS_MSG_LEN / 8)),
|
|
scanPkt->words, (size_t)(NS_MSG_LEN / 8));
|
|
}
|
|
else {
|
|
DBG_ERROR("Failed to packing intraW2AcqFrameMsgs[%d].\n", i);
|
|
goto failed;
|
|
}
|
|
filedSize += NS_MSG_LEN;
|
|
if (filedSize / 8 >= INTRA_W2ACQ_FRAME_LENGTH_MAX) {
|
|
DBG_ERROR("Message is too long to be properly packed.\n");
|
|
goto failed;
|
|
}
|
|
}
|
|
L2_CrcAppend(buff, W2ACQ_FRAME_CRC_DLEN, W2ACQ_FRAME_CRC_POS, L2_CRC16_8005);
|
|
break;
|
|
default:
|
|
DBG_ERROR("Scan Packet type [%d] is not surpported yet.\n", pktType);
|
|
goto failed;
|
|
}
|
|
bit_array_free(scanPkt);
|
|
return SUCCESS;
|
|
failed:
|
|
bit_array_free(scanPkt);
|
|
return FAILURE;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Member node scan net InitScan stage handler.
|
|
*
|
|
* @param instPtr A pointer to l2 instance
|
|
*
|
|
* @return used slot (5ms slot).
|
|
*
|
|
******************************************************************************/
|
|
static int32_t Scan_InitScanHandler(L2_INST* instPtr) {
|
|
ASSERT(instPtr != NULL);
|
|
L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
|
|
DBG_INFO("** Netsetup - InitScan Procedure (Attempts: %u)**\n", scanFsmPtr->scanCnt);
|
|
|
|
if(scanFsmPtr->scanMode == INTRA_CLUSTER_MODE){
|
|
if (scanFsmPtr->isLocked == true) {
|
|
StageUpdate(scanFsmPtr, BM_ADVT_STAGE);
|
|
/* Antenna setting (reserved.) */
|
|
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_BM_FRAME);
|
|
} /* Otherwise remain INIT_SCAN_STAGE */
|
|
else {
|
|
/* Antenna setting (reserved.) */
|
|
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_SCAN_FRAME);
|
|
scanFsmPtr->scanCnt++;
|
|
}
|
|
}
|
|
else if(scanFsmPtr->scanMode == INTER_CLUSTER_MODE){
|
|
if (scanFsmPtr->isLocked == true) {
|
|
StageUpdate(scanFsmPtr, BM_ADVT_STAGE);
|
|
/* Antenna setting (reserved.) */
|
|
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTER_BM_FRAME);
|
|
} /* Otherwise remain INIT_SCAN_STAGE */
|
|
else {
|
|
/* Antenna setting (reserved.) */
|
|
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTER_SCAN_FRAME);
|
|
scanFsmPtr->scanCnt++;
|
|
}
|
|
}
|
|
else{
|
|
DBG_ERROR("UNKNOWN Scan Mode: %d.\n", scanFsmPtr->scanMode);
|
|
return NUM_SLOTS_NONE;
|
|
}
|
|
return (NUM_SLOTS_SKIP);
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
|
|
/**
|
|
* @brief Member node scan net BmAdvt stage handler.
|
|
*
|
|
* @param scanFsmPtr A pointer to scan net fsm.
|
|
* mainState A pointer to main node state.
|
|
*
|
|
* @return used slot (5ms slot).
|
|
*
|
|
******************************************************************************/
|
|
static int32_t Scan_BmAdvtHandler(L2_INST* instPtr) {
|
|
L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
|
|
DBG_INFO("** Netsetup - BmAdvt Procedure (Timeout: %u) **\n", scanFsmPtr->bmTimeout);
|
|
|
|
// if received a valid bm frame.
|
|
if (scanFsmPtr->isBmRcved == true) {
|
|
scanFsmPtr->bmTimeout = 0; /* reset listen counter. */
|
|
DBG_INFO("Received Bm frame, skipping 10ms for turn around...\n");
|
|
// TODO: check functionality.
|
|
StageUpdate(scanFsmPtr, WAY2_ACQ_STAGE);
|
|
return NUM_SLOTS_10MS;
|
|
}
|
|
// no valid bm frame was received.
|
|
else {
|
|
/* if time out reached, go back to init scan stage. */
|
|
if (scanFsmPtr->bmTimeout > BM_LSTEN_TIMEOUT) {
|
|
if(scanFsmPtr->scanMode == INTRA_CLUSTER_MODE){
|
|
/* Antenna setting (reserved.) */
|
|
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_SCAN_FRAME);
|
|
}
|
|
else if(scanFsmPtr->scanMode == INTER_CLUSTER_MODE){
|
|
/* Antenna setting (reserved.) */
|
|
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTER_SCAN_FRAME);
|
|
}
|
|
else{
|
|
DBG_ERROR("UNKNOWN Scan Mode: %d.\n", scanFsmPtr->scanMode);
|
|
return NUM_SLOTS_NONE;
|
|
}
|
|
DBG_WARNING("BM timeout, restart net scan from INIT_SCAN_STAGE.\n");
|
|
scanFsmPtr->bmTimeout = 0; /* reset listen counter. */
|
|
StageUpdate(scanFsmPtr, INIT_SCAN_STAGE);
|
|
}
|
|
else {
|
|
if(scanFsmPtr->scanMode == INTRA_CLUSTER_MODE){
|
|
/* Antenna setting (reserved.) */
|
|
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_BM_FRAME);
|
|
}
|
|
else if(scanFsmPtr->scanMode == INTER_CLUSTER_MODE){
|
|
/* Antenna setting (reserved.) */
|
|
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTER_BM_FRAME);
|
|
}
|
|
else{
|
|
DBG_ERROR("UNKNOWN Scan Mode: %d.\n", scanFsmPtr->scanMode);
|
|
return NUM_SLOTS_NONE;
|
|
}
|
|
scanFsmPtr->bmTimeout++;
|
|
}
|
|
return NUM_SLOTS_SKIP;
|
|
}
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Scan net 2Way-Acq stage handler.
|
|
*
|
|
* @param instPtr a pointer to linklayer layer instance
|
|
*
|
|
* @return used slot (5ms slot).
|
|
*
|
|
******************************************************************************/
|
|
static int32_t Scan_2WAcqHandler (L2_INST* instPtr){
|
|
L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
|
|
DBG_INFO("** Netsetup - 2WAcq Procedure (Timeout: %u) **\n", scanFsmPtr->w2acqTimeout);
|
|
L2_FRAME_TYPE frameType = L2_NONE_FRAME;
|
|
|
|
if(scanFsmPtr->scanMode == INTRA_CLUSTER_MODE){
|
|
frameType = L2_INTRA_W2_ACQ_FRAME;
|
|
}
|
|
else if(scanFsmPtr->scanMode == INTER_CLUSTER_MODE){
|
|
frameType = L2_INTER_W2_ACQ_FRAME;
|
|
}
|
|
else{
|
|
DBG_ERROR("UNKNOWN Scan Mode: %d.\n", scanFsmPtr->scanMode);
|
|
return FAILURE;
|
|
}
|
|
|
|
// 2 way acq failed, back to init stage.
|
|
if (scanFsmPtr->w2acqTimeout >= WAY2_ACQ_SLOT_MAX) {
|
|
DBG_WARNING("2WAcq timeout, restart net scan from INIT_SCAN_STAGE.\n");
|
|
scanFsmPtr->w2acqTimeout = 0; // reset counter for next scan.
|
|
StageUpdate(scanFsmPtr, INIT_SCAN_STAGE);
|
|
}
|
|
else {
|
|
scanFsmPtr->w2acqTimeout++; // increase counter.
|
|
}
|
|
L2_SetupNextSlot(instPtr, L2_TOR_RX, frameType);
|
|
return SUCCESS;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
|
|
/**
|
|
* @brief Scan net packet handler.
|
|
*
|
|
* @param instPtr a pointer to linklayer layer instance
|
|
*
|
|
* @return SUCCESS
|
|
* error code
|
|
*
|
|
******************************************************************************/
|
|
int32_t ScanNet_PktHandler(L2_INST* instPtr) {
|
|
ASSERT(instPtr != NULL);
|
|
uint32_t Status;
|
|
L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
|
|
uint64_t dataChunk;
|
|
BIT_ARRAY* scanMsg = bit_array_create(L2_MSG_MAX);
|
|
if (scanMsg == NULL) {
|
|
free(scanMsg);
|
|
return EALLOC;
|
|
}
|
|
switch (L2_GetLastFrameType(instPtr)) {
|
|
case L2_INTER_SCAN_FRAME:
|
|
case L2_INTRA_SCAN_FRAME:
|
|
// refer to "linklayer814.c/L2_FrameStateUpdate"
|
|
// INTRA_BM_FRAME could be received if current frame type is INTRA_BM_FRAME
|
|
// in net setup stage(for member node).
|
|
// When SCAN frame was considered as received, handle the frame as BM frame.
|
|
break;
|
|
case L2_INTER_BM_FRAME:
|
|
case L2_INTRA_BM_FRAME: {
|
|
for (int i = 0; i < ARRAY_SIZE(intraBmFrameMsgs); i++) {
|
|
memcpy(scanMsg->words,
|
|
instPtr->recvPktData + (size_t)(i * (BM_MSG_LEN / 8)),
|
|
(size_t)(BM_MSG_LEN / 8));
|
|
switch (intraBmFrameMsgs[i]) {
|
|
case L2_MSG_BM:
|
|
FrameGetChunk(scanMsg, BM_MSG_SRC_ADDR_OFFSET, BM_MSG_SRC_ADDR_LEN, &dataChunk);
|
|
scanFsmPtr->BMrcvLog.NodeId = dataChunk & 0x000000000000001F;
|
|
scanFsmPtr->BMrcvLog.ClusterId = (dataChunk>>5) & 0x000000000000001F;
|
|
|
|
FrameGetChunk(scanMsg, BM_MSG_TUNIT_NUM_OFFSET, BM_MSG_TUNIT_NUM_LEN, &dataChunk);
|
|
scanFsmPtr->BMrcvLog.frameNum = (uint32_t) dataChunk;
|
|
|
|
FrameGetChunk(scanMsg, BM_MSG_TFRAME_NUM_OFFSET, BM_MSG_TFRAME_NUM_LEN, &dataChunk);
|
|
scanFsmPtr->BMrcvLog.slotNum = (uint32_t)(dataChunk * L2_SLOTS_PER_SUBFRAME);
|
|
|
|
FrameGetChunk(scanMsg, BM_MSG_TSLOT_NUM_OFFSET, BM_MSG_TSLOT_NUM_LEN, &dataChunk);
|
|
scanFsmPtr->BMrcvLog.slotNum = scanFsmPtr->BMrcvLog.slotNum + (uint32_t) dataChunk;
|
|
|
|
FrameGetChunk(scanMsg, BM_MSG_LONGITUDE_OFFSET, BM_MSG_LONGITUDE_LEN, &dataChunk);
|
|
scanFsmPtr->BMrcvLog.Longitude = (uint32_t)(dataChunk);
|
|
|
|
FrameGetChunk(scanMsg, BM_MSG_LATITUDE_OFFSET, BM_MSG_LATITUDE_LEN, &dataChunk);
|
|
scanFsmPtr->BMrcvLog.Latitude = (uint32_t)(dataChunk);
|
|
|
|
FrameGetChunk(scanMsg, BM_MSG_HEIGHT_OFFSET, BM_MSG_HEIGHT_LEN, &dataChunk);
|
|
scanFsmPtr->BMrcvLog.Height = (uint32_t)(dataChunk);
|
|
|
|
FrameGetChunk(scanMsg, BM_MSG_EAST_SPEED_OFFSET, BM_MSG_EAST_SPEED_LEN, &dataChunk);
|
|
scanFsmPtr->BMrcvLog.EastSpeed = (uint32_t)(dataChunk);
|
|
|
|
FrameGetChunk(scanMsg, BM_MSG_NORTH_SPEED_OFFSET, BM_MSG_NORTH_SPEED_LEN, &dataChunk);
|
|
scanFsmPtr->BMrcvLog.NorthSpeed = (uint32_t)(dataChunk);
|
|
|
|
FrameGetChunk(scanMsg, BM_MSG_UPPER_SPEED_OFFSET, BM_MSG_UPPER_SPEED_LEN, &dataChunk);
|
|
scanFsmPtr->BMrcvLog.UpperSpeed = (uint32_t)(dataChunk);
|
|
break;
|
|
default:
|
|
DBG_ERROR("UNSURPPORTED message type [%d]. \n", intraBmFrameMsgs[i]);
|
|
Status = EIGNORE;
|
|
goto failed;
|
|
}
|
|
}
|
|
if(L2_GetNodeType(instPtr) != HEAD_NODE){
|
|
if (IsNodeInList(instPtr->nodeList, scanFsmPtr->BMrcvLog.NodeId) == false) {
|
|
AppendNode(instPtr->nodeList, scanFsmPtr->BMrcvLog.NodeId, (uint8_t) MEMBER_NODE);
|
|
}
|
|
// TODO: Update Node Info
|
|
|
|
if ( L2_isCxn2Net(instPtr) == false ) {
|
|
// TODO: Check
|
|
L2_TimeAlign(instPtr, scanFsmPtr->BMrcvLog.frameNum, scanFsmPtr->BMrcvLog.slotNum);
|
|
}
|
|
// trigger flag
|
|
BmrecvStateUpdate(scanFsmPtr, true);
|
|
}
|
|
else{
|
|
DBG_ERROR("Node type [%d] is not supposed to receive BM frame", L2_GetNodeType(instPtr));
|
|
Status = EUNEXP;
|
|
goto failed;
|
|
}
|
|
break;
|
|
}
|
|
case L2_INTER_W2_ACQ_FRAME:
|
|
case L2_INTRA_W2_ACQ_FRAME: {
|
|
for (int i = 0; i < ARRAY_SIZE(intraW2AcqFrameMsgs); i++) {
|
|
memcpy(scanMsg->words,
|
|
instPtr->recvPktData + (size_t)(i * (NS_MSG_LEN / 8)),
|
|
(size_t)(NS_MSG_LEN / 8));
|
|
switch (intraW2AcqFrameMsgs[i]) {
|
|
case L2_MSG_SCAN_INIT:
|
|
FrameGetChunk(scanMsg, NS_MSG_INI_SRC_ADDR_OFFSET, NS_MSG_INI_SRC_ADDR_LEN, &dataChunk);
|
|
scanFsmPtr->W2ACQrcvLog.NodeId = dataChunk & 0x001F;
|
|
scanFsmPtr->W2ACQrcvLog.ClusterId = (dataChunk>>5) & 0x0001F;
|
|
|
|
FrameGetChunk(scanMsg, NS_MSG_INI_NET_STAGE_OFFSET, NS_MSG_INI_NET_STAGE_LEN, &dataChunk);
|
|
switch (dataChunk) {
|
|
case NET_INI_STAGE:
|
|
scanFsmPtr->W2ACQrcvLog.NetStage = INIT_STATE;
|
|
break;
|
|
case NET_NORMAL_STAGE:
|
|
scanFsmPtr->W2ACQrcvLog.NetStage = NORMAL_STATE;
|
|
break;
|
|
case NET_INTRA_SEEK_STAGE:
|
|
scanFsmPtr->W2ACQrcvLog.NetStage = INTRA_SEEK_STATE;
|
|
break;
|
|
case NET_INTER_SEEK_STAGE:
|
|
scanFsmPtr->W2ACQrcvLog.NetStage = INTER_SEEK_STATE;
|
|
break;
|
|
case NET_SEEK_STAGE:
|
|
scanFsmPtr->W2ACQrcvLog.NetStage = SEEK_STATE;
|
|
break;
|
|
default:
|
|
scanFsmPtr->W2ACQrcvLog.NetStage = NONE_STATE;
|
|
DBG_WARNING("UNEXPECTED head node state.");
|
|
break;
|
|
}
|
|
|
|
FrameGetChunk(scanMsg, NS_MSG_INI_SCAN_TIME_OFFSET, NS_MSG_INI_SCAN_TIME_LEN, &dataChunk);
|
|
scanFsmPtr->W2ACQrcvLog.NetScanTime = (uint32_t)(dataChunk);
|
|
break;
|
|
case L2_MSG_SCAN_CONT_1:
|
|
FrameGetChunk(scanMsg, NS_MSG_CONT_1_EAST_SPEED_OFFSET, NS_MSG_CONT_1_EAST_SPEED_LEN, &dataChunk);
|
|
scanFsmPtr->W2ACQrcvLog.EastSpeed = (uint16_t)(dataChunk);
|
|
break;
|
|
case L2_MSG_SCAN_CONT_2:
|
|
FrameGetChunk(scanMsg, NS_MSG_CONT_2_NORTH_SPEED_OFFSET, NS_MSG_CONT_2_NORTH_SPEED_LEN, &dataChunk);
|
|
scanFsmPtr->W2ACQrcvLog.NorthSpeed = (uint16_t)(dataChunk);
|
|
|
|
FrameGetChunk(scanMsg, NS_MSG_CONT_2_UPPER_SPEED_OFFSET, NS_MSG_CONT_2_UPPER_SPEED_LEN, &dataChunk);
|
|
scanFsmPtr->W2ACQrcvLog.UpperSpeed = (uint16_t)(dataChunk);
|
|
|
|
FrameGetChunk(scanMsg, NS_MSG_CONT_2_HEIGHT_OFFSET, NS_MSG_CONT_2_HEIGHT_LEN, &dataChunk);
|
|
scanFsmPtr->W2ACQrcvLog.Height = (uint32_t)(dataChunk);
|
|
break;
|
|
case L2_MSG_SCAN_CONT_3:
|
|
FrameGetChunk(scanMsg, NS_MSG_CONT_3_LONGITUDE_OFFSET, NS_MSG_CONT_3_LONGITUDE_LEN, &dataChunk);
|
|
scanFsmPtr->W2ACQrcvLog.Longitude = (uint32_t)(dataChunk);
|
|
|
|
FrameGetChunk(scanMsg, NS_MSG_CONT_3_LATITUDE_OFFSET, NS_MSG_CONT_3_LATITUDE_LEN, &dataChunk);
|
|
scanFsmPtr->W2ACQrcvLog.Latitude = (uint32_t)(dataChunk);
|
|
break;
|
|
case L2_MSG_SCAN_CONT_4:
|
|
break;
|
|
case L2_MSG_SCAN_CONT_5:
|
|
break;
|
|
default:
|
|
DBG_ERROR("UNSURPPORTED message type [%d]. \n", intraW2AcqFrameMsgs[i]);
|
|
Status = EIGNORE;
|
|
goto failed;
|
|
}
|
|
}
|
|
// Update net start time
|
|
if(L2_GetNodeType(instPtr) != HEAD_NODE){
|
|
instPtr->netStartTime = scanFsmPtr->W2ACQrcvLog.NetScanTime;
|
|
}
|
|
|
|
if(L2_GetNodeType(instPtr) == HEAD_NODE){
|
|
if (IsNodeInList(instPtr->nodeList, scanFsmPtr->W2ACQrcvLog.NodeId) == false) {
|
|
AppendNode(instPtr->nodeList, scanFsmPtr->W2ACQrcvLog.NodeId, (uint8_t) MEMBER_NODE);
|
|
}
|
|
// TODO: Update Node Info
|
|
}
|
|
|
|
if(L2_GetNodeType(instPtr) != HEAD_NODE && scanFsmPtr->W2ACQrcvLog.NetStage != NONE_STATE){
|
|
// sync state with head node
|
|
L2_NODE_STATE newState = scanFsmPtr->W2ACQrcvLog.NetStage;
|
|
L2_EVENT stateUpEvent = {
|
|
.event = L2_EV_MAIN_STATE_UPDATE,
|
|
.msg = &newState
|
|
};
|
|
AssertL2Event(instPtr, &stateUpEvent);
|
|
|
|
// check if net setup finished.
|
|
if(L2_GetMainState(instPtr) == INIT_STATE && L2_isCxn2Net(instPtr) == false){
|
|
L2_RTNE_CONFIG newRtneConfig = {0};
|
|
L2_EVENT rtneUpEvent = {
|
|
.event = L2_ROUTINE_UPDATE,
|
|
.msg = &newRtneConfig
|
|
};
|
|
if(scanFsmPtr->W2ACQrcvLog.NetStage == INIT_STATE){
|
|
newRtneConfig.rtneTable = initTable;
|
|
newRtneConfig.rtnePrd = ARRAY_SIZE(initTable);
|
|
AssertL2Event(instPtr, &rtneUpEvent);
|
|
}
|
|
else if(scanFsmPtr->W2ACQrcvLog.NetStage == INTRA_SEEK_STATE){
|
|
newRtneConfig.rtneTable = intraSeekTable;
|
|
newRtneConfig.rtnePrd = ARRAY_SIZE(intraSeekTable);
|
|
AssertL2Event(instPtr, &rtneUpEvent);
|
|
}
|
|
L2_EVENT netSetupDoneEvent = {
|
|
.event = L2_EV_NET_SETUP_DONE,
|
|
.msg = NULL
|
|
};
|
|
AssertL2Event(instPtr, &netSetupDoneEvent);
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
default: {
|
|
DBG_ERROR("Cannot handler message type [%d] properly.\n", L2_GetLastFrameType(instPtr));
|
|
Status = EIGNORE;
|
|
goto failed;
|
|
}
|
|
}
|
|
bit_array_free(scanMsg);
|
|
return SUCCESS;
|
|
failed:
|
|
bit_array_free(scanMsg);
|
|
return Status;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Initialize ScanNet FSM, allocate memory for FSM instance.
|
|
*
|
|
* @param scanFsmPtr a pointer to scan net finite state machine
|
|
*
|
|
* @return SUCCESS Initialization successful.
|
|
* EMAC_MEMORY_ALLOC_ERROR Memory allocate error.
|
|
*
|
|
******************************************************************************/
|
|
int32_t ScanNet_Init(L2_SCAN_FSM** scanFSMPtr) {
|
|
*scanFSMPtr = (L2_SCAN_FSM *) malloc(sizeof(L2_SCAN_FSM));
|
|
if (*scanFSMPtr == NULL) {
|
|
DBG_ERROR("Failed to allocate memory for scanFsmPtr.\n");
|
|
return EALLOC;
|
|
}
|
|
ScanNet_Reset(*scanFSMPtr);
|
|
return (SUCCESS);
|
|
}
|
|
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Reset ScanNet FSM
|
|
*
|
|
* @param scanFsmPtr a pointer to scan net finite state machine
|
|
*
|
|
* @return NONE
|
|
*
|
|
******************************************************************************/
|
|
void ScanNet_Reset(L2_SCAN_FSM* scanFSMPtr) {
|
|
ASSERT(scanFSMPtr != NULL);
|
|
scanFSMPtr->isLocked = false;
|
|
scanFSMPtr->isBmRcved = false;
|
|
scanFSMPtr->isAcked = false;
|
|
scanFSMPtr->scanCnt = 0;
|
|
scanFSMPtr->bmTimeout = 0;
|
|
scanFSMPtr->w2acqTimeout = 0;
|
|
scanFSMPtr->scanStage = NONE_STAGE;
|
|
scanFSMPtr->scanMode = INTRA_CLUSTER_MODE;
|
|
// FIXME: Only for test
|
|
scanFSMPtr->BMrcvLog.Height = 0;
|
|
scanFSMPtr->BMrcvLog.Latitude = 0;
|
|
scanFSMPtr->BMrcvLog.Longitude = 0;
|
|
scanFSMPtr->BMrcvLog.EastSpeed = 0;
|
|
scanFSMPtr->BMrcvLog.NorthSpeed = 0;
|
|
scanFSMPtr->BMrcvLog.UpperSpeed = 0;
|
|
scanFSMPtr->BMrcvLog.frameNum = 0;
|
|
scanFSMPtr->BMrcvLog.slotNum = 0;
|
|
scanFSMPtr->W2ACQrcvLog.Height = 0;
|
|
scanFSMPtr->W2ACQrcvLog.Latitude = 0;
|
|
scanFSMPtr->W2ACQrcvLog.Longitude = 0;
|
|
scanFSMPtr->W2ACQrcvLog.EastSpeed = 0;
|
|
scanFSMPtr->W2ACQrcvLog.NorthSpeed = 0;
|
|
scanFSMPtr->W2ACQrcvLog.UpperSpeed = 0;
|
|
scanFSMPtr->W2ACQrcvLog.NetStage = NONE_STATE;
|
|
scanFSMPtr->W2ACQrcvLog.NetScanTime = 0;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Destroy ScanNet FSM, free memory
|
|
*
|
|
* @param scanFsmPtr a pointer to scan net finite state machine
|
|
*
|
|
* @return NONE
|
|
*
|
|
******************************************************************************/
|
|
void ScanNet_Destroy(L2_SCAN_FSM* scanFSMPtr) {
|
|
free(scanFSMPtr);
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Execute a member node scan net procedure.
|
|
*
|
|
* @param instPtr a pointer to linklayer layer instance
|
|
*
|
|
* @return used slot (5ms slot).
|
|
*
|
|
******************************************************************************/
|
|
int32_t ScanNet_Proc(L2_INST* instPtr) {
|
|
L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
|
|
if (L2_GetNodeType(instPtr) == HEAD_NODE) {
|
|
DBG_ERROR("Only member node needs to execute ScanNet_Proc. \n");
|
|
return NUM_SLOTS_NONE;
|
|
}
|
|
int32_t Status;
|
|
DBG_INFO("-------- ScanNet Process --------\n");
|
|
switch (scanFsmPtr->scanStage) {
|
|
case NONE_STAGE:
|
|
/* Start Scan. */
|
|
StageUpdate(scanFsmPtr, INIT_SCAN_STAGE);
|
|
Status = Scan_InitScanHandler(instPtr);
|
|
break;
|
|
case INIT_SCAN_STAGE:
|
|
Status = Scan_InitScanHandler(instPtr);
|
|
break;
|
|
case BM_ADVT_STAGE:
|
|
Status = Scan_BmAdvtHandler(instPtr);
|
|
break;
|
|
case WAY2_ACQ_STAGE:
|
|
Status = Scan_2WAcqHandler(instPtr);
|
|
break;
|
|
default:
|
|
DBG_ERROR("Unexpected scan net stage: %d.\n", scanFsmPtr->scanStage);
|
|
Status = FAILURE;
|
|
break;
|
|
}
|
|
return Status;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
|
|
/**
|
|
* @brief Execute a scan net Routine procedure.
|
|
*
|
|
* @param rtneEntry Routine configuration
|
|
*
|
|
* @return used slot (5ms slot).
|
|
*
|
|
******************************************************************************/
|
|
int acqSend = 0;
|
|
int32_t ScanNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry) {
|
|
ASSERT(instPtr != NULL);
|
|
L2_TOR_TYPE torState;
|
|
int32_t Status;
|
|
DBG_INFO("-------- ScanNet Routine --------\n");
|
|
|
|
if(rtneEntry->slotSeq != 0){
|
|
return SQUENCE_SCAN_SLOTS;
|
|
}
|
|
|
|
if(L2_GetNodeType(instPtr) != HEAD_NODE){
|
|
if (rtneEntry->dst0 == L2_GetNodeId(instPtr) || rtneEntry->dst0 == BROADCAST_ID){
|
|
torState = L2_TOR_RX;
|
|
}
|
|
else if (rtneEntry->src0 == L2_GetNodeId(instPtr) || rtneEntry->src0 == BROADCAST_ID){
|
|
torState = L2_TOR_TX;
|
|
}
|
|
else {
|
|
torState = L2_TOR_SUSPEND;
|
|
}
|
|
}
|
|
else{
|
|
if (rtneEntry->dst0 == L2_GetNodeId(instPtr)){
|
|
torState = L2_TOR_RX;
|
|
}
|
|
else if (rtneEntry->src0 == L2_GetNodeId(instPtr)){
|
|
torState = L2_TOR_TX;
|
|
}
|
|
else {
|
|
torState = L2_TOR_SUSPEND;
|
|
}
|
|
}
|
|
|
|
switch (rtneEntry->slotType) {
|
|
case SLOT_INTER_SCAN_ACQ:
|
|
// TODO:
|
|
L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_INTER_SCAN_FRAME);
|
|
break;
|
|
|
|
case SLOT_INTRA_SCAN_ACQ:
|
|
if (L2_GetNodeType(instPtr) != HEAD_NODE && torState == L2_TOR_TX) {
|
|
DBG_ERROR("Only head node can send init-scan Frame.\n");
|
|
Status = FAILURE;
|
|
goto routine_err;
|
|
}
|
|
L2_SetupNextSlot(instPtr, torState, L2_INTRA_SCAN_FRAME);
|
|
break;
|
|
|
|
case SLOT_INTER_SCAN_BM:
|
|
// TODO:
|
|
L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_INTER_BM_FRAME);
|
|
break;
|
|
|
|
case SLOT_INTRA_SCAN_BM:
|
|
if (torState == L2_TOR_TX) {
|
|
if (L2_GetNodeType(instPtr) != HEAD_NODE) {
|
|
DBG_ERROR("Only head node can send BM Frame.\n");
|
|
Status = FAILURE;
|
|
goto routine_err;
|
|
}
|
|
memset(instPtr->sendPktData, 0xAA, INTRA_BM_FRAME_LENGTH_MAX);
|
|
if(ScanPkt_Packing(instPtr, SCANPKT_INTRA_BM) != SUCCESS) {
|
|
Status = FAILURE;
|
|
goto routine_err;
|
|
}
|
|
}
|
|
/* Antenna setting (reserved.) */
|
|
L2_SetupNextSlot(instPtr, L2_TOR_TX, L2_INTRA_BM_FRAME);
|
|
break;
|
|
case SLOT_INTER_SCAN_2W:
|
|
// TODO:
|
|
L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_INTER_W2_ACQ_FRAME);
|
|
break;
|
|
case SLOT_INTRA_SCAN_2W:
|
|
if (torState == L2_TOR_TX) {
|
|
if(L2_GetNodeType(instPtr) != HEAD_NODE && L2_isDataAvail(instPtr) == false) {
|
|
/* Antenna setting (reserved.) */
|
|
L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_INTRA_W2_ACQ_FRAME);
|
|
break;
|
|
}
|
|
/* Prepare Request/ACK data. */
|
|
memset(instPtr->sendPktData, 0xAA, (size_t) INTRA_W2ACQ_FRAME_LENGTH_MAX);
|
|
/* Pack message words */
|
|
if(ScanPkt_Packing(instPtr, SCANPKT_INTRA_2WACQ) != SUCCESS) {
|
|
Status = FAILURE;
|
|
goto routine_err;
|
|
}
|
|
W2ackStateUpdate(instPtr->scanNetFSM, true);
|
|
acqSend ++;
|
|
}
|
|
/* Antenna setting (reserved.) */
|
|
L2_SetupNextSlot(instPtr, torState, L2_INTRA_W2_ACQ_FRAME);
|
|
break;
|
|
default:
|
|
DBG_ERROR("Cannot start a routine for slot type [%d] properly.\n", rtneEntry->slotType);
|
|
Status = FAILURE;
|
|
goto routine_err;
|
|
}
|
|
return SUCCESS;
|
|
routine_err:
|
|
return Status;
|
|
}
|
|
|