These changes are the raw update to linux-4.4.6-rt14. Kernel sources

[kvmfornfv.git] / kernel / drivers / iio / common / st_sensors / st_sensors_core.c

diff --git a/kernel/drivers/iio/common/st_sensors/st_sensors_core.c b/kernel/drivers/iio/common/st_sensors/st_sensors_core.c
index 8dd0477..25258e2 100644 (file)
--- a/kernel/drivers/iio/common/st_sensors/st_sensors_core.c
+++ b/kernel/drivers/iio/common/st_sensors/st_sensors_core.c
@@ -44,6 +44,28 @@ st_sensors_write_data_with_mask_error:
        return err;
 }
 
+int st_sensors_debugfs_reg_access(struct iio_dev *indio_dev,
+                                 unsigned reg, unsigned writeval,
+                                 unsigned *readval)
+{
+       struct st_sensor_data *sdata = iio_priv(indio_dev);
+       u8 readdata;
+       int err;
+
+       if (!readval)
+               return sdata->tf->write_byte(&sdata->tb, sdata->dev,
+                                            (u8)reg, (u8)writeval);
+
+       err = sdata->tf->read_byte(&sdata->tb, sdata->dev, (u8)reg, &readdata);
+       if (err < 0)
+               return err;
+
+       *readval = (unsigned)readdata;
+
+       return 0;
+}
+EXPORT_SYMBOL(st_sensors_debugfs_reg_access);
+
 static int st_sensors_match_odr(struct st_sensor_settings *sensor_settings,
                        unsigned int odr, struct st_sensor_odr_avl *odr_out)
 {
@@ -126,6 +148,9 @@ static int st_sensors_set_fullscale(struct iio_dev *indio_dev, unsigned int fs)
        int err, i = 0;
        struct st_sensor_data *sdata = iio_priv(indio_dev);
 
+       if (sdata->sensor_settings->fs.addr == 0)
+               return 0;
+
        err = st_sensors_match_fs(sdata->sensor_settings, fs, &i);
        if (err < 0)
                goto st_accel_set_fullscale_error;
@@ -245,6 +270,16 @@ static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev,
 {
        struct st_sensor_data *sdata = iio_priv(indio_dev);
 
+       /* Sensor does not support interrupts */
+       if (sdata->sensor_settings->drdy_irq.addr == 0) {
+               if (pdata->drdy_int_pin)
+                       dev_info(&indio_dev->dev,
+                                "DRDY on pin INT%d specified, but sensor "
+                                "does not support interrupts\n",
+                                pdata->drdy_int_pin);
+               return 0;
+       }
+
        switch (pdata->drdy_int_pin) {
        case 1:
                if (sdata->sensor_settings->drdy_irq.mask_int1 == 0) {
@@ -285,7 +320,7 @@ static struct st_sensors_platform_data *st_sensors_of_probe(struct device *dev,
        if (!of_property_read_u32(np, "st,drdy-int-pin", &val) && (val <= 2))
                pdata->drdy_int_pin = (u8) val;
        else
-               pdata->drdy_int_pin = defdata ? defdata->drdy_int_pin : 1;
+               pdata->drdy_int_pin = defdata ? defdata->drdy_int_pin : 0;
 
        return pdata;
 }
@@ -332,11 +367,13 @@ int st_sensors_init_sensor(struct iio_dev *indio_dev,
                return err;
 
        /* set BDU */
-       err = st_sensors_write_data_with_mask(indio_dev,
+       if (sdata->sensor_settings->bdu.addr) {
+               err = st_sensors_write_data_with_mask(indio_dev,
                                        sdata->sensor_settings->bdu.addr,
                                        sdata->sensor_settings->bdu.mask, true);
-       if (err < 0)
-               return err;
+               if (err < 0)
+                       return err;
+       }
 
        err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS);
 
@@ -419,7 +456,9 @@ static int st_sensors_read_axis_data(struct iio_dev *indio_dev,
        if (err < 0)
                goto st_sensors_free_memory;
 
-       if (byte_for_channel == 2)
+       if (byte_for_channel == 1)
+               *data = (s8)*outdata;
+       else if (byte_for_channel == 2)
                *data = (s16)get_unaligned_le16(outdata);
        else if (byte_for_channel == 3)
                *data = (s32)st_sensors_get_unaligned_le24(outdata);
@@ -465,45 +504,43 @@ int st_sensors_check_device_support(struct iio_dev *indio_dev,
                        int num_sensors_list,
                        const struct st_sensor_settings *sensor_settings)
 {
-       u8 wai;
        int i, n, err;
+       u8 wai;
        struct st_sensor_data *sdata = iio_priv(indio_dev);
 
-       err = sdata->tf->read_byte(&sdata->tb, sdata->dev,
-                                       ST_SENSORS_DEFAULT_WAI_ADDRESS, &wai);
-       if (err < 0) {
-               dev_err(&indio_dev->dev, "failed to read Who-Am-I register.\n");
-               goto read_wai_error;
-       }
-
        for (i = 0; i < num_sensors_list; i++) {
-               if (sensor_settings[i].wai == wai)
+               for (n = 0; n < ST_SENSORS_MAX_4WAI; n++) {
+                       if (strcmp(indio_dev->name,
+                               sensor_settings[i].sensors_supported[n]) == 0) {
+                               break;
+                       }
+               }
+               if (n < ST_SENSORS_MAX_4WAI)
                        break;
        }
-       if (i == num_sensors_list)
-               goto device_not_supported;
+       if (i == num_sensors_list) {
+               dev_err(&indio_dev->dev, "device name %s not recognized.\n",
+                                                       indio_dev->name);
+               return -ENODEV;
+       }
 
-       for (n = 0; n < ARRAY_SIZE(sensor_settings[i].sensors_supported); n++) {
-               if (strcmp(indio_dev->name,
-                               &sensor_settings[i].sensors_supported[n][0]) == 0)
-                       break;
+       err = sdata->tf->read_byte(&sdata->tb, sdata->dev,
+                                       sensor_settings[i].wai_addr, &wai);
+       if (err < 0) {
+               dev_err(&indio_dev->dev, "failed to read Who-Am-I register.\n");
+               return err;
        }
-       if (n == ARRAY_SIZE(sensor_settings[i].sensors_supported)) {
-               dev_err(&indio_dev->dev, "device name and WhoAmI mismatch.\n");
-               goto sensor_name_mismatch;
+
+       if (sensor_settings[i].wai != wai) {
+               dev_err(&indio_dev->dev, "%s: WhoAmI mismatch (0x%x).\n",
+                                               indio_dev->name, wai);
+               return -EINVAL;
        }
 
        sdata->sensor_settings =
                        (struct st_sensor_settings *)&sensor_settings[i];
 
        return i;
-
-device_not_supported:
-       dev_err(&indio_dev->dev, "device not supported: WhoAmI (0x%x).\n", wai);
-sensor_name_mismatch:
-       err = -ENODEV;
-read_wai_error:
-       return err;
 }
 EXPORT_SYMBOL(st_sensors_check_device_support);