iio-mux.c

  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/cleanup.h>
 11#include <linux/err.h>
 12#include <linux/iio/consumer.h>
 13#include <linux/iio/iio.h>
 14#include <linux/mod_devicetable.h>
 15#include <linux/module.h>
 16#include <linux/mutex.h>
 17#include <linux/mux/consumer.h>
 18#include <linux/platform_device.h>
 19#include <linux/property.h>
 20
 21struct mux_ext_info_cache {
 22	char *data;
 23	ssize_t size;
 24};
 25
 26struct mux_child {
 27	struct mux_ext_info_cache *ext_info_cache;
 28};
 29
 30struct mux {
 31	int cached_state;
 32	struct mux_control *control;
 33	struct iio_channel *parent;
 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_chan_ext_info(struct device *dev, struct mux *mux,
242				       int idx, int num_ext_info)
243{
244	struct mux_child *child = &mux->child[idx];
245	struct iio_chan_spec const *pchan = mux->parent->channel;
246	int i;
247	int ret;
248
249	char *page __free(kfree) = kzalloc(PAGE_SIZE, GFP_KERNEL);
250	if (!page)
251		return -ENOMEM;
252
253	child->ext_info_cache = devm_kcalloc(dev,
254					     num_ext_info,
255					     sizeof(*child->ext_info_cache),
256					     GFP_KERNEL);
257	if (!child->ext_info_cache)
258		return -ENOMEM;
259
260	for (i = 0; i < num_ext_info; ++i) {
261		child->ext_info_cache[i].size = -1;
262
263		if (!pchan->ext_info[i].write)
264			continue;
265		if (!pchan->ext_info[i].read)
266			continue;
267
268		ret = iio_read_channel_ext_info(mux->parent,
269						mux->ext_info[i].name,
270						page);
271		if (ret < 0) {
272			dev_err(dev, "failed to get ext_info '%s'\n",
273				pchan->ext_info[i].name);
274			return ret;
275		}
276		if (ret >= PAGE_SIZE) {
277			dev_err(dev, "too large ext_info '%s'\n",
278				pchan->ext_info[i].name);
279			return -EINVAL;
280		}
281
282		child->ext_info_cache[i].data = devm_kmemdup(dev, page, ret + 1,
283							     GFP_KERNEL);
284		if (!child->ext_info_cache[i].data)
285			return -ENOMEM;
286
287		child->ext_info_cache[i].data[ret] = 0;
288		child->ext_info_cache[i].size = ret;
289	}
290
291	return 0;
292}
293
294static int mux_configure_channel(struct device *dev, struct mux *mux, u32 state,
295				 const char *label, int idx)
296{
297	struct iio_chan_spec *chan = &mux->chan[idx];
298	struct iio_chan_spec const *pchan = mux->parent->channel;
299	int num_ext_info;
300	int ret;
301
302	chan->indexed = 1;
303	chan->output = pchan->output;
304	chan->datasheet_name = label;
305	chan->ext_info = mux->ext_info;
306
307	ret = iio_get_channel_type(mux->parent, &chan->type);
308	if (ret < 0) {
309		dev_err(dev, "failed to get parent channel type\n");
310		return ret;
311	}
312
313	if (iio_channel_has_info(pchan, IIO_CHAN_INFO_RAW))
314		chan->info_mask_separate |= BIT(IIO_CHAN_INFO_RAW);
315	if (iio_channel_has_info(pchan, IIO_CHAN_INFO_SCALE))
316		chan->info_mask_separate |= BIT(IIO_CHAN_INFO_SCALE);
317
318	if (iio_channel_has_available(pchan, IIO_CHAN_INFO_RAW))
319		chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW);
320
321	if (state >= mux_control_states(mux->control)) {
322		dev_err(dev, "too many channels\n");
323		return -EINVAL;
324	}
325
326	chan->channel = state;
327
328	num_ext_info = iio_get_channel_ext_info_count(mux->parent);
329	if (num_ext_info)
330		return mux_configure_chan_ext_info(dev, mux, idx, num_ext_info);
331
332	return 0;
333}
334
335static int mux_probe(struct platform_device *pdev)
336{
337	struct device *dev = &pdev->dev;
338	struct iio_dev *indio_dev;
339	struct iio_channel *parent;
340	struct mux *mux;
341	const char **labels;
342	int all_children;
343	int children;
344	u32 state;
345	int sizeof_ext_info;
346	int sizeof_priv;
347	int i;
348	int ret;
349
350	parent = devm_iio_channel_get(dev, "parent");
351	if (IS_ERR(parent))
352		return dev_err_probe(dev, PTR_ERR(parent),
353				     "failed to get parent channel\n");
354
355	sizeof_ext_info = iio_get_channel_ext_info_count(parent);
356	if (sizeof_ext_info) {
357		sizeof_ext_info += 1; /* one extra entry for the sentinel */
358		sizeof_ext_info *= sizeof(*mux->ext_info);
359	}
360
361	all_children = device_property_string_array_count(dev, "channels");
362	if (all_children < 0)
363		return all_children;
364
365	labels = devm_kmalloc_array(dev, all_children, sizeof(*labels), GFP_KERNEL);
366	if (!labels)
367		return -ENOMEM;
368
369	ret = device_property_read_string_array(dev, "channels", labels, all_children);
370	if (ret < 0)
371		return ret;
372
373	children = 0;
374	for (state = 0; state < all_children; state++) {
375		if (*labels[state])
376			children++;
377	}
378	if (children <= 0) {
379		dev_err(dev, "not even a single child\n");
380		return -EINVAL;
381	}
382
383	sizeof_priv = sizeof(*mux);
384	sizeof_priv += sizeof(*mux->child) * children;
385	sizeof_priv += sizeof(*mux->chan) * children;
386	sizeof_priv += sizeof_ext_info;
387
388	indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
389	if (!indio_dev)
390		return -ENOMEM;
391
392	mux = iio_priv(indio_dev);
393	mux->child = (struct mux_child *)(mux + 1);
394	mux->chan = (struct iio_chan_spec *)(mux->child + children);
395
396	platform_set_drvdata(pdev, indio_dev);
397
398	mux->parent = parent;
399	mux->cached_state = -1;
400
401	mux->delay_us = 0;
402	device_property_read_u32(dev, "settle-time-us", &mux->delay_us);
403
404	indio_dev->name = dev_name(dev);
405	indio_dev->info = &mux_info;
406	indio_dev->modes = INDIO_DIRECT_MODE;
407	indio_dev->channels = mux->chan;
408	indio_dev->num_channels = children;
409	if (sizeof_ext_info) {
410		mux->ext_info = devm_kmemdup(dev,
411					     parent->channel->ext_info,
412					     sizeof_ext_info, GFP_KERNEL);
413		if (!mux->ext_info)
414			return -ENOMEM;
415
416		for (i = 0; mux->ext_info[i].name; ++i) {
417			if (parent->channel->ext_info[i].read)
418				mux->ext_info[i].read = mux_read_ext_info;
419			if (parent->channel->ext_info[i].write)
420				mux->ext_info[i].write = mux_write_ext_info;
421			mux->ext_info[i].private = i;
422		}
423	}
424
425	mux->control = devm_mux_control_get(dev, NULL);
426	if (IS_ERR(mux->control))
427		return dev_err_probe(dev, PTR_ERR(mux->control),
428				     "failed to get control-mux\n");
429
430	i = 0;
431	for (state = 0; state < all_children; state++) {
432		if (!*labels[state])
433			continue;
434
435		ret = mux_configure_channel(dev, mux, state, labels[state], i++);
436		if (ret < 0)
437			return ret;
438	}
439
440	ret = devm_iio_device_register(dev, indio_dev);
441	if (ret) {
442		dev_err(dev, "failed to register iio device\n");
443		return ret;
444	}
445
446	return 0;
447}
448
449static const struct of_device_id mux_match[] = {
450	{ .compatible = "io-channel-mux" },
451	{ }
452};
453MODULE_DEVICE_TABLE(of, mux_match);
454
455static struct platform_driver mux_driver = {
456	.probe = mux_probe,
457	.driver = {
458		.name = "iio-mux",
459		.of_match_table = mux_match,
460	},
461};
462module_platform_driver(mux_driver);
463
464MODULE_DESCRIPTION("IIO multiplexer driver");
465MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
466MODULE_LICENSE("GPL v2");