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

[ Upstream commit a500f3bd787f8224341e44b238f318c407b10897 ]

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: Sasha Levin <sashal@kernel.org>

diff --git a/drivers/iio/adc/at91-sama5d2_adc.c b/drivers/iio/adc/at91-sama5d2_adc.c
index 0898f40c2b8929ac2a4b81bda5f192cffc43e8ba..34639ee2d2ce69f3d13ef539e142c1ac195c7330 100644 (file)
--- a/drivers/iio/adc/at91-sama5d2_adc.c
+++ b/drivers/iio/adc/at91-sama5d2_adc.c
@@ -339,9 +339,24 @@ 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;
+               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));