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 
19 struct mux_ext_info_cache {
20 	char *data;
21 	ssize_t size;
22 };
23 
24 struct mux_child {
25 	struct mux_ext_info_cache *ext_info_cache;
26 };
27 
28 struct 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 
38 static 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 
80 static void iio_mux_deselect(struct mux *mux)
81 {
82 	mux_control_deselect(mux->control);
83 }
84 
85 static 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 
115 static 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 
143 static 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 
169 static 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 
175 static 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 
195 static 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 
238 static 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 
337 static 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 		if (PTR_ERR(parent) != -EPROBE_DEFER)
359 			dev_err(dev, "failed to get parent channel\n");
360 		return PTR_ERR(parent);
361 	}
362 
363 	sizeof_ext_info = iio_get_channel_ext_info_count(parent);
364 	if (sizeof_ext_info) {
365 		sizeof_ext_info += 1; /* one extra entry for the sentinel */
366 		sizeof_ext_info *= sizeof(*mux->ext_info);
367 	}
368 
369 	children = 0;
370 	of_property_for_each_string(np, "channels", prop, label) {
371 		if (*label)
372 			children++;
373 	}
374 	if (children <= 0) {
375 		dev_err(dev, "not even a single child\n");
376 		return -EINVAL;
377 	}
378 
379 	sizeof_priv = sizeof(*mux);
380 	sizeof_priv += sizeof(*mux->child) * children;
381 	sizeof_priv += sizeof(*mux->chan) * children;
382 	sizeof_priv += sizeof_ext_info;
383 
384 	indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
385 	if (!indio_dev)
386 		return -ENOMEM;
387 
388 	mux = iio_priv(indio_dev);
389 	mux->child = (struct mux_child *)(mux + 1);
390 	mux->chan = (struct iio_chan_spec *)(mux->child + children);
391 
392 	platform_set_drvdata(pdev, indio_dev);
393 
394 	mux->parent = parent;
395 	mux->cached_state = -1;
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 		if (PTR_ERR(mux->control) != -EPROBE_DEFER)
421 			dev_err(dev, "failed to get control-mux\n");
422 		return PTR_ERR(mux->control);
423 	}
424 
425 	i = 0;
426 	of_property_for_each_string_index(np, "channels", prop, label, state) {
427 		if (!*label)
428 			continue;
429 
430 		ret = mux_configure_channel(dev, mux, state, label, i++);
431 		if (ret < 0)
432 			return ret;
433 	}
434 
435 	ret = devm_iio_device_register(dev, indio_dev);
436 	if (ret) {
437 		dev_err(dev, "failed to register iio device\n");
438 		return ret;
439 	}
440 
441 	return 0;
442 }
443 
444 static const struct of_device_id mux_match[] = {
445 	{ .compatible = "io-channel-mux" },
446 	{ /* sentinel */ }
447 };
448 MODULE_DEVICE_TABLE(of, mux_match);
449 
450 static struct platform_driver mux_driver = {
451 	.probe = mux_probe,
452 	.driver = {
453 		.name = "iio-mux",
454 		.of_match_table = mux_match,
455 	},
456 };
457 module_platform_driver(mux_driver);
458 
459 MODULE_DESCRIPTION("IIO multiplexer driver");
460 MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
461 MODULE_LICENSE("GPL v2");
462