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
22int 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}
110EXPORT_SYMBOL_NS_GPL(rescale_process_scale, IIO_RESCALE);
111
112int 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}
144EXPORT_SYMBOL_NS_GPL(rescale_process_offset, IIO_RESCALE);
145
146static 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
235static 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
252static const struct iio_info rescale_info = {
253	.read_raw = rescale_read_raw,
254	.read_avail = rescale_read_avail,
255};
256
257static 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
269static 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
281static 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
322static 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
361static 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
382static 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
409static 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
450static 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
475enum rescale_variant {
476	CURRENT_SENSE_AMPLIFIER,
477	CURRENT_SENSE_SHUNT,
478	VOLTAGE_DIVIDER,
479	TEMP_SENSE_RTD,
480	TEMP_TRANSDUCER,
481};
482
483static 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
506static 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};
519MODULE_DEVICE_TABLE(of, rescale_match);
520
521static 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
600static struct platform_driver rescale_driver = {
601	.probe = rescale_probe,
602	.driver = {
603		.name = "iio-rescale",
604		.of_match_table = rescale_match,
605	},
606};
607module_platform_driver(rescale_driver);
608
609MODULE_DESCRIPTION("IIO rescale driver");
610MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
611MODULE_LICENSE("GPL v2");
612