diff options
| -rw-r--r-- | drivers/iio/accel/st_accel_core.c | 12 | ||||
| -rw-r--r-- | drivers/iio/common/st_sensors/st_sensors_core.c | 29 | ||||
| -rw-r--r-- | drivers/iio/gyro/st_gyro_core.c | 12 | ||||
| -rw-r--r-- | drivers/iio/magnetometer/st_magn_core.c | 12 | ||||
| -rw-r--r-- | drivers/iio/pressure/st_pressure_core.c | 12 | ||||
| -rw-r--r-- | include/linux/iio/common/st_sensors.h | 2 |
6 files changed, 57 insertions, 22 deletions
diff --git a/drivers/iio/accel/st_accel_core.c b/drivers/iio/accel/st_accel_core.c index dc73f2d..b8d3c3c 100644 --- 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 --- 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 --- 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 --- 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 --- 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 --- 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); |
