1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * IIO rescale driver
4 *
5 * Copyright (C) 2018 Axentia Technologies AB
6 *
7 * Author: Peter Rosin <peda@axentia.se>
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
20 struct rescale;
21
22 struct rescale_cfg {
23 enum iio_chan_type type;
24 int (*props)(struct device *dev, struct rescale *rescale);
25 };
26
27 struct 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
rescale_read_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int * val,int * val2,long mask)36 static 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
rescale_read_avail(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,const int ** vals,int * type,int * length,long mask)76 static 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
93 static const struct iio_info rescale_info = {
94 .read_raw = rescale_read_raw,
95 .read_avail = rescale_read_avail,
96 };
97
rescale_read_ext_info(struct iio_dev * indio_dev,uintptr_t private,struct iio_chan_spec const * chan,char * buf)98 static 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
rescale_write_ext_info(struct iio_dev * indio_dev,uintptr_t private,struct iio_chan_spec const * chan,const char * buf,size_t len)110 static 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
rescale_configure_channel(struct device * dev,struct rescale * rescale)122 static 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
rescale_current_sense_amplifier_props(struct device * dev,struct rescale * rescale)148 static 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 * Calculate the scaling factor, 1 / (gain * sense), or
169 * gain_div / (gain_mult * sense), while trying to keep the
170 * numerator/denominator from overflowing.
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
rescale_current_sense_shunt_props(struct device * dev,struct rescale * rescale)187 static 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
rescale_voltage_divider_props(struct device * dev,struct rescale * rescale)208 static 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
235 enum rescale_variant {
236 CURRENT_SENSE_AMPLIFIER,
237 CURRENT_SENSE_SHUNT,
238 VOLTAGE_DIVIDER,
239 };
240
241 static 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
256 static 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 { /* sentinel */ }
264 };
265 MODULE_DEVICE_TABLE(of, rescale_match);
266
rescale_probe(struct platform_device * pdev)267 static 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; /* one extra entry for the sentinel */
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
348 static struct platform_driver rescale_driver = {
349 .probe = rescale_probe,
350 .driver = {
351 .name = "iio-rescale",
352 .of_match_table = rescale_match,
353 },
354 };
355 module_platform_driver(rescale_driver);
356
357 MODULE_DESCRIPTION("IIO rescale driver");
358 MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
359 MODULE_LICENSE("GPL v2");
360
