Commit 14f295c8 authored by Gregor Boirie's avatar Gregor Boirie Committed by Jonathan Cameron

iio:st_sensors: fix power regulator usage

Ensure failure to enable power regulators is properly handled.
Signed-off-by: default avatarGregor Boirie <gregor.boirie@parrot.com>
Signed-off-by: default avatarJonathan Cameron <jic23@kernel.org>
parent 169a88c1
...@@ -757,13 +757,15 @@ int st_accel_common_probe(struct iio_dev *indio_dev) ...@@ -757,13 +757,15 @@ int st_accel_common_probe(struct iio_dev *indio_dev)
indio_dev->info = &accel_info; indio_dev->info = &accel_info;
mutex_init(&adata->tb.buf_lock); 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, err = st_sensors_check_device_support(indio_dev,
ARRAY_SIZE(st_accel_sensors_settings), ARRAY_SIZE(st_accel_sensors_settings),
st_accel_sensors_settings); st_accel_sensors_settings);
if (err < 0) if (err < 0)
return err; goto st_accel_power_off;
adata->num_data_channels = ST_ACCEL_NUMBER_DATA_CHANNELS; adata->num_data_channels = ST_ACCEL_NUMBER_DATA_CHANNELS;
adata->multiread_bit = adata->sensor_settings->multi_read_bit; adata->multiread_bit = adata->sensor_settings->multi_read_bit;
...@@ -780,11 +782,11 @@ int st_accel_common_probe(struct iio_dev *indio_dev) ...@@ -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); err = st_sensors_init_sensor(indio_dev, adata->dev->platform_data);
if (err < 0) if (err < 0)
return err; goto st_accel_power_off;
err = st_accel_allocate_ring(indio_dev); err = st_accel_allocate_ring(indio_dev);
if (err < 0) if (err < 0)
return err; goto st_accel_power_off;
if (irq > 0) { if (irq > 0) {
err = st_sensors_allocate_trigger(indio_dev, err = st_sensors_allocate_trigger(indio_dev,
...@@ -807,6 +809,8 @@ int st_accel_common_probe(struct iio_dev *indio_dev) ...@@ -807,6 +809,8 @@ int st_accel_common_probe(struct iio_dev *indio_dev)
st_sensors_deallocate_trigger(indio_dev); st_sensors_deallocate_trigger(indio_dev);
st_accel_probe_trigger_error: st_accel_probe_trigger_error:
st_accel_deallocate_ring(indio_dev); st_accel_deallocate_ring(indio_dev);
st_accel_power_off:
st_sensors_power_disable(indio_dev);
return err; return err;
} }
......
...@@ -228,7 +228,7 @@ int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable) ...@@ -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); 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); struct st_sensor_data *pdata = iio_priv(indio_dev);
int err; int err;
...@@ -237,18 +237,37 @@ void st_sensors_power_enable(struct iio_dev *indio_dev) ...@@ -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"); pdata->vdd = devm_regulator_get_optional(indio_dev->dev.parent, "vdd");
if (!IS_ERR(pdata->vdd)) { if (!IS_ERR(pdata->vdd)) {
err = regulator_enable(pdata->vdd); err = regulator_enable(pdata->vdd);
if (err != 0) if (err != 0) {
dev_warn(&indio_dev->dev, dev_warn(&indio_dev->dev,
"Failed to enable specified Vdd supply\n"); "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"); pdata->vdd_io = devm_regulator_get_optional(indio_dev->dev.parent, "vddio");
if (!IS_ERR(pdata->vdd_io)) { if (!IS_ERR(pdata->vdd_io)) {
err = regulator_enable(pdata->vdd_io); err = regulator_enable(pdata->vdd_io);
if (err != 0) if (err != 0) {
dev_warn(&indio_dev->dev, dev_warn(&indio_dev->dev,
"Failed to enable specified Vdd_IO supply\n"); "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); EXPORT_SYMBOL(st_sensors_power_enable);
...@@ -256,10 +275,10 @@ void st_sensors_power_disable(struct iio_dev *indio_dev) ...@@ -256,10 +275,10 @@ void st_sensors_power_disable(struct iio_dev *indio_dev)
{ {
struct st_sensor_data *pdata = iio_priv(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); regulator_disable(pdata->vdd);
if (!IS_ERR(pdata->vdd_io)) if (!IS_ERR_OR_NULL(pdata->vdd_io))
regulator_disable(pdata->vdd_io); regulator_disable(pdata->vdd_io);
} }
EXPORT_SYMBOL(st_sensors_power_disable); EXPORT_SYMBOL(st_sensors_power_disable);
......
...@@ -425,13 +425,15 @@ int st_gyro_common_probe(struct iio_dev *indio_dev) ...@@ -425,13 +425,15 @@ int st_gyro_common_probe(struct iio_dev *indio_dev)
indio_dev->info = &gyro_info; indio_dev->info = &gyro_info;
mutex_init(&gdata->tb.buf_lock); 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, err = st_sensors_check_device_support(indio_dev,
ARRAY_SIZE(st_gyro_sensors_settings), ARRAY_SIZE(st_gyro_sensors_settings),
st_gyro_sensors_settings); st_gyro_sensors_settings);
if (err < 0) if (err < 0)
return err; goto st_gyro_power_off;
gdata->num_data_channels = ST_GYRO_NUMBER_DATA_CHANNELS; gdata->num_data_channels = ST_GYRO_NUMBER_DATA_CHANNELS;
gdata->multiread_bit = gdata->sensor_settings->multi_read_bit; gdata->multiread_bit = gdata->sensor_settings->multi_read_bit;
...@@ -445,11 +447,11 @@ int st_gyro_common_probe(struct iio_dev *indio_dev) ...@@ -445,11 +447,11 @@ int st_gyro_common_probe(struct iio_dev *indio_dev)
err = st_sensors_init_sensor(indio_dev, err = st_sensors_init_sensor(indio_dev,
(struct st_sensors_platform_data *)&gyro_pdata); (struct st_sensors_platform_data *)&gyro_pdata);
if (err < 0) if (err < 0)
return err; goto st_gyro_power_off;
err = st_gyro_allocate_ring(indio_dev); err = st_gyro_allocate_ring(indio_dev);
if (err < 0) if (err < 0)
return err; goto st_gyro_power_off;
if (irq > 0) { if (irq > 0) {
err = st_sensors_allocate_trigger(indio_dev, err = st_sensors_allocate_trigger(indio_dev,
...@@ -472,6 +474,8 @@ int st_gyro_common_probe(struct iio_dev *indio_dev) ...@@ -472,6 +474,8 @@ int st_gyro_common_probe(struct iio_dev *indio_dev)
st_sensors_deallocate_trigger(indio_dev); st_sensors_deallocate_trigger(indio_dev);
st_gyro_probe_trigger_error: st_gyro_probe_trigger_error:
st_gyro_deallocate_ring(indio_dev); st_gyro_deallocate_ring(indio_dev);
st_gyro_power_off:
st_sensors_power_disable(indio_dev);
return err; return err;
} }
......
...@@ -588,13 +588,15 @@ int st_magn_common_probe(struct iio_dev *indio_dev) ...@@ -588,13 +588,15 @@ int st_magn_common_probe(struct iio_dev *indio_dev)
indio_dev->info = &magn_info; indio_dev->info = &magn_info;
mutex_init(&mdata->tb.buf_lock); 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, err = st_sensors_check_device_support(indio_dev,
ARRAY_SIZE(st_magn_sensors_settings), ARRAY_SIZE(st_magn_sensors_settings),
st_magn_sensors_settings); st_magn_sensors_settings);
if (err < 0) if (err < 0)
return err; goto st_magn_power_off;
mdata->num_data_channels = ST_MAGN_NUMBER_DATA_CHANNELS; mdata->num_data_channels = ST_MAGN_NUMBER_DATA_CHANNELS;
mdata->multiread_bit = mdata->sensor_settings->multi_read_bit; mdata->multiread_bit = mdata->sensor_settings->multi_read_bit;
...@@ -607,11 +609,11 @@ int st_magn_common_probe(struct iio_dev *indio_dev) ...@@ -607,11 +609,11 @@ int st_magn_common_probe(struct iio_dev *indio_dev)
err = st_sensors_init_sensor(indio_dev, NULL); err = st_sensors_init_sensor(indio_dev, NULL);
if (err < 0) if (err < 0)
return err; goto st_magn_power_off;
err = st_magn_allocate_ring(indio_dev); err = st_magn_allocate_ring(indio_dev);
if (err < 0) if (err < 0)
return err; goto st_magn_power_off;
if (irq > 0) { if (irq > 0) {
err = st_sensors_allocate_trigger(indio_dev, err = st_sensors_allocate_trigger(indio_dev,
...@@ -634,6 +636,8 @@ int st_magn_common_probe(struct iio_dev *indio_dev) ...@@ -634,6 +636,8 @@ int st_magn_common_probe(struct iio_dev *indio_dev)
st_sensors_deallocate_trigger(indio_dev); st_sensors_deallocate_trigger(indio_dev);
st_magn_probe_trigger_error: st_magn_probe_trigger_error:
st_magn_deallocate_ring(indio_dev); st_magn_deallocate_ring(indio_dev);
st_magn_power_off:
st_sensors_power_disable(indio_dev);
return err; return err;
} }
......
...@@ -527,13 +527,15 @@ int st_press_common_probe(struct iio_dev *indio_dev) ...@@ -527,13 +527,15 @@ int st_press_common_probe(struct iio_dev *indio_dev)
indio_dev->info = &press_info; indio_dev->info = &press_info;
mutex_init(&press_data->tb.buf_lock); 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, err = st_sensors_check_device_support(indio_dev,
ARRAY_SIZE(st_press_sensors_settings), ARRAY_SIZE(st_press_sensors_settings),
st_press_sensors_settings); st_press_sensors_settings);
if (err < 0) if (err < 0)
return err; goto st_press_power_off;
press_data->num_data_channels = ST_PRESS_NUMBER_DATA_CHANNELS; press_data->num_data_channels = ST_PRESS_NUMBER_DATA_CHANNELS;
press_data->multiread_bit = press_data->sensor_settings->multi_read_bit; 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) ...@@ -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); err = st_sensors_init_sensor(indio_dev, press_data->dev->platform_data);
if (err < 0) if (err < 0)
return err; goto st_press_power_off;
err = st_press_allocate_ring(indio_dev); err = st_press_allocate_ring(indio_dev);
if (err < 0) if (err < 0)
return err; goto st_press_power_off;
if (irq > 0) { if (irq > 0) {
err = st_sensors_allocate_trigger(indio_dev, err = st_sensors_allocate_trigger(indio_dev,
...@@ -581,6 +583,8 @@ int st_press_common_probe(struct iio_dev *indio_dev) ...@@ -581,6 +583,8 @@ int st_press_common_probe(struct iio_dev *indio_dev)
st_sensors_deallocate_trigger(indio_dev); st_sensors_deallocate_trigger(indio_dev);
st_press_probe_trigger_error: st_press_probe_trigger_error:
st_press_deallocate_ring(indio_dev); st_press_deallocate_ring(indio_dev);
st_press_power_off:
st_sensors_power_disable(indio_dev);
return err; return err;
} }
......
...@@ -278,7 +278,7 @@ int st_sensors_set_enable(struct iio_dev *indio_dev, bool enable); ...@@ -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); 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); void st_sensors_power_disable(struct iio_dev *indio_dev);
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment