iio: adc: at91-sama5d2_adc: fix differential channels in triggered mode

commit a500f3bd787f8224341e44b238f318c407b10897 upstream.

The differential channels require writing the channel offset register (COR).
Otherwise they do not work in differential mode.
The configuration of COR is missing in triggered mode.

Fixes: 5e1a1da0f8c9 ("iio: adc: at91-sama5d2_adc: add hw trigger and buffer support")
Signed-off-by: Eugen Hristev <eugen.hristev@microchip.com>
Cc: <Stable@vger.kernel.org>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

diff --git a/drivers/iio/adc/at91-sama5d2_adc.c b/drivers/iio/adc/at91-sama5d2_adc.c
index e1850f3d5cf31358f2fd97ef8d23f256325e5822..2a6950aa6ae9ccf57dffb99bb4dbd3e94193b899 100644 (file)
--- a/drivers/iio/adc/at91-sama5d2_adc.c
+++ b/drivers/iio/adc/at91-sama5d2_adc.c
@@ -723,6 +723,7 @@ static int at91_adc_configure_trigger(struct iio_trigger *trig, bool state)
 
        for_each_set_bit(bit, indio->active_scan_mask, indio->num_channels) {
                struct iio_chan_spec const *chan = at91_adc_chan_get(indio, bit);
+               u32 cor;
 
                if (!chan)
                        continue;
@@ -731,6 +732,20 @@ static int at91_adc_configure_trigger(struct iio_trigger *trig, bool state)
                    chan->type == IIO_PRESSURE)
                        continue;
 
+               if (state) {
+                       cor = at91_adc_readl(st, AT91_SAMA5D2_COR);
+
+                       if (chan->differential)
+                               cor |= (BIT(chan->channel) |
+                                       BIT(chan->channel2)) <<
+                                       AT91_SAMA5D2_COR_DIFF_OFFSET;
+                       else
+                               cor &= ~(BIT(chan->channel) <<
+                                      AT91_SAMA5D2_COR_DIFF_OFFSET);
+
+                       at91_adc_writel(st, AT91_SAMA5D2_COR, cor);
+               }
+
                if (state) {
                        at91_adc_writel(st, AT91_SAMA5D2_CHER,
                                        BIT(chan->channel));