forked from gatsby/df_datalink
555 lines
15 KiB
C
555 lines
15 KiB
C
/*****************************************************************************/
|
|
/**
|
|
* @file scan.c
|
|
* @author FanHao ([email protected])
|
|
* @brief DF Linklayer scan implementation file.
|
|
* @version 0.1
|
|
* @date 2023-05-06
|
|
*
|
|
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
|
* Lab of Communication and Network
|
|
*
|
|
******************************************************************************/
|
|
/**
|
|
* @addtogroup LinkLayer
|
|
* @{
|
|
*/
|
|
|
|
/**
|
|
* @addtogroup LinkLayer_Scan
|
|
* @{
|
|
*/
|
|
|
|
/***************************** Include Files *********************************/
|
|
#include <string.h>
|
|
#include "link_layer.h"
|
|
#include "link_layer_types.h"
|
|
#include "msg_def.h"
|
|
#include "node_list.h"
|
|
#include "error.h"
|
|
#include "common.h"
|
|
#include "../phy_layer/phy_layer.h"
|
|
|
|
/************************** Constant Definitions *****************************/
|
|
#define ACK_FRAME_SLOTS 4
|
|
/***************** Macros (In-line Functions) Definitions *********************/
|
|
|
|
/**************************** Type Definitions *******************************/
|
|
|
|
/************************** Variable Definitions *****************************/
|
|
|
|
/************************** Function Prototypes ******************************/
|
|
static void AdvRecvStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state);
|
|
static int32_t Scan_AdvHandler(L2_INST *instPtr);
|
|
static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
|
|
|
|
/**
|
|
* @brief Update Base-info Advertisement state.
|
|
*/
|
|
static void AdvRecvStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state)
|
|
{
|
|
ASSERT(scanFsmPtr != NULL);
|
|
DBG_INFO("Update AdvRecvState from [%" PRId8 "] to [%" PRId8 "].\n",
|
|
scanFsmPtr->isAdvRecvd, state);
|
|
scanFsmPtr->isAdvRecvd = state;
|
|
return;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Member node scan net BmAdvt stage handler.
|
|
*
|
|
* @param instPtr Pointer to linklayer layer instance.
|
|
*
|
|
* @return used slot (2 ms slot).
|
|
*
|
|
******************************************************************************/
|
|
static int32_t Scan_AdvHandler(L2_INST *instPtr)
|
|
{
|
|
ASSERT(instPtr != NULL);
|
|
L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM;
|
|
DBG_INFO("** Net-Setup - Advertisement Procedure (Attempts: %" PRIu8 ") **\n", scanFsmPtr->advCnt);
|
|
|
|
// Network status is updated in L2_Scan_PktHandler() if valid ADV frame is received.
|
|
if (scanFsmPtr->isAdvRecvd == true)
|
|
{
|
|
DBG_ERROR("Only unconnected node can execute Scan_AdvHandler().\n");
|
|
return EUNEXP;
|
|
}
|
|
// no valid bm frame was received.
|
|
else
|
|
{
|
|
(void)L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_FRAME_ADV);
|
|
scanFsmPtr->advCnt++;
|
|
}
|
|
return SUCCESS;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Packing Scan message to a bit array.
|
|
*
|
|
* @param bitarr pointer to bit array buffer.
|
|
* @param msgType message type.
|
|
*
|
|
* @return EPARAM
|
|
* SUCCESS
|
|
*
|
|
******************************************************************************/
|
|
static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
|
|
{
|
|
ASSERT(instPtr != NULL);
|
|
int32_t Status;
|
|
|
|
switch (type)
|
|
{
|
|
case L2_FRAME_ADV:
|
|
{
|
|
L2_MSG_ADV *message = (L2_MSG_ADV *)(instPtr->sendPktData);
|
|
|
|
// Common section of link layer package
|
|
message->common.frameNum = L2_GetNextFrameNum(instPtr);
|
|
message->common.slotNum = L2_GetNextSlotNum(instPtr);
|
|
message->common.src = L2_GetNodeId(instPtr);
|
|
message->common.dst = dstId;
|
|
message->common.msgType = L2_FRAME_ADV;
|
|
|
|
// insert position and speed info
|
|
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
|
|
message->srcPosX = instPtr->sysInfo.posX;
|
|
message->srcPosY = instPtr->sysInfo.posY;
|
|
message->srcPosZ = instPtr->sysInfo.posZ;
|
|
|
|
// Insert net start time
|
|
message->srcNetStatTime = instPtr->netStartTime;
|
|
|
|
// Packing extra payload
|
|
SysMonitor_GetSendPayload(message->advData, &(message->advDataId), L2_PAYLOAD_LEN_ADV);
|
|
if (message->advDataId == DATA_ID_NONE)
|
|
{
|
|
memset(message->advData, 0xAA, L2_PAYLOAD_LEN_ADV);
|
|
}
|
|
|
|
// Append CRC
|
|
uint32_t crcVal;
|
|
Status = L2_CrcGen((uint8_t *)message, sizeof(L2_MSG_ADV) - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal);
|
|
if (Status != SUCCESS)
|
|
{
|
|
DBG_ERROR("Fail to generate CRC, error code: %" PRId32 ".\n", Status);
|
|
return Status;
|
|
}
|
|
message->crc = crcVal;
|
|
|
|
// Logging...
|
|
memcpy(instPtr->log.txLog.data, message, sizeof(L2_MSG_ADV));
|
|
instPtr->log.txLog.dataId = message->advDataId;
|
|
|
|
break;
|
|
}
|
|
case L2_FRAME_ACK:
|
|
{
|
|
L2_MSG_ACK *message = (L2_MSG_ACK *)(instPtr->sendPktData);
|
|
|
|
// Common section of link layer package
|
|
message->common.frameNum = L2_GetNextFrameNum(instPtr);
|
|
message->common.slotNum = L2_GetNextSlotNum(instPtr);
|
|
message->common.src = L2_GetNodeId(instPtr);
|
|
message->common.dst = dstId;
|
|
message->common.msgType = L2_FRAME_ACK;
|
|
|
|
// insert position and speed info
|
|
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
|
|
message->srcPosX = instPtr->sysInfo.posX;
|
|
message->srcPosY = instPtr->sysInfo.posY;
|
|
message->srcPosZ = instPtr->sysInfo.posZ;
|
|
message->srcSpeedX = instPtr->sysInfo.vX;
|
|
message->srcSpeedY = instPtr->sysInfo.vY;
|
|
message->srcSpeedZ = instPtr->sysInfo.vZ;
|
|
|
|
// Packing extra payload
|
|
SysMonitor_GetSendPayload(message->ackData, &(message->ackDataId), L2_PAYLOAD_LEN_ACK);
|
|
if (message->ackDataId == DATA_ID_NONE)
|
|
{
|
|
memset(message->ackData, 0xAA, L2_PAYLOAD_LEN_ACK);
|
|
}
|
|
|
|
// Append CRC
|
|
uint32_t crcVal;
|
|
Status = L2_CrcGen((uint8_t *)message, sizeof(L2_MSG_ACK) - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal);
|
|
if (Status != SUCCESS)
|
|
{
|
|
DBG_ERROR("Fail to generate CRC, error code: %" PRId32 ".\n", Status);
|
|
return Status;
|
|
}
|
|
message->crc = crcVal;
|
|
|
|
// Logging...
|
|
memcpy(instPtr->log.txLog.data, message, sizeof(L2_MSG_ACK));
|
|
instPtr->log.txLog.dataId = message->ackDataId;
|
|
|
|
break;
|
|
}
|
|
default:
|
|
DBG_ERROR("Unexpected frame type: %" PRId8 ".\n", type);
|
|
return EPARAM;
|
|
}
|
|
return SUCCESS;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Scan net packet handler.
|
|
*
|
|
* @param instPtr a pointer to linklayer layer instance
|
|
* @param msg A pointer to frame data buffer.
|
|
*
|
|
* @return
|
|
*
|
|
******************************************************************************/
|
|
int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
|
|
{
|
|
ASSERT(instPtr != NULL);
|
|
ASSERT(msg != NULL);
|
|
int32_t Status;
|
|
L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM;
|
|
L2_FRAME_TYPE frameType = L2_FRAME_NONE;
|
|
L2_FrameGetChunk(msg, L2_MSG_TYPE, L2_MSG_TYPE_LEN, &frameType);
|
|
|
|
switch (frameType)
|
|
{
|
|
case L2_FRAME_ADV:
|
|
{
|
|
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
|
|
{
|
|
DBG_ERROR("Unexpected call from head node, only member Node can handle L2_FRAME_ADV.\n");
|
|
Status = EUNEXP;
|
|
break;
|
|
}
|
|
|
|
L2_MSG_ADV *message = (L2_MSG_ADV *)msg;
|
|
|
|
// Update node status.
|
|
uint8_t nodeId = message->common.src;
|
|
NODE_ENTRY *node;
|
|
node = L2_GetNodeById(instPtr->nodeList, nodeId);
|
|
if (node == NULL)
|
|
{
|
|
node = L2_AppendNode(instPtr->nodeList, nodeId, L2_NODE_HEAD);
|
|
if (node == NULL)
|
|
{
|
|
DBG_ERROR("Failed to append new node [id: %" PRId8 "]", nodeId);
|
|
Status = ENOMEM;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Update node info.
|
|
node->nodeInfo.pos.x = (double)message->srcPosX;
|
|
node->nodeInfo.pos.y = (double)message->srcPosY;
|
|
node->nodeInfo.pos.z = (double)message->srcPosZ;
|
|
node->nodeInfo.antIndex = Phy_ReceiveAntIndex(instPtr->config.phyInstPtr);
|
|
|
|
// Reset node ttl
|
|
node->ttlCnt = 0;
|
|
|
|
// Update net start time.
|
|
uint32_t netstartTime = message->srcNetStatTime;
|
|
L2_EVENT netSetupDoneEvent =
|
|
{
|
|
.event = L2_EV_NET_ADV_UPDATE,
|
|
.msg = &netstartTime
|
|
};
|
|
AssertL2Event(instPtr, &netSetupDoneEvent);
|
|
|
|
// Calculate time deviation
|
|
uint32_t frameNum = message->common.frameNum;
|
|
uint32_t slotNum = (uint32_t)message->common.slotNum;
|
|
Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev));
|
|
if (Status != SUCCESS)
|
|
{
|
|
DBG_ERROR("Failed to compute time deviation.\n");
|
|
break;
|
|
}
|
|
|
|
// check if net setup finished.
|
|
if (L2_isCxn2Net(instPtr) == false)
|
|
{
|
|
// Assert Net-Setup Done.
|
|
L2_EVENT l2Event = {
|
|
.event = L2_EV_NET_SETUP_DONE,
|
|
.msg = NULL
|
|
};
|
|
(void)AssertL2Event(instPtr, &l2Event);
|
|
}
|
|
// Time sync.
|
|
L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
|
|
|
|
// Get data payload
|
|
uint32_t dataId = message->advDataId;
|
|
SysMonitor_UpdateRecvPayload(message->advData, dataId, L2_PAYLOAD_LEN_ADV);
|
|
|
|
// Logging...
|
|
memcpy(instPtr->log.rxLog.data, msg, sizeof(L2_MSG_ADV));
|
|
instPtr->log.rxLog.dataId = dataId;
|
|
|
|
// trigger flag
|
|
AdvRecvStateUpdate(scanFsmPtr, true);
|
|
Status = SUCCESS;
|
|
break;
|
|
}
|
|
case L2_FRAME_ACK:
|
|
{
|
|
if (L2_GetNodeType(instPtr) == L2_NODE_MEMBER)
|
|
{
|
|
DBG_ERROR("Unexpected call from member node, only head Node can handle L2_FRAME_ACK.\n");
|
|
Status = EUNEXP;
|
|
break;
|
|
}
|
|
|
|
L2_MSG_ACK *message = (L2_MSG_ACK *)msg;
|
|
|
|
// Update node status.
|
|
uint8_t nodeId = message->common.src;
|
|
NODE_ENTRY *node;
|
|
node = L2_GetNodeById(instPtr->nodeList, nodeId);
|
|
if (node == NULL)
|
|
{
|
|
node = L2_AppendNode(instPtr->nodeList, nodeId, L2_NODE_MEMBER);
|
|
if (node == NULL)
|
|
{
|
|
DBG_ERROR("Failed to append new node [id: %" PRId8 "]", nodeId);
|
|
Status = ENOMEM;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Update node info.
|
|
node->nodeInfo.pos.x = (double)message->srcPosX;
|
|
node->nodeInfo.pos.y = (double)message->srcPosY;
|
|
node->nodeInfo.pos.z = (double)message->srcPosZ;
|
|
node->nodeInfo.speed.x = (double)message->srcSpeedX;
|
|
node->nodeInfo.speed.y = (double)message->srcSpeedY;
|
|
node->nodeInfo.speed.z = (double)message->srcSpeedZ;
|
|
node->nodeInfo.antIndex = Phy_ReceiveAntIndex(instPtr->config.phyInstPtr);
|
|
|
|
// Reset node ttl
|
|
node->ttlCnt = 0;
|
|
|
|
// Calculate time deviation
|
|
uint32_t frameNum = message->common.frameNum;
|
|
uint32_t slotNum = (uint32_t)message->common.slotNum;
|
|
Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev));
|
|
if (Status != SUCCESS)
|
|
{
|
|
DBG_ERROR("Failed to compute time deviation.\n");
|
|
break;
|
|
}
|
|
|
|
if (L2_GetMainState(instPtr) == L2_INIT_STATE) // FIXME: state switch should follow routing table
|
|
{
|
|
// Assert Net-Setup Done.
|
|
L2_EVENT l2Event;
|
|
L2_NODE_STATE newState = L2_NORMAL_STATE;
|
|
l2Event.event = L2_EV_MAIN_STATE_UPDATE;
|
|
l2Event.msg = &newState;
|
|
(void)AssertL2Event(instPtr, &l2Event);
|
|
}
|
|
|
|
// Get data payload
|
|
uint32_t dataId = message->ackDataId;
|
|
SysMonitor_UpdateRecvPayload(message->ackData, dataId, L2_PAYLOAD_LEN_ACK);
|
|
|
|
// Logging...
|
|
memcpy(instPtr->log.rxLog.data, msg, sizeof(L2_MSG_ACK));
|
|
instPtr->log.rxLog.dataId = dataId;
|
|
|
|
Status = SUCCESS;
|
|
break;
|
|
}
|
|
default:
|
|
DBG_ERROR("Unsupported frame type: %" PRId8 ".\n", frameType);
|
|
Status = EUNEXP;
|
|
break;
|
|
}
|
|
return Status;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Initialize scan FSM.
|
|
*
|
|
* @param instPtr
|
|
*
|
|
* @return
|
|
*
|
|
******************************************************************************/
|
|
int32_t L2_Scan_Init(L2_SCAN_FSM **scanFSMPtr)
|
|
{
|
|
ASSERT(scanFSMPtr != NULL);
|
|
*scanFSMPtr = (L2_SCAN_FSM *)malloc(sizeof(L2_SCAN_FSM));
|
|
if (*scanFSMPtr == NULL)
|
|
{
|
|
DBG_ERROR("Failed to allocate memory for scanFsmPtr.\n");
|
|
return ENOMEM;
|
|
}
|
|
L2_Scan_Reset(*scanFSMPtr);
|
|
return SUCCESS;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Reset ScanNet FSM
|
|
*
|
|
* @param scanFsmPtr a pointer to scan net finite state machine
|
|
*
|
|
* @return NONE
|
|
*
|
|
******************************************************************************/
|
|
void L2_Scan_Reset(L2_SCAN_FSM *scanFSMPtr)
|
|
{
|
|
ASSERT(scanFSMPtr != NULL);
|
|
scanFSMPtr->isAdvRecvd = false;
|
|
scanFSMPtr->advCnt = 0;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Destroy ScanNet FSM, free memory
|
|
*
|
|
* @param scanFsmPtr a pointer to scan net finite state machine
|
|
*
|
|
* @return NONE
|
|
*
|
|
******************************************************************************/
|
|
void L2_Scan_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 (2 ms slot).
|
|
*
|
|
******************************************************************************/
|
|
int32_t L2_Scan_Proc(L2_INST *instPtr)
|
|
{
|
|
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
|
|
{
|
|
DBG_ERROR("Only member node needs to execute L2_Scan_Proc. \n");
|
|
return FAILURE;
|
|
}
|
|
int32_t Status;
|
|
DBG_INFO("-------- ScanNet Process --------\n");
|
|
|
|
Status = Scan_AdvHandler(instPtr);
|
|
|
|
return Status;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
|
|
/**
|
|
* @brief Execute a scan net Routine procedure.
|
|
*
|
|
* @param rtneEntry Routine configuration
|
|
*
|
|
* @return used slot (2 ms slot).
|
|
*
|
|
******************************************************************************/
|
|
int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
|
|
{
|
|
ASSERT(instPtr != NULL);
|
|
DBG_INFO("-------- Scan Routine --------\n");
|
|
|
|
if (rtneEntry->slotSeq != 0)
|
|
{
|
|
return SUCCESS; // skip other slots, only process in the 1st slot
|
|
}
|
|
|
|
PHY_INST *phyRef = instPtr->config.phyInstPtr;
|
|
PHY_ANT_INDEX antIndex;
|
|
L2_TOR_TYPE torState;
|
|
const NODE_ENTRY *commNode;
|
|
|
|
switch (rtneEntry->slotType)
|
|
{
|
|
case L2_SLOT_ADVERTISE:
|
|
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD && rtneEntry->src == L2_GetNodeId(instPtr))
|
|
{
|
|
// antenna selection based on broadcast id in routing table
|
|
antIndex = L2_AntId2AntIndex(rtneEntry->dst);
|
|
// head node transmits messages in advertise slot
|
|
torState = L2_TOR_TX;
|
|
|
|
// reset memory
|
|
memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX);
|
|
// pack message
|
|
if (ScanMsg_Packing(instPtr, L2_FRAME_ADV, rtneEntry->dst) != SUCCESS)
|
|
{
|
|
return FAILURE;
|
|
}
|
|
}
|
|
else if (L2_GetNodeType(instPtr) == L2_NODE_MEMBER)
|
|
{
|
|
commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
|
|
// antenna selection based on node list
|
|
antIndex = commNode->nodeInfo.antIndex;
|
|
// member node receives message in advertise slot
|
|
torState = L2_TOR_RX;
|
|
}
|
|
else
|
|
{
|
|
// otherwise suspend
|
|
antIndex = PHY_ANT_INDEX_NONE;
|
|
torState = L2_TOR_SUSPEND;
|
|
}
|
|
(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
|
|
(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_ADV);
|
|
break;
|
|
case L2_SLOT_ACKNOWLEDGE:
|
|
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD && rtneEntry->dst == L2_GetNodeId(instPtr))
|
|
{
|
|
// FIXME: is antenna index necessary here?
|
|
antIndex = PHY_ANT_INDEX_NONE;
|
|
// head node receives messages in acknowledge slot
|
|
torState = L2_TOR_RX;
|
|
}
|
|
else if (L2_GetNodeType(instPtr) == L2_NODE_MEMBER && rtneEntry->src == L2_GetNodeId(instPtr))
|
|
{
|
|
commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
|
|
// antenna selection based on node list
|
|
antIndex = commNode->nodeInfo.antIndex;
|
|
// member node transmits message in acknowledge slot
|
|
torState = L2_TOR_TX;
|
|
|
|
// reset memory
|
|
memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX);
|
|
// pack message
|
|
if (ScanMsg_Packing(instPtr, L2_FRAME_ACK, rtneEntry->dst) != SUCCESS)
|
|
{
|
|
return FAILURE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// otherwise suspend
|
|
antIndex = PHY_ANT_INDEX_NONE;
|
|
torState = L2_TOR_SUSPEND;
|
|
}
|
|
(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
|
|
(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_ACK);
|
|
break;
|
|
default:
|
|
DBG_ERROR("Cannot start a routine for slot type [%" PRId8 "] properly.\n", rtneEntry->slotType);
|
|
return FAILURE;
|
|
}
|
|
return SUCCESS;
|
|
}
|
|
|
|
/** @} LinkLayer_Scan*/
|
|
/** @} LinkLayer*/
|