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