1 /*
2  * IIO multiplexer driver
3  *
4  * Copyright (C) 2017 Axentia Technologies AB
5  *
6  * Author: Peter Rosin <peda@axentia.se>
7  *
8  * This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License version 2 as
10  * published by the Free Software Foundation.
11  */
12 
13 #include <linux/err.h>
14 #include <linux/iio/consumer.h>
15 #include <linux/iio/iio.h>
16 #include <linux/module.h>
17 #include <linux/mutex.h>
18 #include <linux/mux/consumer.h>
19 #include <linux/of.h>
20 #include <linux/platform_device.h>
21 
22 struct mux_ext_info_cache {
23 	char *data;
24 	ssize_t size;
25 };
26 
27 struct mux_child {
28 	struct mux_ext_info_cache *ext_info_cache;
29 };
30 
31 struct mux {
32 	int cached_state;
33 	struct mux_control *control;
34 	struct iio_channel *parent;
35 	struct iio_dev *indio_dev;
36 	struct iio_chan_spec *chan;
37 	struct iio_chan_spec_ext_info *ext_info;
38 	struct mux_child *child;
39 };
40 
iio_mux_select(struct mux * mux,int idx)41 static int iio_mux_select(struct mux *mux, int idx)
42 {
43 	struct mux_child *child = &mux->child[idx];
44 	struct iio_chan_spec const *chan = &mux->chan[idx];
45 	int ret;
46 	int i;
47 
48 	ret = mux_control_select(mux->control, chan->channel);
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 
iio_mux_deselect(struct mux * mux)83 static void iio_mux_deselect(struct mux *mux)
84 {
85 	mux_control_deselect(mux->control);
86 }
87 
mux_read_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int * val,int * val2,long mask)88 static 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 
mux_read_avail(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,const int ** vals,int * type,int * length,long mask)118 static 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 
mux_write_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int val,int val2,long mask)146 static 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 
172 static 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 
mux_read_ext_info(struct iio_dev * indio_dev,uintptr_t private,struct iio_chan_spec const * chan,char * buf)178 static 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 
mux_write_ext_info(struct iio_dev * indio_dev,uintptr_t private,struct iio_chan_spec const * chan,const char * buf,size_t len)198 static 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 
mux_configure_channel(struct device * dev,struct mux * mux,u32 state,const char * label,int idx)241 static 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 
328 /*
329  * Same as of_property_for_each_string(), but also keeps track of the
330  * index of each string.
331  */
332 #define of_property_for_each_string_index(np, propname, prop, s, i)	\
333 	for (prop = of_find_property(np, propname, NULL),		\
334 	     s = of_prop_next_string(prop, NULL),			\
335 	     i = 0;							\
336 	     s;								\
337 	     s = of_prop_next_string(prop, s),				\
338 	     i++)
339 
mux_probe(struct platform_device * pdev)340 static int mux_probe(struct platform_device *pdev)
341 {
342 	struct device *dev = &pdev->dev;
343 	struct device_node *np = pdev->dev.of_node;
344 	struct iio_dev *indio_dev;
345 	struct iio_channel *parent;
346 	struct mux *mux;
347 	struct property *prop;
348 	const char *label;
349 	u32 state;
350 	int sizeof_ext_info;
351 	int children;
352 	int sizeof_priv;
353 	int i;
354 	int ret;
355 
356 	if (!np)
357 		return -ENODEV;
358 
359 	parent = devm_iio_channel_get(dev, "parent");
360 	if (IS_ERR(parent)) {
361 		if (PTR_ERR(parent) != -EPROBE_DEFER)
362 			dev_err(dev, "failed to get parent channel\n");
363 		return PTR_ERR(parent);
364 	}
365 
366 	sizeof_ext_info = iio_get_channel_ext_info_count(parent);
367 	if (sizeof_ext_info) {
368 		sizeof_ext_info += 1; /* one extra entry for the sentinel */
369 		sizeof_ext_info *= sizeof(*mux->ext_info);
370 	}
371 
372 	children = 0;
373 	of_property_for_each_string(np, "channels", prop, label) {
374 		if (*label)
375 			children++;
376 	}
377 	if (children <= 0) {
378 		dev_err(dev, "not even a single child\n");
379 		return -EINVAL;
380 	}
381 
382 	sizeof_priv = sizeof(*mux);
383 	sizeof_priv += sizeof(*mux->child) * children;
384 	sizeof_priv += sizeof(*mux->chan) * children;
385 	sizeof_priv += sizeof_ext_info;
386 
387 	indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
388 	if (!indio_dev)
389 		return -ENOMEM;
390 
391 	mux = iio_priv(indio_dev);
392 	mux->child = (struct mux_child *)(mux + 1);
393 	mux->chan = (struct iio_chan_spec *)(mux->child + children);
394 
395 	platform_set_drvdata(pdev, indio_dev);
396 
397 	mux->parent = parent;
398 	mux->cached_state = -1;
399 
400 	indio_dev->name = dev_name(dev);
401 	indio_dev->dev.parent = dev;
402 	indio_dev->info = &mux_info;
403 	indio_dev->modes = INDIO_DIRECT_MODE;
404 	indio_dev->channels = mux->chan;
405 	indio_dev->num_channels = children;
406 	if (sizeof_ext_info) {
407 		mux->ext_info = devm_kmemdup(dev,
408 					     parent->channel->ext_info,
409 					     sizeof_ext_info, GFP_KERNEL);
410 		if (!mux->ext_info)
411 			return -ENOMEM;
412 
413 		for (i = 0; mux->ext_info[i].name; ++i) {
414 			if (parent->channel->ext_info[i].read)
415 				mux->ext_info[i].read = mux_read_ext_info;
416 			if (parent->channel->ext_info[i].write)
417 				mux->ext_info[i].write = mux_write_ext_info;
418 			mux->ext_info[i].private = i;
419 		}
420 	}
421 
422 	mux->control = devm_mux_control_get(dev, NULL);
423 	if (IS_ERR(mux->control)) {
424 		if (PTR_ERR(mux->control) != -EPROBE_DEFER)
425 			dev_err(dev, "failed to get control-mux\n");
426 		return PTR_ERR(mux->control);
427 	}
428 
429 	i = 0;
430 	of_property_for_each_string_index(np, "channels", prop, label, state) {
431 		if (!*label)
432 			continue;
433 
434 		ret = mux_configure_channel(dev, mux, state, label, i++);
435 		if (ret < 0)
436 			return ret;
437 	}
438 
439 	ret = devm_iio_device_register(dev, indio_dev);
440 	if (ret) {
441 		dev_err(dev, "failed to register iio device\n");
442 		return ret;
443 	}
444 
445 	return 0;
446 }
447 
448 static const struct of_device_id mux_match[] = {
449 	{ .compatible = "io-channel-mux" },
450 	{ /* sentinel */ }
451 };
452 MODULE_DEVICE_TABLE(of, mux_match);
453 
454 static struct platform_driver mux_driver = {
455 	.probe = mux_probe,
456 	.driver = {
457 		.name = "iio-mux",
458 		.of_match_table = mux_match,
459 	},
460 };
461 module_platform_driver(mux_driver);
462 
463 MODULE_DESCRIPTION("IIO multiplexer driver");
464 MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
465 MODULE_LICENSE("GPL v2");
466