iio: Added ST-sensors platform data to select the DRDY interrupt pin

This patch add support to redirect the DRDY interrupt on INT1 or INT2
on accelerometer and pressure sensors.

Signed-off-by: Denis Ciocca <denis.ciocca@st.com>
Signed-off-by: Jonathan Cameron <jic23@kernel.org>

1 files changed, 36 insertions, 5 deletions

diff --git a/drivers/iio/common/st_sensors/st_sensors_core.c b/drivers/iio/common/st_sensors/st_sensors_core.c
index 865b1781df66..965ee22d3ac8 100644
--- a/drivers/iio/common/st_sensors/st_sensors_core.c
+++ b/drivers/iio/common/st_sensors/st_sensors_core.c
@@ -22,7 +22,7 @@
 
 static inline u32 st_sensors_get_unaligned_le24(const u8 *p)
 {
-        return ((s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8);
+        return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8;
 }
 
 static int st_sensors_write_data_with_mask(struct iio_dev *indio_dev,
@@ -118,7 +118,7 @@ st_sensors_match_odr_error:
 }
 
 static int st_sensors_set_fullscale(struct iio_dev *indio_dev,
-                                                        unsigned int fs)
+                                                                unsigned int fs)
 {
         int err, i = 0;
         struct st_sensor_data *sdata = iio_priv(indio_dev);
@@ -198,13 +198,39 @@ int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable)
 }
 EXPORT_SYMBOL(st_sensors_set_axis_enable);
 
-int st_sensors_init_sensor(struct iio_dev *indio_dev)
+int st_sensors_init_sensor(struct iio_dev *indio_dev,
+                                        struct st_sensors_platform_data *pdata)
 {
         int err;
         struct st_sensor_data *sdata = iio_priv(indio_dev);
 
         mutex_init(&sdata->tb.buf_lock);
 
+        switch (pdata->drdy_int_pin) {
+        case 1:
+                if (sdata->sensor->drdy_irq.mask_int1 == 0) {
+                        dev_err(&indio_dev->dev,
+                                        "DRDY on INT1 not available.\n");
+                        err = -EINVAL;
+                        goto init_error;
+                }
+                sdata->drdy_int_pin = 1;
+                break;
+        case 2:
+                if (sdata->sensor->drdy_irq.mask_int2 == 0) {
+                        dev_err(&indio_dev->dev,
+                                        "DRDY on INT2 not available.\n");
+                        err = -EINVAL;
+                        goto init_error;
+                }
+                sdata->drdy_int_pin = 2;
+                break;
+        default:
+                dev_err(&indio_dev->dev, "DRDY on pdata not valid.\n");
+                err = -EINVAL;
+                goto init_error;
+        }
+
         err = st_sensors_set_enable(indio_dev, false);
         if (err < 0)
                 goto init_error;
@@ -234,6 +260,7 @@ EXPORT_SYMBOL(st_sensors_init_sensor);
 int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable)
 {
         int err;
+        u8 drdy_mask;
         struct st_sensor_data *sdata = iio_priv(indio_dev);
 
         /* Enable/Disable the interrupt generator 1. */
@@ -245,10 +272,14 @@ int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable)
                         goto st_accel_set_dataready_irq_error;
         }
 
+        if (sdata->drdy_int_pin == 1)
+                drdy_mask = sdata->sensor->drdy_irq.mask_int1;
+        else
+                drdy_mask = sdata->sensor->drdy_irq.mask_int2;
+
         /* Enable/Disable the interrupt generator for data ready. */
         err = st_sensors_write_data_with_mask(indio_dev,
-                        sdata->sensor->drdy_irq.addr,
-                        sdata->sensor->drdy_irq.mask, (int)enable);
+                        sdata->sensor->drdy_irq.addr, drdy_mask, (int)enable);
 
 st_accel_set_dataready_irq_error:
         return err;