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 <[email protected]>
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/mod_devicetable.h>
15 #include <linux/module.h>
16 #include <linux/mutex.h>
17 #include <linux/mux/consumer.h>
18 #include <linux/platform_device.h>
19 #include <linux/property.h>
20
21 struct mux_ext_info_cache {
22 char *data;
23 ssize_t size;
24 };
25
26 struct mux_child {
27 struct mux_ext_info_cache *ext_info_cache;
28 };
29
30 struct mux {
31 int cached_state;
32 struct mux_control *control;
33 struct iio_channel *parent;
34 struct iio_chan_spec *chan;
35 struct iio_chan_spec_ext_info *ext_info;
36 struct mux_child *child;
37 u32 delay_us;
38 };
39
iio_mux_select(struct mux * mux,int idx)40 static int iio_mux_select(struct mux *mux, int idx)
41 {
42 struct mux_child *child = &mux->child[idx];
43 struct iio_chan_spec const *chan = &mux->chan[idx];
44 int ret;
45 int i;
46
47 ret = mux_control_select_delay(mux->control, chan->channel,
48 mux->delay_us);
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_chan_ext_info(struct device * dev,struct mux * mux,int idx,int num_ext_info)241 static int mux_configure_chan_ext_info(struct device *dev, struct mux *mux,
242 int idx, int num_ext_info)
243 {
244 struct mux_child *child = &mux->child[idx];
245 struct iio_chan_spec const *pchan = mux->parent->channel;
246 int i;
247 int ret;
248
249 char *page __free(kfree) = kzalloc(PAGE_SIZE, GFP_KERNEL);
250 if (!page)
251 return -ENOMEM;
252
253 child->ext_info_cache = devm_kcalloc(dev,
254 num_ext_info,
255 sizeof(*child->ext_info_cache),
256 GFP_KERNEL);
257 if (!child->ext_info_cache)
258 return -ENOMEM;
259
260 for (i = 0; i < num_ext_info; ++i) {
261 child->ext_info_cache[i].size = -1;
262
263 if (!pchan->ext_info[i].write)
264 continue;
265 if (!pchan->ext_info[i].read)
266 continue;
267
268 ret = iio_read_channel_ext_info(mux->parent,
269 mux->ext_info[i].name,
270 page);
271 if (ret < 0) {
272 dev_err(dev, "failed to get ext_info '%s'\n",
273 pchan->ext_info[i].name);
274 return ret;
275 }
276 if (ret >= PAGE_SIZE) {
277 dev_err(dev, "too large ext_info '%s'\n",
278 pchan->ext_info[i].name);
279 return -EINVAL;
280 }
281
282 child->ext_info_cache[i].data = devm_kmemdup(dev, page, ret + 1,
283 GFP_KERNEL);
284 if (!child->ext_info_cache[i].data)
285 return -ENOMEM;
286
287 child->ext_info_cache[i].data[ret] = 0;
288 child->ext_info_cache[i].size = ret;
289 }
290
291 return 0;
292 }
293
mux_configure_channel(struct device * dev,struct mux * mux,u32 state,const char * label,int idx)294 static int mux_configure_channel(struct device *dev, struct mux *mux, u32 state,
295 const char *label, int idx)
296 {
297 struct iio_chan_spec *chan = &mux->chan[idx];
298 struct iio_chan_spec const *pchan = mux->parent->channel;
299 int num_ext_info;
300 int ret;
301
302 chan->indexed = 1;
303 chan->output = pchan->output;
304 chan->datasheet_name = label;
305 chan->ext_info = mux->ext_info;
306
307 ret = iio_get_channel_type(mux->parent, &chan->type);
308 if (ret < 0) {
309 dev_err(dev, "failed to get parent channel type\n");
310 return ret;
311 }
312
313 if (iio_channel_has_info(pchan, IIO_CHAN_INFO_RAW))
314 chan->info_mask_separate |= BIT(IIO_CHAN_INFO_RAW);
315 if (iio_channel_has_info(pchan, IIO_CHAN_INFO_SCALE))
316 chan->info_mask_separate |= BIT(IIO_CHAN_INFO_SCALE);
317
318 if (iio_channel_has_available(pchan, IIO_CHAN_INFO_RAW))
319 chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW);
320
321 if (state >= mux_control_states(mux->control)) {
322 dev_err(dev, "too many channels\n");
323 return -EINVAL;
324 }
325
326 chan->channel = state;
327
328 num_ext_info = iio_get_channel_ext_info_count(mux->parent);
329 if (num_ext_info)
330 return mux_configure_chan_ext_info(dev, mux, idx, num_ext_info);
331
332 return 0;
333 }
334
mux_probe(struct platform_device * pdev)335 static int mux_probe(struct platform_device *pdev)
336 {
337 struct device *dev = &pdev->dev;
338 struct iio_dev *indio_dev;
339 struct iio_channel *parent;
340 struct mux *mux;
341 const char **labels;
342 int all_children;
343 int children;
344 u32 state;
345 int sizeof_ext_info;
346 int sizeof_priv;
347 int i;
348 int ret;
349
350 parent = devm_iio_channel_get(dev, "parent");
351 if (IS_ERR(parent))
352 return dev_err_probe(dev, PTR_ERR(parent),
353 "failed to get parent channel\n");
354
355 sizeof_ext_info = iio_get_channel_ext_info_count(parent);
356 if (sizeof_ext_info) {
357 sizeof_ext_info += 1; /* one extra entry for the sentinel */
358 sizeof_ext_info *= sizeof(*mux->ext_info);
359 }
360
361 all_children = device_property_string_array_count(dev, "channels");
362 if (all_children < 0)
363 return all_children;
364
365 labels = devm_kmalloc_array(dev, all_children, sizeof(*labels), GFP_KERNEL);
366 if (!labels)
367 return -ENOMEM;
368
369 ret = device_property_read_string_array(dev, "channels", labels, all_children);
370 if (ret < 0)
371 return ret;
372
373 children = 0;
374 for (state = 0; state < all_children; state++) {
375 if (*labels[state])
376 children++;
377 }
378 if (children <= 0) {
379 dev_err(dev, "not even a single child\n");
380 return -EINVAL;
381 }
382
383 sizeof_priv = sizeof(*mux);
384 sizeof_priv += sizeof(*mux->child) * children;
385 sizeof_priv += sizeof(*mux->chan) * children;
386 sizeof_priv += sizeof_ext_info;
387
388 indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
389 if (!indio_dev)
390 return -ENOMEM;
391
392 mux = iio_priv(indio_dev);
393 mux->child = (struct mux_child *)(mux + 1);
394 mux->chan = (struct iio_chan_spec *)(mux->child + children);
395
396 platform_set_drvdata(pdev, indio_dev);
397
398 mux->parent = parent;
399 mux->cached_state = -1;
400
401 mux->delay_us = 0;
402 device_property_read_u32(dev, "settle-time-us", &mux->delay_us);
403
404 indio_dev->name = dev_name(dev);
405 indio_dev->info = &mux_info;
406 indio_dev->modes = INDIO_DIRECT_MODE;
407 indio_dev->channels = mux->chan;
408 indio_dev->num_channels = children;
409 if (sizeof_ext_info) {
410 mux->ext_info = devm_kmemdup(dev,
411 parent->channel->ext_info,
412 sizeof_ext_info, GFP_KERNEL);
413 if (!mux->ext_info)
414 return -ENOMEM;
415
416 for (i = 0; mux->ext_info[i].name; ++i) {
417 if (parent->channel->ext_info[i].read)
418 mux->ext_info[i].read = mux_read_ext_info;
419 if (parent->channel->ext_info[i].write)
420 mux->ext_info[i].write = mux_write_ext_info;
421 mux->ext_info[i].private = i;
422 }
423 }
424
425 mux->control = devm_mux_control_get(dev, NULL);
426 if (IS_ERR(mux->control))
427 return dev_err_probe(dev, PTR_ERR(mux->control),
428 "failed to get control-mux\n");
429
430 i = 0;
431 for (state = 0; state < all_children; state++) {
432 if (!*labels[state])
433 continue;
434
435 ret = mux_configure_channel(dev, mux, state, labels[state], i++);
436 if (ret < 0)
437 return ret;
438 }
439
440 ret = devm_iio_device_register(dev, indio_dev);
441 if (ret) {
442 dev_err(dev, "failed to register iio device\n");
443 return ret;
444 }
445
446 return 0;
447 }
448
449 static const struct of_device_id mux_match[] = {
450 { .compatible = "io-channel-mux" },
451 { /* sentinel */ }
452 };
453 MODULE_DEVICE_TABLE(of, mux_match);
454
455 static struct platform_driver mux_driver = {
456 .probe = mux_probe,
457 .driver = {
458 .name = "iio-mux",
459 .of_match_table = mux_match,
460 },
461 };
462 module_platform_driver(mux_driver);
463
464 MODULE_DESCRIPTION("IIO multiplexer driver");
465 MODULE_AUTHOR("Peter Rosin <[email protected]>");
466 MODULE_LICENSE("GPL v2");
467