iio:st_sensors: fix power regulator usage

Ensure failure to enable power regulators is properly handled.

Signed-off-by: Gregor Boirie <gregor.boirie@parrot.com>
Signed-off-by: Jonathan Cameron <jic23@kernel.org>

diff --git a/drivers/iio/accel/st_accel_core.c b/drivers/iio/accel/st_accel_core.c
index dc73f2d..b8d3c3c 100644 (file)
--- a/drivers/iio/accel/st_accel_core.c
+++ b/drivers/iio/accel/st_accel_core.c
@@ -757,13 +757,15 @@ int st_accel_common_probe(struct iio_dev *indio_dev)
        indio_dev->info = &accel_info;
        mutex_init(&adata->tb.buf_lock);
 
-       st_sensors_power_enable(indio_dev);
+       err = st_sensors_power_enable(indio_dev);
+       if (err)
+               return err;
 
        err = st_sensors_check_device_support(indio_dev,
                                        ARRAY_SIZE(st_accel_sensors_settings),
                                        st_accel_sensors_settings);
        if (err < 0)
-               return err;
+               goto st_accel_power_off;
 
        adata->num_data_channels = ST_ACCEL_NUMBER_DATA_CHANNELS;
        adata->multiread_bit = adata->sensor_settings->multi_read_bit;
@@ -780,11 +782,11 @@ int st_accel_common_probe(struct iio_dev *indio_dev)
 
        err = st_sensors_init_sensor(indio_dev, adata->dev->platform_data);
        if (err < 0)
-               return err;
+               goto st_accel_power_off;
 
        err = st_accel_allocate_ring(indio_dev);
        if (err < 0)
-               return err;
+               goto st_accel_power_off;
 
        if (irq > 0) {
                err = st_sensors_allocate_trigger(indio_dev,
@@ -807,6 +809,8 @@ st_accel_device_register_error:
                st_sensors_deallocate_trigger(indio_dev);
 st_accel_probe_trigger_error:
        st_accel_deallocate_ring(indio_dev);
+st_accel_power_off:
+       st_sensors_power_disable(indio_dev);
 
        return err;
 }

diff --git a/drivers/iio/common/st_sensors/st_sensors_core.c b/drivers/iio/common/st_sensors/st_sensors_core.c
index dffe006..00078b5 100644 (file)
--- a/drivers/iio/common/st_sensors/st_sensors_core.c
+++ b/drivers/iio/common/st_sensors/st_sensors_core.c
@@ -228,7 +228,7 @@ int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable)
 }
 EXPORT_SYMBOL(st_sensors_set_axis_enable);
 
-void st_sensors_power_enable(struct iio_dev *indio_dev)
+int st_sensors_power_enable(struct iio_dev *indio_dev)
 {
        struct st_sensor_data *pdata = iio_priv(indio_dev);
        int err;
@@ -237,18 +237,37 @@ void st_sensors_power_enable(struct iio_dev *indio_dev)
        pdata->vdd = devm_regulator_get_optional(indio_dev->dev.parent, "vdd");
        if (!IS_ERR(pdata->vdd)) {
                err = regulator_enable(pdata->vdd);
-               if (err != 0)
+               if (err != 0) {
                        dev_warn(&indio_dev->dev,
                                 "Failed to enable specified Vdd supply\n");
+                       return err;
+               }
+       } else {
+               err = PTR_ERR(pdata->vdd);
+               if (err != -ENODEV)
+                       return err;
        }
 
        pdata->vdd_io = devm_regulator_get_optional(indio_dev->dev.parent, "vddio");
        if (!IS_ERR(pdata->vdd_io)) {
                err = regulator_enable(pdata->vdd_io);
-               if (err != 0)
+               if (err != 0) {
                        dev_warn(&indio_dev->dev,
                                 "Failed to enable specified Vdd_IO supply\n");
+                       goto st_sensors_disable_vdd;
+               }
+       } else {
+               err = PTR_ERR(pdata->vdd_io);
+               if (err != -ENODEV)
+                       goto st_sensors_disable_vdd;
        }
+
+       return 0;
+
+st_sensors_disable_vdd:
+       if (!IS_ERR_OR_NULL(pdata->vdd))
+               regulator_disable(pdata->vdd);
+       return err;
 }
 EXPORT_SYMBOL(st_sensors_power_enable);
 
@@ -256,10 +275,10 @@ void st_sensors_power_disable(struct iio_dev *indio_dev)
 {
        struct st_sensor_data *pdata = iio_priv(indio_dev);
 
-       if (!IS_ERR(pdata->vdd))
+       if (!IS_ERR_OR_NULL(pdata->vdd))
                regulator_disable(pdata->vdd);
 
-       if (!IS_ERR(pdata->vdd_io))
+       if (!IS_ERR_OR_NULL(pdata->vdd_io))
                regulator_disable(pdata->vdd_io);
 }
 EXPORT_SYMBOL(st_sensors_power_disable);

diff --git a/drivers/iio/gyro/st_gyro_core.c b/drivers/iio/gyro/st_gyro_core.c
index 52a3c87..ae1377d 100644 (file)
--- a/drivers/iio/gyro/st_gyro_core.c
+++ b/drivers/iio/gyro/st_gyro_core.c
@@ -425,13 +425,15 @@ int st_gyro_common_probe(struct iio_dev *indio_dev)
        indio_dev->info = &gyro_info;
        mutex_init(&gdata->tb.buf_lock);
 
-       st_sensors_power_enable(indio_dev);
+       err = st_sensors_power_enable(indio_dev);
+       if (err)
+               return err;
 
        err = st_sensors_check_device_support(indio_dev,
                                        ARRAY_SIZE(st_gyro_sensors_settings),
                                        st_gyro_sensors_settings);
        if (err < 0)
-               return err;
+               goto st_gyro_power_off;
 
        gdata->num_data_channels = ST_GYRO_NUMBER_DATA_CHANNELS;
        gdata->multiread_bit = gdata->sensor_settings->multi_read_bit;
@@ -445,11 +447,11 @@ int st_gyro_common_probe(struct iio_dev *indio_dev)
        err = st_sensors_init_sensor(indio_dev,
                                (struct st_sensors_platform_data *)&gyro_pdata);
        if (err < 0)
-               return err;
+               goto st_gyro_power_off;
 
        err = st_gyro_allocate_ring(indio_dev);
        if (err < 0)
-               return err;
+               goto st_gyro_power_off;
 
        if (irq > 0) {
                err = st_sensors_allocate_trigger(indio_dev,
@@ -472,6 +474,8 @@ st_gyro_device_register_error:
                st_sensors_deallocate_trigger(indio_dev);
 st_gyro_probe_trigger_error:
        st_gyro_deallocate_ring(indio_dev);
+st_gyro_power_off:
+       st_sensors_power_disable(indio_dev);
 
        return err;
 }

diff --git a/drivers/iio/magnetometer/st_magn_core.c b/drivers/iio/magnetometer/st_magn_core.c
index 62036d2..7c94adc 100644 (file)
--- a/drivers/iio/magnetometer/st_magn_core.c
+++ b/drivers/iio/magnetometer/st_magn_core.c
@@ -588,13 +588,15 @@ int st_magn_common_probe(struct iio_dev *indio_dev)
        indio_dev->info = &magn_info;
        mutex_init(&mdata->tb.buf_lock);
 
-       st_sensors_power_enable(indio_dev);
+       err = st_sensors_power_enable(indio_dev);
+       if (err)
+               return err;
 
        err = st_sensors_check_device_support(indio_dev,
                                        ARRAY_SIZE(st_magn_sensors_settings),
                                        st_magn_sensors_settings);
        if (err < 0)
-               return err;
+               goto st_magn_power_off;
 
        mdata->num_data_channels = ST_MAGN_NUMBER_DATA_CHANNELS;
        mdata->multiread_bit = mdata->sensor_settings->multi_read_bit;
@@ -607,11 +609,11 @@ int st_magn_common_probe(struct iio_dev *indio_dev)
 
        err = st_sensors_init_sensor(indio_dev, NULL);
        if (err < 0)
-               return err;
+               goto st_magn_power_off;
 
        err = st_magn_allocate_ring(indio_dev);
        if (err < 0)
-               return err;
+               goto st_magn_power_off;
 
        if (irq > 0) {
                err = st_sensors_allocate_trigger(indio_dev,
@@ -634,6 +636,8 @@ st_magn_device_register_error:
                st_sensors_deallocate_trigger(indio_dev);
 st_magn_probe_trigger_error:
        st_magn_deallocate_ring(indio_dev);
+st_magn_power_off:
+       st_sensors_power_disable(indio_dev);
 
        return err;
 }

diff --git a/drivers/iio/pressure/st_pressure_core.c b/drivers/iio/pressure/st_pressure_core.c
index c0ff3bf..4d317af 100644 (file)
--- a/drivers/iio/pressure/st_pressure_core.c
+++ b/drivers/iio/pressure/st_pressure_core.c
@@ -527,13 +527,15 @@ int st_press_common_probe(struct iio_dev *indio_dev)
        indio_dev->info = &press_info;
        mutex_init(&press_data->tb.buf_lock);
 
-       st_sensors_power_enable(indio_dev);
+       err = st_sensors_power_enable(indio_dev);
+       if (err)
+               return err;
 
        err = st_sensors_check_device_support(indio_dev,
                                        ARRAY_SIZE(st_press_sensors_settings),
                                        st_press_sensors_settings);
        if (err < 0)
-               return err;
+               goto st_press_power_off;
 
        press_data->num_data_channels = ST_PRESS_NUMBER_DATA_CHANNELS;
        press_data->multiread_bit = press_data->sensor_settings->multi_read_bit;
@@ -554,11 +556,11 @@ int st_press_common_probe(struct iio_dev *indio_dev)
 
        err = st_sensors_init_sensor(indio_dev, press_data->dev->platform_data);
        if (err < 0)
-               return err;
+               goto st_press_power_off;
 
        err = st_press_allocate_ring(indio_dev);
        if (err < 0)
-               return err;
+               goto st_press_power_off;
 
        if (irq > 0) {
                err = st_sensors_allocate_trigger(indio_dev,
@@ -581,6 +583,8 @@ st_press_device_register_error:
                st_sensors_deallocate_trigger(indio_dev);
 st_press_probe_trigger_error:
        st_press_deallocate_ring(indio_dev);
+st_press_power_off:
+       st_sensors_power_disable(indio_dev);
 
        return err;
 }

diff --git a/include/linux/iio/common/st_sensors.h b/include/linux/iio/common/st_sensors.h
index 78a1934..91d5f68 100644 (file)
--- a/include/linux/iio/common/st_sensors.h
+++ b/include/linux/iio/common/st_sensors.h
@@ -278,7 +278,7 @@ int st_sensors_set_enable(struct iio_dev *indio_dev, bool enable);
 
 int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable);
 
-void st_sensors_power_enable(struct iio_dev *indio_dev);
+int st_sensors_power_enable(struct iio_dev *indio_dev);
 
 void st_sensors_power_disable(struct iio_dev *indio_dev);