summaryrefslogtreecommitdiff
path: root/drivers/iio/afe/iio-rescale.c
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/iio/afe/iio-rescale.c')
-rw-r--r--drivers/iio/afe/iio-rescale.c162
1 files changed, 123 insertions, 39 deletions
diff --git a/drivers/iio/afe/iio-rescale.c b/drivers/iio/afe/iio-rescale.c
index cc28713b0dc8..ea0a39882eb2 100644
--- a/drivers/iio/afe/iio-rescale.c
+++ b/drivers/iio/afe/iio-rescale.c
@@ -3,43 +3,91 @@
* IIO rescale driver
*
* Copyright (C) 2018 Axentia Technologies AB
+ * Copyright (C) 2022 Liam Beguin <liambeguin@gmail.com>
*
* Author: Peter Rosin <peda@axentia.se>
*/
#include <linux/err.h>
#include <linux/gcd.h>
-#include <linux/iio/consumer.h>
-#include <linux/iio/iio.h>
#include <linux/module.h>
#include <linux/of.h>
#include <linux/of_device.h>
#include <linux/platform_device.h>
#include <linux/property.h>
-struct rescale;
+#include <linux/iio/afe/rescale.h>
+#include <linux/iio/consumer.h>
+#include <linux/iio/iio.h>
-struct rescale_cfg {
- enum iio_chan_type type;
- int (*props)(struct device *dev, struct rescale *rescale);
-};
+int rescale_process_scale(struct rescale *rescale, int scale_type,
+ int *val, int *val2)
+{
+ s64 tmp;
-struct rescale {
- const struct rescale_cfg *cfg;
- struct iio_channel *source;
- struct iio_chan_spec chan;
- struct iio_chan_spec_ext_info *ext_info;
- bool chan_processed;
- s32 numerator;
- s32 denominator;
-};
+ switch (scale_type) {
+ case IIO_VAL_FRACTIONAL:
+ *val *= rescale->numerator;
+ *val2 *= rescale->denominator;
+ return scale_type;
+ case IIO_VAL_INT:
+ *val *= rescale->numerator;
+ if (rescale->denominator == 1)
+ return scale_type;
+ *val2 = rescale->denominator;
+ return IIO_VAL_FRACTIONAL;
+ case IIO_VAL_FRACTIONAL_LOG2:
+ tmp = (s64)*val * 1000000000LL;
+ tmp = div_s64(tmp, rescale->denominator);
+ tmp *= rescale->numerator;
+ tmp = div_s64(tmp, 1000000000LL);
+ *val = tmp;
+ return scale_type;
+ default:
+ return -EOPNOTSUPP;
+ }
+}
+
+int rescale_process_offset(struct rescale *rescale, int scale_type,
+ int scale, int scale2, int schan_off,
+ int *val, int *val2)
+{
+ s64 tmp, tmp2;
+
+ switch (scale_type) {
+ case IIO_VAL_FRACTIONAL:
+ tmp = (s64)rescale->offset * scale2;
+ *val = div_s64(tmp, scale) + schan_off;
+ return IIO_VAL_INT;
+ case IIO_VAL_INT:
+ *val = div_s64(rescale->offset, scale) + schan_off;
+ return IIO_VAL_INT;
+ case IIO_VAL_FRACTIONAL_LOG2:
+ tmp = (s64)rescale->offset * (1 << scale2);
+ *val = div_s64(tmp, scale) + schan_off;
+ return IIO_VAL_INT;
+ case IIO_VAL_INT_PLUS_NANO:
+ tmp = (s64)rescale->offset * 1000000000LL;
+ tmp2 = ((s64)scale * 1000000000LL) + scale2;
+ *val = div64_s64(tmp, tmp2) + schan_off;
+ return IIO_VAL_INT;
+ case IIO_VAL_INT_PLUS_MICRO:
+ tmp = (s64)rescale->offset * 1000000LL;
+ tmp2 = ((s64)scale * 1000000LL) + scale2;
+ *val = div64_s64(tmp, tmp2) + schan_off;
+ return IIO_VAL_INT;
+ 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);
- s64 tmp;
+ int scale, scale2;
+ int schan_off = 0;
int ret;
switch (mask) {
@@ -65,27 +113,58 @@ static int rescale_read_raw(struct iio_dev *indio_dev,
} else {
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 = (s64)*val * 1000000000LL;
- tmp = div_s64(tmp, rescale->denominator);
- tmp *= rescale->numerator;
- tmp = div_s64(tmp, 1000000000LL);
- *val = tmp;
- return ret;
- default:
- return -EOPNOTSUPP;
+ return rescale_process_scale(rescale, ret, val, val2);
+ case IIO_CHAN_INFO_OFFSET:
+ /*
+ * Processed channels are scaled 1-to-1 and source offset is
+ * already taken into account.
+ *
+ * In other cases, real world measurement are expressed as:
+ *
+ * schan_scale * (raw + schan_offset)
+ *
+ * Given that the rescaler parameters are applied recursively:
+ *
+ * rescaler_scale * (schan_scale * (raw + schan_offset) +
+ * rescaler_offset)
+ *
+ * Or,
+ *
+ * (rescaler_scale * schan_scale) * (raw +
+ * (schan_offset + rescaler_offset / schan_scale)
+ *
+ * Thus, reusing the original expression the parameters exposed
+ * to userspace are:
+ *
+ * scale = schan_scale * rescaler_scale
+ * offset = schan_offset + rescaler_offset / schan_scale
+ */
+ if (rescale->chan_processed) {
+ *val = rescale->offset;
+ return IIO_VAL_INT;
+ }
+
+ if (iio_channel_has_info(rescale->source->channel,
+ IIO_CHAN_INFO_OFFSET)) {
+ ret = iio_read_channel_offset(rescale->source,
+ &schan_off, NULL);
+ if (ret != IIO_VAL_INT)
+ return ret < 0 ? ret : -EOPNOTSUPP;
+ }
+
+ if (iio_channel_has_info(rescale->source->channel,
+ IIO_CHAN_INFO_SCALE)) {
+ ret = iio_read_channel_scale(rescale->source, &scale, &scale2);
+ return rescale_process_offset(rescale, ret, scale, scale2,
+ schan_off, val, val2);
}
+
+ /*
+ * If we get here we have no scale so scale 1:1 but apply
+ * rescaler and offset, if any.
+ */
+ return rescale_process_offset(rescale, IIO_VAL_FRACTIONAL, 1, 1,
+ schan_off, val, val2);
default:
return -EINVAL;
}
@@ -149,8 +228,9 @@ static int rescale_configure_channel(struct device *dev,
chan->type = rescale->cfg->type;
if (iio_channel_has_info(schan, IIO_CHAN_INFO_RAW) &&
- iio_channel_has_info(schan, IIO_CHAN_INFO_SCALE)) {
- dev_info(dev, "using raw+scale source channel\n");
+ (iio_channel_has_info(schan, IIO_CHAN_INFO_SCALE) ||
+ iio_channel_has_info(schan, IIO_CHAN_INFO_OFFSET))) {
+ dev_info(dev, "using raw+scale/offset source channel\n");
} else if (iio_channel_has_info(schan, IIO_CHAN_INFO_PROCESSED)) {
dev_info(dev, "using processed channel\n");
rescale->chan_processed = true;
@@ -162,6 +242,9 @@ static int rescale_configure_channel(struct device *dev,
chan->info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
BIT(IIO_CHAN_INFO_SCALE);
+ if (rescale->offset)
+ chan->info_mask_separate |= BIT(IIO_CHAN_INFO_OFFSET);
+
/*
* Using .read_avail() is fringe to begin with and makes no sense
* whatsoever for processed channels, so we make sure that this cannot
@@ -326,6 +409,7 @@ static int rescale_probe(struct platform_device *pdev)
rescale->cfg = of_device_get_match_data(dev);
rescale->numerator = 1;
rescale->denominator = 1;
+ rescale->offset = 0;
ret = rescale->cfg->props(dev, rescale);
if (ret)
generated by cgit v1.2.3 (git 2.0.4) at 2024-05-23 22:36:37 +0000