T4 - Clear out TX FIFO in RequestFrom and endTransmission

This appears to resolve the hang in the BNO055 test cases.

Basically I detect if the FIFO queue is not empty when you start these two functions and clear it out if it does.  Also NEED to then clear out the FEF status bit after you clear it.

You might want to see if there is a better way.  But @mjs513 said it also resolved his issue with MPL3115

WireIMXRT.cpp

 
 uint8_t TwoWire::endTransmission(uint8_t sendStop)
 {
+	//printf("\nendTransmission %d (%x %x %x) %x\n", txBufferLength,txBuffer[0], txBuffer[1], txBuffer[2], sendStop);
 	uint32_t i=0, len, status;
 
 	len = txBufferLength;
 	}
 	//printf("m=%x\n", status);
 
+	// Wonder if MFSR we should maybe clear it?
+	if ( port->MFSR & 0x7) {
+		port->MCR = LPI2C_MCR_MEN | LPI2C_MCR_RTF;  // clear the FIFO
+		port->MSR = LPI2C_MSR_PLTF | LPI2C_MSR_ALF | LPI2C_MSR_NDF | LPI2C_MSR_SDF | LPI2C_MSR_FEF; // clear flags
+		//printf("Clear TX Fifo %lx %lx\n", port->MSR, port->MFSR);
+	}
 	// TODO: is this correct if the prior use didn't send stop?
 	//port->MSR = LPI2C_MSR_PLTF | LPI2C_MSR_ALF | LPI2C_MSR_NDF | LPI2C_MSR_SDF; // clear flags
 	port->MSR = status;
 	uint32_t cmd=0, status, fifo;
 
 	// wait while bus is busy
+	//printf("\nrequestFrom %x %x %x\n", address, length, sendStop);
 	while (1) {
 		status = port->MSR; // pg 2899 & 2892
 		if (!(status & LPI2C_MSR_BBF)) break; // bus is available
 	//printf("idle2, msr=%x\n", status);
 
 	// TODO: is this correct if the prior use didn't send stop?
-	port->MSR = LPI2C_MSR_PLTF | LPI2C_MSR_ALF | LPI2C_MSR_NDF | LPI2C_MSR_SDF; // clear flags
+	port->MSR = LPI2C_MSR_PLTF | LPI2C_MSR_ALF | LPI2C_MSR_NDF | LPI2C_MSR_SDF | LPI2C_MSR_FEF; // clear flags
 
-	//printf("MSR=%lX, MFSR=%lX\n", status, port->MFSR);
+	//printf("MSR=%lX, MCR:%lx, MFSR=%lX\n", status, port->MCR, port->MFSR);
+
+	// Wonder if MFSR we should maybe clear it?
+	if ( port->MFSR & 0x7) {
+		port->MCR = LPI2C_MCR_MEN | LPI2C_MCR_RTF;  // clear the FIFO
+		port->MSR = LPI2C_MSR_PLTF | LPI2C_MSR_ALF | LPI2C_MSR_NDF | LPI2C_MSR_SDF | LPI2C_MSR_FEF; // clear flags
+		//printf("Clear TX Fifo %lx %lx\n", port->MSR, port->MFSR);
+	}
 	address = (address & 0x7F) << 1;
 	if (length < 1) length = 1;
 	if (length > 255) length = 255;