From a4106b997c20bcd439b60e971b31ed8e9357acba Mon Sep 17 00:00:00 2001
From: Gatsby Van <mail.gatsby.icu>
Date: Mon, 29 Jan 2024 20:39:14 +0800
Subject: [PATCH] bugfix: swap control flow

---
 df_datalink_sp/src/systembus/systembus.c | 51 ++++++++++++------------
 1 file changed, 25 insertions(+), 26 deletions(-)

diff --git a/df_datalink_sp/src/systembus/systembus.c b/df_datalink_sp/src/systembus/systembus.c
index d7819fd..b567c43 100644
--- a/df_datalink_sp/src/systembus/systembus.c
+++ b/df_datalink_sp/src/systembus/systembus.c
@@ -351,6 +351,29 @@ int32_t SystemBus_HandleCmd(SYSBUS *instPtr)
 	{	
 		PHY_INST *phyRef = linkInstPtr->config.phyInstPtr;
 		TMC_INST *tmcRef = linkInstPtr->config.tmcInstPtr;
+		
+		if (data[WD_MODE_CMD] == MODE_CMD_HEAD) 
+		{
+			// head node config
+			linkConfig.myType = L2_NODE_HEAD;
+			linkConfig.myId = 1;
+			Linklayer_Reconfig(linkInstPtr, &linkConfig);
+		}
+		else if (data[WD_MODE_CMD] == MODE_CMD_MEMBER) 
+		{
+			// member node config
+			linkConfig.myType = L2_NODE_MEMBER;
+			linkConfig.myId = 2;
+			Linklayer_Reconfig(linkInstPtr, &linkConfig);
+		}
+		else 
+		{
+			// FIXME: 
+			// Halt linklayer
+			Linklayer_Halt(linkInstPtr);
+			DBG_ERROR("Unexpected config command 0x%02X:0x%02X", 
+				WD_MODE_CMD, data[WD_MODE_CMD]);
+		}
 
 		if(data[WD_EN_CMD] == EN_CMD_ON_HIGH_POWER) 
 		{
@@ -378,33 +401,9 @@ int32_t SystemBus_HandleCmd(SYSBUS *instPtr)
 			{
 				DBG_ERROR("Physical set pull time failed, with error code: %" PRId32 ".\n", Status);
 			}
-			Phy_SetupNextSlot(phyRef, PHY_TOR_TX, L1_10KBPS_10MS_FRAME, NULL);
-		}
-		
-		if (data[WD_MODE_CMD] == MODE_CMD_HEAD) 
-		{
-			// head node config
-			linkConfig.myType = L2_NODE_HEAD;
-			linkConfig.myId = 1;
-			Linklayer_Reconfig(linkInstPtr, &linkConfig);
-			Linklayer_Start(linkInstPtr);
-		}
-		else if (data[WD_MODE_CMD] == MODE_CMD_MEMBER) 
-		{
-			// member node config
-			linkConfig.myType = L2_NODE_MEMBER;
-			linkConfig.myId = 2;
-			Linklayer_Reconfig(linkInstPtr, &linkConfig);
-			Linklayer_Start(linkInstPtr);
-		}
-		else 
-		{
-			// FIXME: 
-			// Halt linklayer
-			Linklayer_Halt(linkInstPtr);
-			DBG_ERROR("Unexpected config command 0x%02X:0x%02X", 
-				WD_MODE_CMD, data[WD_MODE_CMD]);
+			Phy_SetupNextSlot(phyRef, PHY_TOR_RX, L1_10KBPS_10MS_FRAME, NULL);
 		}
+
 		_nop_(); // for debug
 		Status = SUCCESS;
 		break;