diff --git a/drivers/iio/accel/kionix-kx022a.c b/drivers/iio/accel/kionix-kx022a.c index f671fab3dc31..9ff70a373141 100644 --- a/drivers/iio/accel/kionix-kx022a.c +++ b/drivers/iio/accel/kionix-kx022a.c @@ -980,26 +980,44 @@ static int kx022a_fifo_enable(struct kx022a_data *data) guard(mutex)(&data->mutex); ret = __kx022a_turn_on_off(data, false); if (ret) - return ret; + goto err_free_out; /* Update watermark to HW */ ret = kx022a_fifo_set_wmi(data); if (ret) - return ret; + goto err_turn_on_out; /* Enable buffer */ ret = regmap_set_bits(data->regmap, data->chip_info->buf_cntl2, KX022A_MASK_BUF_EN); if (ret) - return ret; + goto err_turn_on_out; data->state |= KX022A_STATE_FIFO; ret = regmap_set_bits(data->regmap, data->ien_reg, KX022A_MASK_WMI); if (ret) - return ret; + goto err_buf_disable_out; - return __kx022a_turn_on_off(data, true); + ret = __kx022a_turn_on_off(data, true); + if (ret) + goto err_wmi_clear_out; + + return ret; + +err_wmi_clear_out: + regmap_clear_bits(data->regmap, data->ien_reg, + KX022A_MASK_WMI); +err_buf_disable_out: + regmap_clear_bits(data->regmap, data->chip_info->buf_cntl2, + KX022A_MASK_BUF_EN); + data->state &= ~KX022A_STATE_FIFO; +err_turn_on_out: + __kx022a_turn_on_off(data, true); +err_free_out: + kfree(data->fifo_buffer); + + return ret; } static int kx022a_buffer_postenable(struct iio_dev *idev)