iio-mux.c 9.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459
  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. #include <linux/err.h>
  10. #include <linux/iio/consumer.h>
  11. #include <linux/iio/iio.h>
  12. #include <linux/module.h>
  13. #include <linux/mutex.h>
  14. #include <linux/mux/consumer.h>
  15. #include <linux/of.h>
  16. #include <linux/platform_device.h>
  17. struct mux_ext_info_cache {
  18. char *data;
  19. ssize_t size;
  20. };
  21. struct mux_child {
  22. struct mux_ext_info_cache *ext_info_cache;
  23. };
  24. struct mux {
  25. int cached_state;
  26. struct mux_control *control;
  27. struct iio_channel *parent;
  28. struct iio_dev *indio_dev;
  29. struct iio_chan_spec *chan;
  30. struct iio_chan_spec_ext_info *ext_info;
  31. struct mux_child *child;
  32. };
  33. static int iio_mux_select(struct mux *mux, int idx)
  34. {
  35. struct mux_child *child = &mux->child[idx];
  36. struct iio_chan_spec const *chan = &mux->chan[idx];
  37. int ret;
  38. int i;
  39. ret = mux_control_select(mux->control, chan->channel);
  40. if (ret < 0) {
  41. mux->cached_state = -1;
  42. return ret;
  43. }
  44. if (mux->cached_state == chan->channel)
  45. return 0;
  46. if (chan->ext_info) {
  47. for (i = 0; chan->ext_info[i].name; ++i) {
  48. const char *attr = chan->ext_info[i].name;
  49. struct mux_ext_info_cache *cache;
  50. cache = &child->ext_info_cache[i];
  51. if (cache->size < 0)
  52. continue;
  53. ret = iio_write_channel_ext_info(mux->parent, attr,
  54. cache->data,
  55. cache->size);
  56. if (ret < 0) {
  57. mux_control_deselect(mux->control);
  58. mux->cached_state = -1;
  59. return ret;
  60. }
  61. }
  62. }
  63. mux->cached_state = chan->channel;
  64. return 0;
  65. }
  66. static void iio_mux_deselect(struct mux *mux)
  67. {
  68. mux_control_deselect(mux->control);
  69. }
  70. static int mux_read_raw(struct iio_dev *indio_dev,
  71. struct iio_chan_spec const *chan,
  72. int *val, int *val2, long mask)
  73. {
  74. struct mux *mux = iio_priv(indio_dev);
  75. int idx = chan - mux->chan;
  76. int ret;
  77. ret = iio_mux_select(mux, idx);
  78. if (ret < 0)
  79. return ret;
  80. switch (mask) {
  81. case IIO_CHAN_INFO_RAW:
  82. ret = iio_read_channel_raw(mux->parent, val);
  83. break;
  84. case IIO_CHAN_INFO_SCALE:
  85. ret = iio_read_channel_scale(mux->parent, val, val2);
  86. break;
  87. default:
  88. ret = -EINVAL;
  89. }
  90. iio_mux_deselect(mux);
  91. return ret;
  92. }
  93. static int mux_read_avail(struct iio_dev *indio_dev,
  94. struct iio_chan_spec const *chan,
  95. const int **vals, int *type, int *length,
  96. long mask)
  97. {
  98. struct mux *mux = iio_priv(indio_dev);
  99. int idx = chan - mux->chan;
  100. int ret;
  101. ret = iio_mux_select(mux, idx);
  102. if (ret < 0)
  103. return ret;
  104. switch (mask) {
  105. case IIO_CHAN_INFO_RAW:
  106. *type = IIO_VAL_INT;
  107. ret = iio_read_avail_channel_raw(mux->parent, vals, length);
  108. break;
  109. default:
  110. ret = -EINVAL;
  111. }
  112. iio_mux_deselect(mux);
  113. return ret;
  114. }
  115. static int mux_write_raw(struct iio_dev *indio_dev,
  116. struct iio_chan_spec const *chan,
  117. int val, int val2, long mask)
  118. {
  119. struct mux *mux = iio_priv(indio_dev);
  120. int idx = chan - mux->chan;
  121. int ret;
  122. ret = iio_mux_select(mux, idx);
  123. if (ret < 0)
  124. return ret;
  125. switch (mask) {
  126. case IIO_CHAN_INFO_RAW:
  127. ret = iio_write_channel_raw(mux->parent, val);
  128. break;
  129. default:
  130. ret = -EINVAL;
  131. }
  132. iio_mux_deselect(mux);
  133. return ret;
  134. }
  135. static const struct iio_info mux_info = {
  136. .read_raw = mux_read_raw,
  137. .read_avail = mux_read_avail,
  138. .write_raw = mux_write_raw,
  139. };
  140. static ssize_t mux_read_ext_info(struct iio_dev *indio_dev, uintptr_t private,
  141. struct iio_chan_spec const *chan, char *buf)
  142. {
  143. struct mux *mux = iio_priv(indio_dev);
  144. int idx = chan - mux->chan;
  145. ssize_t ret;
  146. ret = iio_mux_select(mux, idx);
  147. if (ret < 0)
  148. return ret;
  149. ret = iio_read_channel_ext_info(mux->parent,
  150. mux->ext_info[private].name,
  151. buf);
  152. iio_mux_deselect(mux);
  153. return ret;
  154. }
  155. static ssize_t mux_write_ext_info(struct iio_dev *indio_dev, uintptr_t private,
  156. struct iio_chan_spec const *chan,
  157. const char *buf, size_t len)
  158. {
  159. struct device *dev = indio_dev->dev.parent;
  160. struct mux *mux = iio_priv(indio_dev);
  161. int idx = chan - mux->chan;
  162. char *new;
  163. ssize_t ret;
  164. if (len >= PAGE_SIZE)
  165. return -EINVAL;
  166. ret = iio_mux_select(mux, idx);
  167. if (ret < 0)
  168. return ret;
  169. new = devm_kmemdup(dev, buf, len + 1, GFP_KERNEL);
  170. if (!new) {
  171. iio_mux_deselect(mux);
  172. return -ENOMEM;
  173. }
  174. new[len] = 0;
  175. ret = iio_write_channel_ext_info(mux->parent,
  176. mux->ext_info[private].name,
  177. buf, len);
  178. if (ret < 0) {
  179. iio_mux_deselect(mux);
  180. devm_kfree(dev, new);
  181. return ret;
  182. }
  183. devm_kfree(dev, mux->child[idx].ext_info_cache[private].data);
  184. mux->child[idx].ext_info_cache[private].data = new;
  185. mux->child[idx].ext_info_cache[private].size = len;
  186. iio_mux_deselect(mux);
  187. return ret;
  188. }
  189. static int mux_configure_channel(struct device *dev, struct mux *mux,
  190. u32 state, const char *label, int idx)
  191. {
  192. struct mux_child *child = &mux->child[idx];
  193. struct iio_chan_spec *chan = &mux->chan[idx];
  194. struct iio_chan_spec const *pchan = mux->parent->channel;
  195. char *page = NULL;
  196. int num_ext_info;
  197. int i;
  198. int ret;
  199. chan->indexed = 1;
  200. chan->output = pchan->output;
  201. chan->datasheet_name = label;
  202. chan->ext_info = mux->ext_info;
  203. ret = iio_get_channel_type(mux->parent, &chan->type);
  204. if (ret < 0) {
  205. dev_err(dev, "failed to get parent channel type\n");
  206. return ret;
  207. }
  208. if (iio_channel_has_info(pchan, IIO_CHAN_INFO_RAW))
  209. chan->info_mask_separate |= BIT(IIO_CHAN_INFO_RAW);
  210. if (iio_channel_has_info(pchan, IIO_CHAN_INFO_SCALE))
  211. chan->info_mask_separate |= BIT(IIO_CHAN_INFO_SCALE);
  212. if (iio_channel_has_available(pchan, IIO_CHAN_INFO_RAW))
  213. chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW);
  214. if (state >= mux_control_states(mux->control)) {
  215. dev_err(dev, "too many channels\n");
  216. return -EINVAL;
  217. }
  218. chan->channel = state;
  219. num_ext_info = iio_get_channel_ext_info_count(mux->parent);
  220. if (num_ext_info) {
  221. page = devm_kzalloc(dev, PAGE_SIZE, GFP_KERNEL);
  222. if (!page)
  223. return -ENOMEM;
  224. }
  225. child->ext_info_cache = devm_kcalloc(dev,
  226. num_ext_info,
  227. sizeof(*child->ext_info_cache),
  228. GFP_KERNEL);
  229. if (!child->ext_info_cache)
  230. return -ENOMEM;
  231. for (i = 0; i < num_ext_info; ++i) {
  232. child->ext_info_cache[i].size = -1;
  233. if (!pchan->ext_info[i].write)
  234. continue;
  235. if (!pchan->ext_info[i].read)
  236. continue;
  237. ret = iio_read_channel_ext_info(mux->parent,
  238. mux->ext_info[i].name,
  239. page);
  240. if (ret < 0) {
  241. dev_err(dev, "failed to get ext_info '%s'\n",
  242. pchan->ext_info[i].name);
  243. return ret;
  244. }
  245. if (ret >= PAGE_SIZE) {
  246. dev_err(dev, "too large ext_info '%s'\n",
  247. pchan->ext_info[i].name);
  248. return -EINVAL;
  249. }
  250. child->ext_info_cache[i].data = devm_kmemdup(dev, page, ret + 1,
  251. GFP_KERNEL);
  252. if (!child->ext_info_cache[i].data)
  253. return -ENOMEM;
  254. child->ext_info_cache[i].data[ret] = 0;
  255. child->ext_info_cache[i].size = ret;
  256. }
  257. if (page)
  258. devm_kfree(dev, page);
  259. return 0;
  260. }
  261. /*
  262. * Same as of_property_for_each_string(), but also keeps track of the
  263. * index of each string.
  264. */
  265. #define of_property_for_each_string_index(np, propname, prop, s, i) \
  266. for (prop = of_find_property(np, propname, NULL), \
  267. s = of_prop_next_string(prop, NULL), \
  268. i = 0; \
  269. s; \
  270. s = of_prop_next_string(prop, s), \
  271. i++)
  272. static int mux_probe(struct platform_device *pdev)
  273. {
  274. struct device *dev = &pdev->dev;
  275. struct device_node *np = pdev->dev.of_node;
  276. struct iio_dev *indio_dev;
  277. struct iio_channel *parent;
  278. struct mux *mux;
  279. struct property *prop;
  280. const char *label;
  281. u32 state;
  282. int sizeof_ext_info;
  283. int children;
  284. int sizeof_priv;
  285. int i;
  286. int ret;
  287. if (!np)
  288. return -ENODEV;
  289. parent = devm_iio_channel_get(dev, "parent");
  290. if (IS_ERR(parent))
  291. return dev_err_probe(dev, PTR_ERR(parent),
  292. "failed to get parent channel\n");
  293. sizeof_ext_info = iio_get_channel_ext_info_count(parent);
  294. if (sizeof_ext_info) {
  295. sizeof_ext_info += 1; /* one extra entry for the sentinel */
  296. sizeof_ext_info *= sizeof(*mux->ext_info);
  297. }
  298. children = 0;
  299. of_property_for_each_string(np, "channels", prop, label) {
  300. if (*label)
  301. children++;
  302. }
  303. if (children <= 0) {
  304. dev_err(dev, "not even a single child\n");
  305. return -EINVAL;
  306. }
  307. sizeof_priv = sizeof(*mux);
  308. sizeof_priv += sizeof(*mux->child) * children;
  309. sizeof_priv += sizeof(*mux->chan) * children;
  310. sizeof_priv += sizeof_ext_info;
  311. indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
  312. if (!indio_dev)
  313. return -ENOMEM;
  314. mux = iio_priv(indio_dev);
  315. mux->child = (struct mux_child *)(mux + 1);
  316. mux->chan = (struct iio_chan_spec *)(mux->child + children);
  317. platform_set_drvdata(pdev, indio_dev);
  318. mux->parent = parent;
  319. mux->cached_state = -1;
  320. indio_dev->name = dev_name(dev);
  321. indio_dev->info = &mux_info;
  322. indio_dev->modes = INDIO_DIRECT_MODE;
  323. indio_dev->channels = mux->chan;
  324. indio_dev->num_channels = children;
  325. if (sizeof_ext_info) {
  326. mux->ext_info = devm_kmemdup(dev,
  327. parent->channel->ext_info,
  328. sizeof_ext_info, GFP_KERNEL);
  329. if (!mux->ext_info)
  330. return -ENOMEM;
  331. for (i = 0; mux->ext_info[i].name; ++i) {
  332. if (parent->channel->ext_info[i].read)
  333. mux->ext_info[i].read = mux_read_ext_info;
  334. if (parent->channel->ext_info[i].write)
  335. mux->ext_info[i].write = mux_write_ext_info;
  336. mux->ext_info[i].private = i;
  337. }
  338. }
  339. mux->control = devm_mux_control_get(dev, NULL);
  340. if (IS_ERR(mux->control)) {
  341. if (PTR_ERR(mux->control) != -EPROBE_DEFER)
  342. dev_err(dev, "failed to get control-mux\n");
  343. return PTR_ERR(mux->control);
  344. }
  345. i = 0;
  346. of_property_for_each_string_index(np, "channels", prop, label, state) {
  347. if (!*label)
  348. continue;
  349. ret = mux_configure_channel(dev, mux, state, label, i++);
  350. if (ret < 0)
  351. return ret;
  352. }
  353. ret = devm_iio_device_register(dev, indio_dev);
  354. if (ret) {
  355. dev_err(dev, "failed to register iio device\n");
  356. return ret;
  357. }
  358. return 0;
  359. }
  360. static const struct of_device_id mux_match[] = {
  361. { .compatible = "io-channel-mux" },
  362. { /* sentinel */ }
  363. };
  364. MODULE_DEVICE_TABLE(of, mux_match);
  365. static struct platform_driver mux_driver = {
  366. .probe = mux_probe,
  367. .driver = {
  368. .name = "iio-mux",
  369. .of_match_table = mux_match,
  370. },
  371. };
  372. module_platform_driver(mux_driver);
  373. MODULE_DESCRIPTION("IIO multiplexer driver");
  374. MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
  375. MODULE_LICENSE("GPL v2");