industrialio-buffer.c 35 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423
  1. /* The industrial I/O core
  2. *
  3. * Copyright (c) 2008 Jonathan Cameron
  4. *
  5. * This program is free software; you can redistribute it and/or modify it
  6. * under the terms of the GNU General Public License version 2 as published by
  7. * the Free Software Foundation.
  8. *
  9. * Handling of buffer allocation / resizing.
  10. *
  11. *
  12. * Things to look at here.
  13. * - Better memory allocation techniques?
  14. * - Alternative access techniques?
  15. */
  16. #include <linux/kernel.h>
  17. #include <linux/export.h>
  18. #include <linux/device.h>
  19. #include <linux/fs.h>
  20. #include <linux/cdev.h>
  21. #include <linux/slab.h>
  22. #include <linux/poll.h>
  23. #include <linux/sched.h>
  24. #include <linux/iio/iio.h>
  25. #include "iio_core.h"
  26. #include <linux/iio/sysfs.h>
  27. #include <linux/iio/buffer.h>
  28. static const char * const iio_endian_prefix[] = {
  29. [IIO_BE] = "be",
  30. [IIO_LE] = "le",
  31. };
  32. static bool iio_buffer_is_active(struct iio_buffer *buf)
  33. {
  34. return !list_empty(&buf->buffer_list);
  35. }
  36. static size_t iio_buffer_data_available(struct iio_buffer *buf)
  37. {
  38. return buf->access->data_available(buf);
  39. }
  40. static int iio_buffer_flush_hwfifo(struct iio_dev *indio_dev,
  41. struct iio_buffer *buf, size_t required)
  42. {
  43. if (!indio_dev->info->hwfifo_flush_to_buffer)
  44. return -ENODEV;
  45. return indio_dev->info->hwfifo_flush_to_buffer(indio_dev, required);
  46. }
  47. static bool iio_buffer_ready(struct iio_dev *indio_dev, struct iio_buffer *buf,
  48. size_t to_wait, int to_flush)
  49. {
  50. size_t avail;
  51. int flushed = 0;
  52. /* wakeup if the device was unregistered */
  53. if (!indio_dev->info)
  54. return true;
  55. /* drain the buffer if it was disabled */
  56. if (!iio_buffer_is_active(buf)) {
  57. to_wait = min_t(size_t, to_wait, 1);
  58. to_flush = 0;
  59. }
  60. avail = iio_buffer_data_available(buf);
  61. if (avail >= to_wait) {
  62. /* force a flush for non-blocking reads */
  63. if (!to_wait && avail < to_flush)
  64. iio_buffer_flush_hwfifo(indio_dev, buf,
  65. to_flush - avail);
  66. return true;
  67. }
  68. if (to_flush)
  69. flushed = iio_buffer_flush_hwfifo(indio_dev, buf,
  70. to_wait - avail);
  71. if (flushed <= 0)
  72. return false;
  73. if (avail + flushed >= to_wait)
  74. return true;
  75. return false;
  76. }
  77. /**
  78. * iio_buffer_read_first_n_outer() - chrdev read for buffer access
  79. * @filp: File structure pointer for the char device
  80. * @buf: Destination buffer for iio buffer read
  81. * @n: First n bytes to read
  82. * @f_ps: Long offset provided by the user as a seek position
  83. *
  84. * This function relies on all buffer implementations having an
  85. * iio_buffer as their first element.
  86. *
  87. * Return: negative values corresponding to error codes or ret != 0
  88. * for ending the reading activity
  89. **/
  90. ssize_t iio_buffer_read_first_n_outer(struct file *filp, char __user *buf,
  91. size_t n, loff_t *f_ps)
  92. {
  93. struct iio_dev *indio_dev = filp->private_data;
  94. struct iio_buffer *rb = indio_dev->buffer;
  95. size_t datum_size;
  96. size_t to_wait;
  97. int ret;
  98. if (!indio_dev->info)
  99. return -ENODEV;
  100. if (!rb || !rb->access->read_first_n)
  101. return -EINVAL;
  102. datum_size = rb->bytes_per_datum;
  103. /*
  104. * If datum_size is 0 there will never be anything to read from the
  105. * buffer, so signal end of file now.
  106. */
  107. if (!datum_size)
  108. return 0;
  109. if (filp->f_flags & O_NONBLOCK)
  110. to_wait = 0;
  111. else
  112. to_wait = min_t(size_t, n / datum_size, rb->watermark);
  113. do {
  114. ret = wait_event_interruptible(rb->pollq,
  115. iio_buffer_ready(indio_dev, rb, to_wait, n / datum_size));
  116. if (ret)
  117. return ret;
  118. if (!indio_dev->info)
  119. return -ENODEV;
  120. ret = rb->access->read_first_n(rb, n, buf);
  121. if (ret == 0 && (filp->f_flags & O_NONBLOCK))
  122. ret = -EAGAIN;
  123. } while (ret == 0);
  124. return ret;
  125. }
  126. /**
  127. * iio_buffer_poll() - poll the buffer to find out if it has data
  128. * @filp: File structure pointer for device access
  129. * @wait: Poll table structure pointer for which the driver adds
  130. * a wait queue
  131. *
  132. * Return: (POLLIN | POLLRDNORM) if data is available for reading
  133. * or 0 for other cases
  134. */
  135. unsigned int iio_buffer_poll(struct file *filp,
  136. struct poll_table_struct *wait)
  137. {
  138. struct iio_dev *indio_dev = filp->private_data;
  139. struct iio_buffer *rb = indio_dev->buffer;
  140. if (!indio_dev->info)
  141. return 0;
  142. poll_wait(filp, &rb->pollq, wait);
  143. if (iio_buffer_ready(indio_dev, rb, rb->watermark, 0))
  144. return POLLIN | POLLRDNORM;
  145. return 0;
  146. }
  147. /**
  148. * iio_buffer_wakeup_poll - Wakes up the buffer waitqueue
  149. * @indio_dev: The IIO device
  150. *
  151. * Wakes up the event waitqueue used for poll(). Should usually
  152. * be called when the device is unregistered.
  153. */
  154. void iio_buffer_wakeup_poll(struct iio_dev *indio_dev)
  155. {
  156. if (!indio_dev->buffer)
  157. return;
  158. wake_up(&indio_dev->buffer->pollq);
  159. }
  160. void iio_buffer_init(struct iio_buffer *buffer)
  161. {
  162. INIT_LIST_HEAD(&buffer->demux_list);
  163. INIT_LIST_HEAD(&buffer->buffer_list);
  164. init_waitqueue_head(&buffer->pollq);
  165. kref_init(&buffer->ref);
  166. if (!buffer->watermark)
  167. buffer->watermark = 1;
  168. }
  169. EXPORT_SYMBOL(iio_buffer_init);
  170. static ssize_t iio_show_scan_index(struct device *dev,
  171. struct device_attribute *attr,
  172. char *buf)
  173. {
  174. return sprintf(buf, "%u\n", to_iio_dev_attr(attr)->c->scan_index);
  175. }
  176. static ssize_t iio_show_fixed_type(struct device *dev,
  177. struct device_attribute *attr,
  178. char *buf)
  179. {
  180. struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
  181. u8 type = this_attr->c->scan_type.endianness;
  182. if (type == IIO_CPU) {
  183. #ifdef __LITTLE_ENDIAN
  184. type = IIO_LE;
  185. #else
  186. type = IIO_BE;
  187. #endif
  188. }
  189. if (this_attr->c->scan_type.repeat > 1)
  190. return sprintf(buf, "%s:%c%d/%dX%d>>%u\n",
  191. iio_endian_prefix[type],
  192. this_attr->c->scan_type.sign,
  193. this_attr->c->scan_type.realbits,
  194. this_attr->c->scan_type.storagebits,
  195. this_attr->c->scan_type.repeat,
  196. this_attr->c->scan_type.shift);
  197. else
  198. return sprintf(buf, "%s:%c%d/%d>>%u\n",
  199. iio_endian_prefix[type],
  200. this_attr->c->scan_type.sign,
  201. this_attr->c->scan_type.realbits,
  202. this_attr->c->scan_type.storagebits,
  203. this_attr->c->scan_type.shift);
  204. }
  205. static ssize_t iio_scan_el_show(struct device *dev,
  206. struct device_attribute *attr,
  207. char *buf)
  208. {
  209. int ret;
  210. struct iio_dev *indio_dev = dev_to_iio_dev(dev);
  211. /* Ensure ret is 0 or 1. */
  212. ret = !!test_bit(to_iio_dev_attr(attr)->address,
  213. indio_dev->buffer->scan_mask);
  214. return sprintf(buf, "%d\n", ret);
  215. }
  216. /* Note NULL used as error indicator as it doesn't make sense. */
  217. static const unsigned long *iio_scan_mask_match(const unsigned long *av_masks,
  218. unsigned int masklength,
  219. const unsigned long *mask,
  220. bool strict)
  221. {
  222. if (bitmap_empty(mask, masklength))
  223. return NULL;
  224. while (*av_masks) {
  225. if (strict) {
  226. if (bitmap_equal(mask, av_masks, masklength))
  227. return av_masks;
  228. } else {
  229. if (bitmap_subset(mask, av_masks, masklength))
  230. return av_masks;
  231. }
  232. av_masks += BITS_TO_LONGS(masklength);
  233. }
  234. return NULL;
  235. }
  236. static bool iio_validate_scan_mask(struct iio_dev *indio_dev,
  237. const unsigned long *mask)
  238. {
  239. if (!indio_dev->setup_ops->validate_scan_mask)
  240. return true;
  241. return indio_dev->setup_ops->validate_scan_mask(indio_dev, mask);
  242. }
  243. /**
  244. * iio_scan_mask_set() - set particular bit in the scan mask
  245. * @indio_dev: the iio device
  246. * @buffer: the buffer whose scan mask we are interested in
  247. * @bit: the bit to be set.
  248. *
  249. * Note that at this point we have no way of knowing what other
  250. * buffers might request, hence this code only verifies that the
  251. * individual buffers request is plausible.
  252. */
  253. static int iio_scan_mask_set(struct iio_dev *indio_dev,
  254. struct iio_buffer *buffer, int bit)
  255. {
  256. const unsigned long *mask;
  257. unsigned long *trialmask;
  258. trialmask = kmalloc(sizeof(*trialmask)*
  259. BITS_TO_LONGS(indio_dev->masklength),
  260. GFP_KERNEL);
  261. if (trialmask == NULL)
  262. return -ENOMEM;
  263. if (!indio_dev->masklength) {
  264. WARN(1, "Trying to set scanmask prior to registering buffer\n");
  265. goto err_invalid_mask;
  266. }
  267. bitmap_copy(trialmask, buffer->scan_mask, indio_dev->masklength);
  268. set_bit(bit, trialmask);
  269. if (!iio_validate_scan_mask(indio_dev, trialmask))
  270. goto err_invalid_mask;
  271. if (indio_dev->available_scan_masks) {
  272. mask = iio_scan_mask_match(indio_dev->available_scan_masks,
  273. indio_dev->masklength,
  274. trialmask, false);
  275. if (!mask)
  276. goto err_invalid_mask;
  277. }
  278. bitmap_copy(buffer->scan_mask, trialmask, indio_dev->masklength);
  279. kfree(trialmask);
  280. return 0;
  281. err_invalid_mask:
  282. kfree(trialmask);
  283. return -EINVAL;
  284. }
  285. static int iio_scan_mask_clear(struct iio_buffer *buffer, int bit)
  286. {
  287. clear_bit(bit, buffer->scan_mask);
  288. return 0;
  289. }
  290. static ssize_t iio_scan_el_store(struct device *dev,
  291. struct device_attribute *attr,
  292. const char *buf,
  293. size_t len)
  294. {
  295. int ret;
  296. bool state;
  297. struct iio_dev *indio_dev = dev_to_iio_dev(dev);
  298. struct iio_buffer *buffer = indio_dev->buffer;
  299. struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
  300. ret = strtobool(buf, &state);
  301. if (ret < 0)
  302. return ret;
  303. mutex_lock(&indio_dev->mlock);
  304. if (iio_buffer_is_active(indio_dev->buffer)) {
  305. ret = -EBUSY;
  306. goto error_ret;
  307. }
  308. ret = iio_scan_mask_query(indio_dev, buffer, this_attr->address);
  309. if (ret < 0)
  310. goto error_ret;
  311. if (!state && ret) {
  312. ret = iio_scan_mask_clear(buffer, this_attr->address);
  313. if (ret)
  314. goto error_ret;
  315. } else if (state && !ret) {
  316. ret = iio_scan_mask_set(indio_dev, buffer, this_attr->address);
  317. if (ret)
  318. goto error_ret;
  319. }
  320. error_ret:
  321. mutex_unlock(&indio_dev->mlock);
  322. return ret < 0 ? ret : len;
  323. }
  324. static ssize_t iio_scan_el_ts_show(struct device *dev,
  325. struct device_attribute *attr,
  326. char *buf)
  327. {
  328. struct iio_dev *indio_dev = dev_to_iio_dev(dev);
  329. return sprintf(buf, "%d\n", indio_dev->buffer->scan_timestamp);
  330. }
  331. static ssize_t iio_scan_el_ts_store(struct device *dev,
  332. struct device_attribute *attr,
  333. const char *buf,
  334. size_t len)
  335. {
  336. int ret;
  337. struct iio_dev *indio_dev = dev_to_iio_dev(dev);
  338. bool state;
  339. ret = strtobool(buf, &state);
  340. if (ret < 0)
  341. return ret;
  342. mutex_lock(&indio_dev->mlock);
  343. if (iio_buffer_is_active(indio_dev->buffer)) {
  344. ret = -EBUSY;
  345. goto error_ret;
  346. }
  347. indio_dev->buffer->scan_timestamp = state;
  348. error_ret:
  349. mutex_unlock(&indio_dev->mlock);
  350. return ret ? ret : len;
  351. }
  352. static int iio_buffer_add_channel_sysfs(struct iio_dev *indio_dev,
  353. const struct iio_chan_spec *chan)
  354. {
  355. int ret, attrcount = 0;
  356. struct iio_buffer *buffer = indio_dev->buffer;
  357. ret = __iio_add_chan_devattr("index",
  358. chan,
  359. &iio_show_scan_index,
  360. NULL,
  361. 0,
  362. IIO_SEPARATE,
  363. &indio_dev->dev,
  364. &buffer->scan_el_dev_attr_list);
  365. if (ret)
  366. return ret;
  367. attrcount++;
  368. ret = __iio_add_chan_devattr("type",
  369. chan,
  370. &iio_show_fixed_type,
  371. NULL,
  372. 0,
  373. 0,
  374. &indio_dev->dev,
  375. &buffer->scan_el_dev_attr_list);
  376. if (ret)
  377. return ret;
  378. attrcount++;
  379. if (chan->type != IIO_TIMESTAMP)
  380. ret = __iio_add_chan_devattr("en",
  381. chan,
  382. &iio_scan_el_show,
  383. &iio_scan_el_store,
  384. chan->scan_index,
  385. 0,
  386. &indio_dev->dev,
  387. &buffer->scan_el_dev_attr_list);
  388. else
  389. ret = __iio_add_chan_devattr("en",
  390. chan,
  391. &iio_scan_el_ts_show,
  392. &iio_scan_el_ts_store,
  393. chan->scan_index,
  394. 0,
  395. &indio_dev->dev,
  396. &buffer->scan_el_dev_attr_list);
  397. if (ret)
  398. return ret;
  399. attrcount++;
  400. ret = attrcount;
  401. return ret;
  402. }
  403. static ssize_t iio_buffer_read_length(struct device *dev,
  404. struct device_attribute *attr,
  405. char *buf)
  406. {
  407. struct iio_dev *indio_dev = dev_to_iio_dev(dev);
  408. struct iio_buffer *buffer = indio_dev->buffer;
  409. return sprintf(buf, "%d\n", buffer->length);
  410. }
  411. static ssize_t iio_buffer_write_length(struct device *dev,
  412. struct device_attribute *attr,
  413. const char *buf, size_t len)
  414. {
  415. struct iio_dev *indio_dev = dev_to_iio_dev(dev);
  416. struct iio_buffer *buffer = indio_dev->buffer;
  417. unsigned int val;
  418. int ret;
  419. ret = kstrtouint(buf, 10, &val);
  420. if (ret)
  421. return ret;
  422. if (val == buffer->length)
  423. return len;
  424. mutex_lock(&indio_dev->mlock);
  425. if (iio_buffer_is_active(indio_dev->buffer)) {
  426. ret = -EBUSY;
  427. } else {
  428. buffer->access->set_length(buffer, val);
  429. ret = 0;
  430. }
  431. if (ret)
  432. goto out;
  433. if (buffer->length && buffer->length < buffer->watermark)
  434. buffer->watermark = buffer->length;
  435. out:
  436. mutex_unlock(&indio_dev->mlock);
  437. return ret ? ret : len;
  438. }
  439. static ssize_t iio_buffer_show_enable(struct device *dev,
  440. struct device_attribute *attr,
  441. char *buf)
  442. {
  443. struct iio_dev *indio_dev = dev_to_iio_dev(dev);
  444. return sprintf(buf, "%d\n", iio_buffer_is_active(indio_dev->buffer));
  445. }
  446. static unsigned int iio_storage_bytes_for_si(struct iio_dev *indio_dev,
  447. unsigned int scan_index)
  448. {
  449. const struct iio_chan_spec *ch;
  450. unsigned int bytes;
  451. ch = iio_find_channel_from_si(indio_dev, scan_index);
  452. bytes = ch->scan_type.storagebits / 8;
  453. if (ch->scan_type.repeat > 1)
  454. bytes *= ch->scan_type.repeat;
  455. return bytes;
  456. }
  457. static unsigned int iio_storage_bytes_for_timestamp(struct iio_dev *indio_dev)
  458. {
  459. return iio_storage_bytes_for_si(indio_dev,
  460. indio_dev->scan_index_timestamp);
  461. }
  462. static int iio_compute_scan_bytes(struct iio_dev *indio_dev,
  463. const unsigned long *mask, bool timestamp)
  464. {
  465. unsigned bytes = 0;
  466. int length, i;
  467. /* How much space will the demuxed element take? */
  468. for_each_set_bit(i, mask,
  469. indio_dev->masklength) {
  470. length = iio_storage_bytes_for_si(indio_dev, i);
  471. bytes = ALIGN(bytes, length);
  472. bytes += length;
  473. }
  474. if (timestamp) {
  475. length = iio_storage_bytes_for_timestamp(indio_dev);
  476. bytes = ALIGN(bytes, length);
  477. bytes += length;
  478. }
  479. return bytes;
  480. }
  481. static void iio_buffer_activate(struct iio_dev *indio_dev,
  482. struct iio_buffer *buffer)
  483. {
  484. iio_buffer_get(buffer);
  485. list_add(&buffer->buffer_list, &indio_dev->buffer_list);
  486. }
  487. static void iio_buffer_deactivate(struct iio_buffer *buffer)
  488. {
  489. list_del_init(&buffer->buffer_list);
  490. wake_up_interruptible(&buffer->pollq);
  491. iio_buffer_put(buffer);
  492. }
  493. static void iio_buffer_deactivate_all(struct iio_dev *indio_dev)
  494. {
  495. struct iio_buffer *buffer, *_buffer;
  496. list_for_each_entry_safe(buffer, _buffer,
  497. &indio_dev->buffer_list, buffer_list)
  498. iio_buffer_deactivate(buffer);
  499. }
  500. static int iio_buffer_enable(struct iio_buffer *buffer,
  501. struct iio_dev *indio_dev)
  502. {
  503. if (!buffer->access->enable)
  504. return 0;
  505. return buffer->access->enable(buffer, indio_dev);
  506. }
  507. static int iio_buffer_disable(struct iio_buffer *buffer,
  508. struct iio_dev *indio_dev)
  509. {
  510. if (!buffer->access->disable)
  511. return 0;
  512. return buffer->access->disable(buffer, indio_dev);
  513. }
  514. static void iio_buffer_update_bytes_per_datum(struct iio_dev *indio_dev,
  515. struct iio_buffer *buffer)
  516. {
  517. unsigned int bytes;
  518. if (!buffer->access->set_bytes_per_datum)
  519. return;
  520. bytes = iio_compute_scan_bytes(indio_dev, buffer->scan_mask,
  521. buffer->scan_timestamp);
  522. buffer->access->set_bytes_per_datum(buffer, bytes);
  523. }
  524. static int iio_buffer_request_update(struct iio_dev *indio_dev,
  525. struct iio_buffer *buffer)
  526. {
  527. int ret;
  528. iio_buffer_update_bytes_per_datum(indio_dev, buffer);
  529. if (buffer->access->request_update) {
  530. ret = buffer->access->request_update(buffer);
  531. if (ret) {
  532. dev_dbg(&indio_dev->dev,
  533. "Buffer not started: buffer parameter update failed (%d)\n",
  534. ret);
  535. return ret;
  536. }
  537. }
  538. return 0;
  539. }
  540. static void iio_free_scan_mask(struct iio_dev *indio_dev,
  541. const unsigned long *mask)
  542. {
  543. /* If the mask is dynamically allocated free it, otherwise do nothing */
  544. if (!indio_dev->available_scan_masks)
  545. kfree(mask);
  546. }
  547. struct iio_device_config {
  548. unsigned int mode;
  549. unsigned int watermark;
  550. const unsigned long *scan_mask;
  551. unsigned int scan_bytes;
  552. bool scan_timestamp;
  553. };
  554. static int iio_verify_update(struct iio_dev *indio_dev,
  555. struct iio_buffer *insert_buffer, struct iio_buffer *remove_buffer,
  556. struct iio_device_config *config)
  557. {
  558. unsigned long *compound_mask;
  559. const unsigned long *scan_mask;
  560. bool strict_scanmask = false;
  561. struct iio_buffer *buffer;
  562. bool scan_timestamp;
  563. unsigned int modes;
  564. memset(config, 0, sizeof(*config));
  565. config->watermark = ~0;
  566. /*
  567. * If there is just one buffer and we are removing it there is nothing
  568. * to verify.
  569. */
  570. if (remove_buffer && !insert_buffer &&
  571. list_is_singular(&indio_dev->buffer_list))
  572. return 0;
  573. modes = indio_dev->modes;
  574. list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list) {
  575. if (buffer == remove_buffer)
  576. continue;
  577. modes &= buffer->access->modes;
  578. config->watermark = min(config->watermark, buffer->watermark);
  579. }
  580. if (insert_buffer) {
  581. modes &= insert_buffer->access->modes;
  582. config->watermark = min(config->watermark,
  583. insert_buffer->watermark);
  584. }
  585. /* Definitely possible for devices to support both of these. */
  586. if ((modes & INDIO_BUFFER_TRIGGERED) && indio_dev->trig) {
  587. config->mode = INDIO_BUFFER_TRIGGERED;
  588. } else if (modes & INDIO_BUFFER_HARDWARE) {
  589. /*
  590. * Keep things simple for now and only allow a single buffer to
  591. * be connected in hardware mode.
  592. */
  593. if (insert_buffer && !list_empty(&indio_dev->buffer_list))
  594. return -EINVAL;
  595. config->mode = INDIO_BUFFER_HARDWARE;
  596. strict_scanmask = true;
  597. } else if (modes & INDIO_BUFFER_SOFTWARE) {
  598. config->mode = INDIO_BUFFER_SOFTWARE;
  599. } else {
  600. /* Can only occur on first buffer */
  601. if (indio_dev->modes & INDIO_BUFFER_TRIGGERED)
  602. dev_dbg(&indio_dev->dev, "Buffer not started: no trigger\n");
  603. return -EINVAL;
  604. }
  605. /* What scan mask do we actually have? */
  606. compound_mask = kcalloc(BITS_TO_LONGS(indio_dev->masklength),
  607. sizeof(long), GFP_KERNEL);
  608. if (compound_mask == NULL)
  609. return -ENOMEM;
  610. scan_timestamp = false;
  611. list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list) {
  612. if (buffer == remove_buffer)
  613. continue;
  614. bitmap_or(compound_mask, compound_mask, buffer->scan_mask,
  615. indio_dev->masklength);
  616. scan_timestamp |= buffer->scan_timestamp;
  617. }
  618. if (insert_buffer) {
  619. bitmap_or(compound_mask, compound_mask,
  620. insert_buffer->scan_mask, indio_dev->masklength);
  621. scan_timestamp |= insert_buffer->scan_timestamp;
  622. }
  623. if (indio_dev->available_scan_masks) {
  624. scan_mask = iio_scan_mask_match(indio_dev->available_scan_masks,
  625. indio_dev->masklength,
  626. compound_mask,
  627. strict_scanmask);
  628. kfree(compound_mask);
  629. if (scan_mask == NULL)
  630. return -EINVAL;
  631. } else {
  632. scan_mask = compound_mask;
  633. }
  634. config->scan_bytes = iio_compute_scan_bytes(indio_dev,
  635. scan_mask, scan_timestamp);
  636. config->scan_mask = scan_mask;
  637. config->scan_timestamp = scan_timestamp;
  638. return 0;
  639. }
  640. static int iio_enable_buffers(struct iio_dev *indio_dev,
  641. struct iio_device_config *config)
  642. {
  643. struct iio_buffer *buffer;
  644. int ret;
  645. indio_dev->active_scan_mask = config->scan_mask;
  646. indio_dev->scan_timestamp = config->scan_timestamp;
  647. indio_dev->scan_bytes = config->scan_bytes;
  648. iio_update_demux(indio_dev);
  649. /* Wind up again */
  650. if (indio_dev->setup_ops->preenable) {
  651. ret = indio_dev->setup_ops->preenable(indio_dev);
  652. if (ret) {
  653. dev_dbg(&indio_dev->dev,
  654. "Buffer not started: buffer preenable failed (%d)\n", ret);
  655. goto err_undo_config;
  656. }
  657. }
  658. if (indio_dev->info->update_scan_mode) {
  659. ret = indio_dev->info
  660. ->update_scan_mode(indio_dev,
  661. indio_dev->active_scan_mask);
  662. if (ret < 0) {
  663. dev_dbg(&indio_dev->dev,
  664. "Buffer not started: update scan mode failed (%d)\n",
  665. ret);
  666. goto err_run_postdisable;
  667. }
  668. }
  669. if (indio_dev->info->hwfifo_set_watermark)
  670. indio_dev->info->hwfifo_set_watermark(indio_dev,
  671. config->watermark);
  672. list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list) {
  673. ret = iio_buffer_enable(buffer, indio_dev);
  674. if (ret)
  675. goto err_disable_buffers;
  676. }
  677. indio_dev->currentmode = config->mode;
  678. if (indio_dev->setup_ops->postenable) {
  679. ret = indio_dev->setup_ops->postenable(indio_dev);
  680. if (ret) {
  681. dev_dbg(&indio_dev->dev,
  682. "Buffer not started: postenable failed (%d)\n", ret);
  683. goto err_disable_buffers;
  684. }
  685. }
  686. return 0;
  687. err_disable_buffers:
  688. list_for_each_entry_continue_reverse(buffer, &indio_dev->buffer_list,
  689. buffer_list)
  690. iio_buffer_disable(buffer, indio_dev);
  691. err_run_postdisable:
  692. indio_dev->currentmode = INDIO_DIRECT_MODE;
  693. if (indio_dev->setup_ops->postdisable)
  694. indio_dev->setup_ops->postdisable(indio_dev);
  695. err_undo_config:
  696. indio_dev->active_scan_mask = NULL;
  697. return ret;
  698. }
  699. static int iio_disable_buffers(struct iio_dev *indio_dev)
  700. {
  701. struct iio_buffer *buffer;
  702. int ret = 0;
  703. int ret2;
  704. /* Wind down existing buffers - iff there are any */
  705. if (list_empty(&indio_dev->buffer_list))
  706. return 0;
  707. /*
  708. * If things go wrong at some step in disable we still need to continue
  709. * to perform the other steps, otherwise we leave the device in a
  710. * inconsistent state. We return the error code for the first error we
  711. * encountered.
  712. */
  713. if (indio_dev->setup_ops->predisable) {
  714. ret2 = indio_dev->setup_ops->predisable(indio_dev);
  715. if (ret2 && !ret)
  716. ret = ret2;
  717. }
  718. list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list) {
  719. ret2 = iio_buffer_disable(buffer, indio_dev);
  720. if (ret2 && !ret)
  721. ret = ret2;
  722. }
  723. indio_dev->currentmode = INDIO_DIRECT_MODE;
  724. if (indio_dev->setup_ops->postdisable) {
  725. ret2 = indio_dev->setup_ops->postdisable(indio_dev);
  726. if (ret2 && !ret)
  727. ret = ret2;
  728. }
  729. iio_free_scan_mask(indio_dev, indio_dev->active_scan_mask);
  730. indio_dev->active_scan_mask = NULL;
  731. return ret;
  732. }
  733. static int __iio_update_buffers(struct iio_dev *indio_dev,
  734. struct iio_buffer *insert_buffer,
  735. struct iio_buffer *remove_buffer)
  736. {
  737. struct iio_device_config new_config;
  738. int ret;
  739. ret = iio_verify_update(indio_dev, insert_buffer, remove_buffer,
  740. &new_config);
  741. if (ret)
  742. return ret;
  743. if (insert_buffer) {
  744. ret = iio_buffer_request_update(indio_dev, insert_buffer);
  745. if (ret)
  746. goto err_free_config;
  747. }
  748. ret = iio_disable_buffers(indio_dev);
  749. if (ret)
  750. goto err_deactivate_all;
  751. if (remove_buffer)
  752. iio_buffer_deactivate(remove_buffer);
  753. if (insert_buffer)
  754. iio_buffer_activate(indio_dev, insert_buffer);
  755. /* If no buffers in list, we are done */
  756. if (list_empty(&indio_dev->buffer_list))
  757. return 0;
  758. ret = iio_enable_buffers(indio_dev, &new_config);
  759. if (ret)
  760. goto err_deactivate_all;
  761. return 0;
  762. err_deactivate_all:
  763. /*
  764. * We've already verified that the config is valid earlier. If things go
  765. * wrong in either enable or disable the most likely reason is an IO
  766. * error from the device. In this case there is no good recovery
  767. * strategy. Just make sure to disable everything and leave the device
  768. * in a sane state. With a bit of luck the device might come back to
  769. * life again later and userspace can try again.
  770. */
  771. iio_buffer_deactivate_all(indio_dev);
  772. err_free_config:
  773. iio_free_scan_mask(indio_dev, new_config.scan_mask);
  774. return ret;
  775. }
  776. int iio_update_buffers(struct iio_dev *indio_dev,
  777. struct iio_buffer *insert_buffer,
  778. struct iio_buffer *remove_buffer)
  779. {
  780. int ret;
  781. if (insert_buffer == remove_buffer)
  782. return 0;
  783. mutex_lock(&indio_dev->info_exist_lock);
  784. mutex_lock(&indio_dev->mlock);
  785. if (insert_buffer && iio_buffer_is_active(insert_buffer))
  786. insert_buffer = NULL;
  787. if (remove_buffer && !iio_buffer_is_active(remove_buffer))
  788. remove_buffer = NULL;
  789. if (!insert_buffer && !remove_buffer) {
  790. ret = 0;
  791. goto out_unlock;
  792. }
  793. if (indio_dev->info == NULL) {
  794. ret = -ENODEV;
  795. goto out_unlock;
  796. }
  797. ret = __iio_update_buffers(indio_dev, insert_buffer, remove_buffer);
  798. out_unlock:
  799. mutex_unlock(&indio_dev->mlock);
  800. mutex_unlock(&indio_dev->info_exist_lock);
  801. return ret;
  802. }
  803. EXPORT_SYMBOL_GPL(iio_update_buffers);
  804. void iio_disable_all_buffers(struct iio_dev *indio_dev)
  805. {
  806. iio_disable_buffers(indio_dev);
  807. iio_buffer_deactivate_all(indio_dev);
  808. }
  809. static ssize_t iio_buffer_store_enable(struct device *dev,
  810. struct device_attribute *attr,
  811. const char *buf,
  812. size_t len)
  813. {
  814. int ret;
  815. bool requested_state;
  816. struct iio_dev *indio_dev = dev_to_iio_dev(dev);
  817. bool inlist;
  818. ret = strtobool(buf, &requested_state);
  819. if (ret < 0)
  820. return ret;
  821. mutex_lock(&indio_dev->mlock);
  822. /* Find out if it is in the list */
  823. inlist = iio_buffer_is_active(indio_dev->buffer);
  824. /* Already in desired state */
  825. if (inlist == requested_state)
  826. goto done;
  827. if (requested_state)
  828. ret = __iio_update_buffers(indio_dev,
  829. indio_dev->buffer, NULL);
  830. else
  831. ret = __iio_update_buffers(indio_dev,
  832. NULL, indio_dev->buffer);
  833. done:
  834. mutex_unlock(&indio_dev->mlock);
  835. return (ret < 0) ? ret : len;
  836. }
  837. static const char * const iio_scan_elements_group_name = "scan_elements";
  838. static ssize_t iio_buffer_show_watermark(struct device *dev,
  839. struct device_attribute *attr,
  840. char *buf)
  841. {
  842. struct iio_dev *indio_dev = dev_to_iio_dev(dev);
  843. struct iio_buffer *buffer = indio_dev->buffer;
  844. return sprintf(buf, "%u\n", buffer->watermark);
  845. }
  846. static ssize_t iio_buffer_store_watermark(struct device *dev,
  847. struct device_attribute *attr,
  848. const char *buf,
  849. size_t len)
  850. {
  851. struct iio_dev *indio_dev = dev_to_iio_dev(dev);
  852. struct iio_buffer *buffer = indio_dev->buffer;
  853. unsigned int val;
  854. int ret;
  855. ret = kstrtouint(buf, 10, &val);
  856. if (ret)
  857. return ret;
  858. if (!val)
  859. return -EINVAL;
  860. mutex_lock(&indio_dev->mlock);
  861. if (val > buffer->length) {
  862. ret = -EINVAL;
  863. goto out;
  864. }
  865. if (iio_buffer_is_active(indio_dev->buffer)) {
  866. ret = -EBUSY;
  867. goto out;
  868. }
  869. buffer->watermark = val;
  870. out:
  871. mutex_unlock(&indio_dev->mlock);
  872. return ret ? ret : len;
  873. }
  874. static DEVICE_ATTR(length, S_IRUGO | S_IWUSR, iio_buffer_read_length,
  875. iio_buffer_write_length);
  876. static struct device_attribute dev_attr_length_ro = __ATTR(length,
  877. S_IRUGO, iio_buffer_read_length, NULL);
  878. static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR,
  879. iio_buffer_show_enable, iio_buffer_store_enable);
  880. static DEVICE_ATTR(watermark, S_IRUGO | S_IWUSR,
  881. iio_buffer_show_watermark, iio_buffer_store_watermark);
  882. static struct device_attribute dev_attr_watermark_ro = __ATTR(watermark,
  883. S_IRUGO, iio_buffer_show_watermark, NULL);
  884. static struct attribute *iio_buffer_attrs[] = {
  885. &dev_attr_length.attr,
  886. &dev_attr_enable.attr,
  887. &dev_attr_watermark.attr,
  888. };
  889. int iio_buffer_alloc_sysfs_and_mask(struct iio_dev *indio_dev)
  890. {
  891. struct iio_dev_attr *p;
  892. struct attribute **attr;
  893. struct iio_buffer *buffer = indio_dev->buffer;
  894. int ret, i, attrn, attrcount, attrcount_orig = 0;
  895. const struct iio_chan_spec *channels;
  896. channels = indio_dev->channels;
  897. if (channels) {
  898. int ml = indio_dev->masklength;
  899. for (i = 0; i < indio_dev->num_channels; i++)
  900. ml = max(ml, channels[i].scan_index + 1);
  901. indio_dev->masklength = ml;
  902. }
  903. if (!buffer)
  904. return 0;
  905. attrcount = 0;
  906. if (buffer->attrs) {
  907. while (buffer->attrs[attrcount] != NULL)
  908. attrcount++;
  909. }
  910. attr = kcalloc(attrcount + ARRAY_SIZE(iio_buffer_attrs) + 1,
  911. sizeof(struct attribute *), GFP_KERNEL);
  912. if (!attr)
  913. return -ENOMEM;
  914. memcpy(attr, iio_buffer_attrs, sizeof(iio_buffer_attrs));
  915. if (!buffer->access->set_length)
  916. attr[0] = &dev_attr_length_ro.attr;
  917. if (buffer->access->flags & INDIO_BUFFER_FLAG_FIXED_WATERMARK)
  918. attr[2] = &dev_attr_watermark_ro.attr;
  919. if (buffer->attrs)
  920. memcpy(&attr[ARRAY_SIZE(iio_buffer_attrs)], buffer->attrs,
  921. sizeof(struct attribute *) * attrcount);
  922. attr[attrcount + ARRAY_SIZE(iio_buffer_attrs)] = NULL;
  923. buffer->buffer_group.name = "buffer";
  924. buffer->buffer_group.attrs = attr;
  925. indio_dev->groups[indio_dev->groupcounter++] = &buffer->buffer_group;
  926. if (buffer->scan_el_attrs != NULL) {
  927. attr = buffer->scan_el_attrs->attrs;
  928. while (*attr++ != NULL)
  929. attrcount_orig++;
  930. }
  931. attrcount = attrcount_orig;
  932. INIT_LIST_HEAD(&buffer->scan_el_dev_attr_list);
  933. channels = indio_dev->channels;
  934. if (channels) {
  935. /* new magic */
  936. for (i = 0; i < indio_dev->num_channels; i++) {
  937. if (channels[i].scan_index < 0)
  938. continue;
  939. ret = iio_buffer_add_channel_sysfs(indio_dev,
  940. &channels[i]);
  941. if (ret < 0)
  942. goto error_cleanup_dynamic;
  943. attrcount += ret;
  944. if (channels[i].type == IIO_TIMESTAMP)
  945. indio_dev->scan_index_timestamp =
  946. channels[i].scan_index;
  947. }
  948. if (indio_dev->masklength && buffer->scan_mask == NULL) {
  949. buffer->scan_mask = kcalloc(BITS_TO_LONGS(indio_dev->masklength),
  950. sizeof(*buffer->scan_mask),
  951. GFP_KERNEL);
  952. if (buffer->scan_mask == NULL) {
  953. ret = -ENOMEM;
  954. goto error_cleanup_dynamic;
  955. }
  956. }
  957. }
  958. buffer->scan_el_group.name = iio_scan_elements_group_name;
  959. buffer->scan_el_group.attrs = kcalloc(attrcount + 1,
  960. sizeof(buffer->scan_el_group.attrs[0]),
  961. GFP_KERNEL);
  962. if (buffer->scan_el_group.attrs == NULL) {
  963. ret = -ENOMEM;
  964. goto error_free_scan_mask;
  965. }
  966. if (buffer->scan_el_attrs)
  967. memcpy(buffer->scan_el_group.attrs, buffer->scan_el_attrs,
  968. sizeof(buffer->scan_el_group.attrs[0])*attrcount_orig);
  969. attrn = attrcount_orig;
  970. list_for_each_entry(p, &buffer->scan_el_dev_attr_list, l)
  971. buffer->scan_el_group.attrs[attrn++] = &p->dev_attr.attr;
  972. indio_dev->groups[indio_dev->groupcounter++] = &buffer->scan_el_group;
  973. return 0;
  974. error_free_scan_mask:
  975. kfree(buffer->scan_mask);
  976. error_cleanup_dynamic:
  977. iio_free_chan_devattr_list(&buffer->scan_el_dev_attr_list);
  978. kfree(indio_dev->buffer->buffer_group.attrs);
  979. return ret;
  980. }
  981. void iio_buffer_free_sysfs_and_mask(struct iio_dev *indio_dev)
  982. {
  983. if (!indio_dev->buffer)
  984. return;
  985. kfree(indio_dev->buffer->scan_mask);
  986. kfree(indio_dev->buffer->buffer_group.attrs);
  987. kfree(indio_dev->buffer->scan_el_group.attrs);
  988. iio_free_chan_devattr_list(&indio_dev->buffer->scan_el_dev_attr_list);
  989. }
  990. /**
  991. * iio_validate_scan_mask_onehot() - Validates that exactly one channel is selected
  992. * @indio_dev: the iio device
  993. * @mask: scan mask to be checked
  994. *
  995. * Return true if exactly one bit is set in the scan mask, false otherwise. It
  996. * can be used for devices where only one channel can be active for sampling at
  997. * a time.
  998. */
  999. bool iio_validate_scan_mask_onehot(struct iio_dev *indio_dev,
  1000. const unsigned long *mask)
  1001. {
  1002. return bitmap_weight(mask, indio_dev->masklength) == 1;
  1003. }
  1004. EXPORT_SYMBOL_GPL(iio_validate_scan_mask_onehot);
  1005. int iio_scan_mask_query(struct iio_dev *indio_dev,
  1006. struct iio_buffer *buffer, int bit)
  1007. {
  1008. if (bit > indio_dev->masklength)
  1009. return -EINVAL;
  1010. if (!buffer->scan_mask)
  1011. return 0;
  1012. /* Ensure return value is 0 or 1. */
  1013. return !!test_bit(bit, buffer->scan_mask);
  1014. };
  1015. EXPORT_SYMBOL_GPL(iio_scan_mask_query);
  1016. /**
  1017. * struct iio_demux_table - table describing demux memcpy ops
  1018. * @from: index to copy from
  1019. * @to: index to copy to
  1020. * @length: how many bytes to copy
  1021. * @l: list head used for management
  1022. */
  1023. struct iio_demux_table {
  1024. unsigned from;
  1025. unsigned to;
  1026. unsigned length;
  1027. struct list_head l;
  1028. };
  1029. static const void *iio_demux(struct iio_buffer *buffer,
  1030. const void *datain)
  1031. {
  1032. struct iio_demux_table *t;
  1033. if (list_empty(&buffer->demux_list))
  1034. return datain;
  1035. list_for_each_entry(t, &buffer->demux_list, l)
  1036. memcpy(buffer->demux_bounce + t->to,
  1037. datain + t->from, t->length);
  1038. return buffer->demux_bounce;
  1039. }
  1040. static int iio_push_to_buffer(struct iio_buffer *buffer, const void *data)
  1041. {
  1042. const void *dataout = iio_demux(buffer, data);
  1043. int ret;
  1044. ret = buffer->access->store_to(buffer, dataout);
  1045. if (ret)
  1046. return ret;
  1047. /*
  1048. * We can't just test for watermark to decide if we wake the poll queue
  1049. * because read may request less samples than the watermark.
  1050. */
  1051. wake_up_interruptible_poll(&buffer->pollq, POLLIN | POLLRDNORM);
  1052. return 0;
  1053. }
  1054. static void iio_buffer_demux_free(struct iio_buffer *buffer)
  1055. {
  1056. struct iio_demux_table *p, *q;
  1057. list_for_each_entry_safe(p, q, &buffer->demux_list, l) {
  1058. list_del(&p->l);
  1059. kfree(p);
  1060. }
  1061. }
  1062. int iio_push_to_buffers(struct iio_dev *indio_dev, const void *data)
  1063. {
  1064. int ret;
  1065. struct iio_buffer *buf;
  1066. list_for_each_entry(buf, &indio_dev->buffer_list, buffer_list) {
  1067. ret = iio_push_to_buffer(buf, data);
  1068. if (ret < 0)
  1069. return ret;
  1070. }
  1071. return 0;
  1072. }
  1073. EXPORT_SYMBOL_GPL(iio_push_to_buffers);
  1074. static int iio_buffer_add_demux(struct iio_buffer *buffer,
  1075. struct iio_demux_table **p, unsigned int in_loc, unsigned int out_loc,
  1076. unsigned int length)
  1077. {
  1078. if (*p && (*p)->from + (*p)->length == in_loc &&
  1079. (*p)->to + (*p)->length == out_loc) {
  1080. (*p)->length += length;
  1081. } else {
  1082. *p = kmalloc(sizeof(**p), GFP_KERNEL);
  1083. if (*p == NULL)
  1084. return -ENOMEM;
  1085. (*p)->from = in_loc;
  1086. (*p)->to = out_loc;
  1087. (*p)->length = length;
  1088. list_add_tail(&(*p)->l, &buffer->demux_list);
  1089. }
  1090. return 0;
  1091. }
  1092. static int iio_buffer_update_demux(struct iio_dev *indio_dev,
  1093. struct iio_buffer *buffer)
  1094. {
  1095. int ret, in_ind = -1, out_ind, length;
  1096. unsigned in_loc = 0, out_loc = 0;
  1097. struct iio_demux_table *p = NULL;
  1098. /* Clear out any old demux */
  1099. iio_buffer_demux_free(buffer);
  1100. kfree(buffer->demux_bounce);
  1101. buffer->demux_bounce = NULL;
  1102. /* First work out which scan mode we will actually have */
  1103. if (bitmap_equal(indio_dev->active_scan_mask,
  1104. buffer->scan_mask,
  1105. indio_dev->masklength))
  1106. return 0;
  1107. /* Now we have the two masks, work from least sig and build up sizes */
  1108. for_each_set_bit(out_ind,
  1109. buffer->scan_mask,
  1110. indio_dev->masklength) {
  1111. in_ind = find_next_bit(indio_dev->active_scan_mask,
  1112. indio_dev->masklength,
  1113. in_ind + 1);
  1114. while (in_ind != out_ind) {
  1115. in_ind = find_next_bit(indio_dev->active_scan_mask,
  1116. indio_dev->masklength,
  1117. in_ind + 1);
  1118. length = iio_storage_bytes_for_si(indio_dev, in_ind);
  1119. /* Make sure we are aligned */
  1120. in_loc = roundup(in_loc, length) + length;
  1121. }
  1122. length = iio_storage_bytes_for_si(indio_dev, in_ind);
  1123. out_loc = roundup(out_loc, length);
  1124. in_loc = roundup(in_loc, length);
  1125. ret = iio_buffer_add_demux(buffer, &p, in_loc, out_loc, length);
  1126. if (ret)
  1127. goto error_clear_mux_table;
  1128. out_loc += length;
  1129. in_loc += length;
  1130. }
  1131. /* Relies on scan_timestamp being last */
  1132. if (buffer->scan_timestamp) {
  1133. length = iio_storage_bytes_for_timestamp(indio_dev);
  1134. out_loc = roundup(out_loc, length);
  1135. in_loc = roundup(in_loc, length);
  1136. ret = iio_buffer_add_demux(buffer, &p, in_loc, out_loc, length);
  1137. if (ret)
  1138. goto error_clear_mux_table;
  1139. out_loc += length;
  1140. in_loc += length;
  1141. }
  1142. buffer->demux_bounce = kzalloc(out_loc, GFP_KERNEL);
  1143. if (buffer->demux_bounce == NULL) {
  1144. ret = -ENOMEM;
  1145. goto error_clear_mux_table;
  1146. }
  1147. return 0;
  1148. error_clear_mux_table:
  1149. iio_buffer_demux_free(buffer);
  1150. return ret;
  1151. }
  1152. int iio_update_demux(struct iio_dev *indio_dev)
  1153. {
  1154. struct iio_buffer *buffer;
  1155. int ret;
  1156. list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list) {
  1157. ret = iio_buffer_update_demux(indio_dev, buffer);
  1158. if (ret < 0)
  1159. goto error_clear_mux_table;
  1160. }
  1161. return 0;
  1162. error_clear_mux_table:
  1163. list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list)
  1164. iio_buffer_demux_free(buffer);
  1165. return ret;
  1166. }
  1167. EXPORT_SYMBOL_GPL(iio_update_demux);
  1168. /**
  1169. * iio_buffer_release() - Free a buffer's resources
  1170. * @ref: Pointer to the kref embedded in the iio_buffer struct
  1171. *
  1172. * This function is called when the last reference to the buffer has been
  1173. * dropped. It will typically free all resources allocated by the buffer. Do not
  1174. * call this function manually, always use iio_buffer_put() when done using a
  1175. * buffer.
  1176. */
  1177. static void iio_buffer_release(struct kref *ref)
  1178. {
  1179. struct iio_buffer *buffer = container_of(ref, struct iio_buffer, ref);
  1180. buffer->access->release(buffer);
  1181. }
  1182. /**
  1183. * iio_buffer_get() - Grab a reference to the buffer
  1184. * @buffer: The buffer to grab a reference for, may be NULL
  1185. *
  1186. * Returns the pointer to the buffer that was passed into the function.
  1187. */
  1188. struct iio_buffer *iio_buffer_get(struct iio_buffer *buffer)
  1189. {
  1190. if (buffer)
  1191. kref_get(&buffer->ref);
  1192. return buffer;
  1193. }
  1194. EXPORT_SYMBOL_GPL(iio_buffer_get);
  1195. /**
  1196. * iio_buffer_put() - Release the reference to the buffer
  1197. * @buffer: The buffer to release the reference for, may be NULL
  1198. */
  1199. void iio_buffer_put(struct iio_buffer *buffer)
  1200. {
  1201. if (buffer)
  1202. kref_put(&buffer->ref, iio_buffer_release);
  1203. }
  1204. EXPORT_SYMBOL_GPL(iio_buffer_put);