iio-mux.c 10.0 KB

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