diff --git a/df_datalink_ge/src/mpc/mpc.c b/df_datalink_ge/src/mpc/mpc.c
index e77ce97..3dd3ed2 100644
--- a/df_datalink_ge/src/mpc/mpc.c
+++ b/df_datalink_ge/src/mpc/mpc.c
@@ -151,13 +151,14 @@ static int32_t Mpc_GenSelfTestRsp(MPC *instPtr)
 	ASSERT(instPtr != NULL);
 	MPC_HW_FRAME *mpcRsp;
 	uint8_t *msg;
-	if (instPtr->isEnable == false)
+	if (Mpc_IsEnable(instPtr) == false)
 	{
 		DBG_ERROR("System is not enabled!\n");
 		return EIGNORE;
 	}
 	mpcRsp = malloc(sizeof(MPC_HW_FRAME));
-	if (mpcRsp == NULL) {
+	if (mpcRsp == NULL)
+	{
 		DBG_ERROR("Failed to allocate memory for LJ REMOTE TEST Response!\n");
 		return ENOMEM;
 	}
@@ -169,13 +170,13 @@ static int32_t Mpc_GenSelfTestRsp(MPC *instPtr)
 	msg[WD_CMD] = CMD_SELF_TEST;
 	msg[WD_PRD] = MY_PRD_ID;
 	SysMonitor_GetHardwareStatus(hwStatsPtr);
-	if (linkInstPtr->isEnable == false)
+	if (Mpc_IsEnable(instPtr))
 	{
-		msg[WD_ST_RET] = (hwStatsPtr->isSpOk) ? ST_RET_OK : ST_RET_ERROR;
+		msg[WD_ST_RET] = (hwStatsPtr->isSpOk && hwStatsPtr->isAntOk) ? ST_RET_OK : ST_RET_ERROR;
 	}
 	else
 	{
-		msg[WD_ST_RET] = (hwStatsPtr->isSpOk && hwStatsPtr->isAntOk) ? ST_RET_OK : ST_RET_ERROR;
+		msg[WD_ST_RET] = (hwStatsPtr->isSpOk) ? ST_RET_OK : ST_RET_ERROR;
 	}
 	// apply status and omit 2 bytes of overhead.
 	memcpy(msg + WD_SP_FPGA_VOLT, &(hwStatsPtr->spFpgaVolt), ST_RET_INFO_LEN);
@@ -201,47 +202,49 @@ static int32_t Mpc_SystemConfig(MPC *instPtr, uint8_t *msg)
 	if (msg[WD_MODE_CMD] == MODE_CMD_HEAD)
 	{
 		// omit unnecessary reconfiguration
-		if (L2_GetNodeType(linkInstPtr) != L2_NODE_HEAD)
-		{
-			// head node config
-			linkConfig.myType = L2_NODE_HEAD;
-			linkConfig.myId = 1;
-			reconfig = true;
-		}
+		reconfig = (L2_NODE_HEAD != L2_GetNodeType(linkInstPtr));
+		linkConfig.myType = L2_NODE_HEAD;
+		linkConfig.myId = 2;
 	}
 
-	else if (msg[WD_MODE_CMD] == MODE_CMD_MEMBER )
+	else if (msg[WD_MODE_CMD] == MODE_CMD_MEMBER)
 	{
 		// omit unnecessary reconfiguration
-		if (L2_GetNodeType(linkInstPtr) != L2_NODE_MEMBER)
-		{
-			// member node config
-			linkConfig.myType = L2_NODE_MEMBER;
-			linkConfig.myId = 2;
-			reconfig = true;
-		}
+		reconfig = (L2_NODE_MEMBER != L2_GetNodeType(linkInstPtr));
+		linkConfig.myType = L2_NODE_MEMBER;
+		linkConfig.myId = 2;
 	}
 	else
 	{
 		// Ignore current command
 		DBG_ERROR("Unexpected config command 0x%02X:0x%02X",
-			WD_MODE_CMD, msg[WD_MODE_CMD]);
-		return EPARAM;
+				  WD_MODE_CMD, msg[WD_MODE_CMD]);
+		return EIGNORE;
 	}
 
-	if(msg[WD_EN_CMD] == EN_CMD_ON_HIGH_POWER)
+	if (msg[WD_EN_CMD] == EN_CMD_ON_HIGH_POWER)
 	{
 		// high power config
 		Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_FULL);
+		if (reconfig == true || !Mpc_IsEnable(instPtr))
+		{
+			Linklayer_Reconfig(linkInstPtr, &linkConfig);
+		}
+		Linklayer_Start(linkInstPtr);
 		instPtr->isEnable = true;
 	}
-	else if(msg[WD_EN_CMD] == EN_CMD_ON_LOW_POWER)
+	else if (msg[WD_EN_CMD] == EN_CMD_ON_LOW_POWER)
 	{
 		// low power config
 		Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_LOW);
+		if (reconfig == true || !Mpc_IsEnable(instPtr))
+		{
+			Linklayer_Reconfig(linkInstPtr, &linkConfig);
+		}
+		Linklayer_Start(linkInstPtr);
 		instPtr->isEnable = true;
 	}
-	else if(msg[WD_EN_CMD] == EN_CMD_SILENT)
+	else if (msg[WD_EN_CMD] == EN_CMD_SILENT)
 	{
 		// silence config
 		// TODO: better approach?
@@ -256,20 +259,18 @@ static int32_t Mpc_SystemConfig(MPC *instPtr, uint8_t *msg)
 		}
 		Phy_SetupNextSlot(phyRef, PHY_TOR_RX, L1_10KBPS_10MS_FRAME, NULL);
 	}
-	if (reconfig == true)
+	else
 	{
-		Linklayer_Reconfig(linkInstPtr, &linkConfig);
-	}
-	if (instPtr->isEnable == true)
-	{
-		Linklayer_Start(linkInstPtr);
+		// Ignore current command
+		DBG_ERROR("Unexpected config command 0x%02X:0x%02X",
+				  WD_EN_CMD, msg[WD_EN_CMD]);
+		return EIGNORE;
 	}
 	instPtr->mpcLog.lastConfig[0] = msg[WD_MODE_CMD];
 	instPtr->mpcLog.lastConfig[1] = msg[WD_EN_CMD];
 	return SUCCESS;
 }
 
-
 /*****************************************************************************/
 /**
  * @brief   Initialize MPC instance.
@@ -371,13 +372,14 @@ int32_t Mpc_PendRemoteTest(MPC *instPtr)
 {
 	MPC_HW_FRAME *mpcRsp;
 	uint8_t *msg;
-	if (instPtr->isEnable == false)
+	if (!Mpc_IsEnable(instPtr))
 	{
 		DBG_ERROR("System is not enabled!\n");
 		return EIGNORE;
 	}
 	mpcRsp = malloc(sizeof(MPC_HW_FRAME));
-	if (mpcRsp == NULL) {
+	if (mpcRsp == NULL)
+	{
 		DBG_ERROR("Failed to allocate memory for LJ REMOTE TEST Response!\n");
 		return ENOMEM;
 	}
@@ -407,7 +409,8 @@ int32_t Mpc_PendRemoteTest(MPC *instPtr)
 	memcpy(msg + WD_PHY_LOG, &(instPtr->phyLog), WD_PHY_LOG_LEN);
 	memcpy(msg + WD_HARDWARE_ST, &(instPtr->hwState.spFpgaVolt), WD_HARDWARE_ST_LEN);
 
-	if (CircQueuePush(instPtr->txQueue, (void *)mpcRsp) == false){
+	if (CircQueuePush(instPtr->txQueue, (void *)mpcRsp) == false)
+	{
 		free(mpcRsp);
 		DBG_WARNING("SystemBus::TX Queue overflowed!\n");
 		return EQUEUE;
diff --git a/df_datalink_sp/src/systembus/systembus.c b/df_datalink_sp/src/systembus/systembus.c
index 315fb67..0cbd917 100644
--- a/df_datalink_sp/src/systembus/systembus.c
+++ b/df_datalink_sp/src/systembus/systembus.c
@@ -248,13 +248,13 @@ static int32_t SystemBus_GenSelfTestRsp(SYSBUS *instPtr)
 	sysbusRsp->data[WD_CMD] = CMD_SELF_TEST;
 	sysbusRsp->data[WD_PRD] = MY_PRD_ID;
 	SysMonitor_GetHardwareStatus(hwStatsPtr);
-	if (linkInstPtr->isEnable == false)
+	if (L2_isStarted(linkInstPtr))
 	{
-		sysbusRsp->data[WD_ST_RET] = (hwStatsPtr->isSpOk) ? ST_RET_OK : ST_RET_ERROR;
+		sysbusRsp->data[WD_ST_RET] = (hwStatsPtr->isSpOk && hwStatsPtr->isAntOk) ? ST_RET_OK : ST_RET_ERROR;
 	}
 	else
 	{
-		sysbusRsp->data[WD_ST_RET] = (hwStatsPtr->isSpOk && hwStatsPtr->isAntOk) ? ST_RET_OK : ST_RET_ERROR;
+		sysbusRsp->data[WD_ST_RET] = (hwStatsPtr->isSpOk) ? ST_RET_OK : ST_RET_ERROR;
 	}
 	// apply status and omit 2 bytes of overhead.
 	memcpy(sysbusRsp->data + WD_SP_FPGA_VOLT, &(hwStatsPtr->spFpgaVolt), ST_RET_INFO_LEN);
@@ -316,7 +316,6 @@ static int32_t SystemBus_SystemConfig(SYSBUS *instPtr, uint8_t *msg)
 	{
 		// low power config
 		Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_LOW);
-		reconfig = reconfig || (true != L2_isStarted(linkInstPtr));
 		if (reconfig || !L2_isStarted(linkInstPtr))
 		{
 			Linklayer_Reconfig(linkInstPtr, &linkConfig);
@@ -337,6 +336,13 @@ static int32_t SystemBus_SystemConfig(SYSBUS *instPtr, uint8_t *msg)
 		}
 		Phy_SetupNextSlot(phyRef, PHY_TOR_RX, L1_10KBPS_10MS_FRAME, NULL);
 	}
+	else
+	{
+		// Ignore current command
+		DBG_ERROR("Unexpected config command 0x%02X:0x%02X",
+				  WD_EN_CMD, msg[WD_EN_CMD]);
+		return EIGNORE;
+	}
 	instPtr->sysbusLog.lastConfig[0] = msg[WD_MODE_CMD];
 	instPtr->sysbusLog.lastConfig[1] = msg[WD_EN_CMD];
 	return SUCCESS;