From e427653061dc2d095edcc17faa39d18d23f64d1b Mon Sep 17 00:00:00 2001
From: Brendan Bartels <bbartels@iastate.edu>
Date: Fri, 3 Mar 2017 10:31:42 -0600
Subject: [PATCH] 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 | 185 ++++++++++++++++++++++-
1 file changed, 181 insertions(+), 4 deletions(-)
diff --git a/quad/sw/modular_quad_pid/src/iic_utils.c b/quad/sw/modular_quad_pid/src/iic_utils.c
index 38d2db8dd..33a9228c2 100644
--- a/quad/sw/modular_quad_pid/src/iic_utils.c
+++ b/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;
+}
--
GitLab