1 /*
2 * Copyright (c) 2020-2022 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 "hdf_log.h"
10 #include "hdf_wlan_queue.h"
11 #include "securec.h"
12 #include "osal_sem.h"
13 #include "osal_mem.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 HDF_LOGE("%s: Request memory failed", __func__);
41 return NULL;
42 }
43 priorityQueue->priorityLevelCount = priorityLevelCount;
44 for (i = 0; i < priorityLevelCount; i++) {
45 priorityQueue->queues[i] = CreateQueue(queueSize);
46 if (priorityQueue->queues[i] == NULL) {
47 ret = HDF_FAILURE;
48 HDF_LOGE("%s:Init message queue failed!QueueID=%d,ret=%d", __func__, i, ret);
49 break;
50 }
51 }
52 if (ret != HDF_SUCCESS) {
53 DestroyPriorityQueue((PriorityQueue *)priorityQueue);
54 return NULL;
55 }
56 status = OsalSemInit(&priorityQueue->messageSemaphore, 0);
57 if (status != HDF_SUCCESS) {
58 HDF_LOGE("%s: OsalSemInit failed! status=%d", __func__, status);
59 DestroyPriorityQueue((PriorityQueue *)priorityQueue);
60 return NULL;
61 }
62 HDF_LOGD("%s: CreatePriorityQueue finished!", __func__);
63 return (PriorityQueue *)priorityQueue;
64 }
65
DestroyPriorityQueue(PriorityQueue * queue)66 void DestroyPriorityQueue(PriorityQueue *queue)
67 {
68 uint8_t i;
69 HDF_STATUS status;
70 PriorityQueueImpl *queueImpl = (PriorityQueueImpl *)queue;
71 if (queue == NULL) {
72 HDF_LOGE("%s: Input param is null", __func__);
73 return;
74 }
75
76 for (i = 0; i < queueImpl->priorityLevelCount; i++) {
77 if (queueImpl->queues[i] == NULL) {
78 continue;
79 }
80 DestroyQueue(queueImpl->queues[i]);
81 queueImpl->queues[i] = NULL;
82 }
83 status = OsalSemDestroy(&queueImpl->messageSemaphore);
84 if (status != HDF_SUCCESS) {
85 HDF_LOGE("%s:Destroy message queue semaphore failed! status=%d", __func__, status);
86 }
87
88 OsalMemFree(queueImpl);
89 HDF_LOGD("%s: DestroyPriorityQueue finished!", __func__);
90 }
91
PushPriorityQueue(PriorityQueue * queue,const uint8_t priority,void * context)92 int32_t PushPriorityQueue(PriorityQueue *queue, const uint8_t priority, void *context)
93 {
94 uint32_t ret;
95 uint8_t pri;
96 PriorityQueueImpl *queueImpl = NULL;
97 if (queue == NULL || context == NULL) {
98 HDF_LOGE("%s: Input param is null!", __func__);
99 return HDF_FAILURE;
100 }
101 queueImpl = (PriorityQueueImpl *)queue;
102 pri = priority;
103 if (priority >= queueImpl->priorityLevelCount) {
104 pri = queueImpl->priorityLevelCount - 1;
105 }
106
107 ret = PushQueue(queueImpl->queues[pri], context);
108 if (ret != HDF_SUCCESS) {
109 HDF_LOGE("%s:Write queue failed! ret=%d", __func__, ret);
110 return ret;
111 }
112
113 (void)OsalSemPost(&queueImpl->messageSemaphore);
114 HDF_LOGD("%s:PushPriorityQueue successful!", __func__);
115 return HDF_SUCCESS;
116 }
117
PopQueueByPri(PriorityQueueImpl * queue)118 static void *PopQueueByPri(PriorityQueueImpl *queue)
119 {
120 void *context = NULL;
121 uint8_t i;
122 for (i = 0; i < queue->priorityLevelCount; i++) {
123 context = PopQueue(queue->queues[i]);
124 if (context != NULL) {
125 return context;
126 }
127 }
128 return NULL;
129 }
130
PopPriorityQueue(PriorityQueue * queue,uint32_t waitInMS)131 void *PopPriorityQueue(PriorityQueue *queue, uint32_t waitInMS)
132 {
133 PriorityQueueImpl *queueImpl = (PriorityQueueImpl *)queue;
134 void *context = NULL;
135 HDF_STATUS status;
136 if (queue == NULL) {
137 HDF_LOGE("%s: Input param is null!", __func__);
138 return NULL;
139 }
140
141 context = PopQueueByPri(queueImpl);
142 if (context != NULL || waitInMS == 0) {
143 HDF_LOGD("%s: Context isn't null or waitInMs is zero!", __func__);
144 return context;
145 }
146
147 status = OsalSemWait(&queueImpl->messageSemaphore, waitInMS);
148 if (status != HDF_SUCCESS) {
149 return NULL;
150 }
151 return PopQueueByPri(queueImpl);
152 }
153