feat: 修改通过SDK编译

df_datalink_ge/src/main.c

@@ -88,7 +88,8 @@ L2_CONFIG linkConfig =
 	.clustId = 1,
 	.myId = 1,
 	.headId = 1,
-	.netScale = 2,
+	.netScale = 10,
+	.serviceRate = L2_SVC_128K,
 	.phyInstPtr = NULL, // physical layer reference
 	.tmcInstPtr = NULL
 };

df_datalink_sp/src/main.c

@@ -94,7 +94,6 @@ static PHY_CONFIG phyConfig = {
 	.devId = PHY_DEVICE_ID,
 	.baseAddr = PHY_BASEADDR,
 	.antBramId = PHY_ANTCONFIG_BRAM_DEVICE_ID,
-	.antInfoBramId = PHY_ANTINFO_BRAM_DEVICE_ID,
 	.txBramId = PHY_TXBUFF_BRAM_DEVICE_ID,
 	.rxBramId = PHY_RXBUFF_BRAM_DEVICE_ID,
 	.hwXadcId = PHY_HW_XADC_DEVICE_ID,

df_datalink_sp/src/networking/link_layer/framing.c

@@ -81,7 +81,7 @@ void L2_FrameGetChunk(uint8_t *frame, size_t offset, size_t bytes, void *chunk)
  * 			SUCCESS
  *
  ******************************************************************************/
-static int32_t L2_CrcGen(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t *crc)
+int32_t L2_CrcGen(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t *crc)
 {
 	ASSERT(dataBuff != NULL);
 	int32_t Status;

df_datalink_sp/src/networking/link_layer/link_layer.c

@@ -149,33 +149,33 @@ static void L2_FrameStateUpdate(L2_INST *instPtr)
 		switch (recvType)
 		{
 		case L2_FRAME_ADV:
-			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_ADV, L2_SVC_NONE), L2_CRC16_1021);
+			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_ADV, L2_SVC_NONE), L2_CRC8_31);
 			instPtr->log.advRecvCnt++;
 			instPtr->log.advCorrectCnt = isValid ? instPtr->log.advCorrectCnt + 1 : instPtr->log.advCorrectCnt;
 			break;
 		case L2_FRAME_ACK:
-			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_ACK, L2_SVC_NONE), L2_CRC16_1021);
+			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_ACK, L2_SVC_NONE), L2_CRC8_31);
 			instPtr->log.ackRecvCnt++;
 			instPtr->log.ackCorrectCnt = isValid ? instPtr->log.ackCorrectCnt + 1 : instPtr->log.ackCorrectCnt;
 			break;
 		case L2_FRAME_DIS:
-			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_DIS, L2_SVC_NONE), L2_CRC16_1021);
+			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_DIS, L2_SVC_NONE), L2_CRC8_31);
 			instPtr->log.disRecvCnt++;
 			instPtr->log.disCorrectCnt = isValid ? instPtr->log.disCorrectCnt + 1 : instPtr->log.disCorrectCnt;
 			break;
 		case L2_FRAME_MNG:
-			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_MNG, L2_SVC_NONE), L2_CRC16_1021);
+			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_MNG, L2_SVC_NONE), L2_CRC8_31);
 			instPtr->log.mngRecvCnt++;
 			instPtr->log.mngCorrectCnt = isValid ? instPtr->log.mngCorrectCnt + 1 : instPtr->log.mngCorrectCnt;
 			break;
 		case L2_FRAME_RTT:
-			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_RTT, L2_SVC_NONE), L2_CRC16_1021);
+			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_RTT, L2_SVC_NONE), L2_CRC8_31);
 			instPtr->log.rttRecvCnt++;
 			instPtr->log.rttCorrectCnt = isValid ? instPtr->log.rttCorrectCnt + 1 : instPtr->log.rttCorrectCnt;
 			break;
 		case L2_FRAME_SVC:
 #ifndef DATALINK_ENABLE_BERTEST
-			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_SVC, instPtr->config.serviceRate), L2_CRC16_1021);
+			isValid = L2_CrcCheck(instPtr->recvPktData, L2_GetFrameLen(L2_FRAME_SVC, instPtr->config.serviceRate), L2_CRC8_31);
 #else
 			isValid = true;
 #endif
@@ -439,6 +439,56 @@ L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame, L2_SVC_RATE l2SvcRate)
 	}
 }
 
+/*****************************************************************************/
+/**
+ * @brief   Get L2 service packet length based on service rate
+ *
+ * @param	l2SvcRate
+ *
+ * @return  Packet length.
+ *
+ ******************************************************************************/
+size_t L2_GetSvcPacketLen(L2_SVC_RATE l2SvcRate)
+{
+	switch (l2SvcRate)
+	{
+	case L2_SVC_128K:
+		return sizeof(L2_MSG_SVC_128K);
+	case L2_SVC_512K:
+		return sizeof(L2_MSG_SVC_512K);
+	case L2_SVC_2M:
+		return sizeof(L2_MSG_SVC_2M);
+	default:
+		DBG_ERROR("Unsupported service rate: %" PRId8 ".\n", instPtr->config.serviceRate);
+		return EPARAM;
+	}
+}
+
+/*****************************************************************************/
+/**
+ * @brief   Get L2 service payload length based on service rate
+ *
+ * @param	l2SvcRate
+ *
+ * @return  Payload length.
+ *
+ ******************************************************************************/
+size_t L2_GetSvcPayloadLen(L2_SVC_RATE l2SvcRate)
+{
+	switch (l2SvcRate)
+	{
+	case L2_SVC_128K:
+		return L2_PAYLOAD_LEN_SVC_128K;
+	case L2_SVC_512K:
+		return L2_PAYLOAD_LEN_SVC_512K;
+	case L2_SVC_2M:
+		return L2_PAYLOAD_LEN_SVC_2M;
+	default:
+		DBG_ERROR("Unsupported service rate: %" PRId8 ".\n", instPtr->config.serviceRate);
+		return EPARAM;
+	}
+}
+
 /*****************************************************************************/
 /**
  * @brief   Convert antenna broadcast id to antenna index
@@ -964,9 +1014,8 @@ static int32_t Linklayer_Routine(L2_INST *instPtr)
 	case L2_SLOT_DISTRIBUTE:
 	case L2_SLOT_MANAGE:
 	case L2_SLOT_RTT:
-		Status = L2_Service_Routine(instPtr, &rtneEntry, L2_SVC_128K);
 	case L2_SLOT_SERVICE:
-		Status = L2_Service_Routine(instPtr, &rtneEntry, L2_GetServiceRate(instPtr));
+		Status = L2_Service_Routine(instPtr, &rtneEntry);
 		break;
 	default:
 		DBG_ERROR("UNKNOWN slot type [%" PRId8 "].\n", rtneEntry.slotType);

df_datalink_sp/src/networking/link_layer/link_layer.h

@@ -69,10 +69,10 @@ typedef struct
 	uint32_t ackRecvCnt;	///< acknowledgement received frame count
 	uint32_t ackCorrectCnt; ///< acknowledgement correctly received frame count
 
-	uint32_t distRxCnt;		///< distribution rx frame count
-	uint32_t distTxCnt;		///< distribution tx frame count
-	uint32_t distRecvCnt;	///< distribution received frame count
-	uint32_t distCorrectCnt;///< distribution correctly received frame count
+	uint32_t disRxCnt;		///< distribution rx frame count
+	uint32_t disTxCnt;		///< distribution tx frame count
+	uint32_t disRecvCnt;	///< distribution received frame count
+	uint32_t disCorrectCnt;///< distribution correctly received frame count
 
 	uint32_t mngRxCnt;		///< management rx frame count
 	uint32_t mngTxCnt;		///< management tx frame count
@@ -152,7 +152,7 @@ int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE f
 void L2_FrameInsertChunk(uint8_t *frame, size_t offset, size_t bytes, const void *chunk);
 void L2_FrameGetChunk(uint8_t *frame, size_t offset, size_t bytes, void *chunk);
 int32_t L2_CrcAppend(uint8_t *dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType);
-static int32_t L2_CrcGen(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t *crc);
+int32_t L2_CrcGen(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t *crc);
 bool L2_CrcCheck(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType);
 /** @} LinkLayer_Framing*/
 
@@ -171,9 +171,6 @@ int32_t L2_RescheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint
  * @defgroup LinkLayer_Scan
  * @{
  */
-void L2_Scan_RefreashScanTable(L2_INST *instPtr);
-void L2_Scan_Reset(L2_SCAN_FSM *scanFSMPtr);
-void L2_Scan_Destroy(L2_SCAN_FSM *scanFSMPtr);
 int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg);
 int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *schedEntry);
 /** @} LinkLayer_Scan*/
@@ -224,9 +221,11 @@ uint32_t L2_GetNextSlotNum(L2_INST *instPtr);
 L2_FRAME_TYPE L2_GetNextFrameType(L2_INST *instPtr);
 L2_TOR_TYPE L2_GetNextTor(L2_INST *instPtr);
 L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame, L2_SVC_RATE l2SvcRate);
+size_t L2_GetSvcPacketLen(L2_SVC_RATE l2SvcRate);
+size_t L2_GetSvcPayloadLen(L2_SVC_RATE l2SvcRate);
 PHY_ANT_INDEX L2_AntId2AntIndex(uint8_t antId);
 /** @} LinkLayer_Utility*/
 
 #endif /* __LINK_LAYER_H_ */
 
-/** @} LinkLayer*/
+/** @} LinkLayer*/

df_datalink_sp/src/networking/link_layer/msg_def.h

@@ -88,7 +88,7 @@ typedef struct
  */
 typedef struct
 {
-	L2_MSG_COMMON *common;	///< common message header
+	L2_MSG_COMMON common;	///< common message header
 	double srcPosX;			///< position x
 	double srcPosY;			///< position y
 	double srcPosZ;			///< position z
@@ -104,7 +104,7 @@ typedef struct
  */
 typedef struct
 {
-	L2_MSG_COMMON *common;	///< common message header
+	L2_MSG_COMMON common;	///< common message header
 	double srcPosX;			///< position x
 	double srcPosY;			///< position y
 	double srcPosZ;			///< position z
@@ -122,7 +122,7 @@ typedef struct
  */
 typedef struct
 {
-	L2_MSG_COMMON *common;	///< common message header
+	L2_MSG_COMMON common;	///< common message header
 	double srcPosX;			///< position x
 	double srcPosY;			///< position y
 	double srcPosZ;			///< position z
@@ -140,7 +140,7 @@ typedef struct
  */
 typedef struct
 {
-	L2_MSG_COMMON *common;	///< common message header
+	L2_MSG_COMMON common;	///< common message header
 	double srcPosX;			///< position x
 	double srcPosY;			///< position y
 	double srcPosZ;			///< position z
@@ -158,7 +158,7 @@ typedef struct
  */
 typedef struct
 {
-	L2_MSG_COMMON *common;	///< common message header
+	L2_MSG_COMMON common;	///< common message header
 	double srcPosX;			///< position x
 	double srcPosY;			///< position y
 	double srcPosZ;			///< position z
@@ -176,4 +176,4 @@ typedef struct
 
 /** @} LinkLayer_MSG_Constants*/
 
-/** @} LinkLayer*/
+/** @} LinkLayer*/

df_datalink_sp/src/networking/link_layer/node_list.c

@@ -140,7 +140,8 @@ void L2_NodeList_Destroy(NODE_LIST *list)
 NODE_ENTRY *L2_GetNodeById(NODE_LIST *list, uint8_t nodeId)
 {
 	ASSERT(list != NULL);
-	if (nodeId == L2_BROADCAST_ID || nodeId == L2_NONE_ID)
+	if (nodeId == L2_BROADCAST_ID1 || nodeId == L2_BROADCAST_ID2 ||
+			nodeId == L2_BROADCAST_ID3 || nodeId == L2_NONE_ID)
 	{
 		return NULL;
 	}
@@ -558,4 +559,4 @@ int32_t L2_IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *))
 
 /** @} LinkLayer_NodeList*/
 
-/** @} LinkLayer*/
+/** @} LinkLayer*/

df_datalink_sp/src/networking/link_layer/scan.c

@@ -56,82 +56,74 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
 {
 	ASSERT(instPtr != NULL);
 	int32_t Status;
-	uint32_t dataChunk;
-	if ((type < L2_FRAME_ADV) || (type > L2_FRAME_ACK))
-	{
-		DBG_ERROR("Unexpected frame type: %" PRId8 ".\n", type);
-		return EPARAM;
-	}
 
 	switch (type)
 	{
 	case L2_FRAME_ADV:
 	{
-		L2_MSG_ADV *message = calloc(sizeof(L2_MSG_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->ackDataId == DATA_ID_NONE)
-		{
-			memset(message->ackData, 0xAA, L2_PAYLOAD_LEN_ADV);
-		}
-
-		// Append CRC
-		uint32_t crcVal;
-		Status = L2_CrcGen(message, Phy_GetFrameLen(L2_L2frame2L1frame(L2_FRAME_ADV)) - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal);
-		if (Status != SUCCESS)
-		{
-			return Status;
-		}
-		message->crc = crcVal;
-
-		instPtr->sendPktData = message;
-
-		// Logging...
-		memcpy(instPtr->log.txLog.data, message, Phy_GetFrameLen(L2_L2frame2L1frame(L2_FRAME_ADV)));
-		instPtr->log.txLog.dataId = message->ackDataId;
-
-		free(message);
-		break;
-	}
-	case L2_FRAME_ACK:
-	{
-		L2_MSG_ACK *message = calloc(sizeof(L2_MSG_ACK));
-
-		// 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.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);
-		message->srcSpeedX = &(instPtr->&(instPtr->sysInfo.vX));
-		message->srcSpeedY = &(instPtr->&(instPtr->sysInfo.vY));
-		message->srcSpeedZ = &(instPtr->&(instPtr->sysInfo.vZ));
+		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->ackData, message->ackDataId, L2_PAYLOAD_LEN_ACK);
+		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_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;
+		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);
@@ -139,26 +131,24 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
 
 		// Append CRC
 		uint32_t crcVal;
-		Status = L2_CrcGen(message, Phy_GetFrameLen(L2_L2frame2L1frame(L2_FRAME_ACK)) - L2_MSG_CRC_LEN, L2_CRC8_31, &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, Phy_GetFrameLen(L2_L2frame2L1frame(L2_FRAME_ACK)));
+		memcpy(instPtr->log.txLog.data, message, sizeof(L2_MSG_ACK));
 		instPtr->log.txLog.dataId = message->ackDataId;
 
-		free(message);
 		break;
 	}
 	default:
-	{
 		DBG_ERROR("Unexpected frame type: %" PRId8 ".\n", type);
 		return EPARAM;
 	}
-	}
 	return SUCCESS;
 }
 
@@ -191,7 +181,7 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
 			break;
 		}
 
-		L2_MSG_ADV *message = msg;
+		L2_MSG_ADV *message = (L2_MSG_ADV *)msg;
 
 		// Update node status.
 		uint8_t nodeId = message->common.src;
@@ -207,11 +197,12 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
 				break;
 			}
 		}
+
 		// Update node info.
 		node->nodeInfo.pos.x = message->srcPosX;
 		node->nodeInfo.pos.y = message->srcPosY;
 		node->nodeInfo.pos.z = message->srcPosZ;
-		node->nodeInfo.antIndex = Phy_ListenAntIndex(instPtr->config.phyInstPtr);
+		node->nodeInfo.antIndex = Phy_ReceiveAntIndex(instPtr->config.phyInstPtr);
 
 		// Reset node ttl
 		node->ttlCnt = 0;
@@ -253,7 +244,7 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
 		SysMonitor_UpdateRecvPayload(message->advData, dataId, L2_PAYLOAD_LEN_ADV);
 
 		// Logging...
-		memcpy(instPtr->log.rxLog.data, msg, Phy_GetFrameLen(L2_L2frame2L1frame(L2_FRAME_ADV, L2_SVC_NONE)));
+		memcpy(instPtr->log.rxLog.data, msg, sizeof(L2_MSG_ADV));
 		instPtr->log.rxLog.dataId = dataId;
 
 		Status = SUCCESS;
@@ -268,7 +259,7 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
 			break;
 		}
 
-		L2_MSG_ACK *message = msg;
+		L2_MSG_ACK *message = (L2_MSG_ACK *)msg;
 
 		// Update node status.
 		uint8_t nodeId = message->common.src;
@@ -284,6 +275,7 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
 				break;
 			}
 		}
+
 		// Update node info.
 		node->nodeInfo.pos.x = message->srcPosX;
 		node->nodeInfo.pos.y = message->srcPosY;
@@ -291,7 +283,7 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
 		node->nodeInfo.speed.x = message->srcSpeedX;
 		node->nodeInfo.speed.y = message->srcSpeedY;
 		node->nodeInfo.speed.z = message->srcSpeedZ;
-		node->nodeInfo.antIndex = Phy_ListenAntIndex(instPtr->config.phyInstPtr);
+		node->nodeInfo.antIndex = Phy_ReceiveAntIndex(instPtr->config.phyInstPtr);
 
 		// Reset node ttl
 		node->ttlCnt = 0;
@@ -317,11 +309,11 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
 		}
 
 		// Get data payload
-		uint32_t dataId = message->advDataId;
-		SysMonitor_UpdateRecvPayload(message->advData, dataId, L2_PAYLOAD_LEN_ADV);
+		uint32_t dataId = message->ackDataId;
+		SysMonitor_UpdateRecvPayload(message->ackData, dataId, L2_PAYLOAD_LEN_ACK);
 
 		// Logging...
-		memcpy(instPtr->log.rxLog.data, msg, Phy_GetFrameLen(L2_L2frame2L1frame(L2_FRAME_ACK, L2_SVC_NONE)));
+		memcpy(instPtr->log.rxLog.data, msg, sizeof(L2_MSG_ACK));
 		instPtr->log.rxLog.dataId = dataId;
 
 		Status = SUCCESS;
@@ -335,39 +327,6 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
 	return Status;
 }
 
-/*****************************************************************************/
-/**
- * @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->isLocked = false;
-	scanFSMPtr->isAdvRecvd = false;
-	scanFSMPtr->isAckPending = false;
-	scanFSMPtr->advCnt = 0;
-	scanFSMPtr->cfmTimeout = 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);
-}
-
 /*****************************************************************************/
 
 /**
@@ -381,10 +340,6 @@ void L2_Scan_Destroy(L2_SCAN_FSM *scanFSMPtr)
 int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
 {
 	ASSERT(instPtr != NULL);
-	L2_TOR_TYPE torState;
-	const NODE_ENTRY *commNode;
-	PHY_ANT_INDEX antIndex;
-	PHY_INST *phyRef = instPtr->config.phyInstPtr;
 	DBG_INFO("-------- Scan Routine --------\n");
 
 	if (rtneEntry->slotSeq != 0)
@@ -392,14 +347,24 @@ int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
 		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))
 		{
-			antIndex = L2_AntId2AntIndex(rtneEntry->dst); // antenna selection based on broadcast id in routing table
-			torState = L2_TOR_TX; // head node transmits messages in advertise slot
+			// 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;
@@ -408,13 +373,16 @@ int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
 		else if (L2_GetNodeType(instPtr) == L2_NODE_MEMBER)
 		{
 			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
-			antIndex = commNode->nodeInfo.antIndex; // antenna selection based on node list
-			torState = L2_TOR_RX; // member node receives message in advertise slot
+			// antenna selection based on node list
+			antIndex = commNode->nodeInfo.antIndex;
+			// member node receives message in advertise slot
+			torState = L2_TOR_RX;
 		}
 		else
 		{
-			DBG_ERROR("Unsupported node type" PRId8 "or wrong routine table in L2_SLOT_ADVERTISE.\n", L2_GetNodeType(instPtr));
-			return FAILURE;
+			// 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);
@@ -422,15 +390,22 @@ int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
 	case L2_SLOT_ACKNOWLEDGE:
 		if (L2_GetNodeType(instPtr) == L2_NODE_HEAD && rtneEntry->dst == L2_GetNodeId(instPtr))
 		{
-			antIndex = PHY_ANT_INDEX_NONE; // FIXME: is antenna index necessary here?
-			torState = L2_TOR_RX; // head node receives messages in acknowledge slot
+			// 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)
+		else if (L2_GetNodeType(instPtr) == L2_NODE_MEMBER && rtneEntry->src == L2_GetNodeId(instPtr))
 		{
 			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
-			antIndex = commNode->nodeInfo.antIndex; // antenna selection based on node list
-			torState = L2_TOR_TX; // member node transmits message in acknowledge slot
+			// 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;
@@ -438,8 +413,9 @@ int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
 		}
 		else
 		{
-			DBG_ERROR("Unsupported node type" PRId8 "or wrong routine table in L2_SLOT_ACKNOWLEDGE.\n", L2_GetNodeType(instPtr));
-			return FAILURE;
+			// 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);

df_datalink_sp/src/networking/link_layer/service.c

@@ -36,6 +36,13 @@
 /**************************** Type Definitions *******************************/
 /************************** Function Prototypes ******************************/
 static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
+static void Service_PackingCommon(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
+static int32_t Service_Packing128K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
+static int32_t Service_Packing512K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
+static int32_t Service_Packing2M(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
+static int32_t Service_PktHandler128K(L2_INST *instPtr, uint8_t *msg);
+static int32_t Service_PktHandler512K(L2_INST *instPtr, uint8_t *msg);
+static int32_t Service_PktHandler2M(L2_INST *instPtr, uint8_t *msg);
 /************************** Variable Definitions *****************************/
 
 static uint8_t testData[L2_PACK_LEN_MAX] =
@@ -91,75 +98,242 @@ static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
 #ifndef DATALINK_ENABLE_BERTEST
 	ASSERT(instPtr != NULL);
 	int32_t Status;
-	uint32_t dataChunk;
-	uint8_t *buff = instPtr->sendPktData;
 
-	// Common section of link layer package
-	dataChunk = L2_GetNextFrameNum(instPtr);
-	L2_FrameInsertChunk(buff, L2_MSG_FRAME_NUM, L2_MSG_FRAME_NUM_LEN, &dataChunk);
-	dataChunk = L2_GetNextSlotNum(instPtr);
-	L2_FrameInsertChunk(buff, L2_MSG_SLOT_NUM, L2_MSG_SLOT_NUM_LEN, &dataChunk);
-	dataChunk = L2_GetNodeId(instPtr);
-	L2_FrameInsertChunk(buff, L2_MSG_SRC_ID, L2_MSG_SRC_ID_LEN, &dataChunk);
-	dataChunk = dstId;
-	L2_FrameInsertChunk(buff, L2_MSG_DST_ID, L2_MSG_DST_ID_LEN, &dataChunk);
 	switch (type)
 	{
-	case L2_SVC_FRAME:
+	case L2_FRAME_DIS:
+	case L2_FRAME_MNG:
+	case L2_FRAME_RTT:
 	{
-		dataChunk = L2_SVC_FRAME;
-		L2_FrameInsertChunk(buff, L2_MSG_TYPE, L2_MSG_TYPE_LEN, &dataChunk);
-
-		SysMonitor_GetSysinfo(&(instPtr->sysInfo));
-		// insert position and speed info
-		L2_FrameInsertChunk(buff, L2_MSG_SVC_SRC_POSX, L2_MSG_SVC_SRC_POSX_LEN, &(instPtr->sysInfo.posX));
-		L2_FrameInsertChunk(buff, L2_MSG_SVC_SRC_POSY, L2_MSG_SVC_SRC_POSY_LEN, &(instPtr->sysInfo.posY));
-		L2_FrameInsertChunk(buff, L2_MSG_SVC_SRC_POSZ, L2_MSG_SVC_SRC_POSZ_LEN, &(instPtr->sysInfo.posZ));
-		L2_FrameInsertChunk(buff, L2_MSG_SVC_SRC_SPEEDX, L2_MSG_SVC_SRC_SPEEDX_LEN, &(instPtr->sysInfo.vX));
-		L2_FrameInsertChunk(buff, L2_MSG_SVC_SRC_SPEEDY, L2_MSG_SVC_SRC_SPEEDY_LEN, &(instPtr->sysInfo.vY));
-		L2_FrameInsertChunk(buff, L2_MSG_SVC_SRC_SPEEDZ, L2_MSG_SVC_SRC_SPEEDZ_LEN, &(instPtr->sysInfo.vZ));
-
-		uint32_t dataId;
-		if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
+		Status = Service_Packing128K(instPtr, type, dstId);
+		break;
+	}
+	case L2_FRAME_SVC:
+	{
+		switch (instPtr->config.serviceRate)
 		{
-			SysMonitor_GetSendPayload(buff + L2_MSG_SVC_PAYLOAD, &dataId, PAYLOAD_LEN_FLJ_TX);
+		case L2_SVC_128K:
+			Status = Service_Packing128K(instPtr, type, dstId);
+			break;
+		case L2_SVC_512K:
+			Status = Service_Packing512K(instPtr, type, dstId);
+			break;
+		case L2_SVC_2M:
+			Status = Service_Packing2M(instPtr, type, dstId);
+			break;
+		default:
+			DBG_ERROR("Unexpected service rate: %" PRIu8 ".\n", instPtr->config.serviceRate);
+			return EPARAM;
 		}
-		else
-		{
-			SysMonitor_GetSendPayload(buff + L2_MSG_SVC_PAYLOAD, &dataId, PAYLOAD_LEN_LJ_TX);
-		}
-
-		L2_FrameInsertChunk(buff, L2_MSG_SVC_PAYLOAD_ID, L2_MSG_SVC_PAYLOAD_ID_LEN, &dataId);
-		if (dataId == DATA_ID_NONE)
-		{
-			L2_FrameInsertChunk(buff, L2_MSG_SVC_PAYLOAD, L2_MSG_SVC_PAYLOAD_LEN, testData);
-		}
-
-		Status = L2_CrcAppend(buff, L2_SVC_PACK_LEN - L2_MSG_SVC_CRC_LEN, L2_MSG_SVC_CRC, L2_CRC16_1021);
-		if (Status != SUCCESS)
-		{
-			DBG_ERROR("Fail to append CRC...\n");
-			return Status;
-		}
-
-		memcpy(instPtr->log.txLog.data, buff, L2_SVC_PACK_LEN);
-		instPtr->log.txLog.dataId = dataId;
 		break;
 	}
 	default:
-	{
 		DBG_ERROR("Unexpected frame type: %" PRIu8 ".\n", type);
 		return EPARAM;
-		break;
 	}
-	}
-	return SUCCESS;
+	return Status;
 #else
 	DBG_ERROR("Unexpected call of Service_Packing in BER test mode.\n");
 	return EUNEXP;
 #endif
 }
 
+/*****************************************************************************/
+/**
+ * @brief   Packing Service message (common part)
+ *
+ * @param   instPtr		pointer to linklayer instance
+ *          type		message type.
+ *          dstId		destination node address.
+ *
+ * @return	EPARAM
+ * 			SUCCESS
+ *
+ ******************************************************************************/
+static void Service_PackingCommon(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
+{
+	ASSERT(instPtr != NULL);
+
+	L2_MSG_COMMON *message = (L2_MSG_COMMON *)(instPtr->sendPktData);
+
+	// Common section of link layer package
+	message->frameNum = L2_GetNextFrameNum(instPtr);
+	message->slotNum = L2_GetNextSlotNum(instPtr);
+	message->src = L2_GetNodeId(instPtr);
+	message->dst = dstId;
+	message->msgType = type;
+}
+
+/*****************************************************************************/
+/**
+ * @brief   Packing Service message (128 kbps)
+ *
+ * @param   instPtr		pointer to linklayer instance
+ *          type		message type.
+ *          dstId		destination node address.
+ *
+ * @return	EPARAM
+ * 			SUCCESS
+ *
+ ******************************************************************************/
+static int32_t Service_Packing128K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
+{
+	ASSERT(instPtr != NULL);
+	int32_t Status;
+
+	Service_PackingCommon(instPtr, type, dstId);
+
+	L2_MSG_SVC_128K *message = (L2_MSG_SVC_128K *)(instPtr->sendPktData);
+	size_t msgLen = sizeof(L2_MSG_SVC_128K);
+	size_t dataLen = L2_PAYLOAD_LEN_SVC_128K;
+
+	// 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->svcData, &(message->svcDataId), dataLen);
+	if (message->svcDataId == DATA_ID_NONE)
+	{
+		memset(message->svcData, 0xAA, dataLen);
+	}
+
+	// Append CRC
+	uint32_t crcVal;
+	Status = L2_CrcGen((uint8_t *)message, msgLen - 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, msgLen);
+	instPtr->log.txLog.dataId = message->svcDataId;
+
+	return SUCCESS;
+}
+
+/*****************************************************************************/
+/**
+ * @brief   Packing Service message (512 kbps)
+ *
+ * @param   instPtr		pointer to linklayer instance
+ *          type		message type.
+ *          dstId		destination node address.
+ *
+ * @return	EPARAM
+ * 			SUCCESS
+ *
+ ******************************************************************************/
+static int32_t Service_Packing512K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
+{
+	ASSERT(instPtr != NULL);
+	int32_t Status;
+
+	Service_PackingCommon(instPtr, type, dstId);
+
+	L2_MSG_SVC_512K *message = (L2_MSG_SVC_512K *)(instPtr->sendPktData);
+	size_t msgLen = sizeof(L2_MSG_SVC_512K);
+	size_t dataLen = L2_PAYLOAD_LEN_SVC_512K;
+
+	// 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->svcData, &(message->svcDataId), dataLen);
+	if (message->svcDataId == DATA_ID_NONE)
+	{
+		memset(message->svcData, 0xAA, dataLen);
+	}
+
+	// Append CRC
+	uint32_t crcVal;
+	Status = L2_CrcGen((uint8_t *)message, msgLen - 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, msgLen);
+	instPtr->log.txLog.dataId = message->svcDataId;
+
+
+	return SUCCESS;
+}
+
+/*****************************************************************************/
+/**
+ * @brief   Packing Service message (2 Mbps)
+ *
+ * @param   instPtr		pointer to linklayer instance
+ *          type		message type.
+ *          dstId		destination node address.
+ *
+ * @return	EPARAM
+ * 			SUCCESS
+ *
+ ******************************************************************************/
+static int32_t Service_Packing2M(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
+{
+	ASSERT(instPtr != NULL);
+	int32_t Status;
+
+	Service_PackingCommon(instPtr, type, dstId);
+
+	L2_MSG_SVC_2M *message = (L2_MSG_SVC_2M *)(instPtr->sendPktData);
+	size_t msgLen = sizeof(L2_MSG_SVC_2M);
+	size_t dataLen = L2_PAYLOAD_LEN_SVC_2M;
+
+	// 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->svcData, &(message->svcDataId), dataLen);
+	if (message->svcDataId == DATA_ID_NONE)
+	{
+		memset(message->svcData, 0xAA, dataLen);
+	}
+
+	// Append CRC
+	uint32_t crcVal;
+	Status = L2_CrcGen((uint8_t *)message, msgLen - 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, msgLen);
+	instPtr->log.txLog.dataId = message->svcDataId;
+
+
+	return SUCCESS;
+}
+
 /*****************************************************************************/
 /**
  * @brief   Handle service received packet. Send data up to MPC
@@ -176,103 +350,60 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg)
 	ASSERT(msg != NULL);
 	int32_t Status;
 #ifndef DATALINK_ENABLE_BERTEST
-	L2_FRAME_TYPE frameType = L2_NONE_FRAME;
+	L2_FRAME_TYPE frameType = L2_FRAME_NONE;
 	L2_FrameGetChunk(msg, L2_MSG_TYPE, L2_MSG_TYPE_LEN, &frameType);
+
+	// retrieve basic info
 	switch (frameType)
 	{
-	case L2_SVC_FRAME:
-	{
-		uint8_t nodeId;
-		L2_FrameGetChunk(msg, L2_MSG_SRC_ID, L2_MSG_SRC_ID_LEN, &nodeId);
-		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: %" PRIu8 "]", nodeId);
-				Status = ENOMEM;
-				break;
-			}
-		}
-
-		// Update node info.
-		float nodePos;
-		L2_FrameGetChunk(msg, L2_MSG_SVC_SRC_POSX, L2_MSG_SVC_SRC_POSX_LEN, &nodePos);
-		node->nodeInfo.pos.x = (double)nodePos;
-		L2_FrameGetChunk(msg, L2_MSG_SVC_SRC_POSY, L2_MSG_SVC_SRC_POSY_LEN, &nodePos);
-		node->nodeInfo.pos.y = (double)nodePos;
-		L2_FrameGetChunk(msg, L2_MSG_SVC_SRC_POSZ, L2_MSG_SVC_SRC_POSZ_LEN, &nodePos);
-		node->nodeInfo.pos.z = (double)nodePos;
-		float nodeSpeed;
-		L2_FrameGetChunk(msg, L2_MSG_SVC_SRC_SPEEDX, L2_MSG_SVC_SRC_SPEEDX_LEN, &nodeSpeed);
-		node->nodeInfo.speed.x = (double)nodePos;
-		L2_FrameGetChunk(msg, L2_MSG_SVC_SRC_SPEEDY, L2_MSG_SVC_SRC_SPEEDY_LEN, &nodeSpeed);
-		node->nodeInfo.speed.y = (double)nodePos;
-		L2_FrameGetChunk(msg, L2_MSG_SVC_SRC_SPEEDZ, L2_MSG_SVC_SRC_SPEEDZ_LEN, &nodeSpeed);
-		node->nodeInfo.speed.z = (double)nodePos;
-
-		// Reset node ttl
-		node->ttlCnt = 0;
-
-		// Calculate time deviation
-		uint32_t frameNum, slotNum = 0;
-		L2_FrameGetChunk(msg, L2_MSG_FRAME_NUM, L2_MSG_FRAME_NUM_LEN, &frameNum);
-		L2_FrameGetChunk(msg, L2_MSG_SLOT_NUM, L2_MSG_SLOT_NUM_LEN, &slotNum);
-		Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev));
-		if (Status != SUCCESS)
-		{
-			DBG_ERROR("Failed to compute time deviation.\n")
-			break;
-		}
-
-		uint32_t dataId;
-		L2_FrameGetChunk(msg, L2_MSG_SVC_PAYLOAD_ID, L2_MSG_SVC_PAYLOAD_ID_LEN, &dataId);
-
+	case L2_FRAME_DIS:
 		if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
 		{
-			SysMonitor_UpdateRecvPayload(msg + L2_MSG_SVC_PAYLOAD, dataId, PAYLOAD_LEN_FLJ_RX);
+			DBG_ERROR("Unexpected call from head node, only member Node can handle L2_FRAME_DIS.\n");
+			return EUNEXP;
+		}
+	case L2_FRAME_MNG:
+	case L2_FRAME_RTT:
+		Status = Service_PktHandler128K(instPtr, msg);
+		break;
+	case L2_FRAME_SVC:
+		switch (instPtr->config.serviceRate)
+		{
+		case L2_SVC_128K:
+			Status = Service_PktHandler128K(instPtr, msg);
+			break;
+		case L2_SVC_512K:
+			Status = Service_PktHandler512K(instPtr, msg);
+			break;
+		case L2_SVC_2M:
+			Status = Service_PktHandler2M(instPtr, msg);
+			break;
+		default:
+			DBG_ERROR("Unexpected service rate: %" PRIu8 ".\n", instPtr->config.serviceRate);
+			return EPARAM;
 		}
-		else
-		{
-			SysMonitor_UpdateRecvPayload(msg + L2_MSG_SVC_PAYLOAD, dataId, PAYLOAD_LEN_LJ_RX);
-		}
-
-		// Logging
-		memcpy(instPtr->log.rxLog.data, msg, L2_SVC_PACK_LEN);
-		instPtr->log.rxLog.dataId = dataId;
-		instPtr->serviceFSM->rxdCnt++;
-
-#ifdef DATALINK_ENABLE_AUTOTIMESYNC
-		// Implement a auto time sync.
-		if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
-		{
-			// Time sync.
-			L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
-		}
-#endif
 		break;
-	}
 	default:
 		DBG_ERROR("Unsupported frame type: %" PRIu8 ".\n", frameType);
-		break;
+		return EPARAM;
 	}
 	return Status;
+
 #else
 	uint32_t errorBits = 0;
-	for (int i = 0; i < L2_SVC_PACK_LEN; i++)
+	uint32_t pktLen = L2_GetSvcPacketLen(instPtr->config.serviceRate);
+
+	for (int i = 0; i < pktLen; i++)
 	{
 		errorBits += count_ones(msg[i] ^ testData[i]);
 	}
 
 	instPtr->serviceFSM->bitError += errorBits;
-	instPtr->serviceFSM->bitTotal += L2_SVC_PACK_LEN * 8;
+	instPtr->serviceFSM->bitTotal += pktLen * 8;
 
 	// Calculate time deviation
-	uint32_t frameNum, slotNum = 0;
-	L2_FrameGetChunk(msg, L2_MSG_FRAME_NUM, L2_MSG_FRAME_NUM_LEN, &frameNum);
-	L2_FrameGetChunk(msg, L2_MSG_SLOT_NUM, L2_MSG_SLOT_NUM_LEN, &slotNum);
+	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)
 	{
@@ -289,6 +420,231 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg)
 #endif
 }
 
+/*****************************************************************************/
+/**
+ * @brief   Handle service received packet. Send data up to MPC. (128 kbps)
+ *
+ * @param   msg A point32_ter the the message frame.
+ *
+ * @return  SUCCESS message has been successfully handled.
+ *          other       error occurred.
+ *
+ ******************************************************************************/
+static int32_t Service_PktHandler128K(L2_INST *instPtr, uint8_t *msg)
+{
+	ASSERT(instPtr != NULL);
+	ASSERT(msg != NULL);
+	int32_t Status;
+
+	L2_MSG_SVC_128K *message = (L2_MSG_SVC_128K *)msg;
+	size_t msgLen = sizeof(L2_MSG_SVC_128K);
+	size_t dataLen = L2_PAYLOAD_LEN_SVC_128K;
+
+	// 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);
+			return ENOMEM;
+		}
+	}
+
+	// Update node info.
+	node->nodeInfo.pos.x = message->srcPosX;
+	node->nodeInfo.pos.y = message->srcPosY;
+	node->nodeInfo.pos.z = message->srcPosZ;
+	node->nodeInfo.speed.x = message->srcSpeedX;
+	node->nodeInfo.speed.y = message->srcSpeedY;
+	node->nodeInfo.speed.z = 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")
+		return Status;
+	}
+
+	// Get data payload
+	SysMonitor_UpdateRecvPayload(message->svcData, message->svcDataId, dataLen);
+
+	// Logging...
+	memcpy(instPtr->log.rxLog.data, msg, msgLen);
+	instPtr->log.rxLog.dataId = message->svcDataId;
+
+#ifdef DATALINK_ENABLE_AUTOTIMESYNC
+	// Implement a auto time sync.
+	if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
+	{
+		// Time sync.
+		L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
+	}
+#endif
+
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+ * @brief   Handle service received packet. Send data up to MPC. (512 kbps)
+ *
+ * @param   msg A point32_ter the the message frame.
+ *
+ * @return  SUCCESS message has been successfully handled.
+ *          other       error occurred.
+ *
+ ******************************************************************************/
+static int32_t Service_PktHandler512K(L2_INST *instPtr, uint8_t *msg)
+{
+	ASSERT(instPtr != NULL);
+	ASSERT(msg != NULL);
+	int32_t Status;
+
+	L2_MSG_SVC_512K *message = (L2_MSG_SVC_512K *)msg;
+	size_t msgLen = sizeof(L2_MSG_SVC_512K);
+	size_t dataLen = L2_PAYLOAD_LEN_SVC_512K;
+
+	// 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);
+			return ENOMEM;
+		}
+	}
+
+	// Update node info.
+	node->nodeInfo.pos.x = message->srcPosX;
+	node->nodeInfo.pos.y = message->srcPosY;
+	node->nodeInfo.pos.z = message->srcPosZ;
+	node->nodeInfo.speed.x = message->srcSpeedX;
+	node->nodeInfo.speed.y = message->srcSpeedY;
+	node->nodeInfo.speed.z = 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")
+		return Status;
+	}
+
+	// Get data payload
+	SysMonitor_UpdateRecvPayload(message->svcData, message->svcDataId, dataLen);
+
+	// Logging...
+	memcpy(instPtr->log.rxLog.data, msg, msgLen);
+	instPtr->log.rxLog.dataId = message->svcDataId;
+
+#ifdef DATALINK_ENABLE_AUTOTIMESYNC
+	// Implement a auto time sync.
+	if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
+	{
+		// Time sync.
+		L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
+	}
+#endif
+
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+ * @brief   Handle service received packet. Send data up to MPC. (2 Mbps)
+ *
+ * @param   msg A point32_ter the the message frame.
+ *
+ * @return  SUCCESS message has been successfully handled.
+ *          other       error occurred.
+ *
+ ******************************************************************************/
+static int32_t Service_PktHandler2M(L2_INST *instPtr, uint8_t *msg)
+{
+	ASSERT(instPtr != NULL);
+	ASSERT(msg != NULL);
+	int32_t Status;
+
+	L2_MSG_SVC_2M *message = (L2_MSG_SVC_2M *)msg;
+	size_t msgLen = sizeof(L2_MSG_SVC_2M);
+	size_t dataLen = L2_PAYLOAD_LEN_SVC_2M;
+
+	// 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);
+			return ENOMEM;
+		}
+	}
+
+	// Update node info.
+	node->nodeInfo.pos.x = message->srcPosX;
+	node->nodeInfo.pos.y = message->srcPosY;
+	node->nodeInfo.pos.z = message->srcPosZ;
+	node->nodeInfo.speed.x = message->srcSpeedX;
+	node->nodeInfo.speed.y = message->srcSpeedY;
+	node->nodeInfo.speed.z = 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")
+		return Status;
+	}
+
+	// Get data payload
+	SysMonitor_UpdateRecvPayload(message->svcData, message->svcDataId, dataLen);
+
+	// Logging...
+	memcpy(instPtr->log.rxLog.data, msg, msgLen);
+	instPtr->log.rxLog.dataId = message->svcDataId;
+
+#ifdef DATALINK_ENABLE_AUTOTIMESYNC
+	// Implement a auto time sync.
+	if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
+	{
+		// Time sync.
+		L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
+	}
+#endif
+
+	return Status;
+}
+
 /*****************************************************************************/
 /**
  * @brief   Initialize service net FSM
@@ -338,7 +694,7 @@ void L2_Service_Reset(L2_SVC_FSM *svcFSMPtr)
 /**
  * @brief  Destroy ServiceNet FSM, free memory
  *
- * @param   scanFsmPtr a pointer to service net finite state machine
+ * @param   serviceFSM a pointer to service net finite state machine
  *
  * @return  NONE
  *
@@ -369,55 +725,157 @@ int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntr
 	}
 
 	PHY_INST *phyRef = instPtr->config.phyInstPtr;
+	PHY_ANT_INDEX antIndex;
 	L2_SVC_FSM *svcFSMPtr = instPtr->serviceFSM;
 	L2_TOR_TYPE torState;
 	const NODE_ENTRY *commNode;
-	if (rtneEntry->src == L2_GetNodeId(instPtr))
-	{
-		torState = L2_TOR_TX;
-		commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->dst);
-	}
-	else if (rtneEntry->dst == L2_GetNodeId(instPtr))
-	{
-		torState = L2_TOR_RX;
-		commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
-	}
-	else
-	{
-		torState = L2_TOR_SUSPEND;
-	}
-
-	// Setup antenna beam
-	if (L2_GetNodeType(instPtr) != L2_NODE_HEAD && commNode != NULL)
-	{
-		Phy_SetBeamWidth(phyRef, PHY_ANT_BEAM_16);
-		Phy_SetBeamNo(phyRef, commNode->nodeInfo.beamNo, PHY_ANT_BEAM_16);
-		instPtr->log.lastBeamWidth = PHY_ANT_BEAM_16;
-	}
-	else
-	{
-		Phy_SetBeamWidth(phyRef, PHY_ANT_BEAM_120);
-		instPtr->log.lastBeamWidth = PHY_ANT_BEAM_120;
-	}
 
 	switch (rtneEntry->slotType)
 	{
-	case L2_SLOT_SERVICE:
-		if (torState == L2_TOR_TX)
+	case L2_SLOT_DISTRIBUTE:
+		if (L2_GetNodeType(instPtr) == L2_NODE_HEAD && rtneEntry->src == L2_GetNodeId(instPtr))
 		{
-			svcFSMPtr->txCnt++;
-#ifndef DATALINK_ENABLE_BERTEST
-			// Packing service data
-			Service_Packing(instPtr, L2_SVC_FRAME, rtneEntry->dst);
-#else
-			memcpy(instPtr->sendPktData, testData, L2_SVC_PACK_LEN);
-#endif
+			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->dst);
+			// antenna selection based on destination id in routing table
+			antIndex = commNode->nodeInfo.antIndex;
+			// head node transmits messages in distribute slot
+			torState = L2_TOR_TX;
+
+			// reset memory
+			memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX);
+			// pack message
+			if (Service_Packing(instPtr, L2_FRAME_DIS, rtneEntry->dst) != SUCCESS)
+			{
+				return FAILURE;
+			}
+		}
+		else if (L2_GetNodeType(instPtr) == L2_NODE_MEMBER && rtneEntry->dst == L2_GetNodeId(instPtr))
+		{
+			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_DIS);
+		break;
+	case L2_SLOT_MANAGE:
+		if (rtneEntry->src == L2_GetNodeId(instPtr))
+		{
+			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->dst);
+			// antenna selection based on destination id in routing table
+			antIndex = commNode->nodeInfo.antIndex;
+			// head node transmits messages in distribute slot
+			torState = L2_TOR_TX;
+
+			// reset memory
+			memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX);
+			// pack message
+			if (Service_Packing(instPtr, L2_FRAME_MNG, rtneEntry->dst) != SUCCESS)
+			{
+				return FAILURE;
+			}
+		}
+		else if (rtneEntry->dst == L2_GetNodeId(instPtr))
+		{
+			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_MNG);
+		break;
+	case L2_SLOT_RTT:
+		if (rtneEntry->src == L2_GetNodeId(instPtr))
+		{
+			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->dst);
+			// antenna selection based on destination id in routing table
+			antIndex = commNode->nodeInfo.antIndex;
+			// head node transmits messages in distribute slot
+			torState = L2_TOR_TX;
+
+			// reset memory
+			memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX);
+			// pack message
+			if (Service_Packing(instPtr, L2_FRAME_RTT, rtneEntry->dst) != SUCCESS)
+			{
+				return FAILURE;
+			}
+		}
+		else if (rtneEntry->dst == L2_GetNodeId(instPtr))
+		{
+			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_RTT);
+		break;
+	case L2_SLOT_SERVICE:
+		if (rtneEntry->src == L2_GetNodeId(instPtr))
+		{
+			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->dst);
+			// antenna selection based on destination id in routing table
+			antIndex = commNode->nodeInfo.antIndex;
+			// head node transmits messages in distribute slot
+			torState = L2_TOR_TX;
+
+			svcFSMPtr->txCnt++;
+
+			// reset memory
+			memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX);
+#ifndef DATALINK_ENABLE_BERTEST
+			// pack message
+			if (Service_Packing(instPtr, L2_FRAME_SVC, rtneEntry->dst) != SUCCESS)
+			{
+				return FAILURE;
+			}
+#else
+			size_t msgLen = L2_GetSvcPacketLen(instPtr->config.serviceRate);
+			memcpy(instPtr->sendPktData, testData, msgLen);
+#endif
+		}
+		else if (rtneEntry->dst == L2_GetNodeId(instPtr))
+		{
+			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;
+
 			svcFSMPtr->rxCnt++;
 		}
-		L2_SetupNextSlot(instPtr, torState, L2_SVC_FRAME);
+		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_SVC);
 		break;
 	default:
 		DBG_ERROR("UNKNOWN slot type [%" PRIu8 "].\n", rtneEntry->slotType);
@@ -435,4 +893,4 @@ int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntr
 }
 
 /** @} LinkLayer_Service*/
-/** @} LinkLayer*/
+/** @} LinkLayer*/

df_datalink_sp/src/networking/phy_layer/phy_layer.c

@@ -64,7 +64,7 @@
 
 /************************** Variable Definitions *****************************/
 extern const uint16_t freqTable[L1_FRAME_MAX_HOPS];
-extern const int16_t antCalTable[L1_FRAME_MAX_HOPS][L1_ANT_CH_NUM];
+extern const int16_t antCalTable[L1_ANT_NUM][L1_FRAME_MAX_HOPS][L1_ANT_CH_NUM];
 extern const uint8_t hopPattern_ADV_24MS[L1_FRAME_ADV_24MS_HOPS];
 extern const uint8_t hopPattern_ACK_8MS[L1_FRAME_ACK_8MS_HOPS];
 extern const uint8_t hopPattern_128KBPS_4MS[L1_FRAME_128KBPS_4MS_HOPS];
@@ -388,7 +388,7 @@ int32_t Phy_Init(PHY_INST **instPtr, PHY_CONFIG *phyConfig)
 	Phy_AntConfigInit(&((*instPtr)->antConfig));
 
 	memset(&((*instPtr))->tor, PHY_TOR_SUSPEND, sizeof(PHY_RCD));
-	memset(&((*instPtr)->frameType), L1_NONE_FRAME, sizeof(PHY_RCD));
+	memset(&((*instPtr)->frameType), L1_FRAME_NONE, sizeof(PHY_RCD));
 	memset(&((*instPtr)->antIndex), (int32_t)L1_INIT_ANT_IDX, sizeof(PHY_RCD));
 	memset(&((*instPtr)->hwSate), 0x00, sizeof(HWSTATE));
 	memset(&((*instPtr)->phyLog), 0x00, sizeof(PHYLOG));
@@ -1058,14 +1058,14 @@ void Phy_RefreshHwSate(PHY_INST *instPtr)
  */
 /*****************************************************************************/
 /**
- * @brief   Get the antenna number after physical listening.
+ * @brief   Get the antenna number after physical receive.
  *
  * @param   instPtr  a pointer to physical instance
  *
  * @return  antenna index
  *
  ******************************************************************************/
-uint8_t Phy_ListenAntIndex(PHY_INST *instPtr)
+uint8_t Phy_ReceiveAntIndex(PHY_INST *instPtr)
 {
 	ASSERT(instPtr != NULL);
 	return (uint8_t)PhyRead(instPtr, ANT_SELECT_INDEX);

df_datalink_sp/src/networking/phy_layer/phy_layer.h

@@ -265,7 +265,7 @@ L1_FRAME_TYPE Phy_GetLastSendFrameType(PHY_INST *instPtr);
  * @defgroup PhysicalLayer_Receive_Estimations
  * @{
  */
-uint8_t Phy_ListenAntIndex(PHY_INST *instPtr);
+uint8_t Phy_ReceiveAntIndex(PHY_INST *instPtr);
 double Phy_GetFreqEst(PHY_INST *instPtr);
 double Phy_GetCoarseFreqEst(PHY_INST *instPtr);
 double Phy_GetFineFreqEst(PHY_INST *instPtr);

df_datalink_sp/src/system_monitor.h

@@ -148,9 +148,9 @@ typedef struct __attribute__((packed))
 	uint32_t ackTxCnt;		///< ack frame send count
 	uint32_t ackRecvCnt;	///< ack frame received count
 	uint32_t ackCorrectCnt; ///< ack frame received correctly count
-	uint32_t distTxCnt;		///< dist frame send count
-	uint32_t distRecvCnt;	///< dist frame received count
-	uint32_t distCorrectCnt;///< dist frame received correctly count
+	uint32_t disTxCnt;		///< dist frame send count
+	uint32_t disRecvCnt;	///< dist frame received count
+	uint32_t disCorrectCnt;///< dist frame received correctly count
 	uint32_t mngTxCnt;		///< mng frame send count
 	uint32_t mngRecvCnt;	///< mng frame received count
 	uint32_t mngCorrectCnt; ///< mng frame received correctly count

df_datalink_sp/src/systembus/systembus.c

@@ -310,40 +310,40 @@ static int32_t SystemBus_SystemConfig(SYSBUS *instPtr, uint8_t *msg)
 		return EPARAM;
 	}
 
-	if (msg[WD_EN_CMD] == EN_CMD_ON_HIGH_POWER)
-	{
-		// full power config
-		Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_FULL);
-		if (reconfig || !L2_isStarted(linkInstPtr))
-		{
-			Linklayer_Reconfig(linkInstPtr, &linkConfig);
-		}
-		Linklayer_Start(linkInstPtr);
-	}
-	else if (msg[WD_EN_CMD] == EN_CMD_ON_LOW_POWER)
-	{
-		// low power config
-		Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_LOW);
-		if (reconfig || !L2_isStarted(linkInstPtr))
-		{
-			Linklayer_Reconfig(linkInstPtr, &linkConfig);
-		}
-		Linklayer_Start(linkInstPtr);
-	}
-	else if (msg[WD_EN_CMD] == EN_CMD_SILENT)
-	{
-		// silence config
-		// TODO: better approach?
-		Linklayer_Halt(linkInstPtr);
-		Phy_AntConfigMode(phyRef, PHY_ANT_MODE_STANDBY);
-		TIME_WORD pullTime = {0, 0};
-		pullTime.int_part = TimeCtrl_GetIntCnt(tmcRef) + 1;
-		if (Phy_SetTimeWord(phyRef, &pullTime, PHY_TIME_WORD_POLL) != SUCCESS)
-		{
-			DBG_ERROR("Physical set pull time failed.\n");
-		}
-		Phy_SetupNextSlot(phyRef, PHY_TOR_RX, L1_10KBPS_10MS_FRAME, NULL);
-	}
+//	if (msg[WD_EN_CMD] == EN_CMD_ON_HIGH_POWER)
+//	{
+//		// full power config
+//		Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_FULL);
+//		if (reconfig || !L2_isStarted(linkInstPtr))
+//		{
+//			Linklayer_Reconfig(linkInstPtr, &linkConfig);
+//		}
+//		Linklayer_Start(linkInstPtr);
+//	}
+//	else if (msg[WD_EN_CMD] == EN_CMD_ON_LOW_POWER)
+//	{
+//		// low power config
+//		Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_LOW);
+//		if (reconfig || !L2_isStarted(linkInstPtr))
+//		{
+//			Linklayer_Reconfig(linkInstPtr, &linkConfig);
+//		}
+//		Linklayer_Start(linkInstPtr);
+//	}
+//	else if (msg[WD_EN_CMD] == EN_CMD_SILENT)
+//	{
+//		// silence config
+//		// TODO: better approach?
+//		Linklayer_Halt(linkInstPtr);
+//		Phy_AntConfigMode(phyRef, PHY_ANT_MODE_STANDBY);
+//		TIME_WORD pullTime = {0, 0};
+//		pullTime.int_part = TimeCtrl_GetIntCnt(tmcRef) + 1;
+//		if (Phy_SetTimeWord(phyRef, &pullTime, PHY_TIME_WORD_POLL) != SUCCESS)
+//		{
+//			DBG_ERROR("Physical set pull time failed.\n");
+//		}
+//		Phy_SetupNextSlot(phyRef, PHY_TOR_RX, L1_10KBPS_10MS_FRAME, NULL);
+//	}
 	else
 	{
 		// Ignore current command
@@ -777,4 +777,4 @@ int32_t SystemBus_RxIntrHandler(SYSBUS *instPtr)
 	return SUCCESS;
 }
 
-/** @} SystemBus*/
+/** @} SystemBus*/

df_datalink_sp/unit_test/linklayer_emulate_unit_test.c

@@ -228,7 +228,7 @@ TEST(linklayer_master_routine)
 
 		if (L2_isDataAvail(linkInstPtr))
 		{
-			if (L2_GetLastOptFrameType(linkInstPtr) == L2_CFM_FRAME)
+			if (L2_GetLastFrameType(linkInstPtr) == L2_CFM_FRAME)
 			{
 				ASSERT_EQUAL(1, L2_GetNodeListLen(linkInstPtr->nodeList));
 				ASSERT_TRUE(L2_isNodeInList(linkInstPtr->nodeList, 2));