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