james kain / Mbed 2 deprecated GPS_Incremental

Dependencies:   mbed

Fork of GPS_Incremental by Dan Matthews

Committer:
dannyman939
Date:
Tue Mar 19 02:17:40 2013 +0000
Revision:
0:c746ee34feae
Basic functionality, Chris Version 0.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dannyman939 0:c746ee34feae 1 /*----------------------------------------------------------------------------
dannyman939 0:c746ee34feae 2 * RL-ARM - RTX
dannyman939 0:c746ee34feae 3 *----------------------------------------------------------------------------
dannyman939 0:c746ee34feae 4 * Name: RT_TASK.C
dannyman939 0:c746ee34feae 5 * Purpose: Task functions and system start up.
dannyman939 0:c746ee34feae 6 * Rev.: V4.50
dannyman939 0:c746ee34feae 7 *----------------------------------------------------------------------------
dannyman939 0:c746ee34feae 8 *
dannyman939 0:c746ee34feae 9 * Copyright (c) 1999-2009 KEIL, 2009-2012 ARM Germany GmbH
dannyman939 0:c746ee34feae 10 * All rights reserved.
dannyman939 0:c746ee34feae 11 * Redistribution and use in source and binary forms, with or without
dannyman939 0:c746ee34feae 12 * modification, are permitted provided that the following conditions are met:
dannyman939 0:c746ee34feae 13 * - Redistributions of source code must retain the above copyright
dannyman939 0:c746ee34feae 14 * notice, this list of conditions and the following disclaimer.
dannyman939 0:c746ee34feae 15 * - Redistributions in binary form must reproduce the above copyright
dannyman939 0:c746ee34feae 16 * notice, this list of conditions and the following disclaimer in the
dannyman939 0:c746ee34feae 17 * documentation and/or other materials provided with the distribution.
dannyman939 0:c746ee34feae 18 * - Neither the name of ARM nor the names of its contributors may be used
dannyman939 0:c746ee34feae 19 * to endorse or promote products derived from this software without
dannyman939 0:c746ee34feae 20 * specific prior written permission.
dannyman939 0:c746ee34feae 21 *
dannyman939 0:c746ee34feae 22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
dannyman939 0:c746ee34feae 23 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
dannyman939 0:c746ee34feae 24 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
dannyman939 0:c746ee34feae 25 * ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE
dannyman939 0:c746ee34feae 26 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
dannyman939 0:c746ee34feae 27 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
dannyman939 0:c746ee34feae 28 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
dannyman939 0:c746ee34feae 29 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
dannyman939 0:c746ee34feae 30 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
dannyman939 0:c746ee34feae 31 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
dannyman939 0:c746ee34feae 32 * POSSIBILITY OF SUCH DAMAGE.
dannyman939 0:c746ee34feae 33 *---------------------------------------------------------------------------*/
dannyman939 0:c746ee34feae 34
dannyman939 0:c746ee34feae 35 #include "rt_TypeDef.h"
dannyman939 0:c746ee34feae 36 #include "RTX_Config.h"
dannyman939 0:c746ee34feae 37 #include "rt_System.h"
dannyman939 0:c746ee34feae 38 #include "rt_Task.h"
dannyman939 0:c746ee34feae 39 #include "rt_List.h"
dannyman939 0:c746ee34feae 40 #include "rt_MemBox.h"
dannyman939 0:c746ee34feae 41 #include "rt_Robin.h"
dannyman939 0:c746ee34feae 42 #include "rt_HAL_CM.h"
dannyman939 0:c746ee34feae 43
dannyman939 0:c746ee34feae 44 /*----------------------------------------------------------------------------
dannyman939 0:c746ee34feae 45 * Global Variables
dannyman939 0:c746ee34feae 46 *---------------------------------------------------------------------------*/
dannyman939 0:c746ee34feae 47
dannyman939 0:c746ee34feae 48 /* Running and next task info. */
dannyman939 0:c746ee34feae 49 struct OS_TSK os_tsk;
dannyman939 0:c746ee34feae 50
dannyman939 0:c746ee34feae 51 /* Task Control Blocks of idle demon */
dannyman939 0:c746ee34feae 52 struct OS_TCB os_idle_TCB;
dannyman939 0:c746ee34feae 53
dannyman939 0:c746ee34feae 54
dannyman939 0:c746ee34feae 55 /*----------------------------------------------------------------------------
dannyman939 0:c746ee34feae 56 * Local Functions
dannyman939 0:c746ee34feae 57 *---------------------------------------------------------------------------*/
dannyman939 0:c746ee34feae 58
dannyman939 0:c746ee34feae 59 OS_TID rt_get_TID (void) {
dannyman939 0:c746ee34feae 60 U32 tid;
dannyman939 0:c746ee34feae 61
dannyman939 0:c746ee34feae 62 for (tid = 1; tid <= os_maxtaskrun; tid++) {
dannyman939 0:c746ee34feae 63 if (os_active_TCB[tid-1] == NULL) {
dannyman939 0:c746ee34feae 64 return ((OS_TID)tid);
dannyman939 0:c746ee34feae 65 }
dannyman939 0:c746ee34feae 66 }
dannyman939 0:c746ee34feae 67 return (0);
dannyman939 0:c746ee34feae 68 }
dannyman939 0:c746ee34feae 69
dannyman939 0:c746ee34feae 70 #if defined (__CC_ARM) && !defined (__MICROLIB)
dannyman939 0:c746ee34feae 71 /*--------------------------- __user_perthread_libspace ---------------------*/
dannyman939 0:c746ee34feae 72 extern void *__libspace_start;
dannyman939 0:c746ee34feae 73
dannyman939 0:c746ee34feae 74 void *__user_perthread_libspace (void) {
dannyman939 0:c746ee34feae 75 /* Provide a separate libspace for each task. */
dannyman939 0:c746ee34feae 76 if (os_tsk.run == NULL) {
dannyman939 0:c746ee34feae 77 /* RTX not running yet. */
dannyman939 0:c746ee34feae 78 return (&__libspace_start);
dannyman939 0:c746ee34feae 79 }
dannyman939 0:c746ee34feae 80 return (void *)(os_tsk.run->std_libspace);
dannyman939 0:c746ee34feae 81 }
dannyman939 0:c746ee34feae 82 #endif
dannyman939 0:c746ee34feae 83
dannyman939 0:c746ee34feae 84 /*--------------------------- rt_init_context -------------------------------*/
dannyman939 0:c746ee34feae 85
dannyman939 0:c746ee34feae 86 void rt_init_context (P_TCB p_TCB, U8 priority, FUNCP task_body) {
dannyman939 0:c746ee34feae 87 /* Initialize general part of the Task Control Block. */
dannyman939 0:c746ee34feae 88 p_TCB->cb_type = TCB;
dannyman939 0:c746ee34feae 89 p_TCB->state = READY;
dannyman939 0:c746ee34feae 90 p_TCB->prio = priority;
dannyman939 0:c746ee34feae 91 p_TCB->p_lnk = NULL;
dannyman939 0:c746ee34feae 92 p_TCB->p_rlnk = NULL;
dannyman939 0:c746ee34feae 93 p_TCB->p_dlnk = NULL;
dannyman939 0:c746ee34feae 94 p_TCB->p_blnk = NULL;
dannyman939 0:c746ee34feae 95 p_TCB->delta_time = 0;
dannyman939 0:c746ee34feae 96 p_TCB->interval_time = 0;
dannyman939 0:c746ee34feae 97 p_TCB->events = 0;
dannyman939 0:c746ee34feae 98 p_TCB->waits = 0;
dannyman939 0:c746ee34feae 99 p_TCB->stack_frame = 0;
dannyman939 0:c746ee34feae 100
dannyman939 0:c746ee34feae 101 rt_init_stack (p_TCB, task_body);
dannyman939 0:c746ee34feae 102 }
dannyman939 0:c746ee34feae 103
dannyman939 0:c746ee34feae 104
dannyman939 0:c746ee34feae 105 /*--------------------------- rt_switch_req ---------------------------------*/
dannyman939 0:c746ee34feae 106
dannyman939 0:c746ee34feae 107 void rt_switch_req (P_TCB p_new) {
dannyman939 0:c746ee34feae 108 /* Switch to next task (identified by "p_new"). */
dannyman939 0:c746ee34feae 109 os_tsk.new = p_new;
dannyman939 0:c746ee34feae 110 p_new->state = RUNNING;
dannyman939 0:c746ee34feae 111 DBG_TASK_SWITCH(p_new->task_id);
dannyman939 0:c746ee34feae 112 }
dannyman939 0:c746ee34feae 113
dannyman939 0:c746ee34feae 114
dannyman939 0:c746ee34feae 115 /*--------------------------- rt_dispatch -----------------------------------*/
dannyman939 0:c746ee34feae 116
dannyman939 0:c746ee34feae 117 void rt_dispatch (P_TCB next_TCB) {
dannyman939 0:c746ee34feae 118 /* Dispatch next task if any identified or dispatch highest ready task */
dannyman939 0:c746ee34feae 119 /* "next_TCB" identifies a task to run or has value NULL (=no next task) */
dannyman939 0:c746ee34feae 120 if (next_TCB == NULL) {
dannyman939 0:c746ee34feae 121 /* Running task was blocked: continue with highest ready task */
dannyman939 0:c746ee34feae 122 next_TCB = rt_get_first (&os_rdy);
dannyman939 0:c746ee34feae 123 rt_switch_req (next_TCB);
dannyman939 0:c746ee34feae 124 }
dannyman939 0:c746ee34feae 125 else {
dannyman939 0:c746ee34feae 126 /* Check which task continues */
dannyman939 0:c746ee34feae 127 if (next_TCB->prio > os_tsk.run->prio) {
dannyman939 0:c746ee34feae 128 /* preempt running task */
dannyman939 0:c746ee34feae 129 rt_put_rdy_first (os_tsk.run);
dannyman939 0:c746ee34feae 130 os_tsk.run->state = READY;
dannyman939 0:c746ee34feae 131 rt_switch_req (next_TCB);
dannyman939 0:c746ee34feae 132 }
dannyman939 0:c746ee34feae 133 else {
dannyman939 0:c746ee34feae 134 /* put next task into ready list, no task switch takes place */
dannyman939 0:c746ee34feae 135 next_TCB->state = READY;
dannyman939 0:c746ee34feae 136 rt_put_prio (&os_rdy, next_TCB);
dannyman939 0:c746ee34feae 137 }
dannyman939 0:c746ee34feae 138 }
dannyman939 0:c746ee34feae 139 }
dannyman939 0:c746ee34feae 140
dannyman939 0:c746ee34feae 141
dannyman939 0:c746ee34feae 142 /*--------------------------- rt_block --------------------------------------*/
dannyman939 0:c746ee34feae 143
dannyman939 0:c746ee34feae 144 void rt_block (U16 timeout, U8 block_state) {
dannyman939 0:c746ee34feae 145 /* Block running task and choose next ready task. */
dannyman939 0:c746ee34feae 146 /* "timeout" sets a time-out value or is 0xffff (=no time-out). */
dannyman939 0:c746ee34feae 147 /* "block_state" defines the appropriate task state */
dannyman939 0:c746ee34feae 148 P_TCB next_TCB;
dannyman939 0:c746ee34feae 149
dannyman939 0:c746ee34feae 150 if (timeout) {
dannyman939 0:c746ee34feae 151 if (timeout < 0xffff) {
dannyman939 0:c746ee34feae 152 rt_put_dly (os_tsk.run, timeout);
dannyman939 0:c746ee34feae 153 }
dannyman939 0:c746ee34feae 154 os_tsk.run->state = block_state;
dannyman939 0:c746ee34feae 155 next_TCB = rt_get_first (&os_rdy);
dannyman939 0:c746ee34feae 156 rt_switch_req (next_TCB);
dannyman939 0:c746ee34feae 157 }
dannyman939 0:c746ee34feae 158 }
dannyman939 0:c746ee34feae 159
dannyman939 0:c746ee34feae 160
dannyman939 0:c746ee34feae 161 /*--------------------------- rt_tsk_pass -----------------------------------*/
dannyman939 0:c746ee34feae 162
dannyman939 0:c746ee34feae 163 void rt_tsk_pass (void) {
dannyman939 0:c746ee34feae 164 /* Allow tasks of same priority level to run cooperatively.*/
dannyman939 0:c746ee34feae 165 P_TCB p_new;
dannyman939 0:c746ee34feae 166
dannyman939 0:c746ee34feae 167 p_new = rt_get_same_rdy_prio();
dannyman939 0:c746ee34feae 168 if (p_new != NULL) {
dannyman939 0:c746ee34feae 169 rt_put_prio ((P_XCB)&os_rdy, os_tsk.run);
dannyman939 0:c746ee34feae 170 os_tsk.run->state = READY;
dannyman939 0:c746ee34feae 171 rt_switch_req (p_new);
dannyman939 0:c746ee34feae 172 }
dannyman939 0:c746ee34feae 173 }
dannyman939 0:c746ee34feae 174
dannyman939 0:c746ee34feae 175
dannyman939 0:c746ee34feae 176 /*--------------------------- rt_tsk_self -----------------------------------*/
dannyman939 0:c746ee34feae 177
dannyman939 0:c746ee34feae 178 OS_TID rt_tsk_self (void) {
dannyman939 0:c746ee34feae 179 /* Return own task identifier value. */
dannyman939 0:c746ee34feae 180 if (os_tsk.run == NULL) {
dannyman939 0:c746ee34feae 181 return (0);
dannyman939 0:c746ee34feae 182 }
dannyman939 0:c746ee34feae 183 return (os_tsk.run->task_id);
dannyman939 0:c746ee34feae 184 }
dannyman939 0:c746ee34feae 185
dannyman939 0:c746ee34feae 186
dannyman939 0:c746ee34feae 187 /*--------------------------- rt_tsk_prio -----------------------------------*/
dannyman939 0:c746ee34feae 188
dannyman939 0:c746ee34feae 189 OS_RESULT rt_tsk_prio (OS_TID task_id, U8 new_prio) {
dannyman939 0:c746ee34feae 190 /* Change execution priority of a task to "new_prio". */
dannyman939 0:c746ee34feae 191 P_TCB p_task;
dannyman939 0:c746ee34feae 192
dannyman939 0:c746ee34feae 193 if (task_id == 0) {
dannyman939 0:c746ee34feae 194 /* Change execution priority of calling task. */
dannyman939 0:c746ee34feae 195 os_tsk.run->prio = new_prio;
dannyman939 0:c746ee34feae 196 run:if (rt_rdy_prio() > new_prio) {
dannyman939 0:c746ee34feae 197 rt_put_prio (&os_rdy, os_tsk.run);
dannyman939 0:c746ee34feae 198 os_tsk.run->state = READY;
dannyman939 0:c746ee34feae 199 rt_dispatch (NULL);
dannyman939 0:c746ee34feae 200 }
dannyman939 0:c746ee34feae 201 return (OS_R_OK);
dannyman939 0:c746ee34feae 202 }
dannyman939 0:c746ee34feae 203
dannyman939 0:c746ee34feae 204 /* Find the task in the "os_active_TCB" array. */
dannyman939 0:c746ee34feae 205 if (task_id > os_maxtaskrun || os_active_TCB[task_id-1] == NULL) {
dannyman939 0:c746ee34feae 206 /* Task with "task_id" not found or not started. */
dannyman939 0:c746ee34feae 207 return (OS_R_NOK);
dannyman939 0:c746ee34feae 208 }
dannyman939 0:c746ee34feae 209 p_task = os_active_TCB[task_id-1];
dannyman939 0:c746ee34feae 210 p_task->prio = new_prio;
dannyman939 0:c746ee34feae 211 if (p_task == os_tsk.run) {
dannyman939 0:c746ee34feae 212 goto run;
dannyman939 0:c746ee34feae 213 }
dannyman939 0:c746ee34feae 214 rt_resort_prio (p_task);
dannyman939 0:c746ee34feae 215 if (p_task->state == READY) {
dannyman939 0:c746ee34feae 216 /* Task enqueued in a ready list. */
dannyman939 0:c746ee34feae 217 p_task = rt_get_first (&os_rdy);
dannyman939 0:c746ee34feae 218 rt_dispatch (p_task);
dannyman939 0:c746ee34feae 219 }
dannyman939 0:c746ee34feae 220 return (OS_R_OK);
dannyman939 0:c746ee34feae 221 }
dannyman939 0:c746ee34feae 222
dannyman939 0:c746ee34feae 223 /*--------------------------- rt_tsk_delete ---------------------------------*/
dannyman939 0:c746ee34feae 224
dannyman939 0:c746ee34feae 225 OS_RESULT rt_tsk_delete (OS_TID task_id) {
dannyman939 0:c746ee34feae 226 /* Terminate the task identified with "task_id". */
dannyman939 0:c746ee34feae 227 P_TCB task_context;
dannyman939 0:c746ee34feae 228
dannyman939 0:c746ee34feae 229 if (task_id == 0 || task_id == os_tsk.run->task_id) {
dannyman939 0:c746ee34feae 230 /* Terminate itself. */
dannyman939 0:c746ee34feae 231 os_tsk.run->state = INACTIVE;
dannyman939 0:c746ee34feae 232 os_tsk.run->tsk_stack = rt_get_PSP ();
dannyman939 0:c746ee34feae 233 rt_stk_check ();
dannyman939 0:c746ee34feae 234 os_active_TCB[os_tsk.run->task_id-1] = NULL;
dannyman939 0:c746ee34feae 235
dannyman939 0:c746ee34feae 236 os_tsk.run->stack = NULL;
dannyman939 0:c746ee34feae 237 DBG_TASK_NOTIFY(os_tsk.run, __FALSE);
dannyman939 0:c746ee34feae 238 os_tsk.run = NULL;
dannyman939 0:c746ee34feae 239 rt_dispatch (NULL);
dannyman939 0:c746ee34feae 240 /* The program should never come to this point. */
dannyman939 0:c746ee34feae 241 }
dannyman939 0:c746ee34feae 242 else {
dannyman939 0:c746ee34feae 243 /* Find the task in the "os_active_TCB" array. */
dannyman939 0:c746ee34feae 244 if (task_id > os_maxtaskrun || os_active_TCB[task_id-1] == NULL) {
dannyman939 0:c746ee34feae 245 /* Task with "task_id" not found or not started. */
dannyman939 0:c746ee34feae 246 return (OS_R_NOK);
dannyman939 0:c746ee34feae 247 }
dannyman939 0:c746ee34feae 248 task_context = os_active_TCB[task_id-1];
dannyman939 0:c746ee34feae 249 rt_rmv_list (task_context);
dannyman939 0:c746ee34feae 250 rt_rmv_dly (task_context);
dannyman939 0:c746ee34feae 251 os_active_TCB[task_id-1] = NULL;
dannyman939 0:c746ee34feae 252
dannyman939 0:c746ee34feae 253 task_context->stack = NULL;
dannyman939 0:c746ee34feae 254 DBG_TASK_NOTIFY(task_context, __FALSE);
dannyman939 0:c746ee34feae 255 }
dannyman939 0:c746ee34feae 256 return (OS_R_OK);
dannyman939 0:c746ee34feae 257 }
dannyman939 0:c746ee34feae 258
dannyman939 0:c746ee34feae 259
dannyman939 0:c746ee34feae 260 /*--------------------------- rt_sys_init -----------------------------------*/
dannyman939 0:c746ee34feae 261 void rt_sys_init (void) {
dannyman939 0:c746ee34feae 262 /* Initialize system and start up task declared with "first_task". */
dannyman939 0:c746ee34feae 263 U32 i;
dannyman939 0:c746ee34feae 264
dannyman939 0:c746ee34feae 265 DBG_INIT();
dannyman939 0:c746ee34feae 266
dannyman939 0:c746ee34feae 267 /* Initialize dynamic memory and task TCB pointers to NULL. */
dannyman939 0:c746ee34feae 268 for (i = 0; i < os_maxtaskrun; i++) {
dannyman939 0:c746ee34feae 269 os_active_TCB[i] = NULL;
dannyman939 0:c746ee34feae 270 }
dannyman939 0:c746ee34feae 271
dannyman939 0:c746ee34feae 272 /* Set up TCB of idle demon */
dannyman939 0:c746ee34feae 273 os_idle_TCB.task_id = 255;
dannyman939 0:c746ee34feae 274 os_idle_TCB.priv_stack = idle_task_stack_size;
dannyman939 0:c746ee34feae 275 os_idle_TCB.stack = idle_task_stack;
dannyman939 0:c746ee34feae 276 rt_init_context (&os_idle_TCB, 0, os_idle_demon);
dannyman939 0:c746ee34feae 277
dannyman939 0:c746ee34feae 278 /* Set up ready list: initially empty */
dannyman939 0:c746ee34feae 279 os_rdy.cb_type = HCB;
dannyman939 0:c746ee34feae 280 os_rdy.p_lnk = NULL;
dannyman939 0:c746ee34feae 281 /* Set up delay list: initially empty */
dannyman939 0:c746ee34feae 282 os_dly.cb_type = HCB;
dannyman939 0:c746ee34feae 283 os_dly.p_dlnk = NULL;
dannyman939 0:c746ee34feae 284 os_dly.p_blnk = NULL;
dannyman939 0:c746ee34feae 285 os_dly.delta_time = 0;
dannyman939 0:c746ee34feae 286
dannyman939 0:c746ee34feae 287 /* Fix SP and systemvariables to assume idle task is running */
dannyman939 0:c746ee34feae 288 /* Transform main program into idle task by assuming idle TCB */
dannyman939 0:c746ee34feae 289 os_tsk.run = &os_idle_TCB;
dannyman939 0:c746ee34feae 290 os_tsk.run->state = RUNNING;
dannyman939 0:c746ee34feae 291
dannyman939 0:c746ee34feae 292 /* Initialize ps queue */
dannyman939 0:c746ee34feae 293 os_psq->first = 0;
dannyman939 0:c746ee34feae 294 os_psq->last = 0;
dannyman939 0:c746ee34feae 295 os_psq->size = os_fifo_size;
dannyman939 0:c746ee34feae 296
dannyman939 0:c746ee34feae 297 rt_init_robin ();
dannyman939 0:c746ee34feae 298
dannyman939 0:c746ee34feae 299 /* Intitialize SVC and PendSV */
dannyman939 0:c746ee34feae 300 rt_svc_init ();
dannyman939 0:c746ee34feae 301 }
dannyman939 0:c746ee34feae 302
dannyman939 0:c746ee34feae 303
dannyman939 0:c746ee34feae 304 /*--------------------------- rt_sys_start ----------------------------------*/
dannyman939 0:c746ee34feae 305 void rt_sys_start (void) {
dannyman939 0:c746ee34feae 306 /* Start system */
dannyman939 0:c746ee34feae 307
dannyman939 0:c746ee34feae 308 /* Intitialize and start system clock timer */
dannyman939 0:c746ee34feae 309 os_tick_irqn = os_tick_init ();
dannyman939 0:c746ee34feae 310 if (os_tick_irqn >= 0) {
dannyman939 0:c746ee34feae 311 OS_X_INIT(os_tick_irqn);
dannyman939 0:c746ee34feae 312 }
dannyman939 0:c746ee34feae 313 }
dannyman939 0:c746ee34feae 314
dannyman939 0:c746ee34feae 315 /*----------------------------------------------------------------------------
dannyman939 0:c746ee34feae 316 * end of file
dannyman939 0:c746ee34feae 317 *---------------------------------------------------------------------------*/