1 /* 2 * IIO multiplexer driver 3 * 4 * Copyright (C) 2017 Axentia Technologies AB 5 * 6 * Author: Peter Rosin <peda@axentia.se> 7 * 8 * This program is free software; you can redistribute it and/or modify 9 * it under the terms of the GNU General Public License version 2 as 10 * published by the Free Software Foundation. 11 */ 12 13 #include <linux/err.h> 14 #include <linux/iio/consumer.h> 15 #include <linux/iio/iio.h> 16 #include <linux/module.h> 17 #include <linux/mutex.h> 18 #include <linux/mux/consumer.h> 19 #include <linux/of.h> 20 #include <linux/platform_device.h> 21 22 struct mux_ext_info_cache { 23 char *data; 24 ssize_t size; 25 }; 26 27 struct mux_child { 28 struct mux_ext_info_cache *ext_info_cache; 29 }; 30 31 struct mux { 32 int cached_state; 33 struct mux_control *control; 34 struct iio_channel *parent; 35 struct iio_dev *indio_dev; 36 struct iio_chan_spec *chan; 37 struct iio_chan_spec_ext_info *ext_info; 38 struct mux_child *child; 39 }; 40 41 static int iio_mux_select(struct mux *mux, int idx) 42 { 43 struct mux_child *child = &mux->child[idx]; 44 struct iio_chan_spec const *chan = &mux->chan[idx]; 45 int ret; 46 int i; 47 48 ret = mux_control_select(mux->control, chan->channel); 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 83 static void iio_mux_deselect(struct mux *mux) 84 { 85 mux_control_deselect(mux->control); 86 } 87 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 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 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 .driver_module = THIS_MODULE, 177 }; 178 179 static ssize_t mux_read_ext_info(struct iio_dev *indio_dev, uintptr_t private, 180 struct iio_chan_spec const *chan, char *buf) 181 { 182 struct mux *mux = iio_priv(indio_dev); 183 int idx = chan - mux->chan; 184 ssize_t ret; 185 186 ret = iio_mux_select(mux, idx); 187 if (ret < 0) 188 return ret; 189 190 ret = iio_read_channel_ext_info(mux->parent, 191 mux->ext_info[private].name, 192 buf); 193 194 iio_mux_deselect(mux); 195 196 return ret; 197 } 198 199 static ssize_t mux_write_ext_info(struct iio_dev *indio_dev, uintptr_t private, 200 struct iio_chan_spec const *chan, 201 const char *buf, size_t len) 202 { 203 struct device *dev = indio_dev->dev.parent; 204 struct mux *mux = iio_priv(indio_dev); 205 int idx = chan - mux->chan; 206 char *new; 207 ssize_t ret; 208 209 if (len >= PAGE_SIZE) 210 return -EINVAL; 211 212 ret = iio_mux_select(mux, idx); 213 if (ret < 0) 214 return ret; 215 216 new = devm_kmemdup(dev, buf, len + 1, GFP_KERNEL); 217 if (!new) { 218 iio_mux_deselect(mux); 219 return -ENOMEM; 220 } 221 222 new[len] = 0; 223 224 ret = iio_write_channel_ext_info(mux->parent, 225 mux->ext_info[private].name, 226 buf, len); 227 if (ret < 0) { 228 iio_mux_deselect(mux); 229 devm_kfree(dev, new); 230 return ret; 231 } 232 233 devm_kfree(dev, mux->child[idx].ext_info_cache[private].data); 234 mux->child[idx].ext_info_cache[private].data = new; 235 mux->child[idx].ext_info_cache[private].size = len; 236 237 iio_mux_deselect(mux); 238 239 return ret; 240 } 241 242 static int mux_configure_channel(struct device *dev, struct mux *mux, 243 u32 state, const char *label, int idx) 244 { 245 struct mux_child *child = &mux->child[idx]; 246 struct iio_chan_spec *chan = &mux->chan[idx]; 247 struct iio_chan_spec const *pchan = mux->parent->channel; 248 char *page = NULL; 249 int num_ext_info; 250 int i; 251 int ret; 252 253 chan->indexed = 1; 254 chan->output = pchan->output; 255 chan->datasheet_name = label; 256 chan->ext_info = mux->ext_info; 257 258 ret = iio_get_channel_type(mux->parent, &chan->type); 259 if (ret < 0) { 260 dev_err(dev, "failed to get parent channel type\n"); 261 return ret; 262 } 263 264 if (iio_channel_has_info(pchan, IIO_CHAN_INFO_RAW)) 265 chan->info_mask_separate |= BIT(IIO_CHAN_INFO_RAW); 266 if (iio_channel_has_info(pchan, IIO_CHAN_INFO_SCALE)) 267 chan->info_mask_separate |= BIT(IIO_CHAN_INFO_SCALE); 268 269 if (iio_channel_has_available(pchan, IIO_CHAN_INFO_RAW)) 270 chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW); 271 272 if (state >= mux_control_states(mux->control)) { 273 dev_err(dev, "too many channels\n"); 274 return -EINVAL; 275 } 276 277 chan->channel = state; 278 279 num_ext_info = iio_get_channel_ext_info_count(mux->parent); 280 if (num_ext_info) { 281 page = devm_kzalloc(dev, PAGE_SIZE, GFP_KERNEL); 282 if (!page) 283 return -ENOMEM; 284 } 285 child->ext_info_cache = devm_kzalloc(dev, 286 sizeof(*child->ext_info_cache) * 287 num_ext_info, GFP_KERNEL); 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 child->ext_info_cache[i].data[ret] = 0; 313 child->ext_info_cache[i].size = ret; 314 } 315 316 if (page) 317 devm_kfree(dev, page); 318 319 return 0; 320 } 321 322 /* 323 * Same as of_property_for_each_string(), but also keeps track of the 324 * index of each string. 325 */ 326 #define of_property_for_each_string_index(np, propname, prop, s, i) \ 327 for (prop = of_find_property(np, propname, NULL), \ 328 s = of_prop_next_string(prop, NULL), \ 329 i = 0; \ 330 s; \ 331 s = of_prop_next_string(prop, s), \ 332 i++) 333 334 static int mux_probe(struct platform_device *pdev) 335 { 336 struct device *dev = &pdev->dev; 337 struct device_node *np = pdev->dev.of_node; 338 struct iio_dev *indio_dev; 339 struct iio_channel *parent; 340 struct mux *mux; 341 struct property *prop; 342 const char *label; 343 u32 state; 344 int sizeof_ext_info; 345 int children; 346 int sizeof_priv; 347 int i; 348 int ret; 349 350 if (!np) 351 return -ENODEV; 352 353 parent = devm_iio_channel_get(dev, "parent"); 354 if (IS_ERR(parent)) { 355 if (PTR_ERR(parent) != -EPROBE_DEFER) 356 dev_err(dev, "failed to get parent channel\n"); 357 return PTR_ERR(parent); 358 } 359 360 sizeof_ext_info = iio_get_channel_ext_info_count(parent); 361 if (sizeof_ext_info) { 362 sizeof_ext_info += 1; /* one extra entry for the sentinel */ 363 sizeof_ext_info *= sizeof(*mux->ext_info); 364 } 365 366 children = 0; 367 of_property_for_each_string(np, "channels", prop, label) { 368 if (*label) 369 children++; 370 } 371 if (children <= 0) { 372 dev_err(dev, "not even a single child\n"); 373 return -EINVAL; 374 } 375 376 sizeof_priv = sizeof(*mux); 377 sizeof_priv += sizeof(*mux->child) * children; 378 sizeof_priv += sizeof(*mux->chan) * children; 379 sizeof_priv += sizeof_ext_info; 380 381 indio_dev = devm_iio_device_alloc(dev, sizeof_priv); 382 if (!indio_dev) 383 return -ENOMEM; 384 385 mux = iio_priv(indio_dev); 386 mux->child = (struct mux_child *)(mux + 1); 387 mux->chan = (struct iio_chan_spec *)(mux->child + children); 388 389 platform_set_drvdata(pdev, indio_dev); 390 391 mux->parent = parent; 392 mux->cached_state = -1; 393 394 indio_dev->name = dev_name(dev); 395 indio_dev->dev.parent = 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