fifo.c 2.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124
  1. /*
  2. * fifo.c
  3. *
  4. * Created on: Jan 17, 2022
  5. * Author: radioman
  6. */
  7. #include "fifo.h"
  8. #include <stdlib.h>
  9. #include <string.h>
  10. void fifo_init(FIFO_HandleTypeDef *fifo, uint8_t *buffer, uint32_t size)
  11. {
  12. fifo->head = fifo->tail = 0;
  13. fifo->state = FIFO_EMPTY;
  14. fifo->size = size;
  15. fifo->data = buffer;
  16. }
  17. FIFO_StateTypeDef fifo_state(FIFO_HandleTypeDef *fifo)
  18. {
  19. return fifo->state;
  20. }
  21. int fifo_size(FIFO_HandleTypeDef *fifo)
  22. {
  23. if(fifo->state == FIFO_FULL)
  24. return fifo->size;
  25. if(fifo->state == FIFO_EMPTY)
  26. return 0;
  27. if(fifo->head >= fifo->tail)
  28. return (fifo->head - fifo->tail);
  29. else
  30. return (fifo->size - fifo->tail + fifo->head);
  31. }
  32. int fifo_read(FIFO_HandleTypeDef *fifo, uint8_t *buf, uint32_t len)
  33. {
  34. if(fifo->state != FIFO_EMPTY)
  35. {
  36. if(fifo->tail >= fifo->head)
  37. {
  38. uint32_t cap_avail = fifo->size - fifo->tail + fifo->head;
  39. if((fifo->size - fifo->tail) >= len)
  40. {
  41. memcpy(buf, &(fifo->data[fifo->tail]), len);
  42. fifo->tail = (fifo->tail + len) % fifo->size;
  43. }
  44. else
  45. {
  46. if(len > cap_avail)
  47. len = cap_avail;
  48. uint32_t blk1_sz = fifo->size - fifo->tail;
  49. memcpy(buf, &(fifo->data[fifo->tail]), blk1_sz);
  50. fifo->tail = 0;
  51. memcpy(&buf[blk1_sz], &(fifo->data[fifo->tail]), (len - blk1_sz));
  52. fifo->tail = (len - blk1_sz) % fifo->size;
  53. }
  54. }
  55. else
  56. {
  57. if(len > (fifo->head - fifo->tail))
  58. len = fifo->head - fifo->tail;
  59. memcpy(buf, &(fifo->data[fifo->tail]), len);
  60. fifo->tail = (fifo->tail + len) % fifo->size;
  61. }
  62. if(fifo->tail == fifo->head)
  63. fifo->state = FIFO_EMPTY;
  64. else
  65. fifo->state = FIFO_READY;
  66. return (len);
  67. }
  68. else
  69. {
  70. return (-1);
  71. }
  72. }
  73. int fifo_write(FIFO_HandleTypeDef *fifo, uint8_t *data, uint32_t len)
  74. {
  75. if(fifo->state != FIFO_FULL)
  76. {
  77. if(fifo->head >= fifo->tail)
  78. {
  79. uint32_t cap_avail = fifo->size - fifo->head + fifo->tail;
  80. if((fifo->size - fifo->head) >= len)
  81. {
  82. memcpy(&(fifo->data[fifo->head]), data, len);
  83. fifo->head = (fifo->head + len) % fifo->size;
  84. }
  85. else
  86. {
  87. if(len > cap_avail)
  88. len = cap_avail;
  89. uint32_t blk1_sz = fifo->size - fifo->head;
  90. memcpy(&(fifo->data[fifo->head]), data, blk1_sz);
  91. fifo->head = 0;
  92. memcpy(&(fifo->data[fifo->head]), &data[blk1_sz], (len - blk1_sz));
  93. fifo->head = (len - blk1_sz) % fifo->size;
  94. }
  95. }
  96. else
  97. {
  98. if(len > (fifo->tail - fifo->head))
  99. len = fifo->tail - fifo->head;
  100. memcpy(&(fifo->data[fifo->head]), data, len);
  101. fifo->head = (fifo->head + len) % fifo->size;
  102. }
  103. if(fifo->head == fifo->tail)
  104. fifo->state = FIFO_FULL;
  105. else
  106. fifo->state = FIFO_READY;
  107. return (len);
  108. }
  109. else
  110. {
  111. return (-1);
  112. }
  113. }