1 /*
2 * Copyright (c) 2009-2022 Huawei Technologies Co., Ltd. All rights reserved.
3 *
4 * UniProton is licensed under Mulan PSL v2.
5 * You can use this software according to the terms and conditions of the Mulan PSL v2.
6 * You may obtain a copy of Mulan PSL v2 at:
7 * http://license.coscl.org.cn/MulanPSL2
8 * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
9 * EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
10 * MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
11 * See the Mulan PSL v2 for more details.
12 * Create: 2009-12-22
13 * Description: 队列函数实现
14 */
15 #include "prt_queue_external.h"
16 #include "prt_task_external.h"
17 #include "prt_asm_cpu_external.h"
18
OsGetSrcPid(void)19 OS_SEC_ALW_INLINE INLINE U32 OsGetSrcPid(void)
20 {
21 U32 srcPid;
22
23 if (OS_HWI_ACTIVE) {
24 /* 硬中断创建消息不具体区别中断号 */
25 srcPid = COMPOSE_PID(OsGetHwThreadId(), OS_HWI_HANDLE);
26 } else {
27 srcPid = RUNNING_TASK->taskPid;
28 }
29
30 return srcPid;
31 }
32
33 /*
34 * 描述：内部Pend操作，这个函数在调用之前必须关中断。
35 */
OsInnerPend(U16 * count,struct TagListObject * pendList,U32 timeOut)36 OS_SEC_ALW_INLINE INLINE U32 OsInnerPend(U16 *count, struct TagListObject *pendList, U32 timeOut)
37 {
38 struct TagTskCb *runTsk = NULL;
39
40 /* 判断是否需要阻塞 */
41 if (*count > 0) {
42 (*count)--;
43 return OS_OK;
44 }
45
46 /* 阻塞任务 */
47 if (timeOut == OS_QUEUE_NO_WAIT) {
48 return OS_ERRNO_QUEUE_NO_SOURCE;
49 }
50
51 if (OS_INT_ACTIVE) {
52 return OS_ERRNO_QUEUE_IN_INTERRUPT;
53 }
54
55 /* 如果锁任务的情况下 */
56 if (OS_TASK_LOCK_DATA != 0) {
57 return OS_ERRNO_QUEUE_PEND_IN_LOCK;
58 }
59
60 /* 利用局部变量 runTsk 完成对任务控制块的相关操作，不修改 RUNNING_TASK */
61 runTsk = (struct TagTskCb *)RUNNING_TASK;
62
63 /* 从任务的Ready list上把当前任务删除，添加到pend list上 */
64 OsTskReadyDel(runTsk);
65
66 TSK_STATUS_SET(runTsk, OS_TSK_QUEUE_PEND);
67 ListTailAdd(&runTsk->pendList, pendList);
68
69 /* 如果timeOut > 0,timeOut为等待时间，如果timeOut == OS_QUEUE_WAIT_FOREVER，表示永久等待 */
70 if (timeOut != OS_QUEUE_WAIT_FOREVER) {
71 /* 如果不是永久等待则将任务挂到计时器链表中，设置OS_TSK_TIMEOUT是为了判断是否等待超时 */
72 TSK_STATUS_SET(runTsk, OS_TSK_TIMEOUT);
73 OsTskTimerAdd(runTsk, timeOut);
74 }
75
76 /* 调用函数之前已经关中断，此处关中断进行调度 */
77 /* 触发任务调度 */
78 OsTskSchedule();
79 TSK_STATUS_CLEAR(runTsk, OS_TSK_QUEUE_BUSY);
80
81 /* 判断是否是等待队列超时 */
82 if ((runTsk->taskStatus & OS_TSK_TIMEOUT) != 0) {
83 TSK_STATUS_CLEAR(runTsk, OS_TSK_TIMEOUT);
84
85 /* 在函数的外面会开中断 */
86 return OS_ERRNO_QUEUE_TIMEOUT;
87 }
88 /* 在函数的外面会开中断 */
89 return OS_OK;
90 }
91
OsQueuePendNeedProc(struct TagListObject * objectList)92 OS_SEC_ALW_INLINE INLINE bool OsQueuePendNeedProc(struct TagListObject *objectList)
93 {
94 struct TagTskCb *resumedTask = NULL;
95
96 /* 判断是否有任务阻塞于该队列 */
97 if (ListEmpty(objectList)) {
98 return FALSE;
99 }
100
101 /* 激活阻塞在该队列的首个任务 */
102 resumedTask = GET_TCB_PEND(OS_LIST_FIRST(objectList));
103 ListDelete(OS_LIST_FIRST(objectList));
104
105 /* 去除该任务的队列阻塞位 */
106 TSK_STATUS_CLEAR(resumedTask, OS_TSK_QUEUE_PEND);
107 /* 如果阻塞的任务属于定时等待的任务时候，去掉其定时等待标志位，并将其从去除 */
108 if ((resumedTask->taskStatus & OS_TSK_TIMEOUT) != 0) {
109 /*
110 * 添加PEND状态时，会加上TIMEOUT标志和timer，或者都不加。
111 * 所以此时有TIMEOUT标志就一定有timer，且只有一个，且只用于该PEND方式
112 */
113 OS_TSK_DELAY_LOCKED_DETACH(resumedTask);
114 TSK_STATUS_CLEAR(resumedTask, OS_TSK_TIMEOUT);
115 }
116
117 TSK_STATUS_SET(resumedTask, OS_TSK_QUEUE_BUSY);
118
119 /* 如果去除队列阻塞位后，该任务不处于挂起态则将该任务挂入就绪队列并触发任务调度 */
120 if ((resumedTask->taskStatus & OS_TSK_SUSPEND) == 0) {
121 OsTskReadyAddBgd(resumedTask);
122 }
123 return TRUE;
124 }
125
126 /*
127 * 描述：读指定队列
128 */
PRT_QueueRead(U32 queueId,void * bufferAddr,U32 * len,U32 timeOut)129 OS_SEC_L4_TEXT U32 PRT_QueueRead(U32 queueId, void *bufferAddr, U32 *len, U32 timeOut)
130 {
131 uintptr_t intSave;
132 U32 ret;
133 U32 bufLen;
134 U32 innerId = OS_QUEUE_INNER_ID(queueId);
135 struct TagQueCb *queueCb = NULL;
136 struct QueNode *queueNode = NULL;
137
138 if (innerId >= g_maxQueue) {
139 return OS_ERRNO_QUEUE_INVALID;
140 }
141
142 if ((bufferAddr == NULL) || (len == NULL)) {
143 return OS_ERRNO_QUEUE_PTR_NULL;
144 }
145
146 if (*len == 0) {
147 return OS_ERRNO_QUEUE_SIZE_ZERO;
148 }
149
150 bufLen = *len;
151 /* 获取指定队列控制块 */
152 queueCb = (struct TagQueCb *)GET_QUEUE_HANDLE(innerId);
153
154 intSave = OsIntLock();
155 if (queueCb->queueState == OS_QUEUE_UNUSED) {
156 ret = OS_ERRNO_QUEUE_NOT_CREATE;
157 goto QUEUE_END;
158 }
159
160 /* 读队列PEND */
161 ret = OsInnerPend(&queueCb->readableCnt, &queueCb->readList, timeOut);
162 if (ret != OS_OK) {
163 goto QUEUE_END;
164 }
165
166 /* 获取该队列中第一个有数据结点首地址，并将数据存入buffer中 */
167 queueNode = (struct QueNode *)(uintptr_t)&queueCb->queue[(queueCb->queueHead) * (queueCb->nodeSize)];
168
169 /* 如果buf的长度小于数据长度，则仅返回buf大小的数据，如果大于，则返回全部数据 */
170 if (*len > queueNode->size) {
171 *len = queueNode->size;
172 }
173
174 if (memcpy_s(bufferAddr, bufLen, (void *)queueNode->buf, *len) != EOK) {
175 OS_GOTO_SYS_ERROR1();
176 }
177
178 queueNode->srcPid = OS_QUEUE_PID_INVALID;
179
180 /* 队列头指针加1 */
181 queueCb->queueHead++;
182 /* 如果队列头指针已经到队列尾，那么头指针指向队列头 */
183 if (queueCb->queueHead == queueCb->nodeNum) {
184 queueCb->queueHead = 0;
185 }
186
187 if (OsQueuePendNeedProc(&queueCb->writeList)) {
188 OsTskSchedule();
189 OsIntRestore(intSave);
190 return OS_OK;
191 }
192
193 queueCb->writableCnt++;
194
195 QUEUE_END:
196 OsIntRestore(intSave);
197 return ret;
198 }
199
OsQueueWriteParaCheck(U32 innerId,uintptr_t bufferAddr,U32 bufferSize,U32 prio)200 OS_SEC_L4_TEXT U32 OsQueueWriteParaCheck(U32 innerId, uintptr_t bufferAddr, U32 bufferSize, U32 prio)
201 {
202 if (innerId >= g_maxQueue) {
203 return OS_ERRNO_QUEUE_INVALID;
204 }
205
206 if (bufferAddr == 0) {
207 return OS_ERRNO_QUEUE_PTR_NULL;
208 }
209
210 if (bufferSize == 0) {
211 return OS_ERRNO_QUEUE_SIZE_ZERO;
212 }
213
214 if (prio > (U32)OS_QUEUE_URGENT) {
215 return OS_ERRNO_QUEUE_PRIO_INVALID;
216 }
217
218 return OS_OK;
219 }
220
OsQueueCpData2Node(U32 prio,uintptr_t bufferAddr,U32 bufferSize,struct TagQueCb * queueCb)221 OS_SEC_ALW_INLINE INLINE void OsQueueCpData2Node(U32 prio, uintptr_t bufferAddr, U32 bufferSize,
222 struct TagQueCb *queueCb)
223 {
224 U16 peak;
225 struct QueNode *queueNode = NULL;
226 if (prio == (U32)OS_QUEUE_NORMAL) {
227 /* 普通消息加到队列尾部 */
228 queueNode = (struct QueNode *)(uintptr_t)&queueCb->queue[((queueCb->queueTail) * (queueCb->nodeSize))];
229
230 queueCb->queueTail++;
231 if (queueCb->queueTail == queueCb->nodeNum) {
232 queueCb->queueTail = 0;
233 }
234 } else {
235 /* 紧急消息加到队列头上 */
236 if (queueCb->queueHead == 0) {
237 queueCb->queueHead = queueCb->nodeNum;
238 }
239 queueCb->queueHead--;
240
241 queueNode = (struct QueNode *)(uintptr_t)&queueCb->queue[((queueCb->queueHead) * (queueCb->nodeSize))];
242 }
243
244 if (memcpy_s((void *)queueNode->buf, (queueCb->nodeSize - OS_QUEUE_NODE_HEAD_LEN),
245 (void *)bufferAddr, bufferSize) != EOK) {
246 OS_GOTO_SYS_ERROR1();
247 }
248
249 queueNode->size = (U16)bufferSize;
250 queueNode->srcPid = (U16)OsGetSrcPid();
251
252 peak = queueCb->queueTail > queueCb->queueHead ? queueCb->queueTail - queueCb->queueHead
253 : (queueCb->nodeNum - queueCb->queueHead) + queueCb->queueTail;
254
255 if (peak > queueCb->nodePeak) {
256 queueCb->nodePeak = peak;
257 }
258 }
259
260 /*
261 * 描述：写指定队列
262 */
PRT_QueueWrite(U32 queueId,void * bufferAddr,U32 bufferSize,U32 timeOut,U32 prio)263 OS_SEC_L4_TEXT U32 PRT_QueueWrite(U32 queueId, void *bufferAddr, U32 bufferSize, U32 timeOut, U32 prio)
264 {
265 U32 ret;
266 uintptr_t intSave;
267 U32 innerId = OS_QUEUE_INNER_ID(queueId);
268 struct TagQueCb *queueCb = NULL;
269
270 ret = OsQueueWriteParaCheck(innerId, (uintptr_t)bufferAddr, bufferSize, prio);
271 if (ret != OS_OK) {
272 return ret;
273 }
274
275 /* 获取指定队列控制块 */
276 queueCb = (struct TagQueCb *)GET_QUEUE_HANDLE(innerId);
277 intSave = OsIntLock();
278 if (queueCb->queueState == OS_QUEUE_UNUSED) {
279 ret = OS_ERRNO_QUEUE_NOT_CREATE;
280 goto QUEUE_END;
281 }
282
283 if (bufferSize > (queueCb->nodeSize - OS_QUEUE_NODE_HEAD_LEN)) {
284 ret = OS_ERRNO_QUEUE_SIZE_TOO_BIG;
285 goto QUEUE_END;
286 }
287
288 /* 读队列PEND */
289 ret = OsInnerPend(&queueCb->writableCnt, &queueCb->writeList, timeOut);
290 if (ret != OS_OK) {
291 goto QUEUE_END;
292 }
293
294 /* 选取消息节点，初始化消息节点拷贝数据 */
295 OsQueueCpData2Node(prio, (uintptr_t)bufferAddr, bufferSize, queueCb);
296
297 if (OsQueuePendNeedProc(&queueCb->readList)) {
298 OsTskSchedule();
299 OsIntRestore(intSave);
300 return OS_OK;
301 }
302
303 queueCb->readableCnt++;
304
305 QUEUE_END:
306 OsIntRestore(intSave);
307 return ret;
308 }
309