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