1
2
3
4
5
6
7
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
327
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 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;
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->dev.parent = dev;
399 indio_dev->info = &mux_info;
400 indio_dev->modes = INDIO_DIRECT_MODE;
401 indio_dev->channels = mux->chan;
402 indio_dev->num_channels = children;
403 if (sizeof_ext_info) {
404 mux->ext_info = devm_kmemdup(dev,
405 parent->channel->ext_info,
406 sizeof_ext_info, GFP_KERNEL);
407 if (!mux->ext_info)
408 return -ENOMEM;
409
410 for (i = 0; mux->ext_info[i].name; ++i) {
411 if (parent->channel->ext_info[i].read)
412 mux->ext_info[i].read = mux_read_ext_info;
413 if (parent->channel->ext_info[i].write)
414 mux->ext_info[i].write = mux_write_ext_info;
415 mux->ext_info[i].private = i;
416 }
417 }
418
419 mux->control = devm_mux_control_get(dev, NULL);
420 if (IS_ERR(mux->control)) {
421 if (PTR_ERR(mux->control) != -EPROBE_DEFER)
422 dev_err(dev, "failed to get control-mux\n");
423 return PTR_ERR(mux->control);
424 }
425
426 i = 0;
427 of_property_for_each_string_index(np, "channels", prop, label, state) {
428 if (!*label)
429 continue;
430
431 ret = mux_configure_channel(dev, mux, state, label, i++);
432 if (ret < 0)
433 return ret;
434 }
435
436 ret = devm_iio_device_register(dev, indio_dev);
437 if (ret) {
438 dev_err(dev, "failed to register iio device\n");
439 return ret;
440 }
441
442 return 0;
443}
444
445static const struct of_device_id mux_match[] = {
446 { .compatible = "io-channel-mux" },
447 { }
448};
449MODULE_DEVICE_TABLE(of, mux_match);
450
451static struct platform_driver mux_driver = {
452 .probe = mux_probe,
453 .driver = {
454 .name = "iio-mux",
455 .of_match_table = mux_match,
456 },
457};
458module_platform_driver(mux_driver);
459
460MODULE_DESCRIPTION("IIO multiplexer driver");
461MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
462MODULE_LICENSE("GPL v2");
463
