Repostiory containing DAPLink source code with Reset Pin workaround for HANI_IOT board.

Upstream: https://github.com/ARMmbed/DAPLink

Committer:
Pawel Zarembski
Date:
Tue Apr 07 12:55:42 2020 +0200
Revision:
0:01f31e923fe2
hani: DAPLink with reset workaround

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Pawel Zarembski 0:01f31e923fe2 1 /**
Pawel Zarembski 0:01f31e923fe2 2 * @file rt_List.c
Pawel Zarembski 0:01f31e923fe2 3 * @brief
Pawel Zarembski 0:01f31e923fe2 4 *
Pawel Zarembski 0:01f31e923fe2 5 * DAPLink Interface Firmware
Pawel Zarembski 0:01f31e923fe2 6 * Copyright (c) 2009-2016, ARM Limited, All Rights Reserved
Pawel Zarembski 0:01f31e923fe2 7 * SPDX-License-Identifier: Apache-2.0
Pawel Zarembski 0:01f31e923fe2 8 *
Pawel Zarembski 0:01f31e923fe2 9 * Licensed under the Apache License, Version 2.0 (the "License"); you may
Pawel Zarembski 0:01f31e923fe2 10 * not use this file except in compliance with the License.
Pawel Zarembski 0:01f31e923fe2 11 * You may obtain a copy of the License at
Pawel Zarembski 0:01f31e923fe2 12 *
Pawel Zarembski 0:01f31e923fe2 13 * http://www.apache.org/licenses/LICENSE-2.0
Pawel Zarembski 0:01f31e923fe2 14 *
Pawel Zarembski 0:01f31e923fe2 15 * Unless required by applicable law or agreed to in writing, software
Pawel Zarembski 0:01f31e923fe2 16 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
Pawel Zarembski 0:01f31e923fe2 17 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
Pawel Zarembski 0:01f31e923fe2 18 * See the License for the specific language governing permissions and
Pawel Zarembski 0:01f31e923fe2 19 * limitations under the License.
Pawel Zarembski 0:01f31e923fe2 20 */
Pawel Zarembski 0:01f31e923fe2 21
Pawel Zarembski 0:01f31e923fe2 22 #include "rt_TypeDef.h"
Pawel Zarembski 0:01f31e923fe2 23 #include "RTX_Config.h"
Pawel Zarembski 0:01f31e923fe2 24 #include "rt_System.h"
Pawel Zarembski 0:01f31e923fe2 25 #include "rt_List.h"
Pawel Zarembski 0:01f31e923fe2 26 #include "rt_Task.h"
Pawel Zarembski 0:01f31e923fe2 27 #include "rt_Time.h"
Pawel Zarembski 0:01f31e923fe2 28 #include "rt_HAL_CM.h"
Pawel Zarembski 0:01f31e923fe2 29
Pawel Zarembski 0:01f31e923fe2 30 /*----------------------------------------------------------------------------
Pawel Zarembski 0:01f31e923fe2 31 * Global Variables
Pawel Zarembski 0:01f31e923fe2 32 *---------------------------------------------------------------------------*/
Pawel Zarembski 0:01f31e923fe2 33
Pawel Zarembski 0:01f31e923fe2 34 /* List head of chained ready tasks */
Pawel Zarembski 0:01f31e923fe2 35 struct OS_XCB os_rdy;
Pawel Zarembski 0:01f31e923fe2 36 /* List head of chained delay tasks */
Pawel Zarembski 0:01f31e923fe2 37 struct OS_XCB os_dly;
Pawel Zarembski 0:01f31e923fe2 38
Pawel Zarembski 0:01f31e923fe2 39
Pawel Zarembski 0:01f31e923fe2 40 /*----------------------------------------------------------------------------
Pawel Zarembski 0:01f31e923fe2 41 * Functions
Pawel Zarembski 0:01f31e923fe2 42 *---------------------------------------------------------------------------*/
Pawel Zarembski 0:01f31e923fe2 43
Pawel Zarembski 0:01f31e923fe2 44
Pawel Zarembski 0:01f31e923fe2 45 /*--------------------------- rt_put_prio -----------------------------------*/
Pawel Zarembski 0:01f31e923fe2 46
Pawel Zarembski 0:01f31e923fe2 47 void rt_put_prio (P_XCB p_CB, P_TCB p_task) {
Pawel Zarembski 0:01f31e923fe2 48 /* Put task identified with "p_task" into list ordered by priority. */
Pawel Zarembski 0:01f31e923fe2 49 /* "p_CB" points to head of list; list has always an element at end with */
Pawel Zarembski 0:01f31e923fe2 50 /* a priority less than "p_task->prio". */
Pawel Zarembski 0:01f31e923fe2 51 P_TCB p_CB2;
Pawel Zarembski 0:01f31e923fe2 52 U32 prio;
Pawel Zarembski 0:01f31e923fe2 53 BOOL sem_mbx = __FALSE;
Pawel Zarembski 0:01f31e923fe2 54
Pawel Zarembski 0:01f31e923fe2 55 if (p_CB->cb_type == SCB || p_CB->cb_type == MCB || p_CB->cb_type == MUCB) {
Pawel Zarembski 0:01f31e923fe2 56 sem_mbx = __TRUE;
Pawel Zarembski 0:01f31e923fe2 57 }
Pawel Zarembski 0:01f31e923fe2 58 prio = p_task->prio;
Pawel Zarembski 0:01f31e923fe2 59 p_CB2 = p_CB->p_lnk;
Pawel Zarembski 0:01f31e923fe2 60 /* Search for an entry in the list */
Pawel Zarembski 0:01f31e923fe2 61 while (p_CB2 != NULL && prio <= p_CB2->prio) {
Pawel Zarembski 0:01f31e923fe2 62 p_CB = (P_XCB)p_CB2;
Pawel Zarembski 0:01f31e923fe2 63 p_CB2 = p_CB2->p_lnk;
Pawel Zarembski 0:01f31e923fe2 64 }
Pawel Zarembski 0:01f31e923fe2 65 /* Entry found, insert the task into the list */
Pawel Zarembski 0:01f31e923fe2 66 p_task->p_lnk = p_CB2;
Pawel Zarembski 0:01f31e923fe2 67 p_CB->p_lnk = p_task;
Pawel Zarembski 0:01f31e923fe2 68 if (sem_mbx) {
Pawel Zarembski 0:01f31e923fe2 69 if (p_CB2 != NULL) {
Pawel Zarembski 0:01f31e923fe2 70 p_CB2->p_rlnk = p_task;
Pawel Zarembski 0:01f31e923fe2 71 }
Pawel Zarembski 0:01f31e923fe2 72 p_task->p_rlnk = (P_TCB)p_CB;
Pawel Zarembski 0:01f31e923fe2 73 }
Pawel Zarembski 0:01f31e923fe2 74 else {
Pawel Zarembski 0:01f31e923fe2 75 p_task->p_rlnk = NULL;
Pawel Zarembski 0:01f31e923fe2 76 }
Pawel Zarembski 0:01f31e923fe2 77 }
Pawel Zarembski 0:01f31e923fe2 78
Pawel Zarembski 0:01f31e923fe2 79
Pawel Zarembski 0:01f31e923fe2 80 /*--------------------------- rt_get_first ----------------------------------*/
Pawel Zarembski 0:01f31e923fe2 81
Pawel Zarembski 0:01f31e923fe2 82 P_TCB rt_get_first (P_XCB p_CB) {
Pawel Zarembski 0:01f31e923fe2 83 /* Get task at head of list: it is the task with highest priority. */
Pawel Zarembski 0:01f31e923fe2 84 /* "p_CB" points to head of list. */
Pawel Zarembski 0:01f31e923fe2 85 P_TCB p_first;
Pawel Zarembski 0:01f31e923fe2 86
Pawel Zarembski 0:01f31e923fe2 87 p_first = p_CB->p_lnk;
Pawel Zarembski 0:01f31e923fe2 88 p_CB->p_lnk = p_first->p_lnk;
Pawel Zarembski 0:01f31e923fe2 89 if (p_CB->cb_type == SCB || p_CB->cb_type == MCB || p_CB->cb_type == MUCB) {
Pawel Zarembski 0:01f31e923fe2 90 if (p_first->p_lnk != NULL) {
Pawel Zarembski 0:01f31e923fe2 91 p_first->p_lnk->p_rlnk = (P_TCB)p_CB;
Pawel Zarembski 0:01f31e923fe2 92 p_first->p_lnk = NULL;
Pawel Zarembski 0:01f31e923fe2 93 }
Pawel Zarembski 0:01f31e923fe2 94 p_first->p_rlnk = NULL;
Pawel Zarembski 0:01f31e923fe2 95 }
Pawel Zarembski 0:01f31e923fe2 96 else {
Pawel Zarembski 0:01f31e923fe2 97 p_first->p_lnk = NULL;
Pawel Zarembski 0:01f31e923fe2 98 }
Pawel Zarembski 0:01f31e923fe2 99 return (p_first);
Pawel Zarembski 0:01f31e923fe2 100 }
Pawel Zarembski 0:01f31e923fe2 101
Pawel Zarembski 0:01f31e923fe2 102
Pawel Zarembski 0:01f31e923fe2 103 /*--------------------------- rt_put_rdy_first ------------------------------*/
Pawel Zarembski 0:01f31e923fe2 104
Pawel Zarembski 0:01f31e923fe2 105 void rt_put_rdy_first (P_TCB p_task) {
Pawel Zarembski 0:01f31e923fe2 106 /* Put task identified with "p_task" at the head of the ready list. The */
Pawel Zarembski 0:01f31e923fe2 107 /* task must have at least a priority equal to highest priority in list. */
Pawel Zarembski 0:01f31e923fe2 108 p_task->p_lnk = os_rdy.p_lnk;
Pawel Zarembski 0:01f31e923fe2 109 p_task->p_rlnk = NULL;
Pawel Zarembski 0:01f31e923fe2 110 os_rdy.p_lnk = p_task;
Pawel Zarembski 0:01f31e923fe2 111 }
Pawel Zarembski 0:01f31e923fe2 112
Pawel Zarembski 0:01f31e923fe2 113
Pawel Zarembski 0:01f31e923fe2 114 /*--------------------------- rt_get_same_rdy_prio --------------------------*/
Pawel Zarembski 0:01f31e923fe2 115
Pawel Zarembski 0:01f31e923fe2 116 P_TCB rt_get_same_rdy_prio (void) {
Pawel Zarembski 0:01f31e923fe2 117 /* Remove a task of same priority from ready list if any exists. Other- */
Pawel Zarembski 0:01f31e923fe2 118 /* wise return NULL. */
Pawel Zarembski 0:01f31e923fe2 119 P_TCB p_first;
Pawel Zarembski 0:01f31e923fe2 120
Pawel Zarembski 0:01f31e923fe2 121 p_first = os_rdy.p_lnk;
Pawel Zarembski 0:01f31e923fe2 122 if (p_first->prio == os_tsk.run->prio) {
Pawel Zarembski 0:01f31e923fe2 123 os_rdy.p_lnk = os_rdy.p_lnk->p_lnk;
Pawel Zarembski 0:01f31e923fe2 124 return (p_first);
Pawel Zarembski 0:01f31e923fe2 125 }
Pawel Zarembski 0:01f31e923fe2 126 return (NULL);
Pawel Zarembski 0:01f31e923fe2 127 }
Pawel Zarembski 0:01f31e923fe2 128
Pawel Zarembski 0:01f31e923fe2 129
Pawel Zarembski 0:01f31e923fe2 130 /*--------------------------- rt_resort_prio --------------------------------*/
Pawel Zarembski 0:01f31e923fe2 131
Pawel Zarembski 0:01f31e923fe2 132 void rt_resort_prio (P_TCB p_task) {
Pawel Zarembski 0:01f31e923fe2 133 /* Re-sort ordered lists after the priority of 'p_task' has changed. */
Pawel Zarembski 0:01f31e923fe2 134 P_TCB p_CB;
Pawel Zarembski 0:01f31e923fe2 135
Pawel Zarembski 0:01f31e923fe2 136 if (p_task->p_rlnk == NULL) {
Pawel Zarembski 0:01f31e923fe2 137 if (p_task->state == READY) {
Pawel Zarembski 0:01f31e923fe2 138 /* Task is chained into READY list. */
Pawel Zarembski 0:01f31e923fe2 139 p_CB = (P_TCB)&os_rdy;
Pawel Zarembski 0:01f31e923fe2 140 goto res;
Pawel Zarembski 0:01f31e923fe2 141 }
Pawel Zarembski 0:01f31e923fe2 142 }
Pawel Zarembski 0:01f31e923fe2 143 else {
Pawel Zarembski 0:01f31e923fe2 144 p_CB = p_task->p_rlnk;
Pawel Zarembski 0:01f31e923fe2 145 while (p_CB->cb_type == TCB) {
Pawel Zarembski 0:01f31e923fe2 146 /* Find a header of this task chain list. */
Pawel Zarembski 0:01f31e923fe2 147 p_CB = p_CB->p_rlnk;
Pawel Zarembski 0:01f31e923fe2 148 }
Pawel Zarembski 0:01f31e923fe2 149 res:rt_rmv_list (p_task);
Pawel Zarembski 0:01f31e923fe2 150 rt_put_prio ((P_XCB)p_CB, p_task);
Pawel Zarembski 0:01f31e923fe2 151 }
Pawel Zarembski 0:01f31e923fe2 152 }
Pawel Zarembski 0:01f31e923fe2 153
Pawel Zarembski 0:01f31e923fe2 154
Pawel Zarembski 0:01f31e923fe2 155 /*--------------------------- rt_put_dly ------------------------------------*/
Pawel Zarembski 0:01f31e923fe2 156
Pawel Zarembski 0:01f31e923fe2 157 void rt_put_dly (P_TCB p_task, U16 delay) {
Pawel Zarembski 0:01f31e923fe2 158 /* Put a task identified with "p_task" into chained delay wait list using */
Pawel Zarembski 0:01f31e923fe2 159 /* a delay value of "delay". */
Pawel Zarembski 0:01f31e923fe2 160 P_TCB p;
Pawel Zarembski 0:01f31e923fe2 161 U32 delta,idelay = delay;
Pawel Zarembski 0:01f31e923fe2 162
Pawel Zarembski 0:01f31e923fe2 163 p = (P_TCB)&os_dly;
Pawel Zarembski 0:01f31e923fe2 164 if (p->p_dlnk == NULL) {
Pawel Zarembski 0:01f31e923fe2 165 /* Delay list empty */
Pawel Zarembski 0:01f31e923fe2 166 delta = 0;
Pawel Zarembski 0:01f31e923fe2 167 goto last;
Pawel Zarembski 0:01f31e923fe2 168 }
Pawel Zarembski 0:01f31e923fe2 169 delta = os_dly.delta_time;
Pawel Zarembski 0:01f31e923fe2 170 while (delta < idelay) {
Pawel Zarembski 0:01f31e923fe2 171 if (p->p_dlnk == NULL) {
Pawel Zarembski 0:01f31e923fe2 172 /* End of list found */
Pawel Zarembski 0:01f31e923fe2 173 last: p_task->p_dlnk = NULL;
Pawel Zarembski 0:01f31e923fe2 174 p->p_dlnk = p_task;
Pawel Zarembski 0:01f31e923fe2 175 p_task->p_blnk = p;
Pawel Zarembski 0:01f31e923fe2 176 p->delta_time = (U16)(idelay - delta);
Pawel Zarembski 0:01f31e923fe2 177 p_task->delta_time = 0;
Pawel Zarembski 0:01f31e923fe2 178 return;
Pawel Zarembski 0:01f31e923fe2 179 }
Pawel Zarembski 0:01f31e923fe2 180 p = p->p_dlnk;
Pawel Zarembski 0:01f31e923fe2 181 delta += p->delta_time;
Pawel Zarembski 0:01f31e923fe2 182 }
Pawel Zarembski 0:01f31e923fe2 183 /* Right place found */
Pawel Zarembski 0:01f31e923fe2 184 p_task->p_dlnk = p->p_dlnk;
Pawel Zarembski 0:01f31e923fe2 185 p->p_dlnk = p_task;
Pawel Zarembski 0:01f31e923fe2 186 p_task->p_blnk = p;
Pawel Zarembski 0:01f31e923fe2 187 if (p_task->p_dlnk != NULL) {
Pawel Zarembski 0:01f31e923fe2 188 p_task->p_dlnk->p_blnk = p_task;
Pawel Zarembski 0:01f31e923fe2 189 }
Pawel Zarembski 0:01f31e923fe2 190 p_task->delta_time = (U16)(delta - idelay);
Pawel Zarembski 0:01f31e923fe2 191 p->delta_time -= p_task->delta_time;
Pawel Zarembski 0:01f31e923fe2 192 }
Pawel Zarembski 0:01f31e923fe2 193
Pawel Zarembski 0:01f31e923fe2 194
Pawel Zarembski 0:01f31e923fe2 195 /*--------------------------- rt_dec_dly ------------------------------------*/
Pawel Zarembski 0:01f31e923fe2 196
Pawel Zarembski 0:01f31e923fe2 197 void rt_dec_dly (void) {
Pawel Zarembski 0:01f31e923fe2 198 /* Decrement delta time of list head: remove tasks having a value of zero.*/
Pawel Zarembski 0:01f31e923fe2 199 P_TCB p_rdy;
Pawel Zarembski 0:01f31e923fe2 200
Pawel Zarembski 0:01f31e923fe2 201 if (os_dly.p_dlnk == NULL) {
Pawel Zarembski 0:01f31e923fe2 202 return;
Pawel Zarembski 0:01f31e923fe2 203 }
Pawel Zarembski 0:01f31e923fe2 204 os_dly.delta_time--;
Pawel Zarembski 0:01f31e923fe2 205 while ((os_dly.delta_time == 0) && (os_dly.p_dlnk != NULL)) {
Pawel Zarembski 0:01f31e923fe2 206 p_rdy = os_dly.p_dlnk;
Pawel Zarembski 0:01f31e923fe2 207 if (p_rdy->p_rlnk != NULL) {
Pawel Zarembski 0:01f31e923fe2 208 /* Task is really enqueued, remove task from semaphore/mailbox */
Pawel Zarembski 0:01f31e923fe2 209 /* timeout waiting list. */
Pawel Zarembski 0:01f31e923fe2 210 p_rdy->p_rlnk->p_lnk = p_rdy->p_lnk;
Pawel Zarembski 0:01f31e923fe2 211 if (p_rdy->p_lnk != NULL) {
Pawel Zarembski 0:01f31e923fe2 212 p_rdy->p_lnk->p_rlnk = p_rdy->p_rlnk;
Pawel Zarembski 0:01f31e923fe2 213 p_rdy->p_lnk = NULL;
Pawel Zarembski 0:01f31e923fe2 214 }
Pawel Zarembski 0:01f31e923fe2 215 p_rdy->p_rlnk = NULL;
Pawel Zarembski 0:01f31e923fe2 216 }
Pawel Zarembski 0:01f31e923fe2 217 rt_put_prio (&os_rdy, p_rdy);
Pawel Zarembski 0:01f31e923fe2 218 os_dly.delta_time = p_rdy->delta_time;
Pawel Zarembski 0:01f31e923fe2 219 if (p_rdy->state == WAIT_ITV) {
Pawel Zarembski 0:01f31e923fe2 220 /* Calculate the next time for interval wait. */
Pawel Zarembski 0:01f31e923fe2 221 p_rdy->delta_time = p_rdy->interval_time + (U16)os_time;
Pawel Zarembski 0:01f31e923fe2 222 }
Pawel Zarembski 0:01f31e923fe2 223 p_rdy->state = READY;
Pawel Zarembski 0:01f31e923fe2 224 p_rdy->ret_val = OS_R_TMO;
Pawel Zarembski 0:01f31e923fe2 225 os_dly.p_dlnk = p_rdy->p_dlnk;
Pawel Zarembski 0:01f31e923fe2 226 if (p_rdy->p_dlnk != NULL) {
Pawel Zarembski 0:01f31e923fe2 227 p_rdy->p_dlnk->p_blnk = (P_TCB)&os_dly;
Pawel Zarembski 0:01f31e923fe2 228 p_rdy->p_dlnk = NULL;
Pawel Zarembski 0:01f31e923fe2 229 }
Pawel Zarembski 0:01f31e923fe2 230 p_rdy->p_blnk = NULL;
Pawel Zarembski 0:01f31e923fe2 231 }
Pawel Zarembski 0:01f31e923fe2 232 }
Pawel Zarembski 0:01f31e923fe2 233
Pawel Zarembski 0:01f31e923fe2 234
Pawel Zarembski 0:01f31e923fe2 235 /*--------------------------- rt_rmv_list -----------------------------------*/
Pawel Zarembski 0:01f31e923fe2 236
Pawel Zarembski 0:01f31e923fe2 237 void rt_rmv_list (P_TCB p_task) {
Pawel Zarembski 0:01f31e923fe2 238 /* Remove task identified with "p_task" from ready, semaphore or mailbox */
Pawel Zarembski 0:01f31e923fe2 239 /* waiting list if enqueued. */
Pawel Zarembski 0:01f31e923fe2 240 P_TCB p_b;
Pawel Zarembski 0:01f31e923fe2 241
Pawel Zarembski 0:01f31e923fe2 242 if (p_task->p_rlnk != NULL) {
Pawel Zarembski 0:01f31e923fe2 243 /* A task is enqueued in semaphore / mailbox waiting list. */
Pawel Zarembski 0:01f31e923fe2 244 p_task->p_rlnk->p_lnk = p_task->p_lnk;
Pawel Zarembski 0:01f31e923fe2 245 if (p_task->p_lnk != NULL) {
Pawel Zarembski 0:01f31e923fe2 246 p_task->p_lnk->p_rlnk = p_task->p_rlnk;
Pawel Zarembski 0:01f31e923fe2 247 }
Pawel Zarembski 0:01f31e923fe2 248 return;
Pawel Zarembski 0:01f31e923fe2 249 }
Pawel Zarembski 0:01f31e923fe2 250
Pawel Zarembski 0:01f31e923fe2 251 p_b = (P_TCB)&os_rdy;
Pawel Zarembski 0:01f31e923fe2 252 while (p_b != NULL) {
Pawel Zarembski 0:01f31e923fe2 253 /* Search the ready list for task "p_task" */
Pawel Zarembski 0:01f31e923fe2 254 if (p_b->p_lnk == p_task) {
Pawel Zarembski 0:01f31e923fe2 255 p_b->p_lnk = p_task->p_lnk;
Pawel Zarembski 0:01f31e923fe2 256 return;
Pawel Zarembski 0:01f31e923fe2 257 }
Pawel Zarembski 0:01f31e923fe2 258 p_b = p_b->p_lnk;
Pawel Zarembski 0:01f31e923fe2 259 }
Pawel Zarembski 0:01f31e923fe2 260 }
Pawel Zarembski 0:01f31e923fe2 261
Pawel Zarembski 0:01f31e923fe2 262
Pawel Zarembski 0:01f31e923fe2 263 /*--------------------------- rt_rmv_dly ------------------------------------*/
Pawel Zarembski 0:01f31e923fe2 264
Pawel Zarembski 0:01f31e923fe2 265 void rt_rmv_dly (P_TCB p_task) {
Pawel Zarembski 0:01f31e923fe2 266 /* Remove task identified with "p_task" from delay list if enqueued. */
Pawel Zarembski 0:01f31e923fe2 267 P_TCB p_b;
Pawel Zarembski 0:01f31e923fe2 268
Pawel Zarembski 0:01f31e923fe2 269 p_b = p_task->p_blnk;
Pawel Zarembski 0:01f31e923fe2 270 if (p_b != NULL) {
Pawel Zarembski 0:01f31e923fe2 271 /* Task is really enqueued */
Pawel Zarembski 0:01f31e923fe2 272 p_b->p_dlnk = p_task->p_dlnk;
Pawel Zarembski 0:01f31e923fe2 273 if (p_task->p_dlnk != NULL) {
Pawel Zarembski 0:01f31e923fe2 274 /* 'p_task' is in the middle of list */
Pawel Zarembski 0:01f31e923fe2 275 p_b->delta_time += p_task->delta_time;
Pawel Zarembski 0:01f31e923fe2 276 p_task->p_dlnk->p_blnk = p_b;
Pawel Zarembski 0:01f31e923fe2 277 p_task->p_dlnk = NULL;
Pawel Zarembski 0:01f31e923fe2 278 }
Pawel Zarembski 0:01f31e923fe2 279 else {
Pawel Zarembski 0:01f31e923fe2 280 /* 'p_task' is at the end of list */
Pawel Zarembski 0:01f31e923fe2 281 p_b->delta_time = 0;
Pawel Zarembski 0:01f31e923fe2 282 }
Pawel Zarembski 0:01f31e923fe2 283 p_task->p_blnk = NULL;
Pawel Zarembski 0:01f31e923fe2 284 }
Pawel Zarembski 0:01f31e923fe2 285 }
Pawel Zarembski 0:01f31e923fe2 286
Pawel Zarembski 0:01f31e923fe2 287
Pawel Zarembski 0:01f31e923fe2 288 /*--------------------------- rt_psq_enq ------------------------------------*/
Pawel Zarembski 0:01f31e923fe2 289
Pawel Zarembski 0:01f31e923fe2 290 void rt_psq_enq (OS_ID entry, U32 arg) {
Pawel Zarembski 0:01f31e923fe2 291 /* Insert post service request "entry" into ps-queue. */
Pawel Zarembski 0:01f31e923fe2 292 U32 idx;
Pawel Zarembski 0:01f31e923fe2 293
Pawel Zarembski 0:01f31e923fe2 294 idx = rt_inc_qi (os_psq->size, &os_psq->count, &os_psq->first);
Pawel Zarembski 0:01f31e923fe2 295 if (idx < os_psq->size) {
Pawel Zarembski 0:01f31e923fe2 296 os_psq->q[idx].id = entry;
Pawel Zarembski 0:01f31e923fe2 297 os_psq->q[idx].arg = arg;
Pawel Zarembski 0:01f31e923fe2 298 }
Pawel Zarembski 0:01f31e923fe2 299 else {
Pawel Zarembski 0:01f31e923fe2 300 os_error (OS_ERR_FIFO_OVF);
Pawel Zarembski 0:01f31e923fe2 301 }
Pawel Zarembski 0:01f31e923fe2 302 }
Pawel Zarembski 0:01f31e923fe2 303
Pawel Zarembski 0:01f31e923fe2 304
Pawel Zarembski 0:01f31e923fe2 305 /*----------------------------------------------------------------------------
Pawel Zarembski 0:01f31e923fe2 306 * end of file
Pawel Zarembski 0:01f31e923fe2 307 *---------------------------------------------------------------------------*/
Pawel Zarembski 0:01f31e923fe2 308