2 files changed, 38 insertions, 8 deletions

diff --git a/drivers/iio/adc/ti_am335x_adc.c b/drivers/iio/adc/ti_am335x_adc.c
index 0ad208a69c29..3ceac3e91dde 100644
--- a/drivers/iio/adc/ti_am335x_adc.c
+++ b/drivers/iio/adc/ti_am335x_adc.c
@@ -60,7 +60,6 @@ static void tiadc_step_config(struct tiadc_device *adc_dev)
 {
         unsigned int stepconfig;
         int i, steps;
-        u32 step_en;
 
         /*
          * There are 16 configurable steps and 8 analog input
@@ -86,8 +85,7 @@ static void tiadc_step_config(struct tiadc_device *adc_dev)
                 adc_dev->channel_step[i] = steps;
                 steps++;
         }
-        step_en = get_adc_step_mask(adc_dev);
-        am335x_tsc_se_set(adc_dev->mfd_tscadc, step_en);
+
 }
 
 static const char * const chan_name_ain[] = {
@@ -142,10 +140,22 @@ static int tiadc_read_raw(struct iio_dev *indio_dev,
                 int *val, int *val2, long mask)
 {
         struct tiadc_device *adc_dev = iio_priv(indio_dev);
-        int i;
-        unsigned int fifo1count, read;
+        int i, map_val;
+        unsigned int fifo1count, read, stepid;
         u32 step = UINT_MAX;
         bool found = false;
+        u32 step_en;
+        unsigned long timeout = jiffies + usecs_to_jiffies
+                                (IDLE_TIMEOUT * adc_dev->channels);
+        step_en = get_adc_step_mask(adc_dev);
+        am335x_tsc_se_set(adc_dev->mfd_tscadc, step_en);
+
+        /* Wait for ADC sequencer to complete sampling */
+        while (tiadc_readl(adc_dev, REG_ADCFSM) & SEQ_STATUS) {
+                if (time_after(jiffies, timeout))
+                        return -EAGAIN;
+                }
+        map_val = chan->channel + TOTAL_CHANNELS;
 
         /*
          * When the sub-system is first enabled,
@@ -170,12 +180,16 @@ static int tiadc_read_raw(struct iio_dev *indio_dev,
         fifo1count = tiadc_readl(adc_dev, REG_FIFO1CNT);
         for (i = 0; i < fifo1count; i++) {
                 read = tiadc_readl(adc_dev, REG_FIFO1);
-                if (read >> 16 == step) {
-                        *val = read & 0xfff;
+                stepid = read & FIFOREAD_CHNLID_MASK;
+                stepid = stepid >> 0x10;
+
+                if (stepid == map_val) {
+                        read = read & FIFOREAD_DATA_MASK;
                         found = true;
+                        *val = read;
                 }
         }
-        am335x_tsc_se_update(adc_dev->mfd_tscadc);
+
         if (found == false)
                 return -EBUSY;
         return IIO_VAL_INT;
diff --git a/include/linux/mfd/ti_am335x_tscadc.h b/include/linux/mfd/ti_am335x_tscadc.h
index 8d73fe29796a..db1791bb997a 100644
--- a/include/linux/mfd/ti_am335x_tscadc.h
+++ b/include/linux/mfd/ti_am335x_tscadc.h
@@ -113,11 +113,27 @@
 #define CNTRLREG_8WIRE          CNTRLREG_AFE_CTRL(3)
 #define CNTRLREG_TSCENB         BIT(7)
 
+/* FIFO READ Register */
+#define FIFOREAD_DATA_MASK (0xfff << 0)
+#define FIFOREAD_CHNLID_MASK (0xf << 16)
+
+/* Sequencer Status */
+#define SEQ_STATUS BIT(5)
+
 #define ADC_CLK                 3000000
 #define MAX_CLK_DIV             7
 #define TOTAL_STEPS             16
 #define TOTAL_CHANNELS          8
 
+/*
+* ADC runs at 3MHz, and it takes
+* 15 cycles to latch one data output.
+* Hence the idle time for ADC to
+* process one sample data would be
+* around 5 micro seconds.
+*/
+#define IDLE_TIMEOUT 5 /* microsec */
+
 #define TSCADC_CELLS            2
 
 struct ti_tscadc_dev {