[PATCH v3 2/5] iio: add processed write API

[Romain Gantois <romain.gantois@bootlin.com>]

Add a function to allow IIO consumers to write a processed value to a
channel.

Signed-off-by: Romain Gantois <romain.gantois@bootlin.com>
---
 drivers/iio/inkern.c         | 129 +++++++++++++++++++++++++++++++++++++++++++
 include/linux/iio/consumer.h |  37 +++++++++++++
 2 files changed, 166 insertions(+)

diff --git a/drivers/iio/inkern.c b/drivers/iio/inkern.c
index 1e5eb5a41271..bd0bbaef6045 100644
--- a/drivers/iio/inkern.c
+++ b/drivers/iio/inkern.c
@@ -635,6 +635,57 @@ int iio_multiply_value(int *result, s64 multiplier,
 }
 EXPORT_SYMBOL_NS_GPL(iio_multiply_value, "IIO_UNIT_TEST");
 
+int iio_divide_by_value(int *result, s64 numerator,
+			unsigned int type, int val, int val2)
+{
+	s64 tmp_num, tmp_den;
+
+	switch (type) {
+	case IIO_VAL_INT:
+		tmp_num = numerator;
+		tmp_den = val;
+		break;
+	case IIO_VAL_INT_PLUS_MICRO:
+	case IIO_VAL_INT_PLUS_NANO:
+		switch (type) {
+		case IIO_VAL_INT_PLUS_MICRO:
+			tmp_num = MICRO;
+			tmp_den = MICRO;
+			break;
+
+		case IIO_VAL_INT_PLUS_NANO:
+			tmp_num = NANO;
+			tmp_den = NANO;
+			break;
+		}
+
+		tmp_num *= numerator;
+		tmp_den = (s64)abs(val) * tmp_den + (s64)abs(val2);
+
+		if (val < 0 || val2 < 0)
+			tmp_num *= -1;
+
+		break;
+	case IIO_VAL_FRACTIONAL:
+		tmp_num = (s64)numerator * (s64)val2;
+		tmp_den = val;
+		break;
+	case IIO_VAL_FRACTIONAL_LOG2:
+		tmp_num = (s64)numerator << val2;
+		tmp_den = val;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (!tmp_den)
+		return -ERANGE;
+
+	*result = div64_s64(tmp_num, tmp_den);
+
+	return IIO_VAL_INT;
+}
+
 static int iio_convert_raw_to_processed_unlocked(struct iio_channel *chan,
 						 int raw, int *processed,
 						 unsigned int scale)
@@ -703,6 +754,64 @@ int iio_convert_raw_to_processed(struct iio_channel *chan, int raw,
 }
 EXPORT_SYMBOL_GPL(iio_convert_raw_to_processed);
 
+static int iio_convert_processed_to_raw_unlocked(struct iio_channel *chan,
+						 int processed, int *raw,
+						 unsigned int scale)
+{
+	int scale_type, scale_val, scale_val2;
+	int offset_type, offset_val, offset_val2;
+	int ret, half_step = 0;
+
+	scale_type = iio_channel_read(chan, &scale_val, &scale_val2,
+				      IIO_CHAN_INFO_SCALE);
+	if (scale_type >= 0) {
+		ret = iio_divide_by_value(raw, processed, scale_type, scale_val, scale_val2);
+		if (ret < 0)
+			return ret;
+	} else {
+		*raw = processed;
+	}
+
+	if (!scale)
+		return -ERANGE;
+
+	*raw = div_s64(*raw, scale);
+
+	offset_type = iio_channel_read(chan, &offset_val, &offset_val2,
+				       IIO_CHAN_INFO_OFFSET);
+	if (offset_type >= 0) {
+		switch (offset_type) {
+		case IIO_VAL_INT:
+		case IIO_VAL_INT_PLUS_MICRO:
+			half_step = MICRO / 2;
+			break;
+		case IIO_VAL_INT_PLUS_NANO:
+			half_step = NANO / 2;
+			break;
+		case IIO_VAL_FRACTIONAL:
+			offset_val = DIV_ROUND_CLOSEST(offset_val, offset_val2);
+			break;
+		case IIO_VAL_FRACTIONAL_LOG2:
+			offset_val >>= offset_val2;
+			break;
+		default:
+			return -EINVAL;
+		}
+
+		/* Round fractional part to closest to reduce rounding bias. */
+		if (half_step) {
+			if (offset_val2 >= half_step)
+				*raw -= 1;
+			else if (offset_val2 <= -half_step)
+				*raw += 1;
+		}
+
+		*raw -= offset_val;
+	}
+
+	return 0;
+}
+
 int iio_read_channel_attribute(struct iio_channel *chan, int *val, int *val2,
 			       enum iio_chan_info_enum attribute)
 {
@@ -1039,3 +1148,23 @@ ssize_t iio_read_channel_label(struct iio_channel *chan, char *buf)
 	return do_iio_read_channel_label(chan->indio_dev, chan->channel, buf);
 }
 EXPORT_SYMBOL_GPL(iio_read_channel_label);
+
+int iio_write_channel_processed_scale(struct iio_channel *chan, int val,
+				      unsigned int scale)
+{
+	struct iio_dev_opaque *iio_dev_opaque = to_iio_dev_opaque(chan->indio_dev);
+	int ret, processed;
+
+	guard(mutex)(&iio_dev_opaque->info_exist_lock);
+
+	if (!chan->indio_dev->info)
+		return -ENODEV;
+
+	ret = iio_convert_processed_to_raw_unlocked(chan, val, &processed, scale);
+	if (ret)
+		return ret;
+
+	return iio_channel_write(chan, processed, 0, IIO_CHAN_INFO_RAW);
+}
+EXPORT_SYMBOL_GPL(iio_write_channel_processed_scale);
+
diff --git a/include/linux/iio/consumer.h b/include/linux/iio/consumer.h
index a38b277c2c02..29d08b57bac9 100644
--- a/include/linux/iio/consumer.h
+++ b/include/linux/iio/consumer.h
@@ -399,6 +399,24 @@ int iio_read_channel_scale(struct iio_channel *chan, int *val,
 int iio_multiply_value(int *result, s64 multiplier,
 		       unsigned int type, int val, int val2);
 
+/**
+ * iio_divide_by_value() - Divide by an IIO value
+ * @result:	Destination pointer for the division result
+ * @numerator:	Numerator.
+ * @type:	One of the IIO_VAL_* constants. This decides how the @val
+ *		and @val2 parameters are interpreted.
+ * @val:	Denominator.
+ * @val2:	Denominator. @val2 use depends on type.
+ *
+ * Divide an s64 number by an IIO value, storing the result as
+ * IIO_VAL_INT. This is typically used for scaling.
+ *
+ * Returns:
+ * IIO_VAL_INT on success or a negative error-number on failure.
+ */
+int iio_divide_by_value(int *result, s64 numerator,
+			unsigned int type, int val, int val2);
+
 /**
  * iio_convert_raw_to_processed() - Converts a raw value to a processed value
  * @chan:		The channel being queried
@@ -469,4 +487,23 @@ ssize_t iio_write_channel_ext_info(struct iio_channel *chan, const char *attr,
  */
 ssize_t iio_read_channel_label(struct iio_channel *chan, char *buf);
 
+/**
+ * iio_write_channel_processed_scale() - scale and write processed value to a given channel
+ * @chan:		The channel being queried.
+ * @val:		Value to write.
+ * @scale:		Processed value is divided by this scale factor during the conversion.
+ *
+ * This function writes a processed value to a channel. A processed value means
+ * that this value will have the correct unit and not some device internal
+ * representation. If the device does not support writing a processed value, the
+ * function will query the channel's scale and offset and write an appropriately
+ * transformed raw value.
+ *
+ * Context: May sleep.
+ * Return: an error code or 0.
+ *
+ */
+int iio_write_channel_processed_scale(struct iio_channel *chan, int val,
+				      unsigned int scale);
+
 #endif

-- 
2.51.2