sf_queue.c 4.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202
  1. // SPDX-License-Identifier: GPL-2.0
  2. /*
  3. * Copyright (C) 2022 StarFive Technology Co., Ltd.
  4. */
  5. #include "sf_queue.h"
  6. #include "SF_OMX_Core.h"
  7. SF_Queue* SF_Queue_Create(
  8. OMX_U32 itemCount,
  9. OMX_U32 itemSize
  10. )
  11. {
  12. SF_Queue* queue = NULL;
  13. OMX_U32 size = itemCount*itemSize;
  14. if ((queue=(SF_Queue *)malloc(sizeof(SF_Queue))) == NULL)
  15. return NULL;
  16. queue->size = itemCount;
  17. queue->itemSize = itemSize;
  18. queue->count = 0;
  19. queue->front = 0;
  20. queue->rear = 0;
  21. queue->buffer = (OMX_U8*)malloc(size);
  22. if (pthread_mutex_init(&queue->lock, NULL) < 0) {
  23. free(queue);
  24. LOG(SF_LOG_ERR, "failed to pthread_mutex_init() errno(%d)\n", errno);
  25. return NULL;
  26. }
  27. if (pthread_cond_init(&queue->cond, NULL) < 0) {
  28. free(queue);
  29. LOG(SF_LOG_ERR, "failed to pthread_cond_init() errno(%d)\n", errno);
  30. return NULL;
  31. }
  32. return queue;
  33. }
  34. void SF_Queue_Destroy(
  35. SF_Queue* queue
  36. )
  37. {
  38. if (queue == NULL)
  39. return;
  40. if (queue->buffer)
  41. free(queue->buffer);
  42. free(queue);
  43. }
  44. OMX_ERRORTYPE SF_Queue_Enqueue(
  45. SF_Queue* queue,
  46. void* data
  47. )
  48. {
  49. OMX_U8* ptr;
  50. OMX_U32 offset;
  51. if (queue == NULL) return OMX_ErrorUndefined;
  52. /* SF_Queue is full */
  53. if (queue->count == queue->size)
  54. return OMX_ErrorOverflow;
  55. pthread_mutex_lock(&queue->lock);
  56. offset = queue->rear * queue->itemSize;
  57. ptr = &queue->buffer[offset];
  58. memcpy(ptr, data, queue->itemSize);
  59. queue->rear++;
  60. queue->rear %= queue->size;
  61. queue->count++;
  62. pthread_mutex_unlock(&queue->lock);
  63. pthread_cond_signal(&queue->cond);
  64. return OMX_ErrorNone;
  65. }
  66. void* SF_Queue_Dequeue(
  67. SF_Queue* queue
  68. )
  69. {
  70. void* data;
  71. OMX_U32 offset;
  72. if (queue == NULL)
  73. return NULL;
  74. /* SF_Queue is empty */
  75. if (queue->count == 0)
  76. return NULL;
  77. pthread_mutex_lock(&queue->lock);
  78. offset = queue->front * queue->itemSize;
  79. data = (void*)&queue->buffer[offset];
  80. queue->front++;
  81. queue->front %= queue->size;
  82. queue->count--;
  83. pthread_mutex_unlock(&queue->lock);
  84. return data;
  85. }
  86. void* SF_Queue_Dequeue_Block(
  87. SF_Queue* queue
  88. )
  89. {
  90. void* data;
  91. OMX_U32 offset;
  92. if (queue == NULL)
  93. return NULL;
  94. pthread_mutex_lock(&queue->lock);
  95. /* SF_Queue is empty */
  96. while (queue->count == 0){
  97. pthread_cond_wait(&queue->cond,&queue->lock);
  98. }
  99. offset = queue->front * queue->itemSize;
  100. data = (void*)&queue->buffer[offset];
  101. queue->front++;
  102. queue->front %= queue->size;
  103. queue->count--;
  104. pthread_mutex_unlock(&queue->lock);
  105. return data;
  106. }
  107. void SF_Queue_Flush(
  108. SF_Queue* queue
  109. )
  110. {
  111. if (queue == NULL)
  112. return;
  113. pthread_mutex_lock(&queue->lock);
  114. queue->count = 0;
  115. queue->front = 0;
  116. queue->rear = 0;
  117. pthread_mutex_unlock(&queue->lock);
  118. return;
  119. }
  120. void* SF_Queue_Peek(
  121. SF_Queue* queue
  122. )
  123. {
  124. OMX_U32 offset;
  125. void* temp;
  126. if (queue == NULL)
  127. return NULL;
  128. /* SF_Queue is empty */
  129. if (queue->count == 0)
  130. return NULL;
  131. pthread_mutex_lock(&queue->lock);
  132. offset = queue->front * queue->itemSize;
  133. temp = (void*)&queue->buffer[offset];
  134. pthread_mutex_unlock(&queue->lock);
  135. return temp;
  136. }
  137. OMX_U32 SF_Queue_Get_Cnt(
  138. SF_Queue* queue
  139. )
  140. {
  141. OMX_U32 cnt;
  142. if (queue == NULL)
  143. return 0;
  144. pthread_mutex_lock(&queue->lock);
  145. cnt = queue->count;
  146. pthread_mutex_unlock(&queue->lock);
  147. return cnt;
  148. }
  149. SF_Queue* SF_Queue_Copy(
  150. SF_Queue* dstQ,
  151. SF_Queue* srcQ
  152. )
  153. {
  154. SF_Queue* queue = NULL;
  155. OMX_U32 bufferSize;
  156. if (dstQ == NULL) {
  157. if ((queue=(SF_Queue *)malloc(sizeof(SF_Queue))) == NULL)
  158. return NULL;
  159. memset((void*)queue, 0x00, sizeof(SF_Queue));
  160. }
  161. else {
  162. queue = dstQ;
  163. }
  164. bufferSize = srcQ->size * srcQ->itemSize;
  165. queue->size = srcQ->size;
  166. queue->itemSize = srcQ->itemSize;
  167. queue->count = srcQ->count;
  168. queue->front = srcQ->front;
  169. queue->rear = srcQ->rear;
  170. if (queue->buffer) {
  171. free(queue->buffer);
  172. }
  173. queue->buffer = (OMX_U8*)malloc(bufferSize);
  174. memcpy(queue->buffer, srcQ->buffer, bufferSize);
  175. return queue;
  176. }