1 files changed, 12 insertions, 13 deletions

diff --git a/drivers/input/touchscreen/wm9705.c b/drivers/input/touchscreen/wm9705.c
index 98e61175d3f5..adc13a523ab5 100644
--- a/drivers/input/touchscreen/wm9705.c
+++ b/drivers/input/touchscreen/wm9705.c
@@ -215,8 +215,9 @@ static inline int is_pden(struct wm97xx *wm)
 static int wm9705_poll_sample(struct wm97xx *wm, int adcsel, int *sample)
 {
         int timeout = 5 * delay;
+        bool wants_pen = adcsel & WM97XX_PEN_DOWN;
 
-        if (!wm->pen_probably_down) {
+        if (wants_pen && !wm->pen_probably_down) {
                 u16 data = wm97xx_reg_read(wm, AC97_WM97XX_DIGITISER_RD);
                 if (!(data & WM97XX_PEN_DOWN))
                         return RC_PENUP;
@@ -224,13 +225,10 @@ static int wm9705_poll_sample(struct wm97xx *wm, int adcsel, int *sample)
         }
 
         /* set up digitiser */
-        if (adcsel & 0x8000)
-                adcsel = ((adcsel & 0x7fff) + 3) << 12;
-
         if (wm->mach_ops && wm->mach_ops->pre_sample)
                 wm->mach_ops->pre_sample(adcsel);
-        wm97xx_reg_write(wm, AC97_WM97XX_DIGITISER1,
-                         adcsel | WM97XX_POLL | WM97XX_DELAY(delay));
+        wm97xx_reg_write(wm, AC97_WM97XX_DIGITISER1, (adcsel & WM97XX_ADCSEL_MASK)
+                                | WM97XX_POLL | WM97XX_DELAY(delay));
 
         /* wait 3 AC97 time slots + delay for conversion */
         poll_delay(delay);
@@ -256,13 +254,14 @@ static int wm9705_poll_sample(struct wm97xx *wm, int adcsel, int *sample)
                 wm->mach_ops->post_sample(adcsel);
 
         /* check we have correct sample */
-        if ((*sample & WM97XX_ADCSEL_MASK) != adcsel) {
-                dev_dbg(wm->dev, "adc wrong sample, read %x got %x", adcsel,
-                *sample & WM97XX_ADCSEL_MASK);
+        if ((*sample ^ adcsel) & WM97XX_ADCSEL_MASK) {
+                dev_dbg(wm->dev, "adc wrong sample, wanted %x got %x",
+                        adcsel & WM97XX_ADCSEL_MASK,
+                        *sample & WM97XX_ADCSEL_MASK);
                 return RC_PENUP;
         }
 
-        if (!(*sample & WM97XX_PEN_DOWN)) {
+        if (wants_pen && !(*sample & WM97XX_PEN_DOWN)) {
                 wm->pen_probably_down = 0;
                 return RC_PENUP;
         }
@@ -277,14 +276,14 @@ static int wm9705_poll_touch(struct wm97xx *wm, struct wm97xx_data *data)
 {
         int rc;
 
-        rc = wm9705_poll_sample(wm, WM97XX_ADCSEL_X, &data->x);
+        rc = wm9705_poll_sample(wm, WM97XX_ADCSEL_X | WM97XX_PEN_DOWN, &data->x);
         if (rc != RC_VALID)
                 return rc;
-        rc = wm9705_poll_sample(wm, WM97XX_ADCSEL_Y, &data->y);
+        rc = wm9705_poll_sample(wm, WM97XX_ADCSEL_Y | WM97XX_PEN_DOWN, &data->y);
         if (rc != RC_VALID)
                 return rc;
         if (pil) {
-                rc = wm9705_poll_sample(wm, WM97XX_ADCSEL_PRES, &data->p);
+                rc = wm9705_poll_sample(wm, WM97XX_ADCSEL_PRES | WM97XX_PEN_DOWN, &data->p);
                 if (rc != RC_VALID)
                         return rc;
         } else