459 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * IIO multiplexer driver4 *5 * Copyright (C) 2017 Axentia Technologies AB6 *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/mod_devicetable.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 20struct mux_ext_info_cache {21 char *data;22 ssize_t size;23};24 25struct mux_child {26 struct mux_ext_info_cache *ext_info_cache;27};28 29struct 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 39static 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 82static void iio_mux_deselect(struct mux *mux)83{84 mux_control_deselect(mux->control);85}86 87static 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 117static 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 145static 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 171static 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 177static 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 197static 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 240static int mux_configure_channel(struct device *dev, struct mux *mux,241 u32 state, const char *label, int idx)242{243 struct mux_child *child = &mux->child[idx];244 struct iio_chan_spec *chan = &mux->chan[idx];245 struct iio_chan_spec const *pchan = mux->parent->channel;246 char *page = NULL;247 int num_ext_info;248 int i;249 int ret;250 251 chan->indexed = 1;252 chan->output = pchan->output;253 chan->datasheet_name = label;254 chan->ext_info = mux->ext_info;255 256 ret = iio_get_channel_type(mux->parent, &chan->type);257 if (ret < 0) {258 dev_err(dev, "failed to get parent channel type\n");259 return ret;260 }261 262 if (iio_channel_has_info(pchan, IIO_CHAN_INFO_RAW))263 chan->info_mask_separate |= BIT(IIO_CHAN_INFO_RAW);264 if (iio_channel_has_info(pchan, IIO_CHAN_INFO_SCALE))265 chan->info_mask_separate |= BIT(IIO_CHAN_INFO_SCALE);266 267 if (iio_channel_has_available(pchan, IIO_CHAN_INFO_RAW))268 chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW);269 270 if (state >= mux_control_states(mux->control)) {271 dev_err(dev, "too many channels\n");272 return -EINVAL;273 }274 275 chan->channel = state;276 277 num_ext_info = iio_get_channel_ext_info_count(mux->parent);278 if (num_ext_info) {279 page = devm_kzalloc(dev, PAGE_SIZE, GFP_KERNEL);280 if (!page)281 return -ENOMEM;282 }283 child->ext_info_cache = devm_kcalloc(dev,284 num_ext_info,285 sizeof(*child->ext_info_cache),286 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 327static int mux_probe(struct platform_device *pdev)328{329 struct device *dev = &pdev->dev;330 struct iio_dev *indio_dev;331 struct iio_channel *parent;332 struct mux *mux;333 const char **labels;334 int all_children;335 int children;336 u32 state;337 int sizeof_ext_info;338 int sizeof_priv;339 int i;340 int ret;341 342 parent = devm_iio_channel_get(dev, "parent");343 if (IS_ERR(parent))344 return dev_err_probe(dev, PTR_ERR(parent),345 "failed to get parent channel\n");346 347 sizeof_ext_info = iio_get_channel_ext_info_count(parent);348 if (sizeof_ext_info) {349 sizeof_ext_info += 1; /* one extra entry for the sentinel */350 sizeof_ext_info *= sizeof(*mux->ext_info);351 }352 353 all_children = device_property_string_array_count(dev, "channels");354 if (all_children < 0)355 return all_children;356 357 labels = devm_kmalloc_array(dev, all_children, sizeof(*labels), GFP_KERNEL);358 if (!labels)359 return -ENOMEM;360 361 ret = device_property_read_string_array(dev, "channels", labels, all_children);362 if (ret < 0)363 return ret;364 365 children = 0;366 for (state = 0; state < all_children; state++) {367 if (*labels[state])368 children++;369 }370 if (children <= 0) {371 dev_err(dev, "not even a single child\n");372 return -EINVAL;373 }374 375 sizeof_priv = sizeof(*mux);376 sizeof_priv += sizeof(*mux->child) * children;377 sizeof_priv += sizeof(*mux->chan) * children;378 sizeof_priv += sizeof_ext_info;379 380 indio_dev = devm_iio_device_alloc(dev, sizeof_priv);381 if (!indio_dev)382 return -ENOMEM;383 384 mux = iio_priv(indio_dev);385 mux->child = (struct mux_child *)(mux + 1);386 mux->chan = (struct iio_chan_spec *)(mux->child + children);387 388 platform_set_drvdata(pdev, indio_dev);389 390 mux->parent = parent;391 mux->cached_state = -1;392 393 mux->delay_us = 0;394 device_property_read_u32(dev, "settle-time-us", &mux->delay_us);395 396 indio_dev->name = dev_name(dev);397 indio_dev->info = &mux_info;398 indio_dev->modes = INDIO_DIRECT_MODE;399 indio_dev->channels = mux->chan;400 indio_dev->num_channels = children;401 if (sizeof_ext_info) {402 mux->ext_info = devm_kmemdup(dev,403 parent->channel->ext_info,404 sizeof_ext_info, GFP_KERNEL);405 if (!mux->ext_info)406 return -ENOMEM;407 408 for (i = 0; mux->ext_info[i].name; ++i) {409 if (parent->channel->ext_info[i].read)410 mux->ext_info[i].read = mux_read_ext_info;411 if (parent->channel->ext_info[i].write)412 mux->ext_info[i].write = mux_write_ext_info;413 mux->ext_info[i].private = i;414 }415 }416 417 mux->control = devm_mux_control_get(dev, NULL);418 if (IS_ERR(mux->control))419 return dev_err_probe(dev, PTR_ERR(mux->control),420 "failed to get control-mux\n");421 422 i = 0;423 for (state = 0; state < all_children; state++) {424 if (!*labels[state])425 continue;426 427 ret = mux_configure_channel(dev, mux, state, labels[state], i++);428 if (ret < 0)429 return ret;430 }431 432 ret = devm_iio_device_register(dev, indio_dev);433 if (ret) {434 dev_err(dev, "failed to register iio device\n");435 return ret;436 }437 438 return 0;439}440 441static const struct of_device_id mux_match[] = {442 { .compatible = "io-channel-mux" },443 { /* sentinel */ }444};445MODULE_DEVICE_TABLE(of, mux_match);446 447static struct platform_driver mux_driver = {448 .probe = mux_probe,449 .driver = {450 .name = "iio-mux",451 .of_match_table = mux_match,452 },453};454module_platform_driver(mux_driver);455 456MODULE_DESCRIPTION("IIO multiplexer driver");457MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");458MODULE_LICENSE("GPL v2");459