When the IIO channel has a scale_available attribute, we want the values
contained to be properly converted the same way the scale value is.
Signed-off-by: Paul Cercueil <paul@xxxxxxxxxxxxxxx>
---
drivers/iio/afe/iio-rescale.c | 125 ++++++++++++++++++++++++++++------
1 file changed, 103 insertions(+), 22 deletions(-)
diff --git a/drivers/iio/afe/iio-rescale.c b/drivers/iio/afe/iio-rescale.c
index e9ceee66d1e7..95802d9ee25e 100644
--- a/drivers/iio/afe/iio-rescale.c
+++ b/drivers/iio/afe/iio-rescale.c
@@ -31,14 +31,45 @@ struct rescale {
struct iio_chan_spec_ext_info *ext_info;
s32 numerator;
s32 denominator;
+ int scale_type, scale_len;
+ int *scale_data;
};
+static int rescale_convert(struct rescale *rescale, int type,
+ const int val, const int val2,
+ int *val_out, int *val2_out)
+{
+ unsigned long long tmp;
+
+ switch (type) {
+ case IIO_VAL_FRACTIONAL:
+ *val_out = val * rescale->numerator;
+ *val2_out = val2 * rescale->denominator;
+ return type;
+ case IIO_VAL_INT:
+ *val_out = val * rescale->numerator;
+ if (rescale->denominator == 1)
+ return type;
+ *val2_out = rescale->denominator;
+ return IIO_VAL_FRACTIONAL;
+ case IIO_VAL_FRACTIONAL_LOG2:
+ tmp = val * 1000000000LL;
+ do_div(tmp, rescale->denominator);
+ tmp *= rescale->numerator;
+ do_div(tmp, 1000000000LL);
+ *val_out = tmp;
+ *val2_out = val2;
+ return type;
+ default:
+ return -EOPNOTSUPP;
+ }
+}
+
static int rescale_read_raw(struct iio_dev *indio_dev,
struct iio_chan_spec const *chan,
int *val, int *val2, long mask)
{
struct rescale *rescale = iio_priv(indio_dev);
- unsigned long long tmp;
int ret;
switch (mask) {
@@ -47,27 +78,7 @@ static int rescale_read_raw(struct iio_dev *indio_dev,
case IIO_CHAN_INFO_SCALE:
ret = iio_read_channel_scale(rescale->source, val, val2);
- switch (ret) {
- case IIO_VAL_FRACTIONAL:
- *val *= rescale->numerator;
- *val2 *= rescale->denominator;
- return ret;
- case IIO_VAL_INT:
- *val *= rescale->numerator;
- if (rescale->denominator == 1)
- return ret;
- *val2 = rescale->denominator;
- return IIO_VAL_FRACTIONAL;
- case IIO_VAL_FRACTIONAL_LOG2:
- tmp = *val * 1000000000LL;
- do_div(tmp, rescale->denominator);
- tmp *= rescale->numerator;
- do_div(tmp, 1000000000LL);
- *val = tmp;
- return ret;
- default:
- return -EOPNOTSUPP;
- }
+ return rescale_convert(rescale, ret, *val, *val2, val, val2);
default:
return -EINVAL;
}
@@ -85,6 +96,14 @@ static int rescale_read_avail(struct iio_dev *indio_dev,
*type = IIO_VAL_INT;
return iio_read_avail_channel_raw(rescale->source,
vals, length);
+ case IIO_CHAN_INFO_SCALE:
+ if (rescale->scale_len) {
+ *type = rescale->scale_type;
+ *length = rescale->scale_len;
+ *vals = rescale->scale_data;
+ return IIO_AVAIL_LIST;
+ }
+ /* fall-through */
default:
return -EINVAL;
}
@@ -119,11 +138,65 @@ static ssize_t rescale_write_ext_info(struct iio_dev *indio_dev,
buf, len);
}
+static int rescale_init_scale_avail(struct device *dev, struct rescale *rescale)
+{
+ const int *scale_raw;
+ unsigned int i;
+ int ret;
+
+ ret = iio_read_avail_channel_attribute(rescale->source, &scale_raw,
+ &rescale->scale_type,
+ &rescale->scale_len,
+ IIO_CHAN_INFO_SCALE);
+ if (ret)
+ return ret;
+
+ if (rescale->scale_type == IIO_VAL_INT && rescale->denominator > 1)
+ rescale->scale_len *= 2;
+
+ rescale->scale_data = devm_kzalloc(dev,
+ sizeof(int) * rescale->scale_len,
+ GFP_KERNEL);
+ if (!rescale->scale_len)
+ return -ENOMEM;
+
+ if (rescale->scale_type != IIO_VAL_INT) {
+ for (i = 0; i < rescale->scale_len; i += 2) {
+ ret = rescale_convert(rescale, rescale->scale_type,
+ scale_raw[i], scale_raw[i + 1],
+ &rescale->scale_data[i],
+ &rescale->scale_data[i + 1]);
+ if (ret < 0)
+ return ret;
+ }
+ } else if (rescale->denominator == 1) {
+ for (i = 0; i < rescale->scale_len; i++) {
+ ret = rescale_convert(rescale, IIO_VAL_INT,
+ scale_raw[i], 0,
+ &rescale->scale_data[i], NULL);
+ if (ret < 0)
+ return ret;
+ }
+ } else {
+ for (i = 0; i < rescale->scale_len / 2; i++) {
+ ret = rescale_convert(rescale, IIO_VAL_INT,
+ scale_raw[i], 0,
+ &rescale->scale_data[i * 2],
+ &rescale->scale_data[i * 2 + 1]);
+ if (ret < 0)
+ return ret;
+ }
+ }
+
+ return 0;
+}
+
static int rescale_configure_channel(struct device *dev,
struct rescale *rescale)
{
struct iio_chan_spec *chan = &rescale->chan;
struct iio_chan_spec const *schan = rescale->source->channel;
+ int ret;
chan->indexed = 1;
chan->output = schan->output;
@@ -142,6 +215,14 @@ static int rescale_configure_channel(struct device *dev,
if (iio_channel_has_available(schan, IIO_CHAN_INFO_RAW))
chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW);
+ if (iio_channel_has_available(schan, IIO_CHAN_INFO_SCALE)) {
+ chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_SCALE);
+
+ ret = rescale_init_scale_avail(dev, rescale);
+ if (ret)
+ return ret;
+ }
+
return 0;
}
--
2.25.0
