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 return dev_err_probe(dev, PTR_ERR(parent), 359 "failed to get parent channel\n"); 360 361 sizeof_ext_info = iio_get_channel_ext_info_count(parent); 362 if (sizeof_ext_info) { 363 sizeof_ext_info += 1; /* one extra entry for the sentinel */ 364 sizeof_ext_info *= sizeof(*mux->ext_info); 365 } 366 367 children = 0; 368 of_property_for_each_string(np, "channels", prop, label) { 369 if (*label) 370 children++; 371 } 372 if (children <= 0) { 373 dev_err(dev, "not even a single child\n"); 374 return -EINVAL; 375 } 376 377 sizeof_priv = sizeof(*mux); 378 sizeof_priv += sizeof(*mux->child) * children; 379 sizeof_priv += sizeof(*mux->chan) * children; 380 sizeof_priv += sizeof_ext_info; 381 382 indio_dev = devm_iio_device_alloc(dev, sizeof_priv); 383 if (!indio_dev) 384 return -ENOMEM; 385 386 mux = iio_priv(indio_dev); 387 mux->child = (struct mux_child *)(mux + 1); 388 mux->chan = (struct iio_chan_spec *)(mux->child + children); 389 390 platform_set_drvdata(pdev, indio_dev); 391 392 mux->parent = parent; 393 mux->cached_state = -1; 394 395 indio_dev->name = dev_name(dev); 396 indio_dev->info = &mux_info; 397 indio_dev->modes = INDIO_DIRECT_MODE; 398 indio_dev->channels = mux->chan; 399 indio_dev->num_channels = children; 400 if (sizeof_ext_info) { 401 mux->ext_info = devm_kmemdup(dev, 402 parent->channel->ext_info, 403 sizeof_ext_info, GFP_KERNEL); 404 if (!mux->ext_info) 405 return -ENOMEM; 406 407 for (i = 0; mux->ext_info[i].name; ++i) { 408 if (parent->channel->ext_info[i].read) 409 mux->ext_info[i].read = mux_read_ext_info; 410 if (parent->channel->ext_info[i].write) 411 mux->ext_info[i].write = mux_write_ext_info; 412 mux->ext_info[i].private = i; 413 } 414 } 415 416 mux->control = devm_mux_control_get(dev, NULL); 417 if (IS_ERR(mux->control)) { 418 if (PTR_ERR(mux->control) != -EPROBE_DEFER) 419 dev_err(dev, "failed to get control-mux\n"); 420 return PTR_ERR(mux->control); 421 } 422 423 i = 0; 424 of_property_for_each_string_index(np, "channels", prop, label, state) { 425 if (!*label) 426 continue; 427 428 ret = mux_configure_channel(dev, mux, state, label, i++); 429 if (ret < 0) 430 return ret; 431 } 432 433 ret = devm_iio_device_register(dev, indio_dev); 434 if (ret) { 435 dev_err(dev, "failed to register iio device\n"); 436 return ret; 437 } 438 439 return 0; 440 } 441 442 static const struct of_device_id mux_match[] = { 443 { .compatible = "io-channel-mux" }, 444 { /* sentinel */ } 445 }; 446 MODULE_DEVICE_TABLE(of, mux_match); 447 448 static struct platform_driver mux_driver = { 449 .probe = mux_probe, 450 .driver = { 451 .name = "iio-mux", 452 .of_match_table = mux_match, 453 }, 454 }; 455 module_platform_driver(mux_driver); 456 457 MODULE_DESCRIPTION("IIO multiplexer driver"); 458 MODULE_AUTHOR("Peter Rosin <peda@axentia.se>"); 459 MODULE_LICENSE("GPL v2"); 460