1 /*
2 * Copyright (c) 2020-2021 Huawei Device Co., Ltd.
3 *
4 * HDF is dual licensed: you can use it either under the terms of
5 * the GPL, or the BSD license, at your option.
6 * See the LICENSE file in the root of this repository for complete details.
7 */
8 #include "hdf_wlan_priority_queue.h"
9 #include "securec.h"
10 #include "osal/osal_sem.h"
11 #include "osal/osal_mem.h"
12 #include "utils/hdf_log.h"
13 #include "hdf_wlan_queue.h"
14
15 #define MAX_PRIORITY_LEVEL 8
16
17 #define HDF_LOG_TAG HDF_WIFI_CORE
18
19 typedef struct {
20 PriorityQueue priorityQueue;
21 uint8_t priorityLevelCount;
22 OSAL_DECLARE_SEMAPHORE(messageSemaphore);
23 HdfWlanQueue *queues[0];
24 } PriorityQueueImpl;
25
CreatePriorityQueue(uint16_t queueSize,uint8_t priorityLevelCount)26 PriorityQueue *CreatePriorityQueue(uint16_t queueSize, uint8_t priorityLevelCount)
27 {
28 uint8_t i;
29 uint32_t queueMemSize;
30 PriorityQueueImpl *priorityQueue = NULL;
31 uint32_t ret = HDF_SUCCESS;
32 HDF_STATUS status;
33 if (priorityLevelCount > MAX_PRIORITY_LEVEL || priorityLevelCount == 0) {
34 HDF_LOGE("%s:priorityLevelCount must in 1 to 8", __func__);
35 return NULL;
36 }
37 queueMemSize = sizeof(PriorityQueueImpl) + (priorityLevelCount * sizeof(HdfWlanQueue *));
38 priorityQueue = (PriorityQueueImpl *)OsalMemCalloc(queueMemSize);
39 if (priorityQueue == NULL) {
40 return NULL;
41 }
42 priorityQueue->priorityLevelCount = priorityLevelCount;
43 for (i = 0; i < priorityLevelCount; i++) {
44 priorityQueue->queues[i] = CreateQueue(queueSize);
45 if (priorityQueue->queues[i] == NULL) {
46 ret = HDF_FAILURE;
47 HDF_LOGE("%s:Init message queue failed!QueueID=%d,ret=%d", __func__, i, ret);
48 break;
49 }
50 }
51 if (ret != HDF_SUCCESS) {
52 DestroyPriorityQueue((PriorityQueue *)priorityQueue);
53 return NULL;
54 }
55 status = OsalSemInit(&priorityQueue->messageSemaphore, 0);
56 if (status != HDF_SUCCESS) {
57 DestroyPriorityQueue((PriorityQueue *)priorityQueue);
58 return NULL;
59 }
60
61 return (PriorityQueue *)priorityQueue;
62 }
63
DestroyPriorityQueue(PriorityQueue * queue)64 void DestroyPriorityQueue(PriorityQueue *queue)
65 {
66 uint8_t i;
67 HDF_STATUS status;
68 PriorityQueueImpl *queueImpl = (PriorityQueueImpl *)queue;
69 if (queue == NULL) {
70 return;
71 }
72
73 for (i = 0; i < queueImpl->priorityLevelCount; i++) {
74 if (queueImpl->queues[i] == NULL) {
75 continue;
76 }
77 DestroyQueue(queueImpl->queues[i]);
78 queueImpl->queues[i] = NULL;
79 }
80 status = OsalSemDestroy(&queueImpl->messageSemaphore);
81 if (status != HDF_SUCCESS) {
82 HDF_LOGE("%s:Destroy message queue semaphore failed!status=%d", __func__, status);
83 }
84
85 OsalMemFree(queueImpl);
86 }
87
PushPriorityQueue(PriorityQueue * queue,const uint8_t priority,void * context)88 int32_t PushPriorityQueue(PriorityQueue *queue, const uint8_t priority, void *context)
89 {
90 uint32_t ret;
91 uint8_t pri;
92 PriorityQueueImpl *queueImpl = NULL;
93 if (queue == NULL || context == NULL) {
94 return HDF_FAILURE;
95 }
96 queueImpl = (PriorityQueueImpl *)queue;
97 pri = priority;
98 if (priority >= queueImpl->priorityLevelCount) {
99 pri = queueImpl->priorityLevelCount - 1;
100 }
101
102 ret = PushQueue(queueImpl->queues[pri], context);
103 if (ret != HDF_SUCCESS) {
104 HDF_LOGE("%s:Write queue failed!ret=%d", __func__, ret);
105 return ret;
106 }
107
108 (void)OsalSemPost(&queueImpl->messageSemaphore);
109 return HDF_SUCCESS;
110 }
111
PopQueueByPri(PriorityQueueImpl * queue)112 static void *PopQueueByPri(PriorityQueueImpl *queue)
113 {
114 void *context = NULL;
115 uint8_t i;
116 for (i = 0; i < queue->priorityLevelCount; i++) {
117 context = PopQueue(queue->queues[i]);
118 if (context != NULL) {
119 return context;
120 }
121 }
122 return NULL;
123 }
124
PopPriorityQueue(PriorityQueue * queue,uint32_t waitInMS)125 void *PopPriorityQueue(PriorityQueue *queue, uint32_t waitInMS)
126 {
127 PriorityQueueImpl *queueImpl = (PriorityQueueImpl *)queue;
128 void *context = NULL;
129 HDF_STATUS status;
130 if (queue == NULL) {
131 return NULL;
132 }
133
134 context = PopQueueByPri(queueImpl);
135 if (context != NULL || waitInMS == 0) {
136 return context;
137 }
138
139 status = OsalSemWait(&queueImpl->messageSemaphore, waitInMS);
140 if (status != HDF_SUCCESS) {
141 return NULL;
142 }
143 return PopQueueByPri(queueImpl);
144 }
145