diff --git a/drivers/iio/adc/ti_am335x_adc.c b/drivers/iio/adc/ti_am335x_adc.c
index 4bec91e40bf7..307a7c07be47 100644
--- a/drivers/iio/adc/ti_am335x_adc.c
+++ b/drivers/iio/adc/ti_am335x_adc.c
@@ -75,7 +75,7 @@ static void tiadc_step_config(struct tiadc_device *adc_dev)
 
 	stepconfig = STEPCONFIG_AVG_16 | STEPCONFIG_FIFO1;
 
-	for (i = (steps + 1); i <= TOTAL_STEPS; i++) {
+	for (i = steps; i < TOTAL_STEPS; i++) {
 		tiadc_writel(adc_dev, REG_STEPCONFIG(i),
 				stepconfig | STEPCONFIG_INP(channels));
 		tiadc_writel(adc_dev, REG_STEPDELAY(i),
diff --git a/drivers/input/touchscreen/ti_am335x_tsc.c b/drivers/input/touchscreen/ti_am335x_tsc.c
index ff3215ddf9f5..1bceb2591fc7 100644
--- a/drivers/input/touchscreen/ti_am335x_tsc.c
+++ b/drivers/input/touchscreen/ti_am335x_tsc.c
@@ -120,11 +120,9 @@ static int titsc_config_wires(struct titsc *ts_dev)
 static void titsc_step_config(struct titsc *ts_dev)
 {
 	unsigned int	config;
-	unsigned int	stepenable = 0;
-	int i, total_steps;
-
-	/* Configure the Step registers */
-	total_steps = 2 * ts_dev->coordinate_readouts;
+	int i;
+	int end_step;
+	u32 stepenable;
 
 	config = STEPCONFIG_MODE_HWSYNC |
 			STEPCONFIG_AVG_16 | ts_dev->bit_xp;
@@ -142,7 +140,9 @@ static void titsc_step_config(struct titsc *ts_dev)
 		break;
 	}
 
-	for (i = 1; i <= ts_dev->coordinate_readouts; i++) {
+	/* 1 … coordinate_readouts is for X */
+	end_step = ts_dev->coordinate_readouts;
+	for (i = 0; i < end_step; i++) {
 		titsc_writel(ts_dev, REG_STEPCONFIG(i), config);
 		titsc_writel(ts_dev, REG_STEPDELAY(i), STEPCONFIG_OPENDLY);
 	}
@@ -150,7 +150,7 @@ static void titsc_step_config(struct titsc *ts_dev)
 	config = 0;
 	config = STEPCONFIG_MODE_HWSYNC |
 			STEPCONFIG_AVG_16 | ts_dev->bit_yn |
-			STEPCONFIG_INM_ADCREFM | STEPCONFIG_FIFO1;
+			STEPCONFIG_INM_ADCREFM;
 	switch (ts_dev->wires) {
 	case 4:
 		config |= ts_dev->bit_yp | STEPCONFIG_INP(ts_dev->inp_xp);
@@ -164,12 +164,13 @@ static void titsc_step_config(struct titsc *ts_dev)
 		break;
 	}
 
-	for (i = (ts_dev->coordinate_readouts + 1); i <= total_steps; i++) {
+	/* coordinate_readouts … coordinate_readouts * 2 is for Y */
+	end_step = ts_dev->coordinate_readouts * 2;
+	for (i = ts_dev->coordinate_readouts; i < end_step; i++) {
 		titsc_writel(ts_dev, REG_STEPCONFIG(i), config);
 		titsc_writel(ts_dev, REG_STEPDELAY(i), STEPCONFIG_OPENDLY);
 	}
 
-	config = 0;
 	/* Charge step configuration */
 	config = ts_dev->bit_xp | ts_dev->bit_yn |
 			STEPCHARGE_RFP_XPUL | STEPCHARGE_RFM_XNUR |
@@ -178,35 +179,39 @@ static void titsc_step_config(struct titsc *ts_dev)
 	titsc_writel(ts_dev, REG_CHARGECONFIG, config);
 	titsc_writel(ts_dev, REG_CHARGEDELAY, CHARGEDLY_OPENDLY);
 
-	config = 0;
-	/* Configure to calculate pressure */
+	/* coordinate_readouts * 2 … coordinate_readouts * 2 + 2 is for Z */
 	config = STEPCONFIG_MODE_HWSYNC |
 			STEPCONFIG_AVG_16 | ts_dev->bit_yp |
 			ts_dev->bit_xn | STEPCONFIG_INM_ADCREFM |
 			STEPCONFIG_INP(ts_dev->inp_xp);
-	titsc_writel(ts_dev, REG_STEPCONFIG(total_steps + 1), config);
-	titsc_writel(ts_dev, REG_STEPDELAY(total_steps + 1),
+	titsc_writel(ts_dev, REG_STEPCONFIG(end_step), config);
+	titsc_writel(ts_dev, REG_STEPDELAY(end_step),
 			STEPCONFIG_OPENDLY);
 
-	config |= STEPCONFIG_INP(ts_dev->inp_yn) | STEPCONFIG_FIFO1;
-	titsc_writel(ts_dev, REG_STEPCONFIG(total_steps + 2), config);
-	titsc_writel(ts_dev, REG_STEPDELAY(total_steps + 2),
+	end_step++;
+	config |= STEPCONFIG_INP(ts_dev->inp_yn);
+	titsc_writel(ts_dev, REG_STEPCONFIG(end_step), config);
+	titsc_writel(ts_dev, REG_STEPDELAY(end_step),
 			STEPCONFIG_OPENDLY);
 
 	/* The steps1 … end and bit 0 for TS_Charge */
-	stepenable = (1 << (total_steps + 2)) - 1;
+	stepenable = (1 << (end_step + 2)) - 1;
 	am335x_tsc_se_set(ts_dev->mfd_tscadc, stepenable);
 }
 
 static void titsc_read_coordinates(struct titsc *ts_dev,
-				    unsigned int *x, unsigned int *y)
+		u32 *x, u32 *y, u32 *z1, u32 *z2)
 {
 	unsigned int fifocount = titsc_readl(ts_dev, REG_FIFO0CNT);
 	unsigned int prev_val_x = ~0, prev_val_y = ~0;
 	unsigned int prev_diff_x = ~0, prev_diff_y = ~0;
 	unsigned int read, diff;
 	unsigned int i, channel;
+	unsigned int creads = ts_dev->coordinate_readouts;
 
+	*z1 = *z2 = 0;
+	if (fifocount % (creads * 2 + 2))
+		fifocount -= fifocount % (creads * 2 + 2);
 	/*
 	 * Delta filter is used to remove large variations in sampled
 	 * values from ADC. The filter tries to predict where the next
@@ -215,32 +220,32 @@ static void titsc_read_coordinates(struct titsc *ts_dev,
 	 * algorithm compares the difference with that of a present value,
 	 * if true the value is reported to the sub system.
 	 */
-	for (i = 0; i < fifocount - 1; i++) {
+	for (i = 0; i < fifocount; i++) {
 		read = titsc_readl(ts_dev, REG_FIFO0);
-		channel = read & 0xf0000;
-		channel = channel >> 0x10;
-		if ((channel >= 0) && (channel < ts_dev->coordinate_readouts)) {
-			read &= 0xfff;
+
+		channel = (read & 0xf0000) >> 16;
+		read &= 0xfff;
+		if (channel < creads) {
 			diff = abs(read - prev_val_x);
 			if (diff < prev_diff_x) {
 				prev_diff_x = diff;
 				*x = read;
 			}
 			prev_val_x = read;
-		}
 
-		read = titsc_readl(ts_dev, REG_FIFO1);
-		channel = read & 0xf0000;
-		channel = channel >> 0x10;
-		if ((channel >= ts_dev->coordinate_readouts) &&
-			(channel < (2 * ts_dev->coordinate_readouts - 1))) {
-			read &= 0xfff;
+		} else if (channel < creads * 2) {
 			diff = abs(read - prev_val_y);
 			if (diff < prev_diff_y) {
 				prev_diff_y = diff;
 				*y = read;
 			}
 			prev_val_y = read;
+
+		} else if (channel < creads * 2 + 1) {
+			*z1 = read;
+
+		} else if (channel < creads * 2 + 2) {
+			*z2 = read;
 		}
 	}
 }
@@ -256,10 +261,8 @@ static irqreturn_t titsc_irq(int irq, void *dev)
 
 	status = titsc_readl(ts_dev, REG_IRQSTATUS);
 	if (status & IRQENB_FIFO0THRES) {
-		titsc_read_coordinates(ts_dev, &x, &y);
 
-		z1 = titsc_readl(ts_dev, REG_FIFO0) & 0xfff;
-		z2 = titsc_readl(ts_dev, REG_FIFO1) & 0xfff;
+		titsc_read_coordinates(ts_dev, &x, &y, &z1, &z2);
 
 		if (ts_dev->pen_down && z1 != 0 && z2 != 0) {
 			/*
@@ -267,10 +270,10 @@ static irqreturn_t titsc_irq(int irq, void *dev)
 			 * Resistance(touch) = x plate resistance *
 			 * x postion/4096 * ((z2 / z1) - 1)
 			 */
-			z = z2 - z1;
+			z = z1 - z2;
 			z *= x;
 			z *= ts_dev->x_plate_resistance;
-			z /= z1;
+			z /= z2;
 			z = (z + 2047) >> 12;
 
 			if (z <= MAX_12BIT) {
@@ -391,7 +394,8 @@ static int titsc_probe(struct platform_device *pdev)
 		goto err_free_irq;
 	}
 	titsc_step_config(ts_dev);
-	titsc_writel(ts_dev, REG_FIFO0THR, ts_dev->coordinate_readouts);
+	titsc_writel(ts_dev, REG_FIFO0THR,
+			ts_dev->coordinate_readouts * 2 + 2 - 1);
 
 	input_dev->name = "ti-tsc";
 	input_dev->dev.parent = &pdev->dev;
@@ -468,7 +472,7 @@ static int titsc_resume(struct device *dev)
 	}
 	titsc_step_config(ts_dev);
 	titsc_writel(ts_dev, REG_FIFO0THR,
-			ts_dev->coordinate_readouts);
+			ts_dev->coordinate_readouts * 2 + 2 - 1);
 	return 0;
 }
 
diff --git a/include/linux/mfd/ti_am335x_tscadc.h b/include/linux/mfd/ti_am335x_tscadc.h
index 533f200e6d0e..8d73fe29796a 100644
--- a/include/linux/mfd/ti_am335x_tscadc.h
+++ b/include/linux/mfd/ti_am335x_tscadc.h
@@ -30,8 +30,8 @@
 #define REG_IDLECONFIG		0x058
 #define REG_CHARGECONFIG	0x05C
 #define REG_CHARGEDELAY		0x060
-#define REG_STEPCONFIG(n)	(0x64 + ((n - 1) * 8))
-#define REG_STEPDELAY(n)	(0x68 + ((n - 1) * 8))
+#define REG_STEPCONFIG(n)	(0x64 + ((n) * 8))
+#define REG_STEPDELAY(n)	(0x68 + ((n) * 8))
 #define REG_FIFO0CNT		0xE4
 #define REG_FIFO0THR		0xE8
 #define REG_FIFO1CNT		0xF0