1 files changed, 37 insertions, 42 deletions

diff --git a/drivers/iio/accel/bma220_spi.c b/drivers/iio/accel/bma220_spi.c
index 0622c7936499..bc4c626e454d 100644
--- a/drivers/iio/accel/bma220_spi.c
+++ b/drivers/iio/accel/bma220_spi.c
@@ -218,20 +218,33 @@ static int bma220_init(struct spi_device *spi)
 	return 0;
 }
 
-static int bma220_deinit(struct spi_device *spi)
+static int bma220_power(struct spi_device *spi, bool up)
 {
-	int ret;
-
-	/* Make sure the chip is powered off */
-	ret = bma220_read_reg(spi, BMA220_REG_SUSPEND);
-	if (ret == BMA220_SUSPEND_SLEEP)
+	int i, ret;
+
+	/**
+	 * The chip can be suspended/woken up by a simple register read.
+	 * So, we need up to 2 register reads of the suspend register
+	 * to make sure that the device is in the desired state.
+	 */
+	for (i = 0; i < 2; i++) {
 		ret = bma220_read_reg(spi, BMA220_REG_SUSPEND);
-	if (ret < 0)
-		return ret;
-	if (ret == BMA220_SUSPEND_SLEEP)
-		return -EBUSY;
+		if (ret < 0)
+			return ret;
 
-	return 0;
+		if (up && ret == BMA220_SUSPEND_SLEEP)
+			return 0;
+
+		if (!up && ret == BMA220_SUSPEND_WAKE)
+			return 0;
+	}
+
+	return -EBUSY;
+}
+
+static void bma220_deinit(void *spi)
+{
+	bma220_power(spi, false);
 }
 
 static int bma220_probe(struct spi_device *spi)
@@ -248,7 +261,6 @@ static int bma220_probe(struct spi_device *spi)
 
 	data = iio_priv(indio_dev);
 	data->spi_device = spi;
-	spi_set_drvdata(spi, indio_dev);
 	mutex_init(&data->lock);
 
 	indio_dev->info = &bma220_info;
@@ -262,49 +274,33 @@ static int bma220_probe(struct spi_device *spi)
 	if (ret)
 		return ret;
 
-	ret = iio_triggered_buffer_setup(indio_dev, iio_pollfunc_store_time,
-					 bma220_trigger_handler, NULL);
-	if (ret < 0) {
-		dev_err(&spi->dev, "iio triggered buffer setup failed\n");
-		goto err_suspend;
-	}
+	ret = devm_add_action_or_reset(&spi->dev, bma220_deinit, spi);
+	if (ret)
+		return ret;
 
-	ret = iio_device_register(indio_dev);
+	ret = devm_iio_triggered_buffer_setup(&spi->dev, indio_dev,
+					      iio_pollfunc_store_time,
+					      bma220_trigger_handler, NULL);
 	if (ret < 0) {
-		dev_err(&spi->dev, "iio_device_register failed\n");
-		iio_triggered_buffer_cleanup(indio_dev);
-		goto err_suspend;
+		dev_err(&spi->dev, "iio triggered buffer setup failed\n");
+		return ret;
 	}
 
-	return 0;
-
-err_suspend:
-	return bma220_deinit(spi);
-}
-
-static int bma220_remove(struct spi_device *spi)
-{
-	struct iio_dev *indio_dev = spi_get_drvdata(spi);
-
-	iio_device_unregister(indio_dev);
-	iio_triggered_buffer_cleanup(indio_dev);
-
-	return bma220_deinit(spi);
+	return devm_iio_device_register(&spi->dev, indio_dev);
 }
 
 static __maybe_unused int bma220_suspend(struct device *dev)
 {
-	struct bma220_data *data = iio_priv(dev_get_drvdata(dev));
+	struct spi_device *spi = to_spi_device(dev);
 
-	/* The chip can be suspended/woken up by a simple register read. */
-	return bma220_read_reg(data->spi_device, BMA220_REG_SUSPEND);
+	return bma220_power(spi, false);
 }
 
 static __maybe_unused int bma220_resume(struct device *dev)
 {
-	struct bma220_data *data = iio_priv(dev_get_drvdata(dev));
+	struct spi_device *spi = to_spi_device(dev);
 
-	return bma220_read_reg(data->spi_device, BMA220_REG_SUSPEND);
+	return bma220_power(spi, true);
 }
 static SIMPLE_DEV_PM_OPS(bma220_pm_ops, bma220_suspend, bma220_resume);
 
@@ -326,7 +322,6 @@ static struct spi_driver bma220_driver = {
 		.acpi_match_table = bma220_acpi_id,
 	},
 	.probe =            bma220_probe,
-	.remove =           bma220_remove,
 	.id_table =         bma220_spi_id,
 };
 module_spi_driver(bma220_driver);