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/module.h>
15 #include <linux/mutex.h>
16 #include <linux/mux/consumer.h>
17 #include <linux/platform_device.h>
18 #include <linux/property.h>
19
20 struct mux_ext_info_cache {
21 char *data;
22 ssize_t size;
23 };
24
25 struct mux_child {
26 struct mux_ext_info_cache *ext_info_cache;
27 };
28
29 struct mux {
30 int cached_state;
31 struct mux_control *control;
32 struct iio_channel *parent;
33 struct iio_chan_spec *chan;
34 struct iio_chan_spec_ext_info *ext_info;
35 struct mux_child *child;
36 u32 delay_us;
37 };
38
iio_mux_select(struct mux * mux,int idx)39 static int iio_mux_select(struct mux *mux, int idx)
40 {
41 struct mux_child *child = &mux->child[idx];
42 struct iio_chan_spec const *chan = &mux->chan[idx];
43 int ret;
44 int i;
45
46 ret = mux_control_select_delay(mux->control, chan->channel,
47 mux->delay_us);
48 if (ret < 0) {
49 mux->cached_state = -1;
50 return ret;
51 }
52
53 if (mux->cached_state == chan->channel)
54 return 0;
55
56 if (chan->ext_info) {
57 for (i = 0; chan->ext_info[i].name; ++i) {
58 const char *attr = chan->ext_info[i].name;
59 struct mux_ext_info_cache *cache;
60
61 cache = &child->ext_info_cache[i];
62
63 if (cache->size < 0)
64 continue;
65
66 ret = iio_write_channel_ext_info(mux->parent, attr,
67 cache->data,
68 cache->size);
69
70 if (ret < 0) {
71 mux_control_deselect(mux->control);
72 mux->cached_state = -1;
73 return ret;
74 }
75 }
76 }
77 mux->cached_state = chan->channel;
78
79 return 0;
80 }
81
iio_mux_deselect(struct mux * mux)82 static void iio_mux_deselect(struct mux *mux)
83 {
84 mux_control_deselect(mux->control);
85 }
86
mux_read_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int * val,int * val2,long mask)87 static int mux_read_raw(struct iio_dev *indio_dev,
88 struct iio_chan_spec const *chan,
89 int *val, int *val2, long mask)
90 {
91 struct mux *mux = iio_priv(indio_dev);
92 int idx = chan - mux->chan;
93 int ret;
94
95 ret = iio_mux_select(mux, idx);
96 if (ret < 0)
97 return ret;
98
99 switch (mask) {
100 case IIO_CHAN_INFO_RAW:
101 ret = iio_read_channel_raw(mux->parent, val);
102 break;
103
104 case IIO_CHAN_INFO_SCALE:
105 ret = iio_read_channel_scale(mux->parent, val, val2);
106 break;
107
108 default:
109 ret = -EINVAL;
110 }
111
112 iio_mux_deselect(mux);
113
114 return ret;
115 }
116
mux_read_avail(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,const int ** vals,int * type,int * length,long mask)117 static int mux_read_avail(struct iio_dev *indio_dev,
118 struct iio_chan_spec const *chan,
119 const int **vals, int *type, int *length,
120 long mask)
121 {
122 struct mux *mux = iio_priv(indio_dev);
123 int idx = chan - mux->chan;
124 int ret;
125
126 ret = iio_mux_select(mux, idx);
127 if (ret < 0)
128 return ret;
129
130 switch (mask) {
131 case IIO_CHAN_INFO_RAW:
132 *type = IIO_VAL_INT;
133 ret = iio_read_avail_channel_raw(mux->parent, vals, length);
134 break;
135
136 default:
137 ret = -EINVAL;
138 }
139
140 iio_mux_deselect(mux);
141
142 return ret;
143 }
144
mux_write_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int val,int val2,long mask)145 static int mux_write_raw(struct iio_dev *indio_dev,
146 struct iio_chan_spec const *chan,
147 int val, int val2, long mask)
148 {
149 struct mux *mux = iio_priv(indio_dev);
150 int idx = chan - mux->chan;
151 int ret;
152
153 ret = iio_mux_select(mux, idx);
154 if (ret < 0)
155 return ret;
156
157 switch (mask) {
158 case IIO_CHAN_INFO_RAW:
159 ret = iio_write_channel_raw(mux->parent, val);
160 break;
161
162 default:
163 ret = -EINVAL;
164 }
165
166 iio_mux_deselect(mux);
167
168 return ret;
169 }
170
171 static const struct iio_info mux_info = {
172 .read_raw = mux_read_raw,
173 .read_avail = mux_read_avail,
174 .write_raw = mux_write_raw,
175 };
176
mux_read_ext_info(struct iio_dev * indio_dev,uintptr_t private,struct iio_chan_spec const * chan,char * buf)177 static ssize_t mux_read_ext_info(struct iio_dev *indio_dev, uintptr_t private,
178 struct iio_chan_spec const *chan, char *buf)
179 {
180 struct mux *mux = iio_priv(indio_dev);
181 int idx = chan - mux->chan;
182 ssize_t ret;
183
184 ret = iio_mux_select(mux, idx);
185 if (ret < 0)
186 return ret;
187
188 ret = iio_read_channel_ext_info(mux->parent,
189 mux->ext_info[private].name,
190 buf);
191
192 iio_mux_deselect(mux);
193
194 return ret;
195 }
196
mux_write_ext_info(struct iio_dev * indio_dev,uintptr_t private,struct iio_chan_spec const * chan,const char * buf,size_t len)197 static ssize_t mux_write_ext_info(struct iio_dev *indio_dev, uintptr_t private,
198 struct iio_chan_spec const *chan,
199 const char *buf, size_t len)
200 {
201 struct device *dev = indio_dev->dev.parent;
202 struct mux *mux = iio_priv(indio_dev);
203 int idx = chan - mux->chan;
204 char *new;
205 ssize_t ret;
206
207 if (len >= PAGE_SIZE)
208 return -EINVAL;
209
210 ret = iio_mux_select(mux, idx);
211 if (ret < 0)
212 return ret;
213
214 new = devm_kmemdup(dev, buf, len + 1, GFP_KERNEL);
215 if (!new) {
216 iio_mux_deselect(mux);
217 return -ENOMEM;
218 }
219
220 new[len] = 0;
221
222 ret = iio_write_channel_ext_info(mux->parent,
223 mux->ext_info[private].name,
224 buf, len);
225 if (ret < 0) {
226 iio_mux_deselect(mux);
227 devm_kfree(dev, new);
228 return ret;
229 }
230
231 devm_kfree(dev, mux->child[idx].ext_info_cache[private].data);
232 mux->child[idx].ext_info_cache[private].data = new;
233 mux->child[idx].ext_info_cache[private].size = len;
234
235 iio_mux_deselect(mux);
236
237 return ret;
238 }
239
mux_configure_chan_ext_info(struct device * dev,struct mux * mux,int idx,int num_ext_info)240 static int mux_configure_chan_ext_info(struct device *dev, struct mux *mux,
241 int idx, int num_ext_info)
242 {
243 struct mux_child *child = &mux->child[idx];
244 struct iio_chan_spec const *pchan = mux->parent->channel;
245 int i;
246 int ret;
247
248 char *page __free(kfree) = kzalloc(PAGE_SIZE, GFP_KERNEL);
249 if (!page)
250 return -ENOMEM;
251
252 child->ext_info_cache = devm_kcalloc(dev,
253 num_ext_info,
254 sizeof(*child->ext_info_cache),
255 GFP_KERNEL);
256 if (!child->ext_info_cache)
257 return -ENOMEM;
258
259 for (i = 0; i < num_ext_info; ++i) {
260 child->ext_info_cache[i].size = -1;
261
262 if (!pchan->ext_info[i].write)
263 continue;
264 if (!pchan->ext_info[i].read)
265 continue;
266
267 ret = iio_read_channel_ext_info(mux->parent,
268 mux->ext_info[i].name,
269 page);
270 if (ret < 0) {
271 dev_err(dev, "failed to get ext_info '%s'\n",
272 pchan->ext_info[i].name);
273 return ret;
274 }
275 if (ret >= PAGE_SIZE) {
276 dev_err(dev, "too large ext_info '%s'\n",
277 pchan->ext_info[i].name);
278 return -EINVAL;
279 }
280
281 child->ext_info_cache[i].data = devm_kmemdup(dev, page, ret + 1,
282 GFP_KERNEL);
283 if (!child->ext_info_cache[i].data)
284 return -ENOMEM;
285
286 child->ext_info_cache[i].data[ret] = 0;
287 child->ext_info_cache[i].size = ret;
288 }
289
290 return 0;
291 }
292
mux_configure_channel(struct device * dev,struct mux * mux,u32 state,const char * label,int idx)293 static int mux_configure_channel(struct device *dev, struct mux *mux, u32 state,
294 const char *label, int idx)
295 {
296 struct iio_chan_spec *chan = &mux->chan[idx];
297 struct iio_chan_spec const *pchan = mux->parent->channel;
298 int num_ext_info;
299 int ret;
300
301 chan->indexed = 1;
302 chan->output = pchan->output;
303 chan->datasheet_name = label;
304 chan->ext_info = mux->ext_info;
305
306 ret = iio_get_channel_type(mux->parent, &chan->type);
307 if (ret < 0) {
308 dev_err(dev, "failed to get parent channel type\n");
309 return ret;
310 }
311
312 if (iio_channel_has_info(pchan, IIO_CHAN_INFO_RAW))
313 chan->info_mask_separate |= BIT(IIO_CHAN_INFO_RAW);
314 if (iio_channel_has_info(pchan, IIO_CHAN_INFO_SCALE))
315 chan->info_mask_separate |= BIT(IIO_CHAN_INFO_SCALE);
316
317 if (iio_channel_has_available(pchan, IIO_CHAN_INFO_RAW))
318 chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW);
319
320 if (state >= mux_control_states(mux->control)) {
321 dev_err(dev, "too many channels\n");
322 return -EINVAL;
323 }
324
325 chan->channel = state;
326
327 num_ext_info = iio_get_channel_ext_info_count(mux->parent);
328 if (num_ext_info)
329 return mux_configure_chan_ext_info(dev, mux, idx, num_ext_info);
330
331 return 0;
332 }
333
mux_probe(struct platform_device * pdev)334 static int mux_probe(struct platform_device *pdev)
335 {
336 struct device *dev = &pdev->dev;
337 struct iio_dev *indio_dev;
338 struct iio_channel *parent;
339 struct mux *mux;
340 const char **labels;
341 int all_children;
342 int children;
343 u32 state;
344 int sizeof_ext_info;
345 int sizeof_priv;
346 int i;
347 int ret;
348
349 parent = devm_iio_channel_get(dev, "parent");
350 if (IS_ERR(parent))
351 return dev_err_probe(dev, PTR_ERR(parent),
352 "failed to get parent channel\n");
353
354 sizeof_ext_info = iio_get_channel_ext_info_count(parent);
355 if (sizeof_ext_info) {
356 sizeof_ext_info += 1; /* one extra entry for the sentinel */
357 sizeof_ext_info *= sizeof(*mux->ext_info);
358 }
359
360 all_children = device_property_string_array_count(dev, "channels");
361 if (all_children < 0)
362 return all_children;
363
364 labels = devm_kmalloc_array(dev, all_children, sizeof(*labels), GFP_KERNEL);
365 if (!labels)
366 return -ENOMEM;
367
368 ret = device_property_read_string_array(dev, "channels", labels, all_children);
369 if (ret < 0)
370 return ret;
371
372 children = 0;
373 for (state = 0; state < all_children; state++) {
374 if (*labels[state])
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 mux->delay_us = 0;
401 device_property_read_u32(dev, "settle-time-us", &mux->delay_us);
402
403 indio_dev->name = dev_name(dev);
404 indio_dev->info = &mux_info;
405 indio_dev->modes = INDIO_DIRECT_MODE;
406 indio_dev->channels = mux->chan;
407 indio_dev->num_channels = children;
408 if (sizeof_ext_info) {
409 mux->ext_info = devm_kmemdup(dev,
410 parent->channel->ext_info,
411 sizeof_ext_info, GFP_KERNEL);
412 if (!mux->ext_info)
413 return -ENOMEM;
414
415 for (i = 0; mux->ext_info[i].name; ++i) {
416 if (parent->channel->ext_info[i].read)
417 mux->ext_info[i].read = mux_read_ext_info;
418 if (parent->channel->ext_info[i].write)
419 mux->ext_info[i].write = mux_write_ext_info;
420 mux->ext_info[i].private = i;
421 }
422 }
423
424 mux->control = devm_mux_control_get(dev, NULL);
425 if (IS_ERR(mux->control))
426 return dev_err_probe(dev, PTR_ERR(mux->control),
427 "failed to get control-mux\n");
428
429 i = 0;
430 for (state = 0; state < all_children; state++) {
431 if (!*labels[state])
432 continue;
433
434 ret = mux_configure_channel(dev, mux, state, labels[state], 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 { }
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 MODULE_IMPORT_NS("IIO_CONSUMER");
467