diff --git a/Projects/AVRISP-MKII/Lib/XPROG/TINYNVM.c b/Projects/AVRISP-MKII/Lib/XPROG/TINYNVM.c
index c0f04c6bdf0d01db1191792592507fd6898d0244..7b9ff09148c90ccbca89dcacb2df6f9c3f301d05 100644
--- a/Projects/AVRISP-MKII/Lib/XPROG/TINYNVM.c
+++ b/Projects/AVRISP-MKII/Lib/XPROG/TINYNVM.c
@@ -81,7 +81,15 @@ bool TINYNVM_WaitWhileNVMBusBusy(void)
 	{
 		/* Send the SLDCS command to read the TPI STATUS register to see the NVM bus is active */
 		XPROGTarget_SendByte(TPI_CMD_SLDCS | TPI_STATUS_REG);
-		if (XPROGTarget_ReceiveByte() & TPI_STATUS_NVM)
+
+		uint8_t StatusRegister = XPROGTarget_ReceiveByte();
+
+		/* We might have timed out waiting for the status register read response, check here */
+		if (!(TimeoutMSRemaining))
+		  return false;
+
+		/* Check the status register read response to see if the NVM bus is enabled */
+		if (StatusRegister & TPI_STATUS_NVM)
 		{
 			TimeoutMSRemaining = COMMAND_TIMEOUT_MS;
 			return true;
@@ -111,8 +119,14 @@ bool TINYNVM_WaitWhileNVMControllerBusy(void)
 		/* Send the SIN command to read the TPI STATUS register to see the NVM bus is busy */
 		TINYNVM_SendReadNVMRegister(XPROG_Param_NVMCSRRegAddr);
 
+		uint8_t StatusRegister = XPROGTarget_ReceiveByte();
+
+		/* We might have timed out waiting for the status register read response, check here */
+		if (!(TimeoutMSRemaining))
+		  return false;
+
 		/* Check to see if the BUSY flag is still set */
-		if (!(XPROGTarget_ReceiveByte() & (1 << 7)))
+		if (!(StatusRegister & (1 << 7)))
 		{
 			TimeoutMSRemaining = COMMAND_TIMEOUT_MS;
 			return true;
diff --git a/Projects/AVRISP-MKII/Lib/XPROG/XMEGANVM.c b/Projects/AVRISP-MKII/Lib/XPROG/XMEGANVM.c
index f63f13a97ac65383dd073d811272545b97a0f2ac..7ca0c5e8a9d9b2ac0d5629a9ae24cdb57555b0ec 100644
--- a/Projects/AVRISP-MKII/Lib/XPROG/XMEGANVM.c
+++ b/Projects/AVRISP-MKII/Lib/XPROG/XMEGANVM.c
@@ -76,7 +76,15 @@ bool XMEGANVM_WaitWhileNVMBusBusy(void)
 	{
 		/* Send the LDCS command to read the PDI STATUS register to see the NVM bus is active */
 		XPROGTarget_SendByte(PDI_CMD_LDCS | PDI_STATUS_REG);
-		if (XPROGTarget_ReceiveByte() & PDI_STATUS_NVM)
+		
+		uint8_t StatusRegister = XPROGTarget_ReceiveByte();
+		
+		/* We might have timed out waiting for the status register read response, check here */
+		if (!(TimeoutMSRemaining))
+		  return false;
+		
+		/* Check the status register read response to see if the NVM bus is enabled */
+		if (StatusRegister & PDI_STATUS_NVM)
 		{
 			TimeoutMSRemaining = COMMAND_TIMEOUT_MS;
 			return true;
@@ -107,8 +115,14 @@ bool XMEGANVM_WaitWhileNVMControllerBusy(void)
 		XPROGTarget_SendByte(PDI_CMD_LDS | (PDI_DATSIZE_4BYTES << 2));
 		XMEGANVM_SendNVMRegAddress(XMEGA_NVM_REG_STATUS);
 		
+		uint8_t StatusRegister = XPROGTarget_ReceiveByte();
+
+		/* We might have timed out waiting for the status register read response, check here */
+		if (!(TimeoutMSRemaining))
+		  return false;
+
 		/* Check to see if the BUSY flag is still set */
-		if (!(XPROGTarget_ReceiveByte() & (1 << 7)))
+		if (!(StatusRegister & (1 << 7)))
 		{
 			TimeoutMSRemaining = COMMAND_TIMEOUT_MS;
 			return true;
diff --git a/Projects/AVRISP-MKII/Lib/XPROG/XPROGTarget.c b/Projects/AVRISP-MKII/Lib/XPROG/XPROGTarget.c
index bd269edd70ad03892885eebc3c9be04e0863ec41..6f0d87add6c7dfe44e5f1a24b9278273c4498843 100644
--- a/Projects/AVRISP-MKII/Lib/XPROG/XPROGTarget.c
+++ b/Projects/AVRISP-MKII/Lib/XPROG/XPROGTarget.c
@@ -453,22 +453,33 @@ static void XPROGTarget_SetRxMode(void)
 	{
 		BITBANG_PDIDATA_DDR  &= ~BITBANG_PDIDATA_MASK;
 		BITBANG_PDIDATA_PORT &= ~BITBANG_PDIDATA_MASK;
+
+		/* Wait until DATA line has been pulled up to idle by the target */
+		while (!(BITBANG_PDIDATA_PIN & BITBANG_PDIDATA_MASK) && TimeoutMSRemaining)
+		{
+			/* Manage software timeout */
+			if (TIFR0 & (1 << OCF0A))
+			{
+				TIFR0 |= (1 << OCF0A);
+				TimeoutMSRemaining--;
+			}
+		}
 	}
 	else
 	{
 		BITBANG_TPIDATA_DDR  &= ~BITBANG_TPIDATA_MASK;
-		BITBANG_TPIDATA_PORT &= ~BITBANG_TPIDATA_MASK;	
-	}
-	
-	/* Wait until DATA line has been pulled up to idle by the target */
-	while (!(BITBANG_PDIDATA_PIN & BITBANG_PDIDATA_MASK) && TimeoutMSRemaining)
-	{
-		/* Manage software timeout */
-		if (TIFR0 & (1 << OCF0A))
+		BITBANG_TPIDATA_PORT &= ~BITBANG_TPIDATA_MASK;
+
+		/* Wait until DATA line has been pulled up to idle by the target */
+		while (!(BITBANG_TPIDATA_PIN & BITBANG_TPIDATA_MASK) && TimeoutMSRemaining)
 		{
-			TIFR0 |= (1 << OCF0A);
-			TimeoutMSRemaining--;
-		}
+			/* Manage software timeout */
+			if (TIFR0 & (1 << OCF0A))
+			{
+				TIFR0 |= (1 << OCF0A);
+				TimeoutMSRemaining--;
+			}
+		}	
 	}	
 #endif