sbi_fifo.c 3.7 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192
  1. /*
  2. * SPDX-License-Identifier: BSD-2-Clause
  3. *
  4. * Copyright (c) 2019 Western Digital Corporation or its affiliates.
  5. *
  6. * Authors:
  7. * Atish Patra<atish.patra@wdc.com>
  8. *
  9. */
  10. #include <sbi/riscv_locks.h>
  11. #include <sbi/sbi_error.h>
  12. #include <sbi/sbi_fifo.h>
  13. #include <sbi/sbi_string.h>
  14. void sbi_fifo_init(struct sbi_fifo *fifo, void *queue_mem, u16 entries,
  15. u16 entry_size)
  16. {
  17. fifo->queue = queue_mem;
  18. fifo->num_entries = entries;
  19. fifo->entry_size = entry_size;
  20. SPIN_LOCK_INIT(fifo->qlock);
  21. fifo->avail = fifo->tail = 0;
  22. sbi_memset(fifo->queue, 0, (size_t)entries * entry_size);
  23. }
  24. /* Note: must be called with fifo->qlock held */
  25. static inline bool __sbi_fifo_is_full(struct sbi_fifo *fifo)
  26. {
  27. return (fifo->avail == fifo->num_entries) ? TRUE : FALSE;
  28. }
  29. u16 sbi_fifo_avail(struct sbi_fifo *fifo)
  30. {
  31. u16 ret;
  32. if (!fifo)
  33. return 0;
  34. spin_lock(&fifo->qlock);
  35. ret = fifo->avail;
  36. spin_unlock(&fifo->qlock);
  37. return ret;
  38. }
  39. bool sbi_fifo_is_full(struct sbi_fifo *fifo)
  40. {
  41. bool ret;
  42. spin_lock(&fifo->qlock);
  43. ret = __sbi_fifo_is_full(fifo);
  44. spin_unlock(&fifo->qlock);
  45. return ret;
  46. }
  47. /* Note: must be called with fifo->qlock held */
  48. static inline void __sbi_fifo_enqueue(struct sbi_fifo *fifo, void *data)
  49. {
  50. u32 head;
  51. head = (u32)fifo->tail + fifo->avail;
  52. if (head >= fifo->num_entries)
  53. head = head - fifo->num_entries;
  54. sbi_memcpy(fifo->queue + head * fifo->entry_size, data, fifo->entry_size);
  55. fifo->avail++;
  56. }
  57. /* Note: must be called with fifo->qlock held */
  58. static inline bool __sbi_fifo_is_empty(struct sbi_fifo *fifo)
  59. {
  60. return (fifo->avail == 0) ? TRUE : FALSE;
  61. }
  62. bool sbi_fifo_is_empty(struct sbi_fifo *fifo)
  63. {
  64. bool ret;
  65. spin_lock(&fifo->qlock);
  66. ret = __sbi_fifo_is_empty(fifo);
  67. spin_unlock(&fifo->qlock);
  68. return ret;
  69. }
  70. /* Note: must be called with fifo->qlock held */
  71. static inline void __sbi_fifo_reset(struct sbi_fifo *fifo)
  72. {
  73. size_t size = (size_t)fifo->num_entries * fifo->entry_size;
  74. fifo->avail = 0;
  75. fifo->tail = 0;
  76. sbi_memset(fifo->queue, 0, size);
  77. }
  78. bool sbi_fifo_reset(struct sbi_fifo *fifo)
  79. {
  80. if (!fifo)
  81. return FALSE;
  82. spin_lock(&fifo->qlock);
  83. __sbi_fifo_reset(fifo);
  84. spin_unlock(&fifo->qlock);
  85. return TRUE;
  86. }
  87. /**
  88. * Provide a helper function to do inplace update to the fifo.
  89. * Note: The callback function is called with lock being held.
  90. *
  91. * **Do not** invoke any other fifo function from callback. Otherwise, it will
  92. * lead to deadlock.
  93. */
  94. int sbi_fifo_inplace_update(struct sbi_fifo *fifo, void *in,
  95. int (*fptr)(void *in, void *data))
  96. {
  97. int i, index;
  98. int ret = SBI_FIFO_UNCHANGED;
  99. void *entry;
  100. if (!fifo || !in)
  101. return ret;
  102. spin_lock(&fifo->qlock);
  103. if (__sbi_fifo_is_empty(fifo)) {
  104. spin_unlock(&fifo->qlock);
  105. return ret;
  106. }
  107. for (i = 0; i < fifo->avail; i++) {
  108. index = fifo->tail + i;
  109. if (index >= fifo->num_entries)
  110. index -= fifo->num_entries;
  111. entry = (void *)fifo->queue + (u32)index * fifo->entry_size;
  112. ret = fptr(in, entry);
  113. if (ret == SBI_FIFO_SKIP || ret == SBI_FIFO_UPDATED) {
  114. break;
  115. }
  116. }
  117. spin_unlock(&fifo->qlock);
  118. return ret;
  119. }
  120. int sbi_fifo_enqueue(struct sbi_fifo *fifo, void *data)
  121. {
  122. if (!fifo || !data)
  123. return SBI_EINVAL;
  124. spin_lock(&fifo->qlock);
  125. if (__sbi_fifo_is_full(fifo)) {
  126. spin_unlock(&fifo->qlock);
  127. return SBI_ENOSPC;
  128. }
  129. __sbi_fifo_enqueue(fifo, data);
  130. spin_unlock(&fifo->qlock);
  131. return 0;
  132. }
  133. int sbi_fifo_dequeue(struct sbi_fifo *fifo, void *data)
  134. {
  135. if (!fifo || !data)
  136. return SBI_EINVAL;
  137. spin_lock(&fifo->qlock);
  138. if (__sbi_fifo_is_empty(fifo)) {
  139. spin_unlock(&fifo->qlock);
  140. return SBI_ENOENT;
  141. }
  142. sbi_memcpy(data, fifo->queue + (u32)fifo->tail * fifo->entry_size,
  143. fifo->entry_size);
  144. fifo->avail--;
  145. fifo->tail++;
  146. if (fifo->tail >= fifo->num_entries)
  147. fifo->tail = 0;
  148. spin_unlock(&fifo->qlock);
  149. return 0;
  150. }