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/of.h>
15 #include <linux/of_device.h>
16 #include <linux/platform_device.h>
17 #include <linux/property.h>
18
19 #include <linux/iio/afe/rescale.h>
20 #include <linux/iio/consumer.h>
21 #include <linux/iio/iio.h>
22
rescale_process_scale(struct rescale * rescale,int scale_type,int * val,int * val2)23 int rescale_process_scale(struct rescale *rescale, int scale_type,
24 int *val, int *val2)
25 {
26 s64 tmp;
27
28 switch (scale_type) {
29 case IIO_VAL_FRACTIONAL:
30 *val *= rescale->numerator;
31 *val2 *= rescale->denominator;
32 return scale_type;
33 case IIO_VAL_INT:
34 *val *= rescale->numerator;
35 if (rescale->denominator == 1)
36 return scale_type;
37 *val2 = rescale->denominator;
38 return IIO_VAL_FRACTIONAL;
39 case IIO_VAL_FRACTIONAL_LOG2:
40 tmp = (s64)*val * 1000000000LL;
41 tmp = div_s64(tmp, rescale->denominator);
42 tmp *= rescale->numerator;
43 tmp = div_s64(tmp, 1000000000LL);
44 *val = tmp;
45 return scale_type;
46 default:
47 return -EOPNOTSUPP;
48 }
49 }
50
rescale_process_offset(struct rescale * rescale,int scale_type,int scale,int scale2,int schan_off,int * val,int * val2)51 int rescale_process_offset(struct rescale *rescale, int scale_type,
52 int scale, int scale2, int schan_off,
53 int *val, int *val2)
54 {
55 s64 tmp, tmp2;
56
57 switch (scale_type) {
58 case IIO_VAL_FRACTIONAL:
59 tmp = (s64)rescale->offset * scale2;
60 *val = div_s64(tmp, scale) + schan_off;
61 return IIO_VAL_INT;
62 case IIO_VAL_INT:
63 *val = div_s64(rescale->offset, scale) + schan_off;
64 return IIO_VAL_INT;
65 case IIO_VAL_FRACTIONAL_LOG2:
66 tmp = (s64)rescale->offset * (1 << scale2);
67 *val = div_s64(tmp, scale) + schan_off;
68 return IIO_VAL_INT;
69 case IIO_VAL_INT_PLUS_NANO:
70 tmp = (s64)rescale->offset * 1000000000LL;
71 tmp2 = ((s64)scale * 1000000000LL) + scale2;
72 *val = div64_s64(tmp, tmp2) + schan_off;
73 return IIO_VAL_INT;
74 case IIO_VAL_INT_PLUS_MICRO:
75 tmp = (s64)rescale->offset * 1000000LL;
76 tmp2 = ((s64)scale * 1000000LL) + scale2;
77 *val = div64_s64(tmp, tmp2) + schan_off;
78 return IIO_VAL_INT;
79 default:
80 return -EOPNOTSUPP;
81 }
82 }
83
rescale_read_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int * val,int * val2,long mask)84 static int rescale_read_raw(struct iio_dev *indio_dev,
85 struct iio_chan_spec const *chan,
86 int *val, int *val2, long mask)
87 {
88 struct rescale *rescale = iio_priv(indio_dev);
89 int scale, scale2;
90 int schan_off = 0;
91 int ret;
92
93 switch (mask) {
94 case IIO_CHAN_INFO_RAW:
95 if (rescale->chan_processed)
96 /*
97 * When only processed channels are supported, we
98 * read the processed data and scale it by 1/1
99 * augmented with whatever the rescaler has calculated.
100 */
101 return iio_read_channel_processed(rescale->source, val);
102 else
103 return iio_read_channel_raw(rescale->source, val);
104
105 case IIO_CHAN_INFO_SCALE:
106 if (rescale->chan_processed) {
107 /*
108 * Processed channels are scaled 1-to-1
109 */
110 *val = 1;
111 *val2 = 1;
112 ret = IIO_VAL_FRACTIONAL;
113 } else {
114 ret = iio_read_channel_scale(rescale->source, val, val2);
115 }
116 return rescale_process_scale(rescale, ret, val, val2);
117 case IIO_CHAN_INFO_OFFSET:
118 /*
119 * Processed channels are scaled 1-to-1 and source offset is
120 * already taken into account.
121 *
122 * In other cases, real world measurement are expressed as:
123 *
124 * schan_scale * (raw + schan_offset)
125 *
126 * Given that the rescaler parameters are applied recursively:
127 *
128 * rescaler_scale * (schan_scale * (raw + schan_offset) +
129 * rescaler_offset)
130 *
131 * Or,
132 *
133 * (rescaler_scale * schan_scale) * (raw +
134 * (schan_offset + rescaler_offset / schan_scale)
135 *
136 * Thus, reusing the original expression the parameters exposed
137 * to userspace are:
138 *
139 * scale = schan_scale * rescaler_scale
140 * offset = schan_offset + rescaler_offset / schan_scale
141 */
142 if (rescale->chan_processed) {
143 *val = rescale->offset;
144 return IIO_VAL_INT;
145 }
146
147 if (iio_channel_has_info(rescale->source->channel,
148 IIO_CHAN_INFO_OFFSET)) {
149 ret = iio_read_channel_offset(rescale->source,
150 &schan_off, NULL);
151 if (ret != IIO_VAL_INT)
152 return ret < 0 ? ret : -EOPNOTSUPP;
153 }
154
155 if (iio_channel_has_info(rescale->source->channel,
156 IIO_CHAN_INFO_SCALE)) {
157 ret = iio_read_channel_scale(rescale->source, &scale, &scale2);
158 return rescale_process_offset(rescale, ret, scale, scale2,
159 schan_off, val, val2);
160 }
161
162 /*
163 * If we get here we have no scale so scale 1:1 but apply
164 * rescaler and offset, if any.
165 */
166 return rescale_process_offset(rescale, IIO_VAL_FRACTIONAL, 1, 1,
167 schan_off, val, val2);
168 default:
169 return -EINVAL;
170 }
171 }
172
rescale_read_avail(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,const int ** vals,int * type,int * length,long mask)173 static int rescale_read_avail(struct iio_dev *indio_dev,
174 struct iio_chan_spec const *chan,
175 const int **vals, int *type, int *length,
176 long mask)
177 {
178 struct rescale *rescale = iio_priv(indio_dev);
179
180 switch (mask) {
181 case IIO_CHAN_INFO_RAW:
182 *type = IIO_VAL_INT;
183 return iio_read_avail_channel_raw(rescale->source,
184 vals, length);
185 default:
186 return -EINVAL;
187 }
188 }
189
190 static const struct iio_info rescale_info = {
191 .read_raw = rescale_read_raw,
192 .read_avail = rescale_read_avail,
193 };
194
rescale_read_ext_info(struct iio_dev * indio_dev,uintptr_t private,struct iio_chan_spec const * chan,char * buf)195 static ssize_t rescale_read_ext_info(struct iio_dev *indio_dev,
196 uintptr_t private,
197 struct iio_chan_spec const *chan,
198 char *buf)
199 {
200 struct rescale *rescale = iio_priv(indio_dev);
201
202 return iio_read_channel_ext_info(rescale->source,
203 rescale->ext_info[private].name,
204 buf);
205 }
206
rescale_write_ext_info(struct iio_dev * indio_dev,uintptr_t private,struct iio_chan_spec const * chan,const char * buf,size_t len)207 static ssize_t rescale_write_ext_info(struct iio_dev *indio_dev,
208 uintptr_t private,
209 struct iio_chan_spec const *chan,
210 const char *buf, size_t len)
211 {
212 struct rescale *rescale = iio_priv(indio_dev);
213
214 return iio_write_channel_ext_info(rescale->source,
215 rescale->ext_info[private].name,
216 buf, len);
217 }
218
rescale_configure_channel(struct device * dev,struct rescale * rescale)219 static int rescale_configure_channel(struct device *dev,
220 struct rescale *rescale)
221 {
222 struct iio_chan_spec *chan = &rescale->chan;
223 struct iio_chan_spec const *schan = rescale->source->channel;
224
225 chan->indexed = 1;
226 chan->output = schan->output;
227 chan->ext_info = rescale->ext_info;
228 chan->type = rescale->cfg->type;
229
230 if (iio_channel_has_info(schan, IIO_CHAN_INFO_RAW) &&
231 (iio_channel_has_info(schan, IIO_CHAN_INFO_SCALE) ||
232 iio_channel_has_info(schan, IIO_CHAN_INFO_OFFSET))) {
233 dev_info(dev, "using raw+scale/offset source channel\n");
234 } else if (iio_channel_has_info(schan, IIO_CHAN_INFO_PROCESSED)) {
235 dev_info(dev, "using processed channel\n");
236 rescale->chan_processed = true;
237 } else {
238 dev_err(dev, "source channel is not supported\n");
239 return -EINVAL;
240 }
241
242 chan->info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
243 BIT(IIO_CHAN_INFO_SCALE);
244
245 if (rescale->offset)
246 chan->info_mask_separate |= BIT(IIO_CHAN_INFO_OFFSET);
247
248 /*
249 * Using .read_avail() is fringe to begin with and makes no sense
250 * whatsoever for processed channels, so we make sure that this cannot
251 * be called on a processed channel.
252 */
253 if (iio_channel_has_available(schan, IIO_CHAN_INFO_RAW) &&
254 !rescale->chan_processed)
255 chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW);
256
257 return 0;
258 }
259
rescale_current_sense_amplifier_props(struct device * dev,struct rescale * rescale)260 static int rescale_current_sense_amplifier_props(struct device *dev,
261 struct rescale *rescale)
262 {
263 u32 sense;
264 u32 gain_mult = 1;
265 u32 gain_div = 1;
266 u32 factor;
267 int ret;
268
269 ret = device_property_read_u32(dev, "sense-resistor-micro-ohms",
270 &sense);
271 if (ret) {
272 dev_err(dev, "failed to read the sense resistance: %d\n", ret);
273 return ret;
274 }
275
276 device_property_read_u32(dev, "sense-gain-mult", &gain_mult);
277 device_property_read_u32(dev, "sense-gain-div", &gain_div);
278
279 /*
280 * Calculate the scaling factor, 1 / (gain * sense), or
281 * gain_div / (gain_mult * sense), while trying to keep the
282 * numerator/denominator from overflowing.
283 */
284 factor = gcd(sense, 1000000);
285 rescale->numerator = 1000000 / factor;
286 rescale->denominator = sense / factor;
287
288 factor = gcd(rescale->numerator, gain_mult);
289 rescale->numerator /= factor;
290 rescale->denominator *= gain_mult / factor;
291
292 factor = gcd(rescale->denominator, gain_div);
293 rescale->numerator *= gain_div / factor;
294 rescale->denominator /= factor;
295
296 return 0;
297 }
298
rescale_current_sense_shunt_props(struct device * dev,struct rescale * rescale)299 static int rescale_current_sense_shunt_props(struct device *dev,
300 struct rescale *rescale)
301 {
302 u32 shunt;
303 u32 factor;
304 int ret;
305
306 ret = device_property_read_u32(dev, "shunt-resistor-micro-ohms",
307 &shunt);
308 if (ret) {
309 dev_err(dev, "failed to read the shunt resistance: %d\n", ret);
310 return ret;
311 }
312
313 factor = gcd(shunt, 1000000);
314 rescale->numerator = 1000000 / factor;
315 rescale->denominator = shunt / factor;
316
317 return 0;
318 }
319
rescale_voltage_divider_props(struct device * dev,struct rescale * rescale)320 static int rescale_voltage_divider_props(struct device *dev,
321 struct rescale *rescale)
322 {
323 int ret;
324 u32 factor;
325
326 ret = device_property_read_u32(dev, "output-ohms",
327 &rescale->denominator);
328 if (ret) {
329 dev_err(dev, "failed to read output-ohms: %d\n", ret);
330 return ret;
331 }
332
333 ret = device_property_read_u32(dev, "full-ohms",
334 &rescale->numerator);
335 if (ret) {
336 dev_err(dev, "failed to read full-ohms: %d\n", ret);
337 return ret;
338 }
339
340 factor = gcd(rescale->numerator, rescale->denominator);
341 rescale->numerator /= factor;
342 rescale->denominator /= factor;
343
344 return 0;
345 }
346
347 enum rescale_variant {
348 CURRENT_SENSE_AMPLIFIER,
349 CURRENT_SENSE_SHUNT,
350 VOLTAGE_DIVIDER,
351 };
352
353 static const struct rescale_cfg rescale_cfg[] = {
354 [CURRENT_SENSE_AMPLIFIER] = {
355 .type = IIO_CURRENT,
356 .props = rescale_current_sense_amplifier_props,
357 },
358 [CURRENT_SENSE_SHUNT] = {
359 .type = IIO_CURRENT,
360 .props = rescale_current_sense_shunt_props,
361 },
362 [VOLTAGE_DIVIDER] = {
363 .type = IIO_VOLTAGE,
364 .props = rescale_voltage_divider_props,
365 },
366 };
367
368 static const struct of_device_id rescale_match[] = {
369 { .compatible = "current-sense-amplifier",
370 .data = &rescale_cfg[CURRENT_SENSE_AMPLIFIER], },
371 { .compatible = "current-sense-shunt",
372 .data = &rescale_cfg[CURRENT_SENSE_SHUNT], },
373 { .compatible = "voltage-divider",
374 .data = &rescale_cfg[VOLTAGE_DIVIDER], },
375 { /* sentinel */ }
376 };
377 MODULE_DEVICE_TABLE(of, rescale_match);
378
rescale_probe(struct platform_device * pdev)379 static int rescale_probe(struct platform_device *pdev)
380 {
381 struct device *dev = &pdev->dev;
382 struct iio_dev *indio_dev;
383 struct iio_channel *source;
384 struct rescale *rescale;
385 int sizeof_ext_info;
386 int sizeof_priv;
387 int i;
388 int ret;
389
390 source = devm_iio_channel_get(dev, NULL);
391 if (IS_ERR(source))
392 return dev_err_probe(dev, PTR_ERR(source),
393 "failed to get source channel\n");
394
395 sizeof_ext_info = iio_get_channel_ext_info_count(source);
396 if (sizeof_ext_info) {
397 sizeof_ext_info += 1; /* one extra entry for the sentinel */
398 sizeof_ext_info *= sizeof(*rescale->ext_info);
399 }
400
401 sizeof_priv = sizeof(*rescale) + sizeof_ext_info;
402
403 indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
404 if (!indio_dev)
405 return -ENOMEM;
406
407 rescale = iio_priv(indio_dev);
408
409 rescale->cfg = of_device_get_match_data(dev);
410 rescale->numerator = 1;
411 rescale->denominator = 1;
412 rescale->offset = 0;
413
414 ret = rescale->cfg->props(dev, rescale);
415 if (ret)
416 return ret;
417
418 if (!rescale->numerator || !rescale->denominator) {
419 dev_err(dev, "invalid scaling factor.\n");
420 return -EINVAL;
421 }
422
423 platform_set_drvdata(pdev, indio_dev);
424
425 rescale->source = source;
426
427 indio_dev->name = dev_name(dev);
428 indio_dev->info = &rescale_info;
429 indio_dev->modes = INDIO_DIRECT_MODE;
430 indio_dev->channels = &rescale->chan;
431 indio_dev->num_channels = 1;
432 if (sizeof_ext_info) {
433 rescale->ext_info = devm_kmemdup(dev,
434 source->channel->ext_info,
435 sizeof_ext_info, GFP_KERNEL);
436 if (!rescale->ext_info)
437 return -ENOMEM;
438
439 for (i = 0; rescale->ext_info[i].name; ++i) {
440 struct iio_chan_spec_ext_info *ext_info =
441 &rescale->ext_info[i];
442
443 if (source->channel->ext_info[i].read)
444 ext_info->read = rescale_read_ext_info;
445 if (source->channel->ext_info[i].write)
446 ext_info->write = rescale_write_ext_info;
447 ext_info->private = i;
448 }
449 }
450
451 ret = rescale_configure_channel(dev, rescale);
452 if (ret)
453 return ret;
454
455 return devm_iio_device_register(dev, indio_dev);
456 }
457
458 static struct platform_driver rescale_driver = {
459 .probe = rescale_probe,
460 .driver = {
461 .name = "iio-rescale",
462 .of_match_table = rescale_match,
463 },
464 };
465 module_platform_driver(rescale_driver);
466
467 MODULE_DESCRIPTION("IIO rescale driver");
468 MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
469 MODULE_LICENSE("GPL v2");
470