kfifo.c 12 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. * A generic kernel FIFO implementation
  4. *
  5. * Copyright (C) 2009/2010 Stefani Seibold <stefani@seibold.net>
  6. */
  7. #include <linux/kernel.h>
  8. #include <linux/export.h>
  9. #include <linux/slab.h>
  10. #include <linux/err.h>
  11. #include <linux/log2.h>
  12. #include <linux/uaccess.h>
  13. #include <linux/kfifo.h>
  14. /*
  15. * internal helper to calculate the unused elements in a fifo
  16. */
  17. static inline unsigned int kfifo_unused(struct __kfifo *fifo)
  18. {
  19. return (fifo->mask + 1) - (fifo->in - fifo->out);
  20. }
  21. int __kfifo_alloc(struct __kfifo *fifo, unsigned int size,
  22. size_t esize, gfp_t gfp_mask)
  23. {
  24. /*
  25. * round up to the next power of 2, since our 'let the indices
  26. * wrap' technique works only in this case.
  27. */
  28. size = roundup_pow_of_two(size);
  29. fifo->in = 0;
  30. fifo->out = 0;
  31. fifo->esize = esize;
  32. if (size < 2) {
  33. fifo->data = NULL;
  34. fifo->mask = 0;
  35. return -EINVAL;
  36. }
  37. fifo->data = kmalloc_array(esize, size, gfp_mask);
  38. if (!fifo->data) {
  39. fifo->mask = 0;
  40. return -ENOMEM;
  41. }
  42. fifo->mask = size - 1;
  43. return 0;
  44. }
  45. EXPORT_SYMBOL(__kfifo_alloc);
  46. void __kfifo_free(struct __kfifo *fifo)
  47. {
  48. kfree(fifo->data);
  49. fifo->in = 0;
  50. fifo->out = 0;
  51. fifo->esize = 0;
  52. fifo->data = NULL;
  53. fifo->mask = 0;
  54. }
  55. EXPORT_SYMBOL(__kfifo_free);
  56. int __kfifo_init(struct __kfifo *fifo, void *buffer,
  57. unsigned int size, size_t esize)
  58. {
  59. size /= esize;
  60. if (!is_power_of_2(size))
  61. size = rounddown_pow_of_two(size);
  62. fifo->in = 0;
  63. fifo->out = 0;
  64. fifo->esize = esize;
  65. fifo->data = buffer;
  66. if (size < 2) {
  67. fifo->mask = 0;
  68. return -EINVAL;
  69. }
  70. fifo->mask = size - 1;
  71. return 0;
  72. }
  73. EXPORT_SYMBOL(__kfifo_init);
  74. static void kfifo_copy_in(struct __kfifo *fifo, const void *src,
  75. unsigned int len, unsigned int off)
  76. {
  77. unsigned int size = fifo->mask + 1;
  78. unsigned int esize = fifo->esize;
  79. unsigned int l;
  80. off &= fifo->mask;
  81. if (esize != 1) {
  82. off *= esize;
  83. size *= esize;
  84. len *= esize;
  85. }
  86. l = min(len, size - off);
  87. memcpy(fifo->data + off, src, l);
  88. memcpy(fifo->data, src + l, len - l);
  89. /*
  90. * make sure that the data in the fifo is up to date before
  91. * incrementing the fifo->in index counter
  92. */
  93. smp_wmb();
  94. }
  95. unsigned int __kfifo_in(struct __kfifo *fifo,
  96. const void *buf, unsigned int len)
  97. {
  98. unsigned int l;
  99. l = kfifo_unused(fifo);
  100. if (len > l)
  101. len = l;
  102. kfifo_copy_in(fifo, buf, len, fifo->in);
  103. fifo->in += len;
  104. return len;
  105. }
  106. EXPORT_SYMBOL(__kfifo_in);
  107. static void kfifo_copy_out(struct __kfifo *fifo, void *dst,
  108. unsigned int len, unsigned int off)
  109. {
  110. unsigned int size = fifo->mask + 1;
  111. unsigned int esize = fifo->esize;
  112. unsigned int l;
  113. off &= fifo->mask;
  114. if (esize != 1) {
  115. off *= esize;
  116. size *= esize;
  117. len *= esize;
  118. }
  119. l = min(len, size - off);
  120. memcpy(dst, fifo->data + off, l);
  121. memcpy(dst + l, fifo->data, len - l);
  122. /*
  123. * make sure that the data is copied before
  124. * incrementing the fifo->out index counter
  125. */
  126. smp_wmb();
  127. }
  128. unsigned int __kfifo_out_peek(struct __kfifo *fifo,
  129. void *buf, unsigned int len)
  130. {
  131. unsigned int l;
  132. l = fifo->in - fifo->out;
  133. if (len > l)
  134. len = l;
  135. kfifo_copy_out(fifo, buf, len, fifo->out);
  136. return len;
  137. }
  138. EXPORT_SYMBOL(__kfifo_out_peek);
  139. unsigned int __kfifo_out(struct __kfifo *fifo,
  140. void *buf, unsigned int len)
  141. {
  142. len = __kfifo_out_peek(fifo, buf, len);
  143. fifo->out += len;
  144. return len;
  145. }
  146. EXPORT_SYMBOL(__kfifo_out);
  147. static unsigned long kfifo_copy_from_user(struct __kfifo *fifo,
  148. const void __user *from, unsigned int len, unsigned int off,
  149. unsigned int *copied)
  150. {
  151. unsigned int size = fifo->mask + 1;
  152. unsigned int esize = fifo->esize;
  153. unsigned int l;
  154. unsigned long ret;
  155. off &= fifo->mask;
  156. if (esize != 1) {
  157. off *= esize;
  158. size *= esize;
  159. len *= esize;
  160. }
  161. l = min(len, size - off);
  162. ret = copy_from_user(fifo->data + off, from, l);
  163. if (unlikely(ret))
  164. ret = DIV_ROUND_UP(ret + len - l, esize);
  165. else {
  166. ret = copy_from_user(fifo->data, from + l, len - l);
  167. if (unlikely(ret))
  168. ret = DIV_ROUND_UP(ret, esize);
  169. }
  170. /*
  171. * make sure that the data in the fifo is up to date before
  172. * incrementing the fifo->in index counter
  173. */
  174. smp_wmb();
  175. *copied = len - ret * esize;
  176. /* return the number of elements which are not copied */
  177. return ret;
  178. }
  179. int __kfifo_from_user(struct __kfifo *fifo, const void __user *from,
  180. unsigned long len, unsigned int *copied)
  181. {
  182. unsigned int l;
  183. unsigned long ret;
  184. unsigned int esize = fifo->esize;
  185. int err;
  186. if (esize != 1)
  187. len /= esize;
  188. l = kfifo_unused(fifo);
  189. if (len > l)
  190. len = l;
  191. ret = kfifo_copy_from_user(fifo, from, len, fifo->in, copied);
  192. if (unlikely(ret)) {
  193. len -= ret;
  194. err = -EFAULT;
  195. } else
  196. err = 0;
  197. fifo->in += len;
  198. return err;
  199. }
  200. EXPORT_SYMBOL(__kfifo_from_user);
  201. static unsigned long kfifo_copy_to_user(struct __kfifo *fifo, void __user *to,
  202. unsigned int len, unsigned int off, unsigned int *copied)
  203. {
  204. unsigned int l;
  205. unsigned long ret;
  206. unsigned int size = fifo->mask + 1;
  207. unsigned int esize = fifo->esize;
  208. off &= fifo->mask;
  209. if (esize != 1) {
  210. off *= esize;
  211. size *= esize;
  212. len *= esize;
  213. }
  214. l = min(len, size - off);
  215. ret = copy_to_user(to, fifo->data + off, l);
  216. if (unlikely(ret))
  217. ret = DIV_ROUND_UP(ret + len - l, esize);
  218. else {
  219. ret = copy_to_user(to + l, fifo->data, len - l);
  220. if (unlikely(ret))
  221. ret = DIV_ROUND_UP(ret, esize);
  222. }
  223. /*
  224. * make sure that the data is copied before
  225. * incrementing the fifo->out index counter
  226. */
  227. smp_wmb();
  228. *copied = len - ret * esize;
  229. /* return the number of elements which are not copied */
  230. return ret;
  231. }
  232. int __kfifo_to_user(struct __kfifo *fifo, void __user *to,
  233. unsigned long len, unsigned int *copied)
  234. {
  235. unsigned int l;
  236. unsigned long ret;
  237. unsigned int esize = fifo->esize;
  238. int err;
  239. if (esize != 1)
  240. len /= esize;
  241. l = fifo->in - fifo->out;
  242. if (len > l)
  243. len = l;
  244. ret = kfifo_copy_to_user(fifo, to, len, fifo->out, copied);
  245. if (unlikely(ret)) {
  246. len -= ret;
  247. err = -EFAULT;
  248. } else
  249. err = 0;
  250. fifo->out += len;
  251. return err;
  252. }
  253. EXPORT_SYMBOL(__kfifo_to_user);
  254. static int setup_sgl_buf(struct scatterlist *sgl, void *buf,
  255. int nents, unsigned int len)
  256. {
  257. int n;
  258. unsigned int l;
  259. unsigned int off;
  260. struct page *page;
  261. if (!nents)
  262. return 0;
  263. if (!len)
  264. return 0;
  265. n = 0;
  266. page = virt_to_page(buf);
  267. off = offset_in_page(buf);
  268. l = 0;
  269. while (len >= l + PAGE_SIZE - off) {
  270. struct page *npage;
  271. l += PAGE_SIZE;
  272. buf += PAGE_SIZE;
  273. npage = virt_to_page(buf);
  274. if (page_to_phys(page) != page_to_phys(npage) - l) {
  275. sg_set_page(sgl, page, l - off, off);
  276. sgl = sg_next(sgl);
  277. if (++n == nents || sgl == NULL)
  278. return n;
  279. page = npage;
  280. len -= l - off;
  281. l = off = 0;
  282. }
  283. }
  284. sg_set_page(sgl, page, len, off);
  285. return n + 1;
  286. }
  287. static unsigned int setup_sgl(struct __kfifo *fifo, struct scatterlist *sgl,
  288. int nents, unsigned int len, unsigned int off)
  289. {
  290. unsigned int size = fifo->mask + 1;
  291. unsigned int esize = fifo->esize;
  292. unsigned int l;
  293. unsigned int n;
  294. off &= fifo->mask;
  295. if (esize != 1) {
  296. off *= esize;
  297. size *= esize;
  298. len *= esize;
  299. }
  300. l = min(len, size - off);
  301. n = setup_sgl_buf(sgl, fifo->data + off, nents, l);
  302. n += setup_sgl_buf(sgl + n, fifo->data, nents - n, len - l);
  303. return n;
  304. }
  305. unsigned int __kfifo_dma_in_prepare(struct __kfifo *fifo,
  306. struct scatterlist *sgl, int nents, unsigned int len)
  307. {
  308. unsigned int l;
  309. l = kfifo_unused(fifo);
  310. if (len > l)
  311. len = l;
  312. return setup_sgl(fifo, sgl, nents, len, fifo->in);
  313. }
  314. EXPORT_SYMBOL(__kfifo_dma_in_prepare);
  315. unsigned int __kfifo_dma_out_prepare(struct __kfifo *fifo,
  316. struct scatterlist *sgl, int nents, unsigned int len)
  317. {
  318. unsigned int l;
  319. l = fifo->in - fifo->out;
  320. if (len > l)
  321. len = l;
  322. return setup_sgl(fifo, sgl, nents, len, fifo->out);
  323. }
  324. EXPORT_SYMBOL(__kfifo_dma_out_prepare);
  325. unsigned int __kfifo_max_r(unsigned int len, size_t recsize)
  326. {
  327. unsigned int max = (1 << (recsize << 3)) - 1;
  328. if (len > max)
  329. return max;
  330. return len;
  331. }
  332. EXPORT_SYMBOL(__kfifo_max_r);
  333. #define __KFIFO_PEEK(data, out, mask) \
  334. ((data)[(out) & (mask)])
  335. /*
  336. * __kfifo_peek_n internal helper function for determinate the length of
  337. * the next record in the fifo
  338. */
  339. static unsigned int __kfifo_peek_n(struct __kfifo *fifo, size_t recsize)
  340. {
  341. unsigned int l;
  342. unsigned int mask = fifo->mask;
  343. unsigned char *data = fifo->data;
  344. l = __KFIFO_PEEK(data, fifo->out, mask);
  345. if (--recsize)
  346. l |= __KFIFO_PEEK(data, fifo->out + 1, mask) << 8;
  347. return l;
  348. }
  349. #define __KFIFO_POKE(data, in, mask, val) \
  350. ( \
  351. (data)[(in) & (mask)] = (unsigned char)(val) \
  352. )
  353. /*
  354. * __kfifo_poke_n internal helper function for storeing the length of
  355. * the record into the fifo
  356. */
  357. static void __kfifo_poke_n(struct __kfifo *fifo, unsigned int n, size_t recsize)
  358. {
  359. unsigned int mask = fifo->mask;
  360. unsigned char *data = fifo->data;
  361. __KFIFO_POKE(data, fifo->in, mask, n);
  362. if (recsize > 1)
  363. __KFIFO_POKE(data, fifo->in + 1, mask, n >> 8);
  364. }
  365. unsigned int __kfifo_len_r(struct __kfifo *fifo, size_t recsize)
  366. {
  367. return __kfifo_peek_n(fifo, recsize);
  368. }
  369. EXPORT_SYMBOL(__kfifo_len_r);
  370. unsigned int __kfifo_in_r(struct __kfifo *fifo, const void *buf,
  371. unsigned int len, size_t recsize)
  372. {
  373. if (len + recsize > kfifo_unused(fifo))
  374. return 0;
  375. __kfifo_poke_n(fifo, len, recsize);
  376. kfifo_copy_in(fifo, buf, len, fifo->in + recsize);
  377. fifo->in += len + recsize;
  378. return len;
  379. }
  380. EXPORT_SYMBOL(__kfifo_in_r);
  381. static unsigned int kfifo_out_copy_r(struct __kfifo *fifo,
  382. void *buf, unsigned int len, size_t recsize, unsigned int *n)
  383. {
  384. *n = __kfifo_peek_n(fifo, recsize);
  385. if (len > *n)
  386. len = *n;
  387. kfifo_copy_out(fifo, buf, len, fifo->out + recsize);
  388. return len;
  389. }
  390. unsigned int __kfifo_out_peek_r(struct __kfifo *fifo, void *buf,
  391. unsigned int len, size_t recsize)
  392. {
  393. unsigned int n;
  394. if (fifo->in == fifo->out)
  395. return 0;
  396. return kfifo_out_copy_r(fifo, buf, len, recsize, &n);
  397. }
  398. EXPORT_SYMBOL(__kfifo_out_peek_r);
  399. unsigned int __kfifo_out_r(struct __kfifo *fifo, void *buf,
  400. unsigned int len, size_t recsize)
  401. {
  402. unsigned int n;
  403. if (fifo->in == fifo->out)
  404. return 0;
  405. len = kfifo_out_copy_r(fifo, buf, len, recsize, &n);
  406. fifo->out += n + recsize;
  407. return len;
  408. }
  409. EXPORT_SYMBOL(__kfifo_out_r);
  410. void __kfifo_skip_r(struct __kfifo *fifo, size_t recsize)
  411. {
  412. unsigned int n;
  413. n = __kfifo_peek_n(fifo, recsize);
  414. fifo->out += n + recsize;
  415. }
  416. EXPORT_SYMBOL(__kfifo_skip_r);
  417. int __kfifo_from_user_r(struct __kfifo *fifo, const void __user *from,
  418. unsigned long len, unsigned int *copied, size_t recsize)
  419. {
  420. unsigned long ret;
  421. len = __kfifo_max_r(len, recsize);
  422. if (len + recsize > kfifo_unused(fifo)) {
  423. *copied = 0;
  424. return 0;
  425. }
  426. __kfifo_poke_n(fifo, len, recsize);
  427. ret = kfifo_copy_from_user(fifo, from, len, fifo->in + recsize, copied);
  428. if (unlikely(ret)) {
  429. *copied = 0;
  430. return -EFAULT;
  431. }
  432. fifo->in += len + recsize;
  433. return 0;
  434. }
  435. EXPORT_SYMBOL(__kfifo_from_user_r);
  436. int __kfifo_to_user_r(struct __kfifo *fifo, void __user *to,
  437. unsigned long len, unsigned int *copied, size_t recsize)
  438. {
  439. unsigned long ret;
  440. unsigned int n;
  441. if (fifo->in == fifo->out) {
  442. *copied = 0;
  443. return 0;
  444. }
  445. n = __kfifo_peek_n(fifo, recsize);
  446. if (len > n)
  447. len = n;
  448. ret = kfifo_copy_to_user(fifo, to, len, fifo->out + recsize, copied);
  449. if (unlikely(ret)) {
  450. *copied = 0;
  451. return -EFAULT;
  452. }
  453. fifo->out += n + recsize;
  454. return 0;
  455. }
  456. EXPORT_SYMBOL(__kfifo_to_user_r);
  457. unsigned int __kfifo_dma_in_prepare_r(struct __kfifo *fifo,
  458. struct scatterlist *sgl, int nents, unsigned int len, size_t recsize)
  459. {
  460. BUG_ON(!nents);
  461. len = __kfifo_max_r(len, recsize);
  462. if (len + recsize > kfifo_unused(fifo))
  463. return 0;
  464. return setup_sgl(fifo, sgl, nents, len, fifo->in + recsize);
  465. }
  466. EXPORT_SYMBOL(__kfifo_dma_in_prepare_r);
  467. void __kfifo_dma_in_finish_r(struct __kfifo *fifo,
  468. unsigned int len, size_t recsize)
  469. {
  470. len = __kfifo_max_r(len, recsize);
  471. __kfifo_poke_n(fifo, len, recsize);
  472. fifo->in += len + recsize;
  473. }
  474. EXPORT_SYMBOL(__kfifo_dma_in_finish_r);
  475. unsigned int __kfifo_dma_out_prepare_r(struct __kfifo *fifo,
  476. struct scatterlist *sgl, int nents, unsigned int len, size_t recsize)
  477. {
  478. BUG_ON(!nents);
  479. len = __kfifo_max_r(len, recsize);
  480. if (len + recsize > fifo->in - fifo->out)
  481. return 0;
  482. return setup_sgl(fifo, sgl, nents, len, fifo->out + recsize);
  483. }
  484. EXPORT_SYMBOL(__kfifo_dma_out_prepare_r);
  485. void __kfifo_dma_out_finish_r(struct __kfifo *fifo, size_t recsize)
  486. {
  487. unsigned int len;
  488. len = __kfifo_peek_n(fifo, recsize);
  489. fifo->out += len + recsize;
  490. }
  491. EXPORT_SYMBOL(__kfifo_dma_out_finish_r);