sabme ua / mbed-rtos

Official mbed Real Time Operating System based on the RTX implementation of the CMSIS-RTOS API open standard.

Dependents:   DISCO-F746NG_rtos_test ecte433

Fork of mbed-rtos by mbed official

rtx/TARGET_ARM7/rt_List.c@96:8ac966e2cd3f, 2015-11-11 (annotated)

Committer:
mzta
Date:
Wed Nov 11 07:21:20 2015 +0000
Revision:
96:8ac966e2cd3f
Parent:
80:2dab120a94c2 
Support Cortex-M7 target.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbed_official 80:2dab120a94c2 1 /*----------------------------------------------------------------------------
mbed_official 80:2dab120a94c2 2 * RL-ARM - RTX
mbed_official 80:2dab120a94c2 3 *----------------------------------------------------------------------------
mbed_official 80:2dab120a94c2 4 * Name: RT_LIST.C
mbed_official 80:2dab120a94c2 5 * Purpose: Functions for the management of different lists
mbed_official 80:2dab120a94c2 6 * Rev.: V4.60
mbed_official 80:2dab120a94c2 7 *----------------------------------------------------------------------------
mbed_official 80:2dab120a94c2 8 *
mbed_official 80:2dab120a94c2 9 * Copyright (c) 1999-2009 KEIL, 2009-2015 ARM Germany GmbH
mbed_official 80:2dab120a94c2 10 * All rights reserved.
mbed_official 80:2dab120a94c2 11 * Redistribution and use in source and binary forms, with or without
mbed_official 80:2dab120a94c2 12 * modification, are permitted provided that the following conditions are met:
mbed_official 80:2dab120a94c2 13 * - Redistributions of source code must retain the above copyright
mbed_official 80:2dab120a94c2 14 * notice, this list of conditions and the following disclaimer.
mbed_official 80:2dab120a94c2 15 * - Redistributions in binary form must reproduce the above copyright
mbed_official 80:2dab120a94c2 16 * notice, this list of conditions and the following disclaimer in the
mbed_official 80:2dab120a94c2 17 * documentation and/or other materials provided with the distribution.
mbed_official 80:2dab120a94c2 18 * - Neither the name of ARM nor the names of its contributors may be used
mbed_official 80:2dab120a94c2 19 * to endorse or promote products derived from this software without
mbed_official 80:2dab120a94c2 20 * specific prior written permission.
mbed_official 80:2dab120a94c2 21 *
mbed_official 80:2dab120a94c2 22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
mbed_official 80:2dab120a94c2 23 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
mbed_official 80:2dab120a94c2 24 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
mbed_official 80:2dab120a94c2 25 * ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE
mbed_official 80:2dab120a94c2 26 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
mbed_official 80:2dab120a94c2 27 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
mbed_official 80:2dab120a94c2 28 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
mbed_official 80:2dab120a94c2 29 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
mbed_official 80:2dab120a94c2 30 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
mbed_official 80:2dab120a94c2 31 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
mbed_official 80:2dab120a94c2 32 * POSSIBILITY OF SUCH DAMAGE.
mbed_official 80:2dab120a94c2 33 *---------------------------------------------------------------------------*/
mbed_official 80:2dab120a94c2 34
mbed_official 80:2dab120a94c2 35 #include "rt_TypeDef.h"
mbed_official 80:2dab120a94c2 36 #include "RTX_Conf.h"
mbed_official 80:2dab120a94c2 37 #include "rt_System.h"
mbed_official 80:2dab120a94c2 38 #include "rt_List.h"
mbed_official 80:2dab120a94c2 39 #include "rt_Task.h"
mbed_official 80:2dab120a94c2 40 #include "rt_Time.h"
mbed_official 80:2dab120a94c2 41 #include "rt_HAL_CM.h"
mbed_official 80:2dab120a94c2 42
mbed_official 80:2dab120a94c2 43 /*----------------------------------------------------------------------------
mbed_official 80:2dab120a94c2 44 * Global Variables
mbed_official 80:2dab120a94c2 45 *---------------------------------------------------------------------------*/
mbed_official 80:2dab120a94c2 46
mbed_official 80:2dab120a94c2 47 /* List head of chained ready tasks */
mbed_official 80:2dab120a94c2 48 struct OS_XCB os_rdy;
mbed_official 80:2dab120a94c2 49 /* List head of chained delay tasks */
mbed_official 80:2dab120a94c2 50 struct OS_XCB os_dly;
mbed_official 80:2dab120a94c2 51
mbed_official 80:2dab120a94c2 52
mbed_official 80:2dab120a94c2 53 /*----------------------------------------------------------------------------
mbed_official 80:2dab120a94c2 54 * Functions
mbed_official 80:2dab120a94c2 55 *---------------------------------------------------------------------------*/
mbed_official 80:2dab120a94c2 56
mbed_official 80:2dab120a94c2 57
mbed_official 80:2dab120a94c2 58 /*--------------------------- rt_put_prio -----------------------------------*/
mbed_official 80:2dab120a94c2 59
mbed_official 80:2dab120a94c2 60 void rt_put_prio (P_XCB p_CB, P_TCB p_task) {
mbed_official 80:2dab120a94c2 61 /* Put task identified with "p_task" into list ordered by priority. */
mbed_official 80:2dab120a94c2 62 /* "p_CB" points to head of list; list has always an element at end with */
mbed_official 80:2dab120a94c2 63 /* a priority less than "p_task->prio". */
mbed_official 80:2dab120a94c2 64 P_TCB p_CB2;
mbed_official 80:2dab120a94c2 65 U32 prio;
mbed_official 80:2dab120a94c2 66 BOOL sem_mbx = __FALSE;
mbed_official 80:2dab120a94c2 67
mbed_official 80:2dab120a94c2 68 if (p_CB->cb_type == SCB || p_CB->cb_type == MCB || p_CB->cb_type == MUCB) {
mbed_official 80:2dab120a94c2 69 sem_mbx = __TRUE;
mbed_official 80:2dab120a94c2 70 }
mbed_official 80:2dab120a94c2 71 prio = p_task->prio;
mbed_official 80:2dab120a94c2 72 p_CB2 = p_CB->p_lnk;
mbed_official 80:2dab120a94c2 73 /* Search for an entry in the list */
mbed_official 80:2dab120a94c2 74 while (p_CB2 != NULL && prio <= p_CB2->prio) {
mbed_official 80:2dab120a94c2 75 p_CB = (P_XCB)p_CB2;
mbed_official 80:2dab120a94c2 76 p_CB2 = p_CB2->p_lnk;
mbed_official 80:2dab120a94c2 77 }
mbed_official 80:2dab120a94c2 78 /* Entry found, insert the task into the list */
mbed_official 80:2dab120a94c2 79 p_task->p_lnk = p_CB2;
mbed_official 80:2dab120a94c2 80 p_CB->p_lnk = p_task;
mbed_official 80:2dab120a94c2 81 if (sem_mbx) {
mbed_official 80:2dab120a94c2 82 if (p_CB2 != NULL) {
mbed_official 80:2dab120a94c2 83 p_CB2->p_rlnk = p_task;
mbed_official 80:2dab120a94c2 84 }
mbed_official 80:2dab120a94c2 85 p_task->p_rlnk = (P_TCB)p_CB;
mbed_official 80:2dab120a94c2 86 }
mbed_official 80:2dab120a94c2 87 else {
mbed_official 80:2dab120a94c2 88 p_task->p_rlnk = NULL;
mbed_official 80:2dab120a94c2 89 }
mbed_official 80:2dab120a94c2 90 }
mbed_official 80:2dab120a94c2 91
mbed_official 80:2dab120a94c2 92
mbed_official 80:2dab120a94c2 93 /*--------------------------- rt_get_first ----------------------------------*/
mbed_official 80:2dab120a94c2 94
mbed_official 80:2dab120a94c2 95 P_TCB rt_get_first (P_XCB p_CB) {
mbed_official 80:2dab120a94c2 96 /* Get task at head of list: it is the task with highest priority. */
mbed_official 80:2dab120a94c2 97 /* "p_CB" points to head of list. */
mbed_official 80:2dab120a94c2 98 P_TCB p_first;
mbed_official 80:2dab120a94c2 99
mbed_official 80:2dab120a94c2 100 p_first = p_CB->p_lnk;
mbed_official 80:2dab120a94c2 101 p_CB->p_lnk = p_first->p_lnk;
mbed_official 80:2dab120a94c2 102 if (p_CB->cb_type == SCB || p_CB->cb_type == MCB || p_CB->cb_type == MUCB) {
mbed_official 80:2dab120a94c2 103 if (p_first->p_lnk != NULL) {
mbed_official 80:2dab120a94c2 104 p_first->p_lnk->p_rlnk = (P_TCB)p_CB;
mbed_official 80:2dab120a94c2 105 p_first->p_lnk = NULL;
mbed_official 80:2dab120a94c2 106 }
mbed_official 80:2dab120a94c2 107 p_first->p_rlnk = NULL;
mbed_official 80:2dab120a94c2 108 }
mbed_official 80:2dab120a94c2 109 else {
mbed_official 80:2dab120a94c2 110 p_first->p_lnk = NULL;
mbed_official 80:2dab120a94c2 111 }
mbed_official 80:2dab120a94c2 112 return (p_first);
mbed_official 80:2dab120a94c2 113 }
mbed_official 80:2dab120a94c2 114
mbed_official 80:2dab120a94c2 115
mbed_official 80:2dab120a94c2 116 /*--------------------------- rt_put_rdy_first ------------------------------*/
mbed_official 80:2dab120a94c2 117
mbed_official 80:2dab120a94c2 118 void rt_put_rdy_first (P_TCB p_task) {
mbed_official 80:2dab120a94c2 119 /* Put task identified with "p_task" at the head of the ready list. The */
mbed_official 80:2dab120a94c2 120 /* task must have at least a priority equal to highest priority in list. */
mbed_official 80:2dab120a94c2 121 p_task->p_lnk = os_rdy.p_lnk;
mbed_official 80:2dab120a94c2 122 p_task->p_rlnk = NULL;
mbed_official 80:2dab120a94c2 123 os_rdy.p_lnk = p_task;
mbed_official 80:2dab120a94c2 124 }
mbed_official 80:2dab120a94c2 125
mbed_official 80:2dab120a94c2 126
mbed_official 80:2dab120a94c2 127 /*--------------------------- rt_get_same_rdy_prio --------------------------*/
mbed_official 80:2dab120a94c2 128
mbed_official 80:2dab120a94c2 129 P_TCB rt_get_same_rdy_prio (void) {
mbed_official 80:2dab120a94c2 130 /* Remove a task of same priority from ready list if any exists. Other- */
mbed_official 80:2dab120a94c2 131 /* wise return NULL. */
mbed_official 80:2dab120a94c2 132 P_TCB p_first;
mbed_official 80:2dab120a94c2 133
mbed_official 80:2dab120a94c2 134 p_first = os_rdy.p_lnk;
mbed_official 80:2dab120a94c2 135 if (p_first->prio == os_tsk.run->prio) {
mbed_official 80:2dab120a94c2 136 os_rdy.p_lnk = os_rdy.p_lnk->p_lnk;
mbed_official 80:2dab120a94c2 137 return (p_first);
mbed_official 80:2dab120a94c2 138 }
mbed_official 80:2dab120a94c2 139 return (NULL);
mbed_official 80:2dab120a94c2 140 }
mbed_official 80:2dab120a94c2 141
mbed_official 80:2dab120a94c2 142
mbed_official 80:2dab120a94c2 143 /*--------------------------- rt_resort_prio --------------------------------*/
mbed_official 80:2dab120a94c2 144
mbed_official 80:2dab120a94c2 145 void rt_resort_prio (P_TCB p_task) {
mbed_official 80:2dab120a94c2 146 /* Re-sort ordered lists after the priority of 'p_task' has changed. */
mbed_official 80:2dab120a94c2 147 P_TCB p_CB;
mbed_official 80:2dab120a94c2 148
mbed_official 80:2dab120a94c2 149 if (p_task->p_rlnk == NULL) {
mbed_official 80:2dab120a94c2 150 if (p_task->state == READY) {
mbed_official 80:2dab120a94c2 151 /* Task is chained into READY list. */
mbed_official 80:2dab120a94c2 152 p_CB = (P_TCB)&os_rdy;
mbed_official 80:2dab120a94c2 153 goto res;
mbed_official 80:2dab120a94c2 154 }
mbed_official 80:2dab120a94c2 155 }
mbed_official 80:2dab120a94c2 156 else {
mbed_official 80:2dab120a94c2 157 p_CB = p_task->p_rlnk;
mbed_official 80:2dab120a94c2 158 while (p_CB->cb_type == TCB) {
mbed_official 80:2dab120a94c2 159 /* Find a header of this task chain list. */
mbed_official 80:2dab120a94c2 160 p_CB = p_CB->p_rlnk;
mbed_official 80:2dab120a94c2 161 }
mbed_official 80:2dab120a94c2 162 res:rt_rmv_list (p_task);
mbed_official 80:2dab120a94c2 163 rt_put_prio ((P_XCB)p_CB, p_task);
mbed_official 80:2dab120a94c2 164 }
mbed_official 80:2dab120a94c2 165 }
mbed_official 80:2dab120a94c2 166
mbed_official 80:2dab120a94c2 167
mbed_official 80:2dab120a94c2 168 /*--------------------------- rt_put_dly ------------------------------------*/
mbed_official 80:2dab120a94c2 169
mbed_official 80:2dab120a94c2 170 void rt_put_dly (P_TCB p_task, U16 delay) {
mbed_official 80:2dab120a94c2 171 /* Put a task identified with "p_task" into chained delay wait list using */
mbed_official 80:2dab120a94c2 172 /* a delay value of "delay". */
mbed_official 80:2dab120a94c2 173 P_TCB p;
mbed_official 80:2dab120a94c2 174 U32 delta,idelay = delay;
mbed_official 80:2dab120a94c2 175
mbed_official 80:2dab120a94c2 176 p = (P_TCB)&os_dly;
mbed_official 80:2dab120a94c2 177 if (p->p_dlnk == NULL) {
mbed_official 80:2dab120a94c2 178 /* Delay list empty */
mbed_official 80:2dab120a94c2 179 delta = 0;
mbed_official 80:2dab120a94c2 180 goto last;
mbed_official 80:2dab120a94c2 181 }
mbed_official 80:2dab120a94c2 182 delta = os_dly.delta_time;
mbed_official 80:2dab120a94c2 183 while (delta < idelay) {
mbed_official 80:2dab120a94c2 184 if (p->p_dlnk == NULL) {
mbed_official 80:2dab120a94c2 185 /* End of list found */
mbed_official 80:2dab120a94c2 186 last: p_task->p_dlnk = NULL;
mbed_official 80:2dab120a94c2 187 p->p_dlnk = p_task;
mbed_official 80:2dab120a94c2 188 p_task->p_blnk = p;
mbed_official 80:2dab120a94c2 189 p->delta_time = (U16)(idelay - delta);
mbed_official 80:2dab120a94c2 190 p_task->delta_time = 0;
mbed_official 80:2dab120a94c2 191 return;
mbed_official 80:2dab120a94c2 192 }
mbed_official 80:2dab120a94c2 193 p = p->p_dlnk;
mbed_official 80:2dab120a94c2 194 delta += p->delta_time;
mbed_official 80:2dab120a94c2 195 }
mbed_official 80:2dab120a94c2 196 /* Right place found */
mbed_official 80:2dab120a94c2 197 p_task->p_dlnk = p->p_dlnk;
mbed_official 80:2dab120a94c2 198 p->p_dlnk = p_task;
mbed_official 80:2dab120a94c2 199 p_task->p_blnk = p;
mbed_official 80:2dab120a94c2 200 if (p_task->p_dlnk != NULL) {
mbed_official 80:2dab120a94c2 201 p_task->p_dlnk->p_blnk = p_task;
mbed_official 80:2dab120a94c2 202 }
mbed_official 80:2dab120a94c2 203 p_task->delta_time = (U16)(delta - idelay);
mbed_official 80:2dab120a94c2 204 p->delta_time -= p_task->delta_time;
mbed_official 80:2dab120a94c2 205 }
mbed_official 80:2dab120a94c2 206
mbed_official 80:2dab120a94c2 207
mbed_official 80:2dab120a94c2 208 /*--------------------------- rt_dec_dly ------------------------------------*/
mbed_official 80:2dab120a94c2 209
mbed_official 80:2dab120a94c2 210 void rt_dec_dly (void) {
mbed_official 80:2dab120a94c2 211 /* Decrement delta time of list head: remove tasks having a value of zero.*/
mbed_official 80:2dab120a94c2 212 P_TCB p_rdy;
mbed_official 80:2dab120a94c2 213
mbed_official 80:2dab120a94c2 214 if (os_dly.p_dlnk == NULL) {
mbed_official 80:2dab120a94c2 215 return;
mbed_official 80:2dab120a94c2 216 }
mbed_official 80:2dab120a94c2 217 os_dly.delta_time--;
mbed_official 80:2dab120a94c2 218 while ((os_dly.delta_time == 0) && (os_dly.p_dlnk != NULL)) {
mbed_official 80:2dab120a94c2 219 p_rdy = os_dly.p_dlnk;
mbed_official 80:2dab120a94c2 220 if (p_rdy->p_rlnk != NULL) {
mbed_official 80:2dab120a94c2 221 /* Task is really enqueued, remove task from semaphore/mailbox */
mbed_official 80:2dab120a94c2 222 /* timeout waiting list. */
mbed_official 80:2dab120a94c2 223 p_rdy->p_rlnk->p_lnk = p_rdy->p_lnk;
mbed_official 80:2dab120a94c2 224 if (p_rdy->p_lnk != NULL) {
mbed_official 80:2dab120a94c2 225 p_rdy->p_lnk->p_rlnk = p_rdy->p_rlnk;
mbed_official 80:2dab120a94c2 226 p_rdy->p_lnk = NULL;
mbed_official 80:2dab120a94c2 227 }
mbed_official 80:2dab120a94c2 228 p_rdy->p_rlnk = NULL;
mbed_official 80:2dab120a94c2 229 }
mbed_official 80:2dab120a94c2 230 rt_put_prio (&os_rdy, p_rdy);
mbed_official 80:2dab120a94c2 231 os_dly.delta_time = p_rdy->delta_time;
mbed_official 80:2dab120a94c2 232 if (p_rdy->state == WAIT_ITV) {
mbed_official 80:2dab120a94c2 233 /* Calculate the next time for interval wait. */
mbed_official 80:2dab120a94c2 234 p_rdy->delta_time = p_rdy->interval_time + (U16)os_time;
mbed_official 80:2dab120a94c2 235 }
mbed_official 80:2dab120a94c2 236 p_rdy->state = READY;
mbed_official 80:2dab120a94c2 237 os_dly.p_dlnk = p_rdy->p_dlnk;
mbed_official 80:2dab120a94c2 238 if (p_rdy->p_dlnk != NULL) {
mbed_official 80:2dab120a94c2 239 p_rdy->p_dlnk->p_blnk = (P_TCB)&os_dly;
mbed_official 80:2dab120a94c2 240 p_rdy->p_dlnk = NULL;
mbed_official 80:2dab120a94c2 241 }
mbed_official 80:2dab120a94c2 242 p_rdy->p_blnk = NULL;
mbed_official 80:2dab120a94c2 243 }
mbed_official 80:2dab120a94c2 244 }
mbed_official 80:2dab120a94c2 245
mbed_official 80:2dab120a94c2 246
mbed_official 80:2dab120a94c2 247 /*--------------------------- rt_rmv_list -----------------------------------*/
mbed_official 80:2dab120a94c2 248
mbed_official 80:2dab120a94c2 249 void rt_rmv_list (P_TCB p_task) {
mbed_official 80:2dab120a94c2 250 /* Remove task identified with "p_task" from ready, semaphore or mailbox */
mbed_official 80:2dab120a94c2 251 /* waiting list if enqueued. */
mbed_official 80:2dab120a94c2 252 P_TCB p_b;
mbed_official 80:2dab120a94c2 253
mbed_official 80:2dab120a94c2 254 if (p_task->p_rlnk != NULL) {
mbed_official 80:2dab120a94c2 255 /* A task is enqueued in semaphore / mailbox waiting list. */
mbed_official 80:2dab120a94c2 256 p_task->p_rlnk->p_lnk = p_task->p_lnk;
mbed_official 80:2dab120a94c2 257 if (p_task->p_lnk != NULL) {
mbed_official 80:2dab120a94c2 258 p_task->p_lnk->p_rlnk = p_task->p_rlnk;
mbed_official 80:2dab120a94c2 259 }
mbed_official 80:2dab120a94c2 260 return;
mbed_official 80:2dab120a94c2 261 }
mbed_official 80:2dab120a94c2 262
mbed_official 80:2dab120a94c2 263 p_b = (P_TCB)&os_rdy;
mbed_official 80:2dab120a94c2 264 while (p_b != NULL) {
mbed_official 80:2dab120a94c2 265 /* Search the ready list for task "p_task" */
mbed_official 80:2dab120a94c2 266 if (p_b->p_lnk == p_task) {
mbed_official 80:2dab120a94c2 267 p_b->p_lnk = p_task->p_lnk;
mbed_official 80:2dab120a94c2 268 return;
mbed_official 80:2dab120a94c2 269 }
mbed_official 80:2dab120a94c2 270 p_b = p_b->p_lnk;
mbed_official 80:2dab120a94c2 271 }
mbed_official 80:2dab120a94c2 272 }
mbed_official 80:2dab120a94c2 273
mbed_official 80:2dab120a94c2 274
mbed_official 80:2dab120a94c2 275 /*--------------------------- rt_rmv_dly ------------------------------------*/
mbed_official 80:2dab120a94c2 276
mbed_official 80:2dab120a94c2 277 void rt_rmv_dly (P_TCB p_task) {
mbed_official 80:2dab120a94c2 278 /* Remove task identified with "p_task" from delay list if enqueued. */
mbed_official 80:2dab120a94c2 279 P_TCB p_b;
mbed_official 80:2dab120a94c2 280
mbed_official 80:2dab120a94c2 281 p_b = p_task->p_blnk;
mbed_official 80:2dab120a94c2 282 if (p_b != NULL) {
mbed_official 80:2dab120a94c2 283 /* Task is really enqueued */
mbed_official 80:2dab120a94c2 284 p_b->p_dlnk = p_task->p_dlnk;
mbed_official 80:2dab120a94c2 285 if (p_task->p_dlnk != NULL) {
mbed_official 80:2dab120a94c2 286 /* 'p_task' is in the middle of list */
mbed_official 80:2dab120a94c2 287 p_b->delta_time += p_task->delta_time;
mbed_official 80:2dab120a94c2 288 p_task->p_dlnk->p_blnk = p_b;
mbed_official 80:2dab120a94c2 289 p_task->p_dlnk = NULL;
mbed_official 80:2dab120a94c2 290 }
mbed_official 80:2dab120a94c2 291 else {
mbed_official 80:2dab120a94c2 292 /* 'p_task' is at the end of list */
mbed_official 80:2dab120a94c2 293 p_b->delta_time = 0;
mbed_official 80:2dab120a94c2 294 }
mbed_official 80:2dab120a94c2 295 p_task->p_blnk = NULL;
mbed_official 80:2dab120a94c2 296 }
mbed_official 80:2dab120a94c2 297 }
mbed_official 80:2dab120a94c2 298
mbed_official 80:2dab120a94c2 299
mbed_official 80:2dab120a94c2 300 /*--------------------------- rt_psq_enq ------------------------------------*/
mbed_official 80:2dab120a94c2 301
mbed_official 80:2dab120a94c2 302 void rt_psq_enq (OS_ID entry, U32 arg) {
mbed_official 80:2dab120a94c2 303 /* Insert post service request "entry" into ps-queue. */
mbed_official 80:2dab120a94c2 304 U32 idx;
mbed_official 80:2dab120a94c2 305
mbed_official 80:2dab120a94c2 306 idx = rt_inc_qi (os_psq->size, &os_psq->count, &os_psq->first);
mbed_official 80:2dab120a94c2 307 if (idx < os_psq->size) {
mbed_official 80:2dab120a94c2 308 os_psq->q[idx].id = entry;
mbed_official 80:2dab120a94c2 309 os_psq->q[idx].arg = arg;
mbed_official 80:2dab120a94c2 310 }
mbed_official 80:2dab120a94c2 311 else {
mbed_official 80:2dab120a94c2 312 os_error (OS_ERR_FIFO_OVF);
mbed_official 80:2dab120a94c2 313 }
mbed_official 80:2dab120a94c2 314 }
mbed_official 80:2dab120a94c2 315
mbed_official 80:2dab120a94c2 316
mbed_official 80:2dab120a94c2 317 /*----------------------------------------------------------------------------
mbed_official 80:2dab120a94c2 318 * end of file
mbed_official 80:2dab120a94c2 319 *---------------------------------------------------------------------------*/
mbed_official 80:2dab120a94c2 320