xref: /linux/drivers/iio/afe/iio-rescale.c (revision 0ea5c948cb64bab5bc7a5516774eb8536f05aa0d)
1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * IIO rescale driver
4  *
5  * Copyright (C) 2018 Axentia Technologies AB
6  * Copyright (C) 2022 Liam Beguin <liambeguin@gmail.com>
7  *
8  * Author: Peter Rosin <peda@axentia.se>
9  */
10 
11 #include <linux/err.h>
12 #include <linux/gcd.h>
13 #include <linux/mod_devicetable.h>
14 #include <linux/module.h>
15 #include <linux/platform_device.h>
16 #include <linux/property.h>
17 
18 #include <linux/iio/afe/rescale.h>
19 #include <linux/iio/consumer.h>
20 #include <linux/iio/iio.h>
21 
rescale_process_scale(struct rescale * rescale,int scale_type,int * val,int * val2)22 int rescale_process_scale(struct rescale *rescale, int scale_type,
23 			  int *val, int *val2)
24 {
25 	s64 tmp;
26 	int _val, _val2;
27 	s32 rem, rem2;
28 	u32 mult;
29 	u32 neg;
30 
31 	switch (scale_type) {
32 	case IIO_VAL_INT:
33 		*val *= rescale->numerator;
34 		if (rescale->denominator == 1)
35 			return scale_type;
36 		*val2 = rescale->denominator;
37 		return IIO_VAL_FRACTIONAL;
38 	case IIO_VAL_FRACTIONAL:
39 		/*
40 		 * When the product of both scales doesn't overflow, avoid
41 		 * potential accuracy loss (for in kernel consumers) by
42 		 * keeping a fractional representation.
43 		 */
44 		if (!check_mul_overflow(*val, rescale->numerator, &_val) &&
45 		    !check_mul_overflow(*val2, rescale->denominator, &_val2)) {
46 			*val = _val;
47 			*val2 = _val2;
48 			return IIO_VAL_FRACTIONAL;
49 		}
50 		fallthrough;
51 	case IIO_VAL_FRACTIONAL_LOG2:
52 		tmp = (s64)*val * 1000000000LL;
53 		tmp = div_s64(tmp, rescale->denominator);
54 		tmp *= rescale->numerator;
55 
56 		tmp = div_s64_rem(tmp, 1000000000LL, &rem);
57 		*val = tmp;
58 
59 		if (!rem)
60 			return scale_type;
61 
62 		if (scale_type == IIO_VAL_FRACTIONAL)
63 			tmp = *val2;
64 		else
65 			tmp = ULL(1) << *val2;
66 
67 		rem2 = *val % (int)tmp;
68 		*val = *val / (int)tmp;
69 
70 		*val2 = rem / (int)tmp;
71 		if (rem2)
72 			*val2 += div_s64((s64)rem2 * 1000000000LL, tmp);
73 
74 		return IIO_VAL_INT_PLUS_NANO;
75 	case IIO_VAL_INT_PLUS_NANO:
76 	case IIO_VAL_INT_PLUS_MICRO:
77 		mult = scale_type == IIO_VAL_INT_PLUS_NANO ? 1000000000L : 1000000L;
78 
79 		/*
80 		 * For IIO_VAL_INT_PLUS_{MICRO,NANO} scale types if either *val
81 		 * OR *val2 is negative the schan scale is negative, i.e.
82 		 * *val = 1 and *val2 = -0.5 yields -1.5 not -0.5.
83 		 */
84 		neg = *val < 0 || *val2 < 0;
85 
86 		tmp = (s64)abs(*val) * abs(rescale->numerator);
87 		*val = div_s64_rem(tmp, abs(rescale->denominator), &rem);
88 
89 		tmp = (s64)rem * mult + (s64)abs(*val2) * abs(rescale->numerator);
90 		tmp = div_s64(tmp, abs(rescale->denominator));
91 
92 		*val += div_s64_rem(tmp, mult, val2);
93 
94 		/*
95 		 * If only one of the rescaler elements or the schan scale is
96 		 * negative, the combined scale is negative.
97 		 */
98 		if (neg ^ ((rescale->numerator < 0) ^ (rescale->denominator < 0))) {
99 			if (*val)
100 				*val = -*val;
101 			else
102 				*val2 = -*val2;
103 		}
104 
105 		return scale_type;
106 	default:
107 		return -EOPNOTSUPP;
108 	}
109 }
110 EXPORT_SYMBOL_NS_GPL(rescale_process_scale, IIO_RESCALE);
111 
rescale_process_offset(struct rescale * rescale,int scale_type,int scale,int scale2,int schan_off,int * val,int * val2)112 int rescale_process_offset(struct rescale *rescale, int scale_type,
113 			   int scale, int scale2, int schan_off,
114 			   int *val, int *val2)
115 {
116 	s64 tmp, tmp2;
117 
118 	switch (scale_type) {
119 	case IIO_VAL_FRACTIONAL:
120 		tmp = (s64)rescale->offset * scale2;
121 		*val = div_s64(tmp, scale) + schan_off;
122 		return IIO_VAL_INT;
123 	case IIO_VAL_INT:
124 		*val = div_s64(rescale->offset, scale) + schan_off;
125 		return IIO_VAL_INT;
126 	case IIO_VAL_FRACTIONAL_LOG2:
127 		tmp = (s64)rescale->offset * (1 << scale2);
128 		*val = div_s64(tmp, scale) + schan_off;
129 		return IIO_VAL_INT;
130 	case IIO_VAL_INT_PLUS_NANO:
131 		tmp = (s64)rescale->offset * 1000000000LL;
132 		tmp2 = ((s64)scale * 1000000000LL) + scale2;
133 		*val = div64_s64(tmp, tmp2) + schan_off;
134 		return IIO_VAL_INT;
135 	case IIO_VAL_INT_PLUS_MICRO:
136 		tmp = (s64)rescale->offset * 1000000LL;
137 		tmp2 = ((s64)scale * 1000000LL) + scale2;
138 		*val = div64_s64(tmp, tmp2) + schan_off;
139 		return IIO_VAL_INT;
140 	default:
141 		return -EOPNOTSUPP;
142 	}
143 }
144 EXPORT_SYMBOL_NS_GPL(rescale_process_offset, IIO_RESCALE);
145 
rescale_read_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int * val,int * val2,long mask)146 static int rescale_read_raw(struct iio_dev *indio_dev,
147 			    struct iio_chan_spec const *chan,
148 			    int *val, int *val2, long mask)
149 {
150 	struct rescale *rescale = iio_priv(indio_dev);
151 	int scale, scale2;
152 	int schan_off = 0;
153 	int ret;
154 
155 	switch (mask) {
156 	case IIO_CHAN_INFO_RAW:
157 		if (rescale->chan_processed)
158 			/*
159 			 * When only processed channels are supported, we
160 			 * read the processed data and scale it by 1/1
161 			 * augmented with whatever the rescaler has calculated.
162 			 */
163 			return iio_read_channel_processed(rescale->source, val);
164 		else
165 			return iio_read_channel_raw(rescale->source, val);
166 
167 	case IIO_CHAN_INFO_SCALE:
168 		if (rescale->chan_processed) {
169 			/*
170 			 * Processed channels are scaled 1-to-1
171 			 */
172 			*val = 1;
173 			*val2 = 1;
174 			ret = IIO_VAL_FRACTIONAL;
175 		} else {
176 			ret = iio_read_channel_scale(rescale->source, val, val2);
177 		}
178 		return rescale_process_scale(rescale, ret, val, val2);
179 	case IIO_CHAN_INFO_OFFSET:
180 		/*
181 		 * Processed channels are scaled 1-to-1 and source offset is
182 		 * already taken into account.
183 		 *
184 		 * In other cases, real world measurement are expressed as:
185 		 *
186 		 *	schan_scale * (raw + schan_offset)
187 		 *
188 		 * Given that the rescaler parameters are applied recursively:
189 		 *
190 		 *	rescaler_scale * (schan_scale * (raw + schan_offset) +
191 		 *		rescaler_offset)
192 		 *
193 		 * Or,
194 		 *
195 		 *	(rescaler_scale * schan_scale) * (raw +
196 		 *		(schan_offset +	rescaler_offset / schan_scale)
197 		 *
198 		 * Thus, reusing the original expression the parameters exposed
199 		 * to userspace are:
200 		 *
201 		 *	scale = schan_scale * rescaler_scale
202 		 *	offset = schan_offset + rescaler_offset / schan_scale
203 		 */
204 		if (rescale->chan_processed) {
205 			*val = rescale->offset;
206 			return IIO_VAL_INT;
207 		}
208 
209 		if (iio_channel_has_info(rescale->source->channel,
210 					 IIO_CHAN_INFO_OFFSET)) {
211 			ret = iio_read_channel_offset(rescale->source,
212 						      &schan_off, NULL);
213 			if (ret != IIO_VAL_INT)
214 				return ret < 0 ? ret : -EOPNOTSUPP;
215 		}
216 
217 		if (iio_channel_has_info(rescale->source->channel,
218 					 IIO_CHAN_INFO_SCALE)) {
219 			ret = iio_read_channel_scale(rescale->source, &scale, &scale2);
220 			return rescale_process_offset(rescale, ret, scale, scale2,
221 						      schan_off, val, val2);
222 		}
223 
224 		/*
225 		 * If we get here we have no scale so scale 1:1 but apply
226 		 * rescaler and offset, if any.
227 		 */
228 		return rescale_process_offset(rescale, IIO_VAL_FRACTIONAL, 1, 1,
229 					      schan_off, val, val2);
230 	default:
231 		return -EINVAL;
232 	}
233 }
234 
rescale_read_avail(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,const int ** vals,int * type,int * length,long mask)235 static int rescale_read_avail(struct iio_dev *indio_dev,
236 			      struct iio_chan_spec const *chan,
237 			      const int **vals, int *type, int *length,
238 			      long mask)
239 {
240 	struct rescale *rescale = iio_priv(indio_dev);
241 
242 	switch (mask) {
243 	case IIO_CHAN_INFO_RAW:
244 		*type = IIO_VAL_INT;
245 		return iio_read_avail_channel_raw(rescale->source,
246 						  vals, length);
247 	default:
248 		return -EINVAL;
249 	}
250 }
251 
252 static const struct iio_info rescale_info = {
253 	.read_raw = rescale_read_raw,
254 	.read_avail = rescale_read_avail,
255 };
256 
rescale_read_ext_info(struct iio_dev * indio_dev,uintptr_t private,struct iio_chan_spec const * chan,char * buf)257 static ssize_t rescale_read_ext_info(struct iio_dev *indio_dev,
258 				     uintptr_t private,
259 				     struct iio_chan_spec const *chan,
260 				     char *buf)
261 {
262 	struct rescale *rescale = iio_priv(indio_dev);
263 
264 	return iio_read_channel_ext_info(rescale->source,
265 					 rescale->ext_info[private].name,
266 					 buf);
267 }
268 
rescale_write_ext_info(struct iio_dev * indio_dev,uintptr_t private,struct iio_chan_spec const * chan,const char * buf,size_t len)269 static ssize_t rescale_write_ext_info(struct iio_dev *indio_dev,
270 				      uintptr_t private,
271 				      struct iio_chan_spec const *chan,
272 				      const char *buf, size_t len)
273 {
274 	struct rescale *rescale = iio_priv(indio_dev);
275 
276 	return iio_write_channel_ext_info(rescale->source,
277 					  rescale->ext_info[private].name,
278 					  buf, len);
279 }
280 
rescale_configure_channel(struct device * dev,struct rescale * rescale)281 static int rescale_configure_channel(struct device *dev,
282 				     struct rescale *rescale)
283 {
284 	struct iio_chan_spec *chan = &rescale->chan;
285 	struct iio_chan_spec const *schan = rescale->source->channel;
286 
287 	chan->indexed = 1;
288 	chan->output = schan->output;
289 	chan->ext_info = rescale->ext_info;
290 	chan->type = rescale->cfg->type;
291 
292 	if (iio_channel_has_info(schan, IIO_CHAN_INFO_RAW) &&
293 	    (iio_channel_has_info(schan, IIO_CHAN_INFO_SCALE) ||
294 	     iio_channel_has_info(schan, IIO_CHAN_INFO_OFFSET))) {
295 		dev_info(dev, "using raw+scale/offset source channel\n");
296 	} else if (iio_channel_has_info(schan, IIO_CHAN_INFO_PROCESSED)) {
297 		dev_info(dev, "using processed channel\n");
298 		rescale->chan_processed = true;
299 	} else {
300 		dev_err(dev, "source channel is not supported\n");
301 		return -EINVAL;
302 	}
303 
304 	chan->info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
305 		BIT(IIO_CHAN_INFO_SCALE);
306 
307 	if (rescale->offset)
308 		chan->info_mask_separate |= BIT(IIO_CHAN_INFO_OFFSET);
309 
310 	/*
311 	 * Using .read_avail() is fringe to begin with and makes no sense
312 	 * whatsoever for processed channels, so we make sure that this cannot
313 	 * be called on a processed channel.
314 	 */
315 	if (iio_channel_has_available(schan, IIO_CHAN_INFO_RAW) &&
316 	    !rescale->chan_processed)
317 		chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW);
318 
319 	return 0;
320 }
321 
rescale_current_sense_amplifier_props(struct device * dev,struct rescale * rescale)322 static int rescale_current_sense_amplifier_props(struct device *dev,
323 						 struct rescale *rescale)
324 {
325 	u32 sense;
326 	u32 gain_mult = 1;
327 	u32 gain_div = 1;
328 	u32 factor;
329 	int ret;
330 
331 	ret = device_property_read_u32(dev, "sense-resistor-micro-ohms",
332 				       &sense);
333 	if (ret) {
334 		dev_err(dev, "failed to read the sense resistance: %d\n", ret);
335 		return ret;
336 	}
337 
338 	device_property_read_u32(dev, "sense-gain-mult", &gain_mult);
339 	device_property_read_u32(dev, "sense-gain-div", &gain_div);
340 
341 	/*
342 	 * Calculate the scaling factor, 1 / (gain * sense), or
343 	 * gain_div / (gain_mult * sense), while trying to keep the
344 	 * numerator/denominator from overflowing.
345 	 */
346 	factor = gcd(sense, 1000000);
347 	rescale->numerator = 1000000 / factor;
348 	rescale->denominator = sense / factor;
349 
350 	factor = gcd(rescale->numerator, gain_mult);
351 	rescale->numerator /= factor;
352 	rescale->denominator *= gain_mult / factor;
353 
354 	factor = gcd(rescale->denominator, gain_div);
355 	rescale->numerator *= gain_div / factor;
356 	rescale->denominator /= factor;
357 
358 	return 0;
359 }
360 
rescale_current_sense_shunt_props(struct device * dev,struct rescale * rescale)361 static int rescale_current_sense_shunt_props(struct device *dev,
362 					     struct rescale *rescale)
363 {
364 	u32 shunt;
365 	u32 factor;
366 	int ret;
367 
368 	ret = device_property_read_u32(dev, "shunt-resistor-micro-ohms",
369 				       &shunt);
370 	if (ret) {
371 		dev_err(dev, "failed to read the shunt resistance: %d\n", ret);
372 		return ret;
373 	}
374 
375 	factor = gcd(shunt, 1000000);
376 	rescale->numerator = 1000000 / factor;
377 	rescale->denominator = shunt / factor;
378 
379 	return 0;
380 }
381 
rescale_voltage_divider_props(struct device * dev,struct rescale * rescale)382 static int rescale_voltage_divider_props(struct device *dev,
383 					 struct rescale *rescale)
384 {
385 	int ret;
386 	u32 factor;
387 
388 	ret = device_property_read_u32(dev, "output-ohms",
389 				       &rescale->denominator);
390 	if (ret) {
391 		dev_err(dev, "failed to read output-ohms: %d\n", ret);
392 		return ret;
393 	}
394 
395 	ret = device_property_read_u32(dev, "full-ohms",
396 				       &rescale->numerator);
397 	if (ret) {
398 		dev_err(dev, "failed to read full-ohms: %d\n", ret);
399 		return ret;
400 	}
401 
402 	factor = gcd(rescale->numerator, rescale->denominator);
403 	rescale->numerator /= factor;
404 	rescale->denominator /= factor;
405 
406 	return 0;
407 }
408 
rescale_temp_sense_rtd_props(struct device * dev,struct rescale * rescale)409 static int rescale_temp_sense_rtd_props(struct device *dev,
410 					struct rescale *rescale)
411 {
412 	u32 factor;
413 	u32 alpha;
414 	u32 iexc;
415 	u32 tmp;
416 	int ret;
417 	u32 r0;
418 
419 	ret = device_property_read_u32(dev, "excitation-current-microamp",
420 				       &iexc);
421 	if (ret) {
422 		dev_err(dev, "failed to read excitation-current-microamp: %d\n",
423 			ret);
424 		return ret;
425 	}
426 
427 	ret = device_property_read_u32(dev, "alpha-ppm-per-celsius", &alpha);
428 	if (ret) {
429 		dev_err(dev, "failed to read alpha-ppm-per-celsius: %d\n",
430 			ret);
431 		return ret;
432 	}
433 
434 	ret = device_property_read_u32(dev, "r-naught-ohms", &r0);
435 	if (ret) {
436 		dev_err(dev, "failed to read r-naught-ohms: %d\n", ret);
437 		return ret;
438 	}
439 
440 	tmp = r0 * iexc * alpha / 1000000;
441 	factor = gcd(tmp, 1000000);
442 	rescale->numerator = 1000000 / factor;
443 	rescale->denominator = tmp / factor;
444 
445 	rescale->offset = -1 * ((r0 * iexc) / 1000);
446 
447 	return 0;
448 }
449 
rescale_temp_transducer_props(struct device * dev,struct rescale * rescale)450 static int rescale_temp_transducer_props(struct device *dev,
451 					 struct rescale *rescale)
452 {
453 	s32 offset = 0;
454 	s32 sense = 1;
455 	s32 alpha;
456 	int ret;
457 
458 	device_property_read_u32(dev, "sense-offset-millicelsius", &offset);
459 	device_property_read_u32(dev, "sense-resistor-ohms", &sense);
460 	ret = device_property_read_u32(dev, "alpha-ppm-per-celsius", &alpha);
461 	if (ret) {
462 		dev_err(dev, "failed to read alpha-ppm-per-celsius: %d\n", ret);
463 		return ret;
464 	}
465 
466 	rescale->numerator = 1000000;
467 	rescale->denominator = alpha * sense;
468 
469 	rescale->offset = div_s64((s64)offset * rescale->denominator,
470 				  rescale->numerator);
471 
472 	return 0;
473 }
474 
475 enum rescale_variant {
476 	CURRENT_SENSE_AMPLIFIER,
477 	CURRENT_SENSE_SHUNT,
478 	VOLTAGE_DIVIDER,
479 	TEMP_SENSE_RTD,
480 	TEMP_TRANSDUCER,
481 };
482 
483 static const struct rescale_cfg rescale_cfg[] = {
484 	[CURRENT_SENSE_AMPLIFIER] = {
485 		.type = IIO_CURRENT,
486 		.props = rescale_current_sense_amplifier_props,
487 	},
488 	[CURRENT_SENSE_SHUNT] = {
489 		.type = IIO_CURRENT,
490 		.props = rescale_current_sense_shunt_props,
491 	},
492 	[VOLTAGE_DIVIDER] = {
493 		.type = IIO_VOLTAGE,
494 		.props = rescale_voltage_divider_props,
495 	},
496 	[TEMP_SENSE_RTD] = {
497 		.type = IIO_TEMP,
498 		.props = rescale_temp_sense_rtd_props,
499 	},
500 	[TEMP_TRANSDUCER] = {
501 		.type = IIO_TEMP,
502 		.props = rescale_temp_transducer_props,
503 	},
504 };
505 
506 static const struct of_device_id rescale_match[] = {
507 	{ .compatible = "current-sense-amplifier",
508 	  .data = &rescale_cfg[CURRENT_SENSE_AMPLIFIER], },
509 	{ .compatible = "current-sense-shunt",
510 	  .data = &rescale_cfg[CURRENT_SENSE_SHUNT], },
511 	{ .compatible = "voltage-divider",
512 	  .data = &rescale_cfg[VOLTAGE_DIVIDER], },
513 	{ .compatible = "temperature-sense-rtd",
514 	  .data = &rescale_cfg[TEMP_SENSE_RTD], },
515 	{ .compatible = "temperature-transducer",
516 	  .data = &rescale_cfg[TEMP_TRANSDUCER], },
517 	{ /* sentinel */ }
518 };
519 MODULE_DEVICE_TABLE(of, rescale_match);
520 
rescale_probe(struct platform_device * pdev)521 static int rescale_probe(struct platform_device *pdev)
522 {
523 	struct device *dev = &pdev->dev;
524 	struct iio_dev *indio_dev;
525 	struct iio_channel *source;
526 	struct rescale *rescale;
527 	int sizeof_ext_info;
528 	int sizeof_priv;
529 	int i;
530 	int ret;
531 
532 	source = devm_iio_channel_get(dev, NULL);
533 	if (IS_ERR(source))
534 		return dev_err_probe(dev, PTR_ERR(source),
535 				     "failed to get source channel\n");
536 
537 	sizeof_ext_info = iio_get_channel_ext_info_count(source);
538 	if (sizeof_ext_info) {
539 		sizeof_ext_info += 1; /* one extra entry for the sentinel */
540 		sizeof_ext_info *= sizeof(*rescale->ext_info);
541 	}
542 
543 	sizeof_priv = sizeof(*rescale) + sizeof_ext_info;
544 
545 	indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
546 	if (!indio_dev)
547 		return -ENOMEM;
548 
549 	rescale = iio_priv(indio_dev);
550 
551 	rescale->cfg = device_get_match_data(dev);
552 	rescale->numerator = 1;
553 	rescale->denominator = 1;
554 	rescale->offset = 0;
555 
556 	ret = rescale->cfg->props(dev, rescale);
557 	if (ret)
558 		return ret;
559 
560 	if (!rescale->numerator || !rescale->denominator) {
561 		dev_err(dev, "invalid scaling factor.\n");
562 		return -EINVAL;
563 	}
564 
565 	platform_set_drvdata(pdev, indio_dev);
566 
567 	rescale->source = source;
568 
569 	indio_dev->name = dev_name(dev);
570 	indio_dev->info = &rescale_info;
571 	indio_dev->modes = INDIO_DIRECT_MODE;
572 	indio_dev->channels = &rescale->chan;
573 	indio_dev->num_channels = 1;
574 	if (sizeof_ext_info) {
575 		rescale->ext_info = devm_kmemdup(dev,
576 						 source->channel->ext_info,
577 						 sizeof_ext_info, GFP_KERNEL);
578 		if (!rescale->ext_info)
579 			return -ENOMEM;
580 
581 		for (i = 0; rescale->ext_info[i].name; ++i) {
582 			struct iio_chan_spec_ext_info *ext_info =
583 				&rescale->ext_info[i];
584 
585 			if (source->channel->ext_info[i].read)
586 				ext_info->read = rescale_read_ext_info;
587 			if (source->channel->ext_info[i].write)
588 				ext_info->write = rescale_write_ext_info;
589 			ext_info->private = i;
590 		}
591 	}
592 
593 	ret = rescale_configure_channel(dev, rescale);
594 	if (ret)
595 		return ret;
596 
597 	return devm_iio_device_register(dev, indio_dev);
598 }
599 
600 static struct platform_driver rescale_driver = {
601 	.probe = rescale_probe,
602 	.driver = {
603 		.name = "iio-rescale",
604 		.of_match_table = rescale_match,
605 	},
606 };
607 module_platform_driver(rescale_driver);
608 
609 MODULE_DESCRIPTION("IIO rescale driver");
610 MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
611 MODULE_LICENSE("GPL v2");
612