1// SPDX-License-Identifier: GPL-2.0
2/*
3 * IIO multiplexer driver
4 *
5 * Copyright (C) 2017 Axentia Technologies AB
6 *
7 * Author: Peter Rosin <peda@axentia.se>
8 */
9
10#include <linux/err.h>
11#include <linux/iio/consumer.h>
12#include <linux/iio/iio.h>
13#include <linux/module.h>
14#include <linux/mutex.h>
15#include <linux/mux/consumer.h>
16#include <linux/of.h>
17#include <linux/platform_device.h>
18
19struct mux_ext_info_cache {
20	char *data;
21	ssize_t size;
22};
23
24struct mux_child {
25	struct mux_ext_info_cache *ext_info_cache;
26};
27
28struct mux {
29	int cached_state;
30	struct mux_control *control;
31	struct iio_channel *parent;
32	struct iio_dev *indio_dev;
33	struct iio_chan_spec *chan;
34	struct iio_chan_spec_ext_info *ext_info;
35	struct mux_child *child;
36};
37
38static int iio_mux_select(struct mux *mux, int idx)
39{
40	struct mux_child *child = &mux->child[idx];
41	struct iio_chan_spec const *chan = &mux->chan[idx];
42	int ret;
43	int i;
44
45	ret = mux_control_select(mux->control, chan->channel);
46	if (ret < 0) {
47		mux->cached_state = -1;
48		return ret;
49	}
50
51	if (mux->cached_state == chan->channel)
52		return 0;
53
54	if (chan->ext_info) {
55		for (i = 0; chan->ext_info[i].name; ++i) {
56			const char *attr = chan->ext_info[i].name;
57			struct mux_ext_info_cache *cache;
58
59			cache = &child->ext_info_cache[i];
60
61			if (cache->size < 0)
62				continue;
63
64			ret = iio_write_channel_ext_info(mux->parent, attr,
65							 cache->data,
66							 cache->size);
67
68			if (ret < 0) {
69				mux_control_deselect(mux->control);
70				mux->cached_state = -1;
71				return ret;
72			}
73		}
74	}
75	mux->cached_state = chan->channel;
76
77	return 0;
78}
79
80static void iio_mux_deselect(struct mux *mux)
81{
82	mux_control_deselect(mux->control);
83}
84
85static int mux_read_raw(struct iio_dev *indio_dev,
86			struct iio_chan_spec const *chan,
87			int *val, int *val2, long mask)
88{
89	struct mux *mux = iio_priv(indio_dev);
90	int idx = chan - mux->chan;
91	int ret;
92
93	ret = iio_mux_select(mux, idx);
94	if (ret < 0)
95		return ret;
96
97	switch (mask) {
98	case IIO_CHAN_INFO_RAW:
99		ret = iio_read_channel_raw(mux->parent, val);
100		break;
101
102	case IIO_CHAN_INFO_SCALE:
103		ret = iio_read_channel_scale(mux->parent, val, val2);
104		break;
105
106	default:
107		ret = -EINVAL;
108	}
109
110	iio_mux_deselect(mux);
111
112	return ret;
113}
114
115static int mux_read_avail(struct iio_dev *indio_dev,
116			  struct iio_chan_spec const *chan,
117			  const int **vals, int *type, int *length,
118			  long mask)
119{
120	struct mux *mux = iio_priv(indio_dev);
121	int idx = chan - mux->chan;
122	int ret;
123
124	ret = iio_mux_select(mux, idx);
125	if (ret < 0)
126		return ret;
127
128	switch (mask) {
129	case IIO_CHAN_INFO_RAW:
130		*type = IIO_VAL_INT;
131		ret = iio_read_avail_channel_raw(mux->parent, vals, length);
132		break;
133
134	default:
135		ret = -EINVAL;
136	}
137
138	iio_mux_deselect(mux);
139
140	return ret;
141}
142
143static int mux_write_raw(struct iio_dev *indio_dev,
144			 struct iio_chan_spec const *chan,
145			 int val, int val2, long mask)
146{
147	struct mux *mux = iio_priv(indio_dev);
148	int idx = chan - mux->chan;
149	int ret;
150
151	ret = iio_mux_select(mux, idx);
152	if (ret < 0)
153		return ret;
154
155	switch (mask) {
156	case IIO_CHAN_INFO_RAW:
157		ret = iio_write_channel_raw(mux->parent, val);
158		break;
159
160	default:
161		ret = -EINVAL;
162	}
163
164	iio_mux_deselect(mux);
165
166	return ret;
167}
168
169static const struct iio_info mux_info = {
170	.read_raw = mux_read_raw,
171	.read_avail = mux_read_avail,
172	.write_raw = mux_write_raw,
173};
174
175static ssize_t mux_read_ext_info(struct iio_dev *indio_dev, uintptr_t private,
176				 struct iio_chan_spec const *chan, char *buf)
177{
178	struct mux *mux = iio_priv(indio_dev);
179	int idx = chan - mux->chan;
180	ssize_t ret;
181
182	ret = iio_mux_select(mux, idx);
183	if (ret < 0)
184		return ret;
185
186	ret = iio_read_channel_ext_info(mux->parent,
187					mux->ext_info[private].name,
188					buf);
189
190	iio_mux_deselect(mux);
191
192	return ret;
193}
194
195static ssize_t mux_write_ext_info(struct iio_dev *indio_dev, uintptr_t private,
196				  struct iio_chan_spec const *chan,
197				  const char *buf, size_t len)
198{
199	struct device *dev = indio_dev->dev.parent;
200	struct mux *mux = iio_priv(indio_dev);
201	int idx = chan - mux->chan;
202	char *new;
203	ssize_t ret;
204
205	if (len >= PAGE_SIZE)
206		return -EINVAL;
207
208	ret = iio_mux_select(mux, idx);
209	if (ret < 0)
210		return ret;
211
212	new = devm_kmemdup(dev, buf, len + 1, GFP_KERNEL);
213	if (!new) {
214		iio_mux_deselect(mux);
215		return -ENOMEM;
216	}
217
218	new[len] = 0;
219
220	ret = iio_write_channel_ext_info(mux->parent,
221					 mux->ext_info[private].name,
222					 buf, len);
223	if (ret < 0) {
224		iio_mux_deselect(mux);
225		devm_kfree(dev, new);
226		return ret;
227	}
228
229	devm_kfree(dev, mux->child[idx].ext_info_cache[private].data);
230	mux->child[idx].ext_info_cache[private].data = new;
231	mux->child[idx].ext_info_cache[private].size = len;
232
233	iio_mux_deselect(mux);
234
235	return ret;
236}
237
238static int mux_configure_channel(struct device *dev, struct mux *mux,
239				 u32 state, const char *label, int idx)
240{
241	struct mux_child *child = &mux->child[idx];
242	struct iio_chan_spec *chan = &mux->chan[idx];
243	struct iio_chan_spec const *pchan = mux->parent->channel;
244	char *page = NULL;
245	int num_ext_info;
246	int i;
247	int ret;
248
249	chan->indexed = 1;
250	chan->output = pchan->output;
251	chan->datasheet_name = label;
252	chan->ext_info = mux->ext_info;
253
254	ret = iio_get_channel_type(mux->parent, &chan->type);
255	if (ret < 0) {
256		dev_err(dev, "failed to get parent channel type\n");
257		return ret;
258	}
259
260	if (iio_channel_has_info(pchan, IIO_CHAN_INFO_RAW))
261		chan->info_mask_separate |= BIT(IIO_CHAN_INFO_RAW);
262	if (iio_channel_has_info(pchan, IIO_CHAN_INFO_SCALE))
263		chan->info_mask_separate |= BIT(IIO_CHAN_INFO_SCALE);
264
265	if (iio_channel_has_available(pchan, IIO_CHAN_INFO_RAW))
266		chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW);
267
268	if (state >= mux_control_states(mux->control)) {
269		dev_err(dev, "too many channels\n");
270		return -EINVAL;
271	}
272
273	chan->channel = state;
274
275	num_ext_info = iio_get_channel_ext_info_count(mux->parent);
276	if (num_ext_info) {
277		page = devm_kzalloc(dev, PAGE_SIZE, GFP_KERNEL);
278		if (!page)
279			return -ENOMEM;
280	}
281	child->ext_info_cache = devm_kcalloc(dev,
282					     num_ext_info,
283					     sizeof(*child->ext_info_cache),
284					     GFP_KERNEL);
285	if (!child->ext_info_cache)
286		return -ENOMEM;
287
288	for (i = 0; i < num_ext_info; ++i) {
289		child->ext_info_cache[i].size = -1;
290
291		if (!pchan->ext_info[i].write)
292			continue;
293		if (!pchan->ext_info[i].read)
294			continue;
295
296		ret = iio_read_channel_ext_info(mux->parent,
297						mux->ext_info[i].name,
298						page);
299		if (ret < 0) {
300			dev_err(dev, "failed to get ext_info '%s'\n",
301				pchan->ext_info[i].name);
302			return ret;
303		}
304		if (ret >= PAGE_SIZE) {
305			dev_err(dev, "too large ext_info '%s'\n",
306				pchan->ext_info[i].name);
307			return -EINVAL;
308		}
309
310		child->ext_info_cache[i].data = devm_kmemdup(dev, page, ret + 1,
311							     GFP_KERNEL);
312		if (!child->ext_info_cache[i].data)
313			return -ENOMEM;
314
315		child->ext_info_cache[i].data[ret] = 0;
316		child->ext_info_cache[i].size = ret;
317	}
318
319	if (page)
320		devm_kfree(dev, page);
321
322	return 0;
323}
324
325/*
326 * Same as of_property_for_each_string(), but also keeps track of the
327 * index of each string.
328 */
329#define of_property_for_each_string_index(np, propname, prop, s, i)	\
330	for (prop = of_find_property(np, propname, NULL),		\
331	     s = of_prop_next_string(prop, NULL),			\
332	     i = 0;							\
333	     s;								\
334	     s = of_prop_next_string(prop, s),				\
335	     i++)
336
337static int mux_probe(struct platform_device *pdev)
338{
339	struct device *dev = &pdev->dev;
340	struct device_node *np = pdev->dev.of_node;
341	struct iio_dev *indio_dev;
342	struct iio_channel *parent;
343	struct mux *mux;
344	struct property *prop;
345	const char *label;
346	u32 state;
347	int sizeof_ext_info;
348	int children;
349	int sizeof_priv;
350	int i;
351	int ret;
352
353	if (!np)
354		return -ENODEV;
355
356	parent = devm_iio_channel_get(dev, "parent");
357	if (IS_ERR(parent))
358		return dev_err_probe(dev, PTR_ERR(parent),
359				     "failed to get parent channel\n");
360
361	sizeof_ext_info = iio_get_channel_ext_info_count(parent);
362	if (sizeof_ext_info) {
363		sizeof_ext_info += 1; /* one extra entry for the sentinel */
364		sizeof_ext_info *= sizeof(*mux->ext_info);
365	}
366
367	children = 0;
368	of_property_for_each_string(np, "channels", prop, label) {
369		if (*label)
370			children++;
371	}
372	if (children <= 0) {
373		dev_err(dev, "not even a single child\n");
374		return -EINVAL;
375	}
376
377	sizeof_priv = sizeof(*mux);
378	sizeof_priv += sizeof(*mux->child) * children;
379	sizeof_priv += sizeof(*mux->chan) * children;
380	sizeof_priv += sizeof_ext_info;
381
382	indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
383	if (!indio_dev)
384		return -ENOMEM;
385
386	mux = iio_priv(indio_dev);
387	mux->child = (struct mux_child *)(mux + 1);
388	mux->chan = (struct iio_chan_spec *)(mux->child + children);
389
390	platform_set_drvdata(pdev, indio_dev);
391
392	mux->parent = parent;
393	mux->cached_state = -1;
394
395	indio_dev->name = dev_name(dev);
396	indio_dev->info = &mux_info;
397	indio_dev->modes = INDIO_DIRECT_MODE;
398	indio_dev->channels = mux->chan;
399	indio_dev->num_channels = children;
400	if (sizeof_ext_info) {
401		mux->ext_info = devm_kmemdup(dev,
402					     parent->channel->ext_info,
403					     sizeof_ext_info, GFP_KERNEL);
404		if (!mux->ext_info)
405			return -ENOMEM;
406
407		for (i = 0; mux->ext_info[i].name; ++i) {
408			if (parent->channel->ext_info[i].read)
409				mux->ext_info[i].read = mux_read_ext_info;
410			if (parent->channel->ext_info[i].write)
411				mux->ext_info[i].write = mux_write_ext_info;
412			mux->ext_info[i].private = i;
413		}
414	}
415
416	mux->control = devm_mux_control_get(dev, NULL);
417	if (IS_ERR(mux->control)) {
418		if (PTR_ERR(mux->control) != -EPROBE_DEFER)
419			dev_err(dev, "failed to get control-mux\n");
420		return PTR_ERR(mux->control);
421	}
422
423	i = 0;
424	of_property_for_each_string_index(np, "channels", prop, label, state) {
425		if (!*label)
426			continue;
427
428		ret = mux_configure_channel(dev, mux, state, label, i++);
429		if (ret < 0)
430			return ret;
431	}
432
433	ret = devm_iio_device_register(dev, indio_dev);
434	if (ret) {
435		dev_err(dev, "failed to register iio device\n");
436		return ret;
437	}
438
439	return 0;
440}
441
442static const struct of_device_id mux_match[] = {
443	{ .compatible = "io-channel-mux" },
444	{ /* sentinel */ }
445};
446MODULE_DEVICE_TABLE(of, mux_match);
447
448static struct platform_driver mux_driver = {
449	.probe = mux_probe,
450	.driver = {
451		.name = "iio-mux",
452		.of_match_table = mux_match,
453	},
454};
455module_platform_driver(mux_driver);
456
457MODULE_DESCRIPTION("IIO multiplexer driver");
458MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
459MODULE_LICENSE("GPL v2");
460