quad: Fix I2C polling function freezing issue

Using the original Xilinx polling I2C send functions, if a slaved
NACKed, then the polling function would poll forever (infinite
loop). Copied the Xilinx function into our src and added a check
for NACK.

This resolves #1

quad/sw/modular_quad_pid/src/iic_utils.c

@@ -17,6 +17,9 @@
 XIicPs_Config* i2c_config;
 XIicPs I2C0;
 double magX_correction = -1, magY_correction, magZ_correction;
+int XIicPs_MasterSendPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr, int ByteCount, u16 SlaveAddr);
+int XIicPs_SetupMaster(XIicPs *InstancePtr, int Role);
+int TransmitFifoFill(XIicPs *InstancePtr);
 
 int iic0_init(){
 
@@ -108,7 +111,7 @@ void iic0_mpu9150_write(u8 register_addr, u8 data){
 		device_addr = MPU9150_COMPASS_ADDR;
 	}
 
-	XIicPs_MasterSendPolled(&I2C0, buf, 2, device_addr);
+	XIicPs_MasterSendPolled_ours(&I2C0, buf, 2, device_addr);
 
 }
 
@@ -127,7 +130,7 @@ void iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size){
 	}
 
 
-	XIicPs_MasterSendPolled(&I2C0, buf, 1, device_addr);
+	XIicPs_MasterSendPolled_ours(&I2C0, buf, 1, device_addr);
 	XIicPs_MasterRecvPolled(&I2C0, recv_buffer,size,device_addr);
 }
 
@@ -239,14 +242,14 @@ int iic0_mpu9150_read_gam(gam_t* gam) {
 int iic0_lidarlite_write(u8 register_addr, u8 data) {
 	u8 buf[] = {register_addr, data};
 
-	return XIicPs_MasterSendPolled(&I2C0, buf, 2, LIDARLITE_DEVICE_ADDR);
+	return XIicPs_MasterSendPolled_ours(&I2C0, buf, 2, LIDARLITE_DEVICE_ADDR);
 }
 
 int iic0_lidarlite_read(u8* recv_buffer, u8 register_addr, int size) {
 	u8 buf[] = {register_addr};
 	int status = 0;
 
-	status = XIicPs_MasterSendPolled(&I2C0, buf, 1, LIDARLITE_DEVICE_ADDR);
+	status = XIicPs_MasterSendPolled_ours(&I2C0, buf, 1, LIDARLITE_DEVICE_ADDR);
 	status |= XIicPs_MasterRecvPolled(&I2C0, recv_buffer,size, LIDARLITE_DEVICE_ADDR);
 	return status;
 }
@@ -279,3 +282,177 @@ int iic0_lidarlite_read_distance(lidar_t *lidar) {
 
 	return status;
 }
+
+/*****************************************************************************/
+/**
+* NOTE to MicroCART: This function is originally from the Xilinx library,
+* but we noticed that the original function didn't check for a NACK, which
+* would cause the original polling function to enter an infinite loop in the
+* event of a NACK. Notice that we have added that NACK check at the final
+* while loop of this function.
+*
+*
+* This function initiates a polled mode send in master mode.
+*
+* It sends data to the FIFO and waits for the slave to pick them up.
+* If slave fails to remove data from FIFO, the send fails with
+* time out.
+*
+* @param	InstancePtr is a pointer to the XIicPs instance.
+* @param	MsgPtr is the pointer to the send buffer.
+* @param	ByteCount is the number of bytes to be sent.
+* @param	SlaveAddr is the address of the slave we are sending to.
+*
+* @return
+*		- XST_SUCCESS if everything went well.
+*		- XST_FAILURE if timed out.
+*
+* @note		This send routine is for polled mode transfer only.
+*
+****************************************************************************/
+int XIicPs_MasterSendPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
+		 int ByteCount, u16 SlaveAddr)
+{
+	u32 IntrStatusReg;
+	u32 StatusReg;
+	u32 BaseAddr;
+	u32 Intrs;
+
+	/*
+	 * Assert validates the input arguments.
+	 */
+	Xil_AssertNonvoid(InstancePtr != NULL);
+	Xil_AssertNonvoid(MsgPtr != NULL);
+	Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
+	Xil_AssertNonvoid(XIICPS_ADDR_MASK >= SlaveAddr);
+
+	BaseAddr = InstancePtr->Config.BaseAddress;
+	InstancePtr->SendBufferPtr = MsgPtr;
+	InstancePtr->SendByteCount = ByteCount;
+
+	XIicPs_SetupMaster(InstancePtr, SENDING_ROLE);
+
+	XIicPs_WriteReg(BaseAddr, XIICPS_ADDR_OFFSET, SlaveAddr);
+
+	/*
+	 * Intrs keeps all the error-related interrupts.
+	 */
+	Intrs = XIICPS_IXR_ARB_LOST_MASK | XIICPS_IXR_TX_OVR_MASK |
+			XIICPS_IXR_TO_MASK | XIICPS_IXR_NACK_MASK;
+
+	/*
+	 * Clear the interrupt status register before use it to monitor.
+	 */
+	IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
+	XIicPs_WriteReg(BaseAddr, XIICPS_ISR_OFFSET, IntrStatusReg);
+
+	/*
+	 * Transmit first FIFO full of data.
+	 */
+	TransmitFifoFill(InstancePtr);
+
+	IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
+
+	/*
+	 * Continue sending as long as there is more data and
+	 * there are no errors.
+	 */
+	while ((InstancePtr->SendByteCount > 0) &&
+		((IntrStatusReg & Intrs) == 0)) {
+		StatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_SR_OFFSET);
+
+		/*
+		 * Wait until transmit FIFO is empty.
+		 */
+		if ((StatusReg & XIICPS_SR_TXDV_MASK) != 0) {
+			IntrStatusReg = XIicPs_ReadReg(BaseAddr,
+					XIICPS_ISR_OFFSET);
+			continue;
+		}
+
+		/*
+		 * Send more data out through transmit FIFO.
+		 */
+		TransmitFifoFill(InstancePtr);
+	}
+
+	/*
+	 * Check for completion of transfer.
+	 */
+	// NOTE for MicroCART: Corrected function. Original left for reference.
+//	while ((XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET) &
+//		XIICPS_IXR_COMP_MASK) != XIICPS_IXR_COMP_MASK);
+	while (!(IntrStatusReg & (Intrs | XIICPS_IXR_COMP_MASK))) {
+		IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
+	}
+
+	/*
+	 * If there is an error, tell the caller.
+	 */
+	if (IntrStatusReg & Intrs) {
+		return XST_FAILURE;
+	}
+
+	return XST_SUCCESS;
+}
+
+/*****************************************************************************/
+/*
+* NOTE to MicroCART: This function is required by the send polling method above,
+* but it was originally static, so we had to copy it word-for-word here.
+*
+* This function prepares a device to transfers as a master.
+*
+* @param	InstancePtr is a pointer to the XIicPs instance.
+*
+* @param	Role specifies whether the device is sending or receiving.
+*
+* @return
+*		- XST_SUCCESS if everything went well.
+*		- XST_FAILURE if bus is busy.
+*
+* @note		Interrupts are always disabled, device which needs to use
+*		interrupts needs to setup interrupts after this call.
+*
+****************************************************************************/
+int XIicPs_SetupMaster(XIicPs *InstancePtr, int Role)
+{
+	u32 ControlReg;
+	u32 BaseAddr;
+	u32 EnabledIntr = 0x0;
+
+	Xil_AssertNonvoid(InstancePtr != NULL);
+
+	BaseAddr = InstancePtr->Config.BaseAddress;
+	ControlReg = XIicPs_ReadReg(BaseAddr, XIICPS_CR_OFFSET);
+
+
+	/*
+	 * Only check if bus is busy when repeated start option is not set.
+	 */
+	if ((ControlReg & XIICPS_CR_HOLD_MASK) == 0) {
+		if (XIicPs_BusIsBusy(InstancePtr)) {
+			return XST_FAILURE;
+		}
+	}
+
+	/*
+	 * Set up master, AckEn, nea and also clear fifo.
+	 */
+	ControlReg |= XIICPS_CR_ACKEN_MASK | XIICPS_CR_CLR_FIFO_MASK |
+		 	XIICPS_CR_NEA_MASK | XIICPS_CR_MS_MASK;
+
+	if (Role == RECVING_ROLE) {
+		ControlReg |= XIICPS_CR_RD_WR_MASK;
+		EnabledIntr = XIICPS_IXR_DATA_MASK |XIICPS_IXR_RX_OVR_MASK;
+	}else {
+		ControlReg &= ~XIICPS_CR_RD_WR_MASK;
+	}
+	EnabledIntr |= XIICPS_IXR_COMP_MASK | XIICPS_IXR_ARB_LOST_MASK;
+
+	XIicPs_WriteReg(BaseAddr, XIICPS_CR_OFFSET, ControlReg);
+
+	XIicPs_DisableAllInterrupts(BaseAddr);
+
+	return XST_SUCCESS;
+}