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_scan.c
+259 -172
View File
@@ -30,28 +30,29 @@
#define INIT_BEAM_NO 0
#define BEAM_PER_ANT 37
#define INIT_ANT_NO 0
#define ANT_NUM 6
#define ANT_NUM 6
#define BM_LSTEN_TIMEOUT 40
#define SQUENCE_SCAN_SLOTS 0
#define SQUENCE_SCAN_SLOTS 0
#define INTRA_INIT_SCAN_SLOTS 2
#define INTRA_BM_ADVT_SLOTS 4
#define INTRA_WAY2ACQ_SLOTS 1
#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 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
// #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 {
typedef enum
{
SCANPKT_NONE = 0,
SCANPKT_INTER_BM = 1,
SCANPKT_INTRA_BM = 2,
@@ -61,25 +62,26 @@ typedef enum {
/************************** 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 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);
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){
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);
scanFsmPtr->isLocked = state;
return;
}
@@ -87,10 +89,11 @@ static inline void LockStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state){
/**
* @brief Update Base-info Advertisement state.
*/
static void BmrecvStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool 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);
scanFsmPtr->isBmRcved = state;
return;
}
@@ -98,10 +101,11 @@ static void BmrecvStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state){
/**
* @brief Update 2-Way Acquisition ack state.
*/
static void W2ackStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool 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);
scanFsmPtr->isAcked = state;
return;
}
@@ -117,9 +121,10 @@ static void W2ackStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state){
* @return NONE.
*
******************************************************************************/
static void StageUpdate(L2_SCAN_FSM* scanFsmPtr, SCAN_STAGE newStage) {
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);
scanFsmPtr->scanStage = newStage;
}
/*****************************************************************************/
@@ -133,17 +138,20 @@ static void StageUpdate(L2_SCAN_FSM* scanFsmPtr, SCAN_STAGE newStage) {
* SUCCESS
*
******************************************************************************/
static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE msgType) {
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) {
if (bitarr->num_of_bits < NS_MSG_LEN || msgType < L2_MSG_BM || msgType > L2_MSG_SCAN_CONT_5)
{
return (EPARAM);
}
uint64_t dataChunk = 0;
switch (msgType) {
switch (msgType)
{
case L2_MSG_BM:
if (bitarr->num_of_bits < BM_MSG_LEN) {
if (bitarr->num_of_bits < BM_MSG_LEN)
{
return EPARAM;
}
/* message type all zero */
@@ -196,11 +204,12 @@ static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
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);
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) {
if (bitarr->num_of_bits < NS_MSG_LEN)
{
return EPARAM;
}
/* Sequence tag */
@@ -228,18 +237,18 @@ static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
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);
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) == L2_NODE_HEAD) ?
(L2_BROADCAST_ID | (L2_BROADCAST_ID << 5))
: (L2_GetHeadId(instPtr) & 0x1F) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
dataChunk = (L2_GetNodeId(instPtr) == L2_NODE_HEAD) ? (L2_BROADCAST_ID | (L2_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) == L2_NODE_HEAD) {
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
{
/* current net stage */
switch (instPtr->mainState) {
switch (instPtr->mainState)
{
case L2_INIT_STATE:
dataChunk = NET_INI_STAGE;
break;
@@ -255,13 +264,14 @@ static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
}
FrameInsertChunk(bitarr, NS_MSG_INI_NET_STAGE_OFFSET, NS_MSG_INI_NET_STAGE_LEN, dataChunk);
// Begin Scan Time Word
// 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) {
if (bitarr->num_of_bits < NS_MSG_LEN)
{
return EPARAM;
}
/* word tag */
@@ -282,7 +292,8 @@ static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
break;
case L2_MSG_SCAN_CONT_2:
if (bitarr->num_of_bits < NS_MSG_LEN) {
if (bitarr->num_of_bits < NS_MSG_LEN)
{
return EPARAM;
}
/* word tag */
@@ -307,7 +318,8 @@ static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
break;
case L2_MSG_SCAN_CONT_3:
if (bitarr->num_of_bits < NS_MSG_LEN) {
if (bitarr->num_of_bits < NS_MSG_LEN)
{
return EPARAM;
}
/* word tag */
@@ -328,7 +340,8 @@ static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
break;
case L2_MSG_SCAN_CONT_4:
if (bitarr->num_of_bits < NS_MSG_LEN) {
if (bitarr->num_of_bits < NS_MSG_LEN)
{
return EPARAM;
}
/* word tag */
@@ -344,7 +357,8 @@ static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
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) {
if (bitarr->num_of_bits < NS_MSG_LEN)
{
return EPARAM;
}
/* word tag */
@@ -366,34 +380,42 @@ static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
return Status;
}
static int32_t ScanPkt_Packing(L2_INST* instPtr, SCANPKT_TYPE pktType){
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) {
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 FAILURE;
}
switch (pktType) {
switch (pktType)
{
case SCANPKT_INTRA_BM:
if (ScanMsg_Packing(instPtr, scanPkt, L2_MSG_BM) != SUCCESS) {
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));
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 {
else
{
DBG_ERROR("Failed to packing intraW2AcqFrameMsgs[%d].\n", i);
goto failed;
}
filedSize += NS_MSG_LEN;
if (filedSize / 8 >= INTRA_W2ACQ_FRAME_LENGTH_MAX) {
if (filedSize / 8 >= INTRA_W2ACQ_FRAME_LENGTH_MAX)
{
DBG_ERROR("Message is too long to be properly packed.\n");
goto failed;
}
@@ -420,36 +442,44 @@ failed:
* @return used slot (5ms slot).
*
******************************************************************************/
static int32_t Scan_InitScanHandler(L2_INST* instPtr) {
static int32_t Scan_InitScanHandler(L2_INST *instPtr)
{
ASSERT(instPtr != NULL);
L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
L2_SCAN_FSM *scanFsmPtr = instPtr->scanNetFSM;
DBG_INFO("** Netsetup - InitScan Procedure (Attempts: %u)**\n", scanFsmPtr->scanCnt);
if(scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE){
if (scanFsmPtr->isLocked == true) {
if (scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE)
{
if (scanFsmPtr->isLocked == true)
{
StageUpdate(scanFsmPtr, L2_SCAN_BMADVT_STAGE);
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_BM_FRAME);
} /* Otherwise remain L2_SCAN_INIT_STAGE */
else {
else
{
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_SCAN_FRAME);
scanFsmPtr->scanCnt++;
}
}
else if(scanFsmPtr->scanMode == L2_SCAN_INTER_MODE){
if (scanFsmPtr->isLocked == true) {
else if (scanFsmPtr->scanMode == L2_SCAN_INTER_MODE)
{
if (scanFsmPtr->isLocked == true)
{
StageUpdate(scanFsmPtr, L2_SCAN_BMADVT_STAGE);
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTER_BM_FRAME);
} /* Otherwise remain L2_SCAN_INIT_STAGE */
else {
else
{
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTER_SCAN_FRAME);
scanFsmPtr->scanCnt++;
}
}
else{
else
{
DBG_ERROR("UNKNOWN Scan Mode: %d.\n", scanFsmPtr->scanMode);
return FAILURE;
}
@@ -467,12 +497,14 @@ static int32_t Scan_InitScanHandler(L2_INST* instPtr) {
* @return used slot (5ms slot).
*
******************************************************************************/
static int32_t Scan_BmAdvtHandler(L2_INST* instPtr) {
L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
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) {
if (scanFsmPtr->isBmRcved == true)
{
scanFsmPtr->bmTimeout = 0; /* reset listen counter. */
DBG_INFO("Received Bm frame, skipping 10ms for turn around...\n");
// TODO: check functionality.
@@ -480,18 +512,23 @@ static int32_t Scan_BmAdvtHandler(L2_INST* instPtr) {
return SUCCESS;
}
// no valid bm frame was received.
else {
else
{
/* if time out reached, go back to init scan stage. */
if (scanFsmPtr->bmTimeout > BM_LSTEN_TIMEOUT) {
if(scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE){
if (scanFsmPtr->bmTimeout > BM_LSTEN_TIMEOUT)
{
if (scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE)
{
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_SCAN_FRAME);
}
else if(scanFsmPtr->scanMode == L2_SCAN_INTER_MODE){
else if (scanFsmPtr->scanMode == L2_SCAN_INTER_MODE)
{
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTER_SCAN_FRAME);
}
else{
else
{
DBG_ERROR("UNKNOWN Scan Mode: %d.\n", scanFsmPtr->scanMode);
return FAILURE;
}
@@ -499,16 +536,20 @@ static int32_t Scan_BmAdvtHandler(L2_INST* instPtr) {
scanFsmPtr->bmTimeout = 0; /* reset listen counter. */
StageUpdate(scanFsmPtr, L2_SCAN_INIT_STAGE);
}
else {
if(scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE){
else
{
if (scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE)
{
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_BM_FRAME);
}
else if(scanFsmPtr->scanMode == L2_SCAN_INTER_MODE){
else if (scanFsmPtr->scanMode == L2_SCAN_INTER_MODE)
{
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTER_BM_FRAME);
}
else{
else
{
DBG_ERROR("UNKNOWN Scan Mode: %d.\n", scanFsmPtr->scanMode);
return FAILURE;
}
@@ -527,33 +568,39 @@ static int32_t Scan_BmAdvtHandler(L2_INST* instPtr) {
* @return used slot (5ms slot).
*
******************************************************************************/
static int32_t Scan_2WAcqHandler (L2_INST* instPtr){
L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
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 == L2_SCAN_INTRA_MODE){
if (scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE)
{
frameType = L2_INTRA_W2_ACQ_FRAME;
}
else if(scanFsmPtr->scanMode == L2_SCAN_INTER_MODE){
else if (scanFsmPtr->scanMode == L2_SCAN_INTER_MODE)
{
frameType = L2_INTER_W2_ACQ_FRAME;
}
else{
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) {
if (scanFsmPtr->w2acqTimeout >= WAY2_ACQ_SLOT_MAX)
{
DBG_WARNING("2WAcq timeout, restart net scan from L2_SCAN_INIT_STAGE.\n");
scanFsmPtr->w2acqTimeout = 0; // reset counter for next scan.
StageUpdate(scanFsmPtr, L2_SCAN_INIT_STAGE);
}
else {
else
{
scanFsmPtr->w2acqTimeout++; // increase counter.
}
// using pull mode to receive 2wacq frame
// since first time sync procedure is accomplished in BM stage.
// since first time sync procedure is accomplished in BM stage.
Phy_SetMode(instPtr->config.phyInstPtr, PHY_MODE_PULL);
L2_SetupNextSlot(instPtr, L2_TOR_RX, frameType);
return SUCCESS;
@@ -570,20 +617,23 @@ static int32_t Scan_2WAcqHandler (L2_INST* instPtr){
* error code
*
******************************************************************************/
int32_t ScanNet_PktHandler(L2_INST* instPtr) {
int32_t ScanNet_PktHandler(L2_INST *instPtr)
{
ASSERT(instPtr != NULL);
uint32_t Status;
L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
L2_SCAN_FSM *scanFsmPtr = instPtr->scanNetFSM;
uint64_t dataChunk;
DBG_INFO("Scan net handling packet...\n");
BIT_ARRAY* scanMsg = bit_array_create(L2_MSG_MAX);
if (scanMsg == NULL) {
BIT_ARRAY *scanMsg = bit_array_create(L2_MSG_MAX);
if (scanMsg == NULL)
{
DBG_INFO("Failed to start handling process...\n");
return EALLOC;
}
switch (L2_GetLastFrameType(instPtr)) {
switch (L2_GetLastFrameType(instPtr))
{
case L2_INTER_SCAN_FRAME:
case L2_INTRA_SCAN_FRAME:
// INTRA_BM_FRAME could be received if current frame type is L2_INTRA_SCAN_FRAME
@@ -592,41 +642,44 @@ int32_t ScanNet_PktHandler(L2_INST* instPtr) {
LockStateUpdate(scanFsmPtr, true);
StageUpdate(scanFsmPtr, L2_SCAN_BMADVT_STAGE);
case L2_INTER_BM_FRAME:
case L2_INTRA_BM_FRAME: {
for (int i = 0; i < ARRAY_SIZE(intraBmFrameMsgs); i++) {
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]) {
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;
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;
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;
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;
@@ -636,45 +689,54 @@ int32_t ScanNet_PktHandler(L2_INST* instPtr) {
goto failed;
}
}
if(L2_GetNodeType(instPtr) != L2_NODE_HEAD){
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
{
NODE_ENTRY* nodeEntry = L2_GetNodeById(instPtr->nodeList, scanFsmPtr->BMrcvLog.NodeId);
if(nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = L2_GetNodeById(instPtr->nodeList, scanFsmPtr->BMrcvLog.NodeId);
if (nodeEntry == NULL)
{
nodeEntry = L2_AppendNode(instPtr->nodeList, scanFsmPtr->BMrcvLog.NodeId, L2_NODE_HEAD);
// TODO: Insert other node information.
}
else {
else
{
// TODO: Update node information.
}
if ( L2_isCxn2Net(instPtr) == false ) {
if (L2_isCxn2Net(instPtr) == false)
{
// TODO: Check
L2_TimeAlign(instPtr, scanFsmPtr->BMrcvLog.frameNum, scanFsmPtr->BMrcvLog.slotNum, true);
}
// trigger flag
BmrecvStateUpdate(scanFsmPtr, true);
}
else{
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++) {
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]) {
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;
scanFsmPtr->W2ACQrcvLog.ClusterId = (dataChunk >> 5) & 0x0001F;
FrameGetChunk(scanMsg, NS_MSG_INI_NET_STAGE_OFFSET, NS_MSG_INI_NET_STAGE_LEN, &dataChunk);
switch (dataChunk) {
switch (dataChunk)
{
case NET_INI_STAGE:
scanFsmPtr->W2ACQrcvLog.NetStage = L2_INIT_STATE;
break;
@@ -709,7 +771,7 @@ int32_t ScanNet_PktHandler(L2_INST* instPtr) {
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;
@@ -731,27 +793,30 @@ int32_t ScanNet_PktHandler(L2_INST* instPtr) {
}
}
NODE_ENTRY* nodeEntry = L2_GetNodeById(instPtr->nodeList, scanFsmPtr->W2ACQrcvLog.NodeId);
if(nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = L2_GetNodeById(instPtr->nodeList, scanFsmPtr->W2ACQrcvLog.NodeId);
if (nodeEntry == NULL)
{
nodeEntry = L2_AppendNode(instPtr->nodeList, scanFsmPtr->W2ACQrcvLog.NodeId, L2_NODE_MEMBER);
// TODO: Insert other node information.
}
else {
else
{
// TODO: Update node information.
}
if(L2_GetNodeType(instPtr) != L2_NODE_HEAD){
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
{
// check if net setup finished.
if(L2_GetMainState(instPtr) == L2_INIT_STATE && L2_isCxn2Net(instPtr) == false){
if (L2_GetMainState(instPtr) == L2_INIT_STATE && L2_isCxn2Net(instPtr) == false)
{
// Assert net setup done event...
uint32_t netStartTime = scanFsmPtr->W2ACQrcvLog.NetScanTime;
L2_EVENT netSetupDoneEvent = {
.event = L2_EV_NET_SETUP_DONE,
.msg = &netStartTime
};
.msg = &netStartTime};
AssertL2Event(instPtr, &netSetupDoneEvent);
// Update time sync since netStartTime updated.
L2_TimeAlign(instPtr, L2_GetLastFrameNum(instPtr), L2_GetLastSlotNum(instPtr), true);
@@ -759,14 +824,15 @@ int32_t ScanNet_PktHandler(L2_INST* instPtr) {
L2_RTNE_CONFIG newRtneConfig = {0};
L2_EVENT rtneUpEvent = {
.event = L2_ROUTINE_UPDATE,
.msg = &newRtneConfig
};
if(scanFsmPtr->W2ACQrcvLog.NetStage == L2_INIT_STATE){
.msg = &newRtneConfig};
if (scanFsmPtr->W2ACQrcvLog.NetStage == L2_INIT_STATE)
{
newRtneConfig.rtneTable = initTable;
newRtneConfig.rtnePrd = ARRAY_SIZE(initTable);
AssertL2Event(instPtr, &rtneUpEvent);
}
else if(scanFsmPtr->W2ACQrcvLog.NetStage == L2_INTRA_SEEK_STATE){
else if (scanFsmPtr->W2ACQrcvLog.NetStage == L2_INTRA_SEEK_STATE)
{
newRtneConfig.rtneTable = intraSeekTable;
newRtneConfig.rtnePrd = ARRAY_SIZE(intraSeekTable);
AssertL2Event(instPtr, &rtneUpEvent);
@@ -777,15 +843,13 @@ int32_t ScanNet_PktHandler(L2_INST* instPtr) {
L2_NODE_STATE newState = scanFsmPtr->W2ACQrcvLog.NetStage;
L2_EVENT stateUpEvent = {
.event = L2_EV_MAIN_STATE_UPDATE,
.msg = &newState
};
.msg = &newState};
AssertL2Event(instPtr, &stateUpEvent);
}
break;
}
default: {
default:
{
DBG_ERROR("Cannot handler message type [%d] properly.\n", L2_GetLastFrameType(instPtr));
Status = EIGNORE;
goto failed;
@@ -808,9 +872,11 @@ failed:
* 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) {
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;
}
@@ -818,7 +884,6 @@ int32_t ScanNet_Init(L2_SCAN_FSM** scanFSMPtr) {
return (SUCCESS);
}
/*****************************************************************************/
/**
* @brief Reset ScanNet FSM
@@ -828,7 +893,8 @@ int32_t ScanNet_Init(L2_SCAN_FSM** scanFSMPtr) {
* @return NONE
*
******************************************************************************/
void ScanNet_Reset(L2_SCAN_FSM* scanFSMPtr) {
void ScanNet_Reset(L2_SCAN_FSM *scanFSMPtr)
{
ASSERT(scanFSMPtr != NULL);
scanFSMPtr->isLocked = false;
scanFSMPtr->isBmRcved = false;
@@ -865,7 +931,8 @@ void ScanNet_Reset(L2_SCAN_FSM* scanFSMPtr) {
* @return NONE
*
******************************************************************************/
void ScanNet_Destroy(L2_SCAN_FSM* scanFSMPtr) {
void ScanNet_Destroy(L2_SCAN_FSM *scanFSMPtr)
{
free(scanFSMPtr);
}
@@ -878,15 +945,18 @@ void ScanNet_Destroy(L2_SCAN_FSM* scanFSMPtr) {
* @return used slot (5ms slot).
*
******************************************************************************/
int32_t ScanNet_Proc(L2_INST* instPtr) {
L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD) {
int32_t ScanNet_Proc(L2_INST *instPtr)
{
L2_SCAN_FSM *scanFsmPtr = instPtr->scanNetFSM;
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
{
DBG_ERROR("Only member node needs to execute ScanNet_Proc. \n");
return FAILURE;
}
int32_t Status;
DBG_INFO("-------- ScanNet Process --------\n");
switch (scanFsmPtr->scanStage) {
switch (scanFsmPtr->scanStage)
{
case L2_SCAN_NONE_STAGE:
/* Start Scan. */
StageUpdate(scanFsmPtr, L2_SCAN_INIT_STAGE);
@@ -920,47 +990,59 @@ int32_t ScanNet_Proc(L2_INST* instPtr) {
*
******************************************************************************/
int acqSend = 0;
int32_t ScanNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry) {
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){
if (rtneEntry->slotSeq != 0)
{
return SQUENCE_SCAN_SLOTS;
}
if(L2_GetNodeType(instPtr) != L2_NODE_HEAD){
if (rtneEntry->dst0 == L2_GetNodeId(instPtr) || rtneEntry->dst0 == L2_BROADCAST_ID){
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
{
if (rtneEntry->dst0 == L2_GetNodeId(instPtr) || rtneEntry->dst0 == L2_BROADCAST_ID)
{
torState = L2_TOR_RX;
}
else if (rtneEntry->src0 == L2_GetNodeId(instPtr) || rtneEntry->src0 == L2_BROADCAST_ID){
else if (rtneEntry->src0 == L2_GetNodeId(instPtr) || rtneEntry->src0 == L2_BROADCAST_ID)
{
torState = L2_TOR_TX;
}
else {
else
{
torState = L2_TOR_SUSPEND;
}
}
else{
if (rtneEntry->dst0 == L2_GetNodeId(instPtr)){
else
{
if (rtneEntry->dst0 == L2_GetNodeId(instPtr))
{
torState = L2_TOR_RX;
}
else if (rtneEntry->src0 == L2_GetNodeId(instPtr)){
else if (rtneEntry->src0 == L2_GetNodeId(instPtr))
{
torState = L2_TOR_TX;
}
else {
else
{
torState = L2_TOR_SUSPEND;
}
}
switch (rtneEntry->slotType) {
switch (rtneEntry->slotType)
{
case L2_SLOT_INTER_SCAN_ACQ:
// TODO:
L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_INTER_SCAN_FRAME);
break;
case L2_SLOT_INTRA_SCAN_ACQ:
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD && torState == L2_TOR_TX) {
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD && torState == L2_TOR_TX)
{
DBG_ERROR("Only head node can send init-scan Frame.\n");
Status = FAILURE;
goto routine_err;
@@ -974,14 +1056,17 @@ int32_t ScanNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry)
break;
case L2_SLOT_INTRA_SCAN_BM:
if (torState == L2_TOR_TX) {
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD) {
if (torState == L2_TOR_TX)
{
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
{
DBG_ERROR("Only head node can send BM Frame.\n");
Status = FAILURE;
goto routine_err;
}
memset(instPtr->sendPktData, 0xAA, L2_PACKET_LEN_MAX);
if(ScanPkt_Packing(instPtr, SCANPKT_INTRA_BM) != SUCCESS) {
if (ScanPkt_Packing(instPtr, SCANPKT_INTRA_BM) != SUCCESS)
{
Status = FAILURE;
goto routine_err;
}
@@ -994,20 +1079,23 @@ int32_t ScanNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry)
L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_INTER_W2_ACQ_FRAME);
break;
case L2_SLOT_INTRA_SCAN_2W:
if (torState == L2_TOR_TX) {
if(L2_GetNodeType(instPtr) != L2_NODE_HEAD && L2_isDataAvail(instPtr) == false) {
if (torState == L2_TOR_TX)
{
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD && L2_isDataAvail(instPtr) == false)
{
// skip...
break;
}
/* Prepare Request/ACK data. */
memset(instPtr->sendPktData, 0xAA, L2_PACKET_LEN_MAX);
/* Pack message words */
if(ScanPkt_Packing(instPtr, SCANPKT_INTRA_2WACQ) != SUCCESS) {
if (ScanPkt_Packing(instPtr, SCANPKT_INTRA_2WACQ) != SUCCESS)
{
Status = FAILURE;
goto routine_err;
}
W2ackStateUpdate(instPtr->scanNetFSM, true);
acqSend ++;
acqSend++;
}
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, torState, L2_INTRA_W2_ACQ_FRAME);
@@ -1021,4 +1109,3 @@ int32_t ScanNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry)
routine_err:
return Status;
}