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 ret = iio_read_channel_scale(rescale->source, &scale, &scale2);
218 return rescale_process_offset(rescale, ret, scale, scale2,
219 schan_off, val, val2);
220 default:
221 return -EINVAL;
222 }
223 }
224
rescale_read_avail(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,const int ** vals,int * type,int * length,long mask)225 static int rescale_read_avail(struct iio_dev *indio_dev,
226 struct iio_chan_spec const *chan,
227 const int **vals, int *type, int *length,
228 long mask)
229 {
230 struct rescale *rescale = iio_priv(indio_dev);
231
232 switch (mask) {
233 case IIO_CHAN_INFO_RAW:
234 *type = IIO_VAL_INT;
235 return iio_read_avail_channel_raw(rescale->source,
236 vals, length);
237 default:
238 return -EINVAL;
239 }
240 }
241
242 static const struct iio_info rescale_info = {
243 .read_raw = rescale_read_raw,
244 .read_avail = rescale_read_avail,
245 };
246
rescale_read_ext_info(struct iio_dev * indio_dev,uintptr_t private,struct iio_chan_spec const * chan,char * buf)247 static ssize_t rescale_read_ext_info(struct iio_dev *indio_dev,
248 uintptr_t private,
249 struct iio_chan_spec const *chan,
250 char *buf)
251 {
252 struct rescale *rescale = iio_priv(indio_dev);
253
254 return iio_read_channel_ext_info(rescale->source,
255 rescale->ext_info[private].name,
256 buf);
257 }
258
rescale_write_ext_info(struct iio_dev * indio_dev,uintptr_t private,struct iio_chan_spec const * chan,const char * buf,size_t len)259 static ssize_t rescale_write_ext_info(struct iio_dev *indio_dev,
260 uintptr_t private,
261 struct iio_chan_spec const *chan,
262 const char *buf, size_t len)
263 {
264 struct rescale *rescale = iio_priv(indio_dev);
265
266 return iio_write_channel_ext_info(rescale->source,
267 rescale->ext_info[private].name,
268 buf, len);
269 }
270
rescale_configure_channel(struct device * dev,struct rescale * rescale)271 static int rescale_configure_channel(struct device *dev,
272 struct rescale *rescale)
273 {
274 struct iio_chan_spec *chan = &rescale->chan;
275 struct iio_chan_spec const *schan = rescale->source->channel;
276
277 chan->indexed = 1;
278 chan->output = schan->output;
279 chan->ext_info = rescale->ext_info;
280 chan->type = rescale->cfg->type;
281
282 if (iio_channel_has_info(schan, IIO_CHAN_INFO_RAW) &&
283 iio_channel_has_info(schan, IIO_CHAN_INFO_SCALE)) {
284 dev_info(dev, "using raw+scale source channel\n");
285 } else if (iio_channel_has_info(schan, IIO_CHAN_INFO_PROCESSED)) {
286 dev_info(dev, "using processed channel\n");
287 rescale->chan_processed = true;
288 } else {
289 dev_err(dev, "source channel is not supported\n");
290 return -EINVAL;
291 }
292
293 chan->info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
294 BIT(IIO_CHAN_INFO_SCALE);
295
296 if (rescale->offset)
297 chan->info_mask_separate |= BIT(IIO_CHAN_INFO_OFFSET);
298
299 /*
300 * Using .read_avail() is fringe to begin with and makes no sense
301 * whatsoever for processed channels, so we make sure that this cannot
302 * be called on a processed channel.
303 */
304 if (iio_channel_has_available(schan, IIO_CHAN_INFO_RAW) &&
305 !rescale->chan_processed)
306 chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW);
307
308 return 0;
309 }
310
rescale_current_sense_amplifier_props(struct device * dev,struct rescale * rescale)311 static int rescale_current_sense_amplifier_props(struct device *dev,
312 struct rescale *rescale)
313 {
314 u32 sense;
315 u32 gain_mult = 1;
316 u32 gain_div = 1;
317 u32 factor;
318 int ret;
319
320 ret = device_property_read_u32(dev, "sense-resistor-micro-ohms",
321 &sense);
322 if (ret) {
323 dev_err(dev, "failed to read the sense resistance: %d\n", ret);
324 return ret;
325 }
326
327 device_property_read_u32(dev, "sense-gain-mult", &gain_mult);
328 device_property_read_u32(dev, "sense-gain-div", &gain_div);
329
330 /*
331 * Calculate the scaling factor, 1 / (gain * sense), or
332 * gain_div / (gain_mult * sense), while trying to keep the
333 * numerator/denominator from overflowing.
334 */
335 factor = gcd(sense, 1000000);
336 rescale->numerator = 1000000 / factor;
337 rescale->denominator = sense / factor;
338
339 factor = gcd(rescale->numerator, gain_mult);
340 rescale->numerator /= factor;
341 rescale->denominator *= gain_mult / factor;
342
343 factor = gcd(rescale->denominator, gain_div);
344 rescale->numerator *= gain_div / factor;
345 rescale->denominator /= factor;
346
347 return 0;
348 }
349
rescale_current_sense_shunt_props(struct device * dev,struct rescale * rescale)350 static int rescale_current_sense_shunt_props(struct device *dev,
351 struct rescale *rescale)
352 {
353 u32 shunt;
354 u32 factor;
355 int ret;
356
357 ret = device_property_read_u32(dev, "shunt-resistor-micro-ohms",
358 &shunt);
359 if (ret) {
360 dev_err(dev, "failed to read the shunt resistance: %d\n", ret);
361 return ret;
362 }
363
364 factor = gcd(shunt, 1000000);
365 rescale->numerator = 1000000 / factor;
366 rescale->denominator = shunt / factor;
367
368 return 0;
369 }
370
rescale_voltage_divider_props(struct device * dev,struct rescale * rescale)371 static int rescale_voltage_divider_props(struct device *dev,
372 struct rescale *rescale)
373 {
374 int ret;
375 u32 factor;
376
377 ret = device_property_read_u32(dev, "output-ohms",
378 &rescale->denominator);
379 if (ret) {
380 dev_err(dev, "failed to read output-ohms: %d\n", ret);
381 return ret;
382 }
383
384 ret = device_property_read_u32(dev, "full-ohms",
385 &rescale->numerator);
386 if (ret) {
387 dev_err(dev, "failed to read full-ohms: %d\n", ret);
388 return ret;
389 }
390
391 factor = gcd(rescale->numerator, rescale->denominator);
392 rescale->numerator /= factor;
393 rescale->denominator /= factor;
394
395 return 0;
396 }
397
rescale_temp_sense_rtd_props(struct device * dev,struct rescale * rescale)398 static int rescale_temp_sense_rtd_props(struct device *dev,
399 struct rescale *rescale)
400 {
401 u32 factor;
402 u32 alpha;
403 u32 iexc;
404 u32 tmp;
405 int ret;
406 u32 r0;
407
408 ret = device_property_read_u32(dev, "excitation-current-microamp",
409 &iexc);
410 if (ret) {
411 dev_err(dev, "failed to read excitation-current-microamp: %d\n",
412 ret);
413 return ret;
414 }
415
416 ret = device_property_read_u32(dev, "alpha-ppm-per-celsius", &alpha);
417 if (ret) {
418 dev_err(dev, "failed to read alpha-ppm-per-celsius: %d\n",
419 ret);
420 return ret;
421 }
422
423 ret = device_property_read_u32(dev, "r-naught-ohms", &r0);
424 if (ret) {
425 dev_err(dev, "failed to read r-naught-ohms: %d\n", ret);
426 return ret;
427 }
428
429 tmp = r0 * iexc * alpha / 1000000;
430 factor = gcd(tmp, 1000000);
431 rescale->numerator = 1000000 / factor;
432 rescale->denominator = tmp / factor;
433
434 rescale->offset = -1 * ((r0 * iexc) / 1000);
435
436 return 0;
437 }
438
rescale_temp_transducer_props(struct device * dev,struct rescale * rescale)439 static int rescale_temp_transducer_props(struct device *dev,
440 struct rescale *rescale)
441 {
442 s32 offset = 0;
443 s32 sense = 1;
444 s32 alpha;
445 int ret;
446
447 device_property_read_u32(dev, "sense-offset-millicelsius", &offset);
448 device_property_read_u32(dev, "sense-resistor-ohms", &sense);
449 ret = device_property_read_u32(dev, "alpha-ppm-per-celsius", &alpha);
450 if (ret) {
451 dev_err(dev, "failed to read alpha-ppm-per-celsius: %d\n", ret);
452 return ret;
453 }
454
455 rescale->numerator = 1000000;
456 rescale->denominator = alpha * sense;
457
458 rescale->offset = div_s64((s64)offset * rescale->denominator,
459 rescale->numerator);
460
461 return 0;
462 }
463
464 enum rescale_variant {
465 CURRENT_SENSE_AMPLIFIER,
466 CURRENT_SENSE_SHUNT,
467 VOLTAGE_DIVIDER,
468 TEMP_SENSE_RTD,
469 TEMP_TRANSDUCER,
470 };
471
472 static const struct rescale_cfg rescale_cfg[] = {
473 [CURRENT_SENSE_AMPLIFIER] = {
474 .type = IIO_CURRENT,
475 .props = rescale_current_sense_amplifier_props,
476 },
477 [CURRENT_SENSE_SHUNT] = {
478 .type = IIO_CURRENT,
479 .props = rescale_current_sense_shunt_props,
480 },
481 [VOLTAGE_DIVIDER] = {
482 .type = IIO_VOLTAGE,
483 .props = rescale_voltage_divider_props,
484 },
485 [TEMP_SENSE_RTD] = {
486 .type = IIO_TEMP,
487 .props = rescale_temp_sense_rtd_props,
488 },
489 [TEMP_TRANSDUCER] = {
490 .type = IIO_TEMP,
491 .props = rescale_temp_transducer_props,
492 },
493 };
494
495 static const struct of_device_id rescale_match[] = {
496 { .compatible = "current-sense-amplifier",
497 .data = &rescale_cfg[CURRENT_SENSE_AMPLIFIER], },
498 { .compatible = "current-sense-shunt",
499 .data = &rescale_cfg[CURRENT_SENSE_SHUNT], },
500 { .compatible = "voltage-divider",
501 .data = &rescale_cfg[VOLTAGE_DIVIDER], },
502 { .compatible = "temperature-sense-rtd",
503 .data = &rescale_cfg[TEMP_SENSE_RTD], },
504 { .compatible = "temperature-transducer",
505 .data = &rescale_cfg[TEMP_TRANSDUCER], },
506 { /* sentinel */ }
507 };
508 MODULE_DEVICE_TABLE(of, rescale_match);
509
rescale_probe(struct platform_device * pdev)510 static int rescale_probe(struct platform_device *pdev)
511 {
512 struct device *dev = &pdev->dev;
513 struct iio_dev *indio_dev;
514 struct iio_channel *source;
515 struct rescale *rescale;
516 int sizeof_ext_info;
517 int sizeof_priv;
518 int i;
519 int ret;
520
521 source = devm_iio_channel_get(dev, NULL);
522 if (IS_ERR(source))
523 return dev_err_probe(dev, PTR_ERR(source),
524 "failed to get source channel\n");
525
526 sizeof_ext_info = iio_get_channel_ext_info_count(source);
527 if (sizeof_ext_info) {
528 sizeof_ext_info += 1; /* one extra entry for the sentinel */
529 sizeof_ext_info *= sizeof(*rescale->ext_info);
530 }
531
532 sizeof_priv = sizeof(*rescale) + sizeof_ext_info;
533
534 indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
535 if (!indio_dev)
536 return -ENOMEM;
537
538 rescale = iio_priv(indio_dev);
539
540 rescale->cfg = device_get_match_data(dev);
541 rescale->numerator = 1;
542 rescale->denominator = 1;
543 rescale->offset = 0;
544
545 ret = rescale->cfg->props(dev, rescale);
546 if (ret)
547 return ret;
548
549 if (!rescale->numerator || !rescale->denominator) {
550 dev_err(dev, "invalid scaling factor.\n");
551 return -EINVAL;
552 }
553
554 platform_set_drvdata(pdev, indio_dev);
555
556 rescale->source = source;
557
558 indio_dev->name = dev_name(dev);
559 indio_dev->info = &rescale_info;
560 indio_dev->modes = INDIO_DIRECT_MODE;
561 indio_dev->channels = &rescale->chan;
562 indio_dev->num_channels = 1;
563 if (sizeof_ext_info) {
564 rescale->ext_info = devm_kmemdup(dev,
565 source->channel->ext_info,
566 sizeof_ext_info, GFP_KERNEL);
567 if (!rescale->ext_info)
568 return -ENOMEM;
569
570 for (i = 0; rescale->ext_info[i].name; ++i) {
571 struct iio_chan_spec_ext_info *ext_info =
572 &rescale->ext_info[i];
573
574 if (source->channel->ext_info[i].read)
575 ext_info->read = rescale_read_ext_info;
576 if (source->channel->ext_info[i].write)
577 ext_info->write = rescale_write_ext_info;
578 ext_info->private = i;
579 }
580 }
581
582 ret = rescale_configure_channel(dev, rescale);
583 if (ret)
584 return ret;
585
586 return devm_iio_device_register(dev, indio_dev);
587 }
588
589 static struct platform_driver rescale_driver = {
590 .probe = rescale_probe,
591 .driver = {
592 .name = "iio-rescale",
593 .of_match_table = rescale_match,
594 },
595 };
596 module_platform_driver(rescale_driver);
597
598 MODULE_DESCRIPTION("IIO rescale driver");
599 MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
600 MODULE_LICENSE("GPL v2");
601