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/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 19 struct mux_ext_info_cache { 20 char *data; 21 ssize_t size; 22 }; 23 24 struct mux_child { 25 struct mux_ext_info_cache *ext_info_cache; 26 }; 27 28 struct 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 38 static 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 80 static void iio_mux_deselect(struct mux *mux) 81 { 82 mux_control_deselect(mux->control); 83 } 84 85 static 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 115 static 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 143 static 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 169 static 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 175 static 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 195 static 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 238 static 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 * Same as of_property_for_each_string(), but also keeps track of the 327 * index of each string. 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 337 static 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; /* one extra entry for the sentinel */ 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 445 static const struct of_device_id mux_match[] = { 446 { .compatible = "io-channel-mux" }, 447 { /* sentinel */ } 448 }; 449 MODULE_DEVICE_TABLE(of, mux_match); 450 451 static struct platform_driver mux_driver = { 452 .probe = mux_probe, 453 .driver = { 454 .name = "iio-mux", 455 .of_match_table = mux_match, 456 }, 457 }; 458 module_platform_driver(mux_driver); 459 460 MODULE_DESCRIPTION("IIO multiplexer driver"); 461 MODULE_AUTHOR("Peter Rosin <peda@axentia.se>"); 462 MODULE_LICENSE("GPL v2"); 463