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
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	s64 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 = (s64)*val * 1000000000LL;
63			tmp = div_s64(tmp, rescale->denominator);
64			tmp *= rescale->numerator;
65			tmp = div_s64(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	 * 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
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	{ /* sentinel */ }
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		return dev_err_probe(dev, PTR_ERR(source),
281				     "failed to get source channel\n");
282
283	sizeof_ext_info = iio_get_channel_ext_info_count(source);
284	if (sizeof_ext_info) {
285		sizeof_ext_info += 1; /* one extra entry for the sentinel */
286		sizeof_ext_info *= sizeof(*rescale->ext_info);
287	}
288
289	sizeof_priv = sizeof(*rescale) + sizeof_ext_info;
290
291	indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
292	if (!indio_dev)
293		return -ENOMEM;
294
295	rescale = iio_priv(indio_dev);
296
297	rescale->cfg = of_device_get_match_data(dev);
298	rescale->numerator = 1;
299	rescale->denominator = 1;
300
301	ret = rescale->cfg->props(dev, rescale);
302	if (ret)
303		return ret;
304
305	if (!rescale->numerator || !rescale->denominator) {
306		dev_err(dev, "invalid scaling factor.\n");
307		return -EINVAL;
308	}
309
310	platform_set_drvdata(pdev, indio_dev);
311
312	rescale->source = source;
313
314	indio_dev->name = dev_name(dev);
315	indio_dev->info = &rescale_info;
316	indio_dev->modes = INDIO_DIRECT_MODE;
317	indio_dev->channels = &rescale->chan;
318	indio_dev->num_channels = 1;
319	if (sizeof_ext_info) {
320		rescale->ext_info = devm_kmemdup(dev,
321						 source->channel->ext_info,
322						 sizeof_ext_info, GFP_KERNEL);
323		if (!rescale->ext_info)
324			return -ENOMEM;
325
326		for (i = 0; rescale->ext_info[i].name; ++i) {
327			struct iio_chan_spec_ext_info *ext_info =
328				&rescale->ext_info[i];
329
330			if (source->channel->ext_info[i].read)
331				ext_info->read = rescale_read_ext_info;
332			if (source->channel->ext_info[i].write)
333				ext_info->write = rescale_write_ext_info;
334			ext_info->private = i;
335		}
336	}
337
338	ret = rescale_configure_channel(dev, rescale);
339	if (ret)
340		return ret;
341
342	return devm_iio_device_register(dev, indio_dev);
343}
344
345static struct platform_driver rescale_driver = {
346	.probe = rescale_probe,
347	.driver = {
348		.name = "iio-rescale",
349		.of_match_table = rescale_match,
350	},
351};
352module_platform_driver(rescale_driver);
353
354MODULE_DESCRIPTION("IIO rescale driver");
355MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
356MODULE_LICENSE("GPL v2");
357