[PATCH 2/4] iio: adc: sd modulator: add scale support

[Olivier Moysan <olivier.moysan@st.com>]

Add scale support to sigma delta modulator.

Signed-off-by: Olivier Moysan <olivier.moysan@st.com>
---
 drivers/iio/adc/sd_adc_modulator.c | 108 ++++++++++++++++++++++++++---
 1 file changed, 100 insertions(+), 8 deletions(-)

diff --git a/drivers/iio/adc/sd_adc_modulator.c b/drivers/iio/adc/sd_adc_modulator.c
index 560d8c7d9d86..a83f35832050 100644
--- a/drivers/iio/adc/sd_adc_modulator.c
+++ b/drivers/iio/adc/sd_adc_modulator.c
@@ -10,8 +10,7 @@
 #include <linux/iio/triggered_buffer.h>
 #include <linux/module.h>
 #include <linux/of_device.h>
-
-static const struct iio_info iio_sd_mod_iio_info;
+#include <linux/regulator/consumer.h>
 
 static const struct iio_chan_spec iio_sd_mod_ch = {
 	.type = IIO_VOLTAGE,
@@ -21,34 +20,126 @@ static const struct iio_chan_spec iio_sd_mod_ch = {
 		.realbits = 1,
 		.shift = 0,
 	},
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
+};
+
+static const struct iio_chan_spec iio_sd_mod_ch_ads = {
+	.type = IIO_VOLTAGE,
+	.indexed = 1,
+	.scan_type = {
+		.sign = 'u',
+		.realbits = 1,
+		.shift = 0,
+	},
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
+	.differential = 1,
+};
+
+struct iio_sd_mod_priv {
+	struct regulator *vref;
+	int vref_mv;
+};
+
+static int iio_sd_mod_read_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan, int *val,
+			       int *val2, long mask)
+{
+	struct iio_sd_mod_priv *priv = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SCALE:
+		*val = priv->vref_mv;
+		*val2 = chan->scan_type.realbits;
+		return IIO_VAL_INT;
+	}
+
+	return -EINVAL;
+}
+
+static const struct iio_info iio_sd_mod_iio_info = {
+	.read_raw = iio_sd_mod_read_raw,
 };
 
 static int iio_sd_mod_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
+	struct iio_sd_mod_priv *priv;
 	struct iio_dev *iio;
+	int ret;
 
-	iio = devm_iio_device_alloc(dev, 0);
+	iio = devm_iio_device_alloc(dev, sizeof(*priv));
 	if (!iio)
 		return -ENOMEM;
 
+	iio->channels = (const struct iio_chan_spec *)
+					of_device_get_match_data(&pdev->dev);
+
+	priv = iio_priv(iio);
+
 	iio->dev.parent = dev;
 	iio->dev.of_node = dev->of_node;
 	iio->name = dev_name(dev);
 	iio->info = &iio_sd_mod_iio_info;
 	iio->modes = INDIO_BUFFER_HARDWARE;
-
 	iio->num_channels = 1;
-	iio->channels = &iio_sd_mod_ch;
 
 	platform_set_drvdata(pdev, iio);
 
-	return devm_iio_device_register(&pdev->dev, iio);
+	priv->vref = devm_regulator_get_optional(dev, "vref");
+	if (IS_ERR(priv->vref)) {
+		ret = PTR_ERR(priv->vref);
+		if (ret != -ENODEV) {
+			if (ret != -EPROBE_DEFER)
+				dev_err(dev, "vref get failed, %d\n", ret);
+			return ret;
+		}
+	}
+
+	if (!IS_ERR(priv->vref)) {
+		ret = regulator_enable(priv->vref);
+		if (ret < 0) {
+			dev_err(dev, "vref enable failed %d\n", ret);
+			return ret;
+		}
+
+		ret = regulator_get_voltage(priv->vref);
+		if (ret < 0) {
+			dev_err(dev, "vref get failed, %d\n", ret);
+			goto err_regulator_disable;
+		}
+
+		priv->vref_mv = ret / 1000;
+		dev_dbg(dev, "vref+=%dmV\n", priv->vref_mv);
+	}
+
+	ret = devm_iio_device_register(&pdev->dev, iio);
+	if (ret < 0) {
+		dev_err(dev, "Failed to register sd-modulator, %d\n", ret);
+		goto err_regulator_disable;
+	}
+
+	return 0;
+
+err_regulator_disable:
+	regulator_disable(priv->vref);
+
+	return ret;
+}
+
+static int iio_sd_mod_remove(struct platform_device *pdev)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct iio_sd_mod_priv *priv = iio_priv(indio_dev);
+
+	if (priv->vref)
+		return regulator_disable(priv->vref);
+
+	return 0;
 }
 
 static const struct of_device_id sd_adc_of_match[] = {
-	{ .compatible = "sd-modulator" },
-	{ .compatible = "ads1201" },
+	{ .compatible = "sd-modulator", .data = &iio_sd_mod_ch },
+	{ .compatible = "ads1201", .data = &iio_sd_mod_ch_ads },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, sd_adc_of_match);
@@ -59,6 +150,7 @@ static struct platform_driver iio_sd_mod_adc = {
 		.of_match_table = of_match_ptr(sd_adc_of_match),
 	},
 	.probe = iio_sd_mod_probe,
+	.remove = iio_sd_mod_remove,
 };
 
 module_platform_driver(iio_sd_mod_adc);
-- 
2.17.1