1
2
3
4
5
6
7
8
9
10#include <linux/err.h>
11#include <linux/gcd.h>
12#include <linux/iio/consumer.h>
13#include <linux/iio/iio.h>
14#include <linux/module.h>
15#include <linux/of.h>
16#include <linux/of_device.h>
17#include <linux/platform_device.h>
18#include <linux/property.h>
19
20struct rescale;
21
22struct rescale_cfg {
23 enum iio_chan_type type;
24 int (*props)(struct device *dev, struct rescale *rescale);
25};
26
27struct rescale {
28 const struct rescale_cfg *cfg;
29 struct iio_channel *source;
30 struct iio_chan_spec chan;
31 struct iio_chan_spec_ext_info *ext_info;
32 bool chan_processed;
33 s32 numerator;
34 s32 denominator;
35};
36
37static int rescale_read_raw(struct iio_dev *indio_dev,
38 struct iio_chan_spec const *chan,
39 int *val, int *val2, long mask)
40{
41 struct rescale *rescale = iio_priv(indio_dev);
42 unsigned long long tmp;
43 int ret;
44
45 switch (mask) {
46 case IIO_CHAN_INFO_RAW:
47 if (rescale->chan_processed)
48
49
50
51
52
53 return iio_read_channel_processed(rescale->source, val);
54 else
55 return iio_read_channel_raw(rescale->source, val);
56
57 case IIO_CHAN_INFO_SCALE:
58 if (rescale->chan_processed) {
59
60
61
62 *val = 1;
63 *val2 = 1;
64 ret = IIO_VAL_FRACTIONAL;
65 } else {
66 ret = iio_read_channel_scale(rescale->source, val, val2);
67 }
68 switch (ret) {
69 case IIO_VAL_FRACTIONAL:
70 *val *= rescale->numerator;
71 *val2 *= rescale->denominator;
72 return ret;
73 case IIO_VAL_INT:
74 *val *= rescale->numerator;
75 if (rescale->denominator == 1)
76 return ret;
77 *val2 = rescale->denominator;
78 return IIO_VAL_FRACTIONAL;
79 case IIO_VAL_FRACTIONAL_LOG2:
80 tmp = *val * 1000000000LL;
81 do_div(tmp, rescale->denominator);
82 tmp *= rescale->numerator;
83 do_div(tmp, 1000000000LL);
84 *val = tmp;
85 return ret;
86 default:
87 return -EOPNOTSUPP;
88 }
89 default:
90 return -EINVAL;
91 }
92}
93
94static int rescale_read_avail(struct iio_dev *indio_dev,
95 struct iio_chan_spec const *chan,
96 const int **vals, int *type, int *length,
97 long mask)
98{
99 struct rescale *rescale = iio_priv(indio_dev);
100
101 switch (mask) {
102 case IIO_CHAN_INFO_RAW:
103 *type = IIO_VAL_INT;
104 return iio_read_avail_channel_raw(rescale->source,
105 vals, length);
106 default:
107 return -EINVAL;
108 }
109}
110
111static const struct iio_info rescale_info = {
112 .read_raw = rescale_read_raw,
113 .read_avail = rescale_read_avail,
114};
115
116static ssize_t rescale_read_ext_info(struct iio_dev *indio_dev,
117 uintptr_t private,
118 struct iio_chan_spec const *chan,
119 char *buf)
120{
121 struct rescale *rescale = iio_priv(indio_dev);
122
123 return iio_read_channel_ext_info(rescale->source,
124 rescale->ext_info[private].name,
125 buf);
126}
127
128static ssize_t rescale_write_ext_info(struct iio_dev *indio_dev,
129 uintptr_t private,
130 struct iio_chan_spec const *chan,
131 const char *buf, size_t len)
132{
133 struct rescale *rescale = iio_priv(indio_dev);
134
135 return iio_write_channel_ext_info(rescale->source,
136 rescale->ext_info[private].name,
137 buf, len);
138}
139
140static int rescale_configure_channel(struct device *dev,
141 struct rescale *rescale)
142{
143 struct iio_chan_spec *chan = &rescale->chan;
144 struct iio_chan_spec const *schan = rescale->source->channel;
145
146 chan->indexed = 1;
147 chan->output = schan->output;
148 chan->ext_info = rescale->ext_info;
149 chan->type = rescale->cfg->type;
150
151 if (iio_channel_has_info(schan, IIO_CHAN_INFO_RAW) ||
152 iio_channel_has_info(schan, IIO_CHAN_INFO_SCALE)) {
153 dev_info(dev, "using raw+scale source channel\n");
154 } else if (iio_channel_has_info(schan, IIO_CHAN_INFO_PROCESSED)) {
155 dev_info(dev, "using processed channel\n");
156 rescale->chan_processed = true;
157 } else {
158 dev_err(dev, "source channel is not supported\n");
159 return -EINVAL;
160 }
161
162 chan->info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
163 BIT(IIO_CHAN_INFO_SCALE);
164
165
166
167
168
169
170 if (iio_channel_has_available(schan, IIO_CHAN_INFO_RAW) &&
171 !rescale->chan_processed)
172 chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW);
173
174 return 0;
175}
176
177static int rescale_current_sense_amplifier_props(struct device *dev,
178 struct rescale *rescale)
179{
180 u32 sense;
181 u32 gain_mult = 1;
182 u32 gain_div = 1;
183 u32 factor;
184 int ret;
185
186 ret = device_property_read_u32(dev, "sense-resistor-micro-ohms",
187 &sense);
188 if (ret) {
189 dev_err(dev, "failed to read the sense resistance: %d\n", ret);
190 return ret;
191 }
192
193 device_property_read_u32(dev, "sense-gain-mult", &gain_mult);
194 device_property_read_u32(dev, "sense-gain-div", &gain_div);
195
196
197
198
199
200
201 factor = gcd(sense, 1000000);
202 rescale->numerator = 1000000 / factor;
203 rescale->denominator = sense / factor;
204
205 factor = gcd(rescale->numerator, gain_mult);
206 rescale->numerator /= factor;
207 rescale->denominator *= gain_mult / factor;
208
209 factor = gcd(rescale->denominator, gain_div);
210 rescale->numerator *= gain_div / factor;
211 rescale->denominator /= factor;
212
213 return 0;
214}
215
216static int rescale_current_sense_shunt_props(struct device *dev,
217 struct rescale *rescale)
218{
219 u32 shunt;
220 u32 factor;
221 int ret;
222
223 ret = device_property_read_u32(dev, "shunt-resistor-micro-ohms",
224 &shunt);
225 if (ret) {
226 dev_err(dev, "failed to read the shunt resistance: %d\n", ret);
227 return ret;
228 }
229
230 factor = gcd(shunt, 1000000);
231 rescale->numerator = 1000000 / factor;
232 rescale->denominator = shunt / factor;
233
234 return 0;
235}
236
237static int rescale_voltage_divider_props(struct device *dev,
238 struct rescale *rescale)
239{
240 int ret;
241 u32 factor;
242
243 ret = device_property_read_u32(dev, "output-ohms",
244 &rescale->denominator);
245 if (ret) {
246 dev_err(dev, "failed to read output-ohms: %d\n", ret);
247 return ret;
248 }
249
250 ret = device_property_read_u32(dev, "full-ohms",
251 &rescale->numerator);
252 if (ret) {
253 dev_err(dev, "failed to read full-ohms: %d\n", ret);
254 return ret;
255 }
256
257 factor = gcd(rescale->numerator, rescale->denominator);
258 rescale->numerator /= factor;
259 rescale->denominator /= factor;
260
261 return 0;
262}
263
264enum rescale_variant {
265 CURRENT_SENSE_AMPLIFIER,
266 CURRENT_SENSE_SHUNT,
267 VOLTAGE_DIVIDER,
268};
269
270static const struct rescale_cfg rescale_cfg[] = {
271 [CURRENT_SENSE_AMPLIFIER] = {
272 .type = IIO_CURRENT,
273 .props = rescale_current_sense_amplifier_props,
274 },
275 [CURRENT_SENSE_SHUNT] = {
276 .type = IIO_CURRENT,
277 .props = rescale_current_sense_shunt_props,
278 },
279 [VOLTAGE_DIVIDER] = {
280 .type = IIO_VOLTAGE,
281 .props = rescale_voltage_divider_props,
282 },
283};
284
285static const struct of_device_id rescale_match[] = {
286 { .compatible = "current-sense-amplifier",
287 .data = &rescale_cfg[CURRENT_SENSE_AMPLIFIER], },
288 { .compatible = "current-sense-shunt",
289 .data = &rescale_cfg[CURRENT_SENSE_SHUNT], },
290 { .compatible = "voltage-divider",
291 .data = &rescale_cfg[VOLTAGE_DIVIDER], },
292 { }
293};
294MODULE_DEVICE_TABLE(of, rescale_match);
295
296static int rescale_probe(struct platform_device *pdev)
297{
298 struct device *dev = &pdev->dev;
299 struct iio_dev *indio_dev;
300 struct iio_channel *source;
301 struct rescale *rescale;
302 int sizeof_ext_info;
303 int sizeof_priv;
304 int i;
305 int ret;
306
307 source = devm_iio_channel_get(dev, NULL);
308 if (IS_ERR(source))
309 return dev_err_probe(dev, PTR_ERR(source),
310 "failed to get source channel\n");
311
312 sizeof_ext_info = iio_get_channel_ext_info_count(source);
313 if (sizeof_ext_info) {
314 sizeof_ext_info += 1;
315 sizeof_ext_info *= sizeof(*rescale->ext_info);
316 }
317
318 sizeof_priv = sizeof(*rescale) + sizeof_ext_info;
319
320 indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
321 if (!indio_dev)
322 return -ENOMEM;
323
324 rescale = iio_priv(indio_dev);
325
326 rescale->cfg = of_device_get_match_data(dev);
327 rescale->numerator = 1;
328 rescale->denominator = 1;
329
330 ret = rescale->cfg->props(dev, rescale);
331 if (ret)
332 return ret;
333
334 if (!rescale->numerator || !rescale->denominator) {
335 dev_err(dev, "invalid scaling factor.\n");
336 return -EINVAL;
337 }
338
339 platform_set_drvdata(pdev, indio_dev);
340
341 rescale->source = source;
342
343 indio_dev->name = dev_name(dev);
344 indio_dev->info = &rescale_info;
345 indio_dev->modes = INDIO_DIRECT_MODE;
346 indio_dev->channels = &rescale->chan;
347 indio_dev->num_channels = 1;
348 if (sizeof_ext_info) {
349 rescale->ext_info = devm_kmemdup(dev,
350 source->channel->ext_info,
351 sizeof_ext_info, GFP_KERNEL);
352 if (!rescale->ext_info)
353 return -ENOMEM;
354
355 for (i = 0; rescale->ext_info[i].name; ++i) {
356 struct iio_chan_spec_ext_info *ext_info =
357 &rescale->ext_info[i];
358
359 if (source->channel->ext_info[i].read)
360 ext_info->read = rescale_read_ext_info;
361 if (source->channel->ext_info[i].write)
362 ext_info->write = rescale_write_ext_info;
363 ext_info->private = i;
364 }
365 }
366
367 ret = rescale_configure_channel(dev, rescale);
368 if (ret)
369 return ret;
370
371 return devm_iio_device_register(dev, indio_dev);
372}
373
374static struct platform_driver rescale_driver = {
375 .probe = rescale_probe,
376 .driver = {
377 .name = "iio-rescale",
378 .of_match_table = rescale_match,
379 },
380};
381module_platform_driver(rescale_driver);
382
383MODULE_DESCRIPTION("IIO rescale driver");
384MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
385MODULE_LICENSE("GPL v2");
386
