1
2
3
4
5
6
7
8
9
10
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
22struct mux_ext_info_cache {
23 char *data;
24 ssize_t size;
25};
26
27struct mux_child {
28 struct mux_ext_info_cache *ext_info_cache;
29};
30
31struct 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
41static 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
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_kzalloc(dev,
285 sizeof(*child->ext_info_cache) *
286 num_ext_info, GFP_KERNEL);
287 if (!child->ext_info_cache)
288 return -ENOMEM;
289
290 for (i = 0; i < num_ext_info; ++i) {
291 child->ext_info_cache[i].size = -1;
292
293 if (!pchan->ext_info[i].write)
294 continue;
295 if (!pchan->ext_info[i].read)
296 continue;
297
298 ret = iio_read_channel_ext_info(mux->parent,
299 mux->ext_info[i].name,
300 page);
301 if (ret < 0) {
302 dev_err(dev, "failed to get ext_info '%s'\n",
303 pchan->ext_info[i].name);
304 return ret;
305 }
306 if (ret >= PAGE_SIZE) {
307 dev_err(dev, "too large ext_info '%s'\n",
308 pchan->ext_info[i].name);
309 return -EINVAL;
310 }
311
312 child->ext_info_cache[i].data = devm_kmemdup(dev, page, ret + 1,
313 GFP_KERNEL);
314 if (!child->ext_info_cache[i].data)
315 return -ENOMEM;
316
317 child->ext_info_cache[i].data[ret] = 0;
318 child->ext_info_cache[i].size = ret;
319 }
320
321 if (page)
322 devm_kfree(dev, page);
323
324 return 0;
325}
326
327
328
329
330
331#define of_property_for_each_string_index(np, propname, prop, s, i) \
332 for (prop = of_find_property(np, propname, NULL), \
333 s = of_prop_next_string(prop, NULL), \
334 i = 0; \
335 s; \
336 s = of_prop_next_string(prop, s), \
337 i++)
338
339static int mux_probe(struct platform_device *pdev)
340{
341 struct device *dev = &pdev->dev;
342 struct device_node *np = pdev->dev.of_node;
343 struct iio_dev *indio_dev;
344 struct iio_channel *parent;
345 struct mux *mux;
346 struct property *prop;
347 const char *label;
348 u32 state;
349 int sizeof_ext_info;
350 int children;
351 int sizeof_priv;
352 int i;
353 int ret;
354
355 if (!np)
356 return -ENODEV;
357
358 parent = devm_iio_channel_get(dev, "parent");
359 if (IS_ERR(parent)) {
360 if (PTR_ERR(parent) != -EPROBE_DEFER)
361 dev_err(dev, "failed to get parent channel\n");
362 return PTR_ERR(parent);
363 }
364
365 sizeof_ext_info = iio_get_channel_ext_info_count(parent);
366 if (sizeof_ext_info) {
367 sizeof_ext_info += 1;
368 sizeof_ext_info *= sizeof(*mux->ext_info);
369 }
370
371 children = 0;
372 of_property_for_each_string(np, "channels", prop, label) {
373 if (*label)
374 children++;
375 }
376 if (children <= 0) {
377 dev_err(dev, "not even a single child\n");
378 return -EINVAL;
379 }
380
381 sizeof_priv = sizeof(*mux);
382 sizeof_priv += sizeof(*mux->child) * children;
383 sizeof_priv += sizeof(*mux->chan) * children;
384 sizeof_priv += sizeof_ext_info;
385
386 indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
387 if (!indio_dev)
388 return -ENOMEM;
389
390 mux = iio_priv(indio_dev);
391 mux->child = (struct mux_child *)(mux + 1);
392 mux->chan = (struct iio_chan_spec *)(mux->child + children);
393
394 platform_set_drvdata(pdev, indio_dev);
395
396 mux->parent = parent;
397 mux->cached_state = -1;
398
399 indio_dev->name = dev_name(dev);
400 indio_dev->dev.parent = dev;
401 indio_dev->info = &mux_info;
402 indio_dev->modes = INDIO_DIRECT_MODE;
403 indio_dev->channels = mux->chan;
404 indio_dev->num_channels = children;
405 if (sizeof_ext_info) {
406 mux->ext_info = devm_kmemdup(dev,
407 parent->channel->ext_info,
408 sizeof_ext_info, GFP_KERNEL);
409 if (!mux->ext_info)
410 return -ENOMEM;
411
412 for (i = 0; mux->ext_info[i].name; ++i) {
413 if (parent->channel->ext_info[i].read)
414 mux->ext_info[i].read = mux_read_ext_info;
415 if (parent->channel->ext_info[i].write)
416 mux->ext_info[i].write = mux_write_ext_info;
417 mux->ext_info[i].private = i;
418 }
419 }
420
421 mux->control = devm_mux_control_get(dev, NULL);
422 if (IS_ERR(mux->control)) {
423 if (PTR_ERR(mux->control) != -EPROBE_DEFER)
424 dev_err(dev, "failed to get control-mux\n");
425 return PTR_ERR(mux->control);
426 }
427
428 i = 0;
429 of_property_for_each_string_index(np, "channels", prop, label, state) {
430 if (!*label)
431 continue;
432
433 ret = mux_configure_channel(dev, mux, state, label, i++);
434 if (ret < 0)
435 return ret;
436 }
437
438 ret = devm_iio_device_register(dev, indio_dev);
439 if (ret) {
440 dev_err(dev, "failed to register iio device\n");
441 return ret;
442 }
443
444 return 0;
445}
446
447static const struct of_device_id mux_match[] = {
448 { .compatible = "io-channel-mux" },
449 { }
450};
451MODULE_DEVICE_TABLE(of, mux_match);
452
453static struct platform_driver mux_driver = {
454 .probe = mux_probe,
455 .driver = {
456 .name = "iio-mux",
457 .of_match_table = mux_match,
458 },
459};
460module_platform_driver(mux_driver);
461
462MODULE_DESCRIPTION("IIO multiplexer driver");
463MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
464MODULE_LICENSE("GPL v2");
465
