Ported DueX I2C fixes from release 3.2.2

1 files changed, 8 insertions, 3 deletions

diff --git a/src/Hardware/I2C.cpp b/src/Hardware/I2C.cpp
index c62db5e7..50db157b 100644
--- a/src/Hardware/I2C.cpp
+++ b/src/Hardware/I2C.cpp
@@ -21,6 +21,7 @@ void I2C::Init() noexcept
 		MutexLocker lock(Tasks::GetI2CMutex());
 		if (!i2cInitialised)					// test it again, now that we own the mutex
 		{
+			NVIC_SetPriority(I2C_IRQn, NvicPriorityTwi);	// we use I2C to talk to the DueX before Platform::InitialiseInterrupts is called, so need to do this here
 			I2C_IFACE.BeginMaster(I2cClockFreq);
 			i2cInitialised = true;
 		}
@@ -43,15 +44,19 @@ extern "C" void WIRE_ISR_HANDLER() noexcept
 
 uint32_t I2C::statusWaitFunc(Twi *twi, uint32_t bitsToWaitFor) noexcept
 {
+	bool ok = true;
 	uint32_t sr = twi->TWI_SR;
-	if ((sr & bitsToWaitFor) == 0)
+	while (ok && (sr & bitsToWaitFor) == 0)
 	{
 		// Suspend this task until we get an interrupt indicating that a status bit that we are interested in has been set
 		twiTask = TaskBase::GetCallerTaskHandle();
+		twi->TWI_IDR = 0xFFFFFFFF;
 		twi->TWI_IER = bitsToWaitFor;
-		(void)TaskBase::Take(2);
-		sr = twi->TWI_SR;
+		NVIC_EnableIRQ(I2C_IRQn);
+		ok = TaskBase::Take(2);
+		twiTask = nullptr;
 		twi->TWI_IDR = 0xFFFFFFFF;
+		sr = twi->TWI_SR;
 	}
 	return sr;
 }