sbi_fifo.c 3.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184
  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 <plat/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. memset(fifo->queue, 0, 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 bool __sbi_fifo_is_empty(struct sbi_fifo *fifo)
  49. {
  50. return (fifo->avail == 0) ? TRUE : FALSE;
  51. }
  52. bool sbi_fifo_is_empty(struct sbi_fifo *fifo)
  53. {
  54. bool ret;
  55. spin_lock(&fifo->qlock);
  56. ret = __sbi_fifo_is_empty(fifo);
  57. spin_unlock(&fifo->qlock);
  58. return ret;
  59. }
  60. /* Note: must be called with fifo->qlock held */
  61. static inline void __sbi_fifo_reset(struct sbi_fifo *fifo)
  62. {
  63. fifo->avail = 0;
  64. fifo->tail = 0;
  65. memset(fifo->queue, 0, fifo->num_entries * fifo->entry_size);
  66. }
  67. bool sbi_fifo_reset(struct sbi_fifo *fifo)
  68. {
  69. if (!fifo)
  70. return FALSE;
  71. spin_lock(&fifo->qlock);
  72. __sbi_fifo_reset(fifo);
  73. spin_unlock(&fifo->qlock);
  74. return TRUE;
  75. }
  76. /**
  77. * Provide a helper function to do inplace update to the fifo.
  78. * Note: The callback function is called with lock being held.
  79. *
  80. * **Do not** invoke any other fifo function from callback. Otherwise, it will
  81. * lead to deadlock.
  82. */
  83. int sbi_fifo_inplace_update(struct sbi_fifo *fifo, void *in,
  84. int (*fptr)(void *in, void *data))
  85. {
  86. int i, index = 0;
  87. int ret = SBI_FIFO_UNCHANGED;
  88. void *entry;
  89. if (!fifo || !in)
  90. return ret;
  91. spin_lock(&fifo->qlock);
  92. if (__sbi_fifo_is_empty(fifo)) {
  93. spin_unlock(&fifo->qlock);
  94. return ret;
  95. }
  96. for (i = 0; i < fifo->avail; i++) {
  97. index = fifo->tail + i;
  98. if (index >= fifo->num_entries)
  99. index = index - fifo->num_entries;
  100. entry = (void *)fifo->queue + (u32)index * fifo->entry_size;
  101. ret = fptr(in, entry);
  102. if (ret == SBI_FIFO_SKIP || ret == SBI_FIFO_UPDATED) {
  103. break;
  104. } else if (ret == SBI_FIFO_RESET) {
  105. __sbi_fifo_reset(fifo);
  106. break;
  107. }
  108. }
  109. spin_unlock(&fifo->qlock);
  110. return ret;
  111. }
  112. int sbi_fifo_enqueue(struct sbi_fifo *fifo, void *data)
  113. {
  114. u32 head;
  115. if (!fifo || !data)
  116. return SBI_EINVAL;
  117. spin_lock(&fifo->qlock);
  118. if (__sbi_fifo_is_full(fifo)) {
  119. spin_unlock(&fifo->qlock);
  120. return SBI_ENOSPC;
  121. }
  122. head = (u32)fifo->tail + fifo->avail;
  123. if (head >= fifo->num_entries)
  124. head = head - fifo->num_entries;
  125. memcpy(fifo->queue + head * fifo->entry_size, data, fifo->entry_size);
  126. fifo->avail++;
  127. spin_unlock(&fifo->qlock);
  128. return 0;
  129. }
  130. int sbi_fifo_dequeue(struct sbi_fifo *fifo, void *data)
  131. {
  132. if (!fifo || !data)
  133. return SBI_EINVAL;
  134. spin_lock(&fifo->qlock);
  135. if (__sbi_fifo_is_empty(fifo)) {
  136. spin_unlock(&fifo->qlock);
  137. return SBI_ENOENT;
  138. }
  139. memcpy(data, fifo->queue + (u32)fifo->tail * fifo->entry_size,
  140. fifo->entry_size);
  141. fifo->avail--;
  142. fifo->tail++;
  143. if (fifo->tail >= fifo->num_entries)
  144. fifo->tail = 0;
  145. spin_unlock(&fifo->qlock);
  146. return 0;
  147. }