Andrew L / mbos

Dependents:   rtest LeonardoMbos OS_test Labo_TRSE_Drone ... more

mbos.cpp@4:e740e08cbea9, 2011-01-31 (annotated)

Committer:
AndrewL
Date:
Mon Jan 31 03:39:39 2011 +0000
Revision:
4:e740e08cbea9
Parent:
0:1dafafe7d505
Child:
5:6eef5e47e154
Submission

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AndrewL 0:1dafafe7d505 1 /***********************************************************************************
AndrewL 0:1dafafe7d505 2 * m b o s R T O S F O R m b e d (ARM CORTEX M3)
AndrewL 0:1dafafe7d505 3 *
AndrewL 0:1dafafe7d505 4 * Copyright (c) 2010 - 2011 Andrew Levido
AndrewL 0:1dafafe7d505 5 *
AndrewL 0:1dafafe7d505 6 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR "AS IS" AND ANY EXPRESS OR IMPLIED
AndrewL 0:1dafafe7d505 7 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
AndrewL 0:1dafafe7d505 8 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
AndrewL 0:1dafafe7d505 9 * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
AndrewL 0:1dafafe7d505 10 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
AndrewL 0:1dafafe7d505 11 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
AndrewL 0:1dafafe7d505 12 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
AndrewL 0:1dafafe7d505 13 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
AndrewL 0:1dafafe7d505 14 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
AndrewL 0:1dafafe7d505 15 */
AndrewL 0:1dafafe7d505 16 #include "mbos.h"
AndrewL 0:1dafafe7d505 17 #include "mbed.h"
AndrewL 0:1dafafe7d505 18
AndrewL 0:1dafafe7d505 19 #define MAX_PRIO 99 // maximum priority
AndrewL 0:1dafafe7d505 20 #define READY MAX_PRIO + 1 // always > MAX_PRIO
AndrewL 0:1dafafe7d505 21 #define WAITING 0 // aways 0
AndrewL 0:1dafafe7d505 22 #define DEFAULT_IDLE_STACK_SZ 32 // in words
AndrewL 0:1dafafe7d505 23 #define MAX_TASKS 99 // tasks 0.. 99 including idle
AndrewL 0:1dafafe7d505 24 #define MAX_TIMERS 99 // timers 0..99
AndrewL 0:1dafafe7d505 25 #define MAX_RESOURCES 99 // resources 0..99
AndrewL 0:1dafafe7d505 26 #define MIN_STACKSZ 32 // enough to run an empty function
AndrewL 0:1dafafe7d505 27 #define TICKS_PER_SECOND 1000 // 1ms Ticks
AndrewL 0:1dafafe7d505 28
AndrewL 0:1dafafe7d505 29 typedef struct _tcb{ // task control block
AndrewL 0:1dafafe7d505 30 uint id;
AndrewL 0:1dafafe7d505 31 uint *stack;
AndrewL 0:1dafafe7d505 32 uint priostate;
AndrewL 0:1dafafe7d505 33 uint eventlist;
AndrewL 0:1dafafe7d505 34 uint eventmask;
AndrewL 0:1dafafe7d505 35 uint *stacklimit;
AndrewL 0:1dafafe7d505 36 }_tcb_t;
AndrewL 0:1dafafe7d505 37
AndrewL 0:1dafafe7d505 38 typedef struct _timer{ // timer cotrol block
AndrewL 0:1dafafe7d505 39 uint timer;
AndrewL 0:1dafafe7d505 40 uint reload;
AndrewL 0:1dafafe7d505 41 uint event;
AndrewL 0:1dafafe7d505 42 uint task;
AndrewL 0:1dafafe7d505 43 }_timer_t;
AndrewL 0:1dafafe7d505 44
AndrewL 0:1dafafe7d505 45 typedef struct _rcb{ // resource control block
AndrewL 0:1dafafe7d505 46 uint lock;
AndrewL 0:1dafafe7d505 47 uint priority;
AndrewL 0:1dafafe7d505 48 uint taskprio;
AndrewL 0:1dafafe7d505 49 }_resource_t;
AndrewL 0:1dafafe7d505 50
AndrewL 0:1dafafe7d505 51 extern "C" void _startos(void); // Assembly routine to start the scheduler
AndrewL 0:1dafafe7d505 52 extern "C" void _swap(void); // Assembly routine to set PendSV exception
AndrewL 0:1dafafe7d505 53 extern "C" void SysTick_Handler(void); // Override weak declaration in startup file
AndrewL 0:1dafafe7d505 54 extern "C" void _stackerror(uint task); // Stack error function called by assembler
AndrewL 0:1dafafe7d505 55
AndrewL 0:1dafafe7d505 56 extern void __attribute__((__weak__)) mbosIdleTask(void); // Allow user to write their own idle task
AndrewL 0:1dafafe7d505 57
AndrewL 0:1dafafe7d505 58 // Static Variables
AndrewL 0:1dafafe7d505 59 uint _hardstacklimit; // stack limit
AndrewL 0:1dafafe7d505 60 _tcb_t **_tasklist; // pointer to task list
AndrewL 0:1dafafe7d505 61 _tcb_t *_tasks; // pointer to task control block 0
AndrewL 0:1dafafe7d505 62 _timer_t *_timers; // pointer to timer control block 0
AndrewL 0:1dafafe7d505 63 _resource_t *_resources; // pointer to resource control block 0
AndrewL 0:1dafafe7d505 64 uint _numtasks; // number of tasks (including idle)
AndrewL 0:1dafafe7d505 65 uint _numtimers; // number of timers
AndrewL 0:1dafafe7d505 66 uint _numresources; // number of resources
AndrewL 0:1dafafe7d505 67 uint _tasklistsize; // task list length in bytes
AndrewL 0:1dafafe7d505 68
AndrewL 0:1dafafe7d505 69 // Default Idle Task -------------------------------------------------------------------
AndrewL 0:1dafafe7d505 70 void _idletask(void) { while(1); } // Default idle task
AndrewL 0:1dafafe7d505 71
AndrewL 0:1dafafe7d505 72 // Constructor -------------------------------------------------------------------------
AndrewL 0:1dafafe7d505 73 mbos::mbos(uint ntasks, uint ntimers, uint nresources)
AndrewL 0:1dafafe7d505 74 {
AndrewL 0:1dafafe7d505 75 uint i;
AndrewL 0:1dafafe7d505 76
AndrewL 0:1dafafe7d505 77 ntasks += 1;
AndrewL 0:1dafafe7d505 78 // Create the tcblist, zero initialised
AndrewL 0:1dafafe7d505 79 if(ntasks > MAX_TASKS || ntasks < 1) error("mbos::mbos - %i is an invalid number of tasks\n", ntasks);
AndrewL 0:1dafafe7d505 80 _tasklist = (_tcb_t**)calloc(sizeof(_tcb_t*), ntasks);
AndrewL 0:1dafafe7d505 81 if(_tasklist == 0) error("mbos::mbos - Insufficient memory to create Tasklist\n");
AndrewL 0:1dafafe7d505 82
AndrewL 0:1dafafe7d505 83 // Create the tcbs
AndrewL 0:1dafafe7d505 84 _tasklist[0] = (_tcb_t*)calloc(sizeof(_tcb_t), ntasks);
AndrewL 0:1dafafe7d505 85 if(_tasklist[0] == 0) error("mbos::mbos - Insufficient memory to create TCBs\n");
AndrewL 0:1dafafe7d505 86 for(i = 1; i < ntasks; i++){
AndrewL 0:1dafafe7d505 87 _tasklist[i] = _tasklist[i - 1] + 1;
AndrewL 0:1dafafe7d505 88 }
AndrewL 0:1dafafe7d505 89 // Create the timers and clear them
AndrewL 0:1dafafe7d505 90 if(ntimers > MAX_TIMERS + 1) error("mbos::mbos - %i is an invalid number of Timers\n", ntimers);
AndrewL 0:1dafafe7d505 91 _timers = (_timer_t*)calloc(sizeof(_timer_t), ntimers);
AndrewL 0:1dafafe7d505 92 if(_timers == 0) error("mbos::mbos - Insufficient memory to create Timers");
AndrewL 0:1dafafe7d505 93
AndrewL 0:1dafafe7d505 94 // create the resources & clear them
AndrewL 0:1dafafe7d505 95 if(nresources > MAX_RESOURCES + 1) error("mbos::mbos - %i is an invalid number of Resources\n", nresources);
AndrewL 0:1dafafe7d505 96 _resources = (_resource_t*)calloc(sizeof(_resource_t), nresources);
AndrewL 0:1dafafe7d505 97 if(_resources == 0) error("mbos::mbos - Insufficient memory to create Resources");
AndrewL 0:1dafafe7d505 98 _hardstacklimit = (uint)malloc(1);
AndrewL 0:1dafafe7d505 99 if(_hardstacklimit == 0) error("mbos::mbos - Insufficient memory to create HardStackLimit");
AndrewL 0:1dafafe7d505 100 _numtasks = ntasks;
AndrewL 0:1dafafe7d505 101 _numtimers = ntimers;
AndrewL 0:1dafafe7d505 102 _numresources = nresources;
AndrewL 0:1dafafe7d505 103 _tasks = _tasklist[0];
AndrewL 0:1dafafe7d505 104 }
AndrewL 0:1dafafe7d505 105 // Create Tasks Function --------------------------------------------------------------
AndrewL 0:1dafafe7d505 106 void mbos::CreateTask(uint id, uint priority, uint stacksz, void (*fun)(void))
AndrewL 0:1dafafe7d505 107 {
AndrewL 0:1dafafe7d505 108 uint* stackbase;
AndrewL 0:1dafafe7d505 109
AndrewL 0:1dafafe7d505 110 // check bounds
AndrewL 0:1dafafe7d505 111 if(id >= _numtasks || id < 1) error("mbos::CreateTask - %i is an invalid task id\n", id);
AndrewL 0:1dafafe7d505 112 if(priority > MAX_PRIO) error("mbos::CreateTask - %i is an invalid priority for Task %i\n", priority, id);
AndrewL 4:e740e08cbea9 113 if(stacksz < MIN_STACKSZ) stacksz = MIN_STACKSZ;
AndrewL 0:1dafafe7d505 114
AndrewL 0:1dafafe7d505 115 // fill tcb
AndrewL 0:1dafafe7d505 116 if(_tasks[id].id == 0) _tasks[id].id = id;
AndrewL 0:1dafafe7d505 117 else error("mbos::CreateTask - Task %i already created\n", id);
AndrewL 0:1dafafe7d505 118 _tasks[id].priostate = READY + priority;
AndrewL 0:1dafafe7d505 119 _tasks[id].eventlist = 0;
AndrewL 0:1dafafe7d505 120 _tasks[id].eventmask = 0;
AndrewL 0:1dafafe7d505 121 stackbase = (uint*)malloc(stacksz * 4);
AndrewL 0:1dafafe7d505 122 if(stackbase == 0) error("mbos::CreateTask - Insufficient memory to create Task %i\n", id);
AndrewL 0:1dafafe7d505 123 _tasks[id].stacklimit = stackbase;
AndrewL 0:1dafafe7d505 124 _tasks[id].stack = _initstack(stackbase + stacksz, fun);
AndrewL 0:1dafafe7d505 125 }
AndrewL 0:1dafafe7d505 126 // Start OS function -------------------------------------------------------------------
AndrewL 0:1dafafe7d505 127 void mbos::Start(uint stacksize)
AndrewL 0:1dafafe7d505 128 {
AndrewL 0:1dafafe7d505 129 uint * stackbase;
AndrewL 0:1dafafe7d505 130
AndrewL 0:1dafafe7d505 131 // Fill idle tcb and initialise idle stack
AndrewL 0:1dafafe7d505 132 _tasks[0].priostate = READY;
AndrewL 0:1dafafe7d505 133 _tasks[0].id = 0;
AndrewL 0:1dafafe7d505 134 _tasks[0].eventlist = 0;
AndrewL 0:1dafafe7d505 135 _tasks[0].eventmask = 0;
AndrewL 0:1dafafe7d505 136 if(mbosIdleTask){
AndrewL 4:e740e08cbea9 137 if(stacksize < MIN_STACKSZ) stacksize = MIN_STACKSZ;
AndrewL 0:1dafafe7d505 138 stackbase = (uint*)malloc(stacksize * 4);
AndrewL 0:1dafafe7d505 139 if(stackbase == 0) error("mbos::Start - Insufficient memory to create idle task\n");
AndrewL 0:1dafafe7d505 140 _tasks[0].stacklimit = stackbase;
AndrewL 0:1dafafe7d505 141 _tasks[0].stack = _initstack(stackbase + stacksize, mbosIdleTask);
AndrewL 0:1dafafe7d505 142 }
AndrewL 0:1dafafe7d505 143 else {
AndrewL 0:1dafafe7d505 144 stackbase = (uint*)malloc(DEFAULT_IDLE_STACK_SZ * 4);
AndrewL 0:1dafafe7d505 145 if(stackbase == 0) error("mbos::Start - Insufficient memory to create idle task\n");
AndrewL 0:1dafafe7d505 146 _tasks[0].stacklimit = stackbase;
AndrewL 0:1dafafe7d505 147 _tasks[0].stack = _initstack(stackbase + DEFAULT_IDLE_STACK_SZ, _idletask);
AndrewL 0:1dafafe7d505 148 }
AndrewL 0:1dafafe7d505 149 _tasklistsize = 4 * (_numtasks - 1);
AndrewL 0:1dafafe7d505 150 SysTick_Config(SystemCoreClock / TICKS_PER_SECOND);
AndrewL 0:1dafafe7d505 151 NVIC_SetPriority(SysTick_IRQn, 0);
AndrewL 0:1dafafe7d505 152 _startos();
AndrewL 0:1dafafe7d505 153
AndrewL 0:1dafafe7d505 154 while(1);
AndrewL 0:1dafafe7d505 155 }
AndrewL 0:1dafafe7d505 156 // OS Tick Function -------------------------------------------------------------------
AndrewL 0:1dafafe7d505 157 void SysTick_Handler(void)
AndrewL 0:1dafafe7d505 158 {
AndrewL 0:1dafafe7d505 159 uint i;
AndrewL 0:1dafafe7d505 160
AndrewL 0:1dafafe7d505 161 __disable_irq();
AndrewL 0:1dafafe7d505 162
AndrewL 0:1dafafe7d505 163 for(i = 0; i < _numtimers; i++){
AndrewL 0:1dafafe7d505 164 if(_timers[i].timer){
AndrewL 0:1dafafe7d505 165 _timers[i].timer--;
AndrewL 0:1dafafe7d505 166 if(_timers[i].timer == 0){
AndrewL 0:1dafafe7d505 167 _timers[i].timer = _timers[i].reload;
AndrewL 0:1dafafe7d505 168 if(_tasks[_timers[i].task].eventmask & _timers[i].event){
AndrewL 0:1dafafe7d505 169 _tasks[_timers[i].task].eventlist |= _timers[i].event;
AndrewL 0:1dafafe7d505 170 _tasks[_timers[i].task].priostate += READY;
AndrewL 0:1dafafe7d505 171 }
AndrewL 0:1dafafe7d505 172 }
AndrewL 0:1dafafe7d505 173 }
AndrewL 0:1dafafe7d505 174 }
AndrewL 0:1dafafe7d505 175 if((__return_address() & 0x08) == 0) { // called from Handler, so swap later
AndrewL 0:1dafafe7d505 176 __enable_irq();
AndrewL 0:1dafafe7d505 177 return;
AndrewL 0:1dafafe7d505 178 }
AndrewL 0:1dafafe7d505 179 _swap();
AndrewL 0:1dafafe7d505 180 __enable_irq();
AndrewL 0:1dafafe7d505 181 }
AndrewL 0:1dafafe7d505 182 // Get Task id Function -----------------------------------------------------------------
AndrewL 0:1dafafe7d505 183 uint mbos::GetTask(void)
AndrewL 0:1dafafe7d505 184 {
AndrewL 0:1dafafe7d505 185 return _tasklist[0]->id;
AndrewL 0:1dafafe7d505 186 }
AndrewL 0:1dafafe7d505 187 // Set Priority Function ----------------------------------------------------------------
AndrewL 0:1dafafe7d505 188 void mbos::SetPriority(uint priority)
AndrewL 0:1dafafe7d505 189 {
AndrewL 0:1dafafe7d505 190 if(_tasklist[0]->id == 0) return;
AndrewL 0:1dafafe7d505 191 if(priority > MAX_PRIO) error("mbos::SetPriority - %i is an invalid priority\n", priority);
AndrewL 0:1dafafe7d505 192 _tasklist[0]->priostate = priority + READY;
AndrewL 0:1dafafe7d505 193 }
AndrewL 0:1dafafe7d505 194 // Get Priority Function ----------------------------------------------------------------
AndrewL 0:1dafafe7d505 195 uint mbos::GetPriority(void)
AndrewL 0:1dafafe7d505 196 {
AndrewL 0:1dafafe7d505 197 return _tasklist[0]->priostate - READY;
AndrewL 0:1dafafe7d505 198 }
AndrewL 0:1dafafe7d505 199 // Wait Event Function ------------------------------------------------------------------
AndrewL 0:1dafafe7d505 200 void mbos::WaitEvent(uint event)
AndrewL 0:1dafafe7d505 201 {
AndrewL 0:1dafafe7d505 202 if(_tasklist[0]->id == 0) return;
AndrewL 0:1dafafe7d505 203 if(event == 0) return;
AndrewL 0:1dafafe7d505 204 __disable_irq();
AndrewL 0:1dafafe7d505 205 _tasklist[0]->eventlist = 0;
AndrewL 0:1dafafe7d505 206 _tasklist[0]->eventmask = event;
AndrewL 0:1dafafe7d505 207 _tasklist[0]->priostate -= READY;
AndrewL 0:1dafafe7d505 208 _swap();
AndrewL 0:1dafafe7d505 209 __enable_irq();
AndrewL 0:1dafafe7d505 210 }
AndrewL 0:1dafafe7d505 211 // Set Event Function --------------------------------------------------------------------
AndrewL 0:1dafafe7d505 212 void mbos::SetEvent(uint event, uint task)
AndrewL 0:1dafafe7d505 213 {
AndrewL 0:1dafafe7d505 214 // check bounds
AndrewL 0:1dafafe7d505 215 if(task >= _numtasks || (task < 1)) return;
AndrewL 0:1dafafe7d505 216
AndrewL 0:1dafafe7d505 217 __disable_irq();
AndrewL 0:1dafafe7d505 218 if(_tasks[task].eventmask & event){
AndrewL 0:1dafafe7d505 219 _tasks[task].eventlist |= event;
AndrewL 0:1dafafe7d505 220 _tasks[task].priostate += READY;
AndrewL 0:1dafafe7d505 221 }
AndrewL 0:1dafafe7d505 222 else{
AndrewL 0:1dafafe7d505 223 __enable_irq();
AndrewL 0:1dafafe7d505 224 return;
AndrewL 0:1dafafe7d505 225 }
AndrewL 0:1dafafe7d505 226 _swap();
AndrewL 0:1dafafe7d505 227 __enable_irq();
AndrewL 0:1dafafe7d505 228 }
AndrewL 0:1dafafe7d505 229 // Get event Function -----------------------------------------------------------------
AndrewL 0:1dafafe7d505 230 uint mbos::GetEvent(void)
AndrewL 0:1dafafe7d505 231 {
AndrewL 0:1dafafe7d505 232 return _tasklist[0]->eventlist;
AndrewL 0:1dafafe7d505 233 }
AndrewL 0:1dafafe7d505 234 // Create Timer Function --------------------------------------------------------------
AndrewL 0:1dafafe7d505 235 void mbos::CreateTimer(uint id, uint task, uint event)
AndrewL 0:1dafafe7d505 236 {
AndrewL 0:1dafafe7d505 237 // check bounds
AndrewL 0:1dafafe7d505 238 if(id >= _numtimers) error("mbos::CreateTimer - %i is an invalid timer id\n", id);
AndrewL 0:1dafafe7d505 239 if(task < 1|| task >= _numtasks) error("mbos::CreateTimer - %i is an invalid task id for Timer %i\n", task, id);
AndrewL 0:1dafafe7d505 240 if(event == 0) error("mbos::CreateTimer - Can't use null event for Timer %i\n", id);
AndrewL 0:1dafafe7d505 241
AndrewL 0:1dafafe7d505 242 // fill tcb
AndrewL 0:1dafafe7d505 243 _timers[id].timer = 0;
AndrewL 0:1dafafe7d505 244 _timers[id].reload = 0;
AndrewL 0:1dafafe7d505 245 _timers[id].task = task;
AndrewL 0:1dafafe7d505 246 _timers[id].event = event;
AndrewL 0:1dafafe7d505 247 }
AndrewL 0:1dafafe7d505 248 // Set Timer Function -------------------------------------------------------------------
AndrewL 0:1dafafe7d505 249 void mbos::SetTimer(uint id, uint time, uint reload)
AndrewL 0:1dafafe7d505 250 {
AndrewL 0:1dafafe7d505 251 // check bounds
AndrewL 0:1dafafe7d505 252 if(id >= _numtimers) error("mbos::SetTimer - %i is an invalid timer id\n", id);
AndrewL 0:1dafafe7d505 253
AndrewL 0:1dafafe7d505 254 __disable_irq();
AndrewL 0:1dafafe7d505 255 _timers[id].timer = time;
AndrewL 0:1dafafe7d505 256 _timers[id].reload = reload;
AndrewL 0:1dafafe7d505 257 __enable_irq();
AndrewL 4:e740e08cbea9 258 }
AndrewL 4:e740e08cbea9 259 // Redirect Timer Function -----------------------------------------------------------------
AndrewL 4:e740e08cbea9 260 void mbos::RedirectTimer(uint id, uint task, uint event)
AndrewL 4:e740e08cbea9 261 {
AndrewL 4:e740e08cbea9 262 // check bounds
AndrewL 4:e740e08cbea9 263 if(id >= _numtimers) error("mbos::RedirectTimer - %i is an invalid timer id\n", id);
AndrewL 4:e740e08cbea9 264 if(task < 1|| task >= _numtasks) error("mbos::RedirectTimer - %i is an invalid task id for Timer %i\n", task, id);
AndrewL 4:e740e08cbea9 265 if(event == 0) error("mbos::RedirectTimer - Can't use null event for Timer %i\n", id);
AndrewL 4:e740e08cbea9 266
AndrewL 4:e740e08cbea9 267 __disable_irq();
AndrewL 4:e740e08cbea9 268 if( _timers[id].timer == 0){
AndrewL 4:e740e08cbea9 269 _timers[id].task = task;
AndrewL 4:e740e08cbea9 270 _timers[id].event = event;
AndrewL 4:e740e08cbea9 271 }
AndrewL 4:e740e08cbea9 272 __enable_irq();
AndrewL 4:e740e08cbea9 273 }
AndrewL 0:1dafafe7d505 274 // Clear Timer Function -------------------------------------------------------------------
AndrewL 0:1dafafe7d505 275 void mbos::ClearTimer(uint id)
AndrewL 0:1dafafe7d505 276 {
AndrewL 0:1dafafe7d505 277 // check bounds
AndrewL 0:1dafafe7d505 278 if(id >= _numtimers) error("mbos::ClearTimer - %i is an invalid timer id\n", id);
AndrewL 0:1dafafe7d505 279
AndrewL 0:1dafafe7d505 280 __disable_irq();
AndrewL 0:1dafafe7d505 281 _timers[id].timer = 0;
AndrewL 0:1dafafe7d505 282 _timers[id].reload = 0;
AndrewL 0:1dafafe7d505 283 __enable_irq();
AndrewL 0:1dafafe7d505 284 }
AndrewL 0:1dafafe7d505 285 // Create resources Function --------------------------------------------------------------
AndrewL 0:1dafafe7d505 286 void mbos::CreateResource(uint id, uint priority)
AndrewL 0:1dafafe7d505 287 {
AndrewL 0:1dafafe7d505 288 // check bounds
AndrewL 0:1dafafe7d505 289 if(id >= _numresources) error("mbos::CreateResource - %i is an invalid resource id\n", id);
AndrewL 0:1dafafe7d505 290 if(priority > MAX_PRIO) error("mbos::CreateResource - %i is an invalid priority for Resource %i\n", priority, id);
AndrewL 0:1dafafe7d505 291
AndrewL 0:1dafafe7d505 292 // fill rcb
AndrewL 0:1dafafe7d505 293 _resources[id].priority = priority;
AndrewL 0:1dafafe7d505 294 _resources[id].lock = 0;
AndrewL 0:1dafafe7d505 295 }
AndrewL 0:1dafafe7d505 296 // Lock Resource Function --------------------------------------------------------------
AndrewL 0:1dafafe7d505 297 uint mbos::LockResource(uint id)
AndrewL 0:1dafafe7d505 298 {
AndrewL 0:1dafafe7d505 299 // check bounds
AndrewL 0:1dafafe7d505 300 if(id >= _numresources) error("mbos::LockResource - %i is an invalid resource id\n", id);
AndrewL 4:e740e08cbea9 301 if(_tasklist[0]->id == 0 ||_resources[id].lock != 0) return _resources[id].lock;
AndrewL 0:1dafafe7d505 302
AndrewL 0:1dafafe7d505 303 __disable_irq();
AndrewL 0:1dafafe7d505 304 _resources[id].lock = _tasklist[0]->id;
AndrewL 0:1dafafe7d505 305 _resources[id].taskprio = _tasklist[0]->priostate;
AndrewL 0:1dafafe7d505 306 _tasklist[0]->priostate = _resources[id].priority + READY;
AndrewL 0:1dafafe7d505 307 __enable_irq();
AndrewL 0:1dafafe7d505 308 return 0;
AndrewL 0:1dafafe7d505 309 }
AndrewL 0:1dafafe7d505 310 // Test Resource Function --------------------------------------------------------------
AndrewL 0:1dafafe7d505 311 uint mbos::TestResource(uint id)
AndrewL 0:1dafafe7d505 312 {
AndrewL 0:1dafafe7d505 313 // check bounds
AndrewL 0:1dafafe7d505 314 if(id >= _numresources) error("mbos::TestResource - %i is an invalid resource id\n", id);
AndrewL 0:1dafafe7d505 315 return(_resources[id].lock);
AndrewL 0:1dafafe7d505 316 }
AndrewL 0:1dafafe7d505 317 // Free Resource Function --------------------------------------------------------------
AndrewL 0:1dafafe7d505 318 uint mbos::FreeResource(uint id)
AndrewL 0:1dafafe7d505 319 {
AndrewL 0:1dafafe7d505 320 // check bounds
AndrewL 0:1dafafe7d505 321 if(id >= _numresources) error("mbos::FreeResource - %i is an invalid resource id\n", id);
AndrewL 4:e740e08cbea9 322 if(_tasklist[0]->id == 0 || _tasklist[0]->id != _resources[id].lock) return _resources[id].lock;
AndrewL 0:1dafafe7d505 323
AndrewL 0:1dafafe7d505 324 __disable_irq();
AndrewL 0:1dafafe7d505 325 _resources[id].lock = 0;
AndrewL 0:1dafafe7d505 326 _tasklist[0]->priostate = _resources[id].taskprio;
AndrewL 0:1dafafe7d505 327 __enable_irq();
AndrewL 0:1dafafe7d505 328 return 0;
AndrewL 0:1dafafe7d505 329 }
AndrewL 0:1dafafe7d505 330 // Initialise stack function -----------------------------------------------------------
AndrewL 0:1dafafe7d505 331 uint* mbos::_initstack(uint *stack, void (*fun)())
AndrewL 0:1dafafe7d505 332 {
AndrewL 0:1dafafe7d505 333 stack -= 3; // leave a spare word
AndrewL 0:1dafafe7d505 334 *stack = 0x01000000; // Initial xPSR (reset value)
AndrewL 0:1dafafe7d505 335 stack--;
AndrewL 0:1dafafe7d505 336 *stack = (uint)fun - 1; // Start address of the task corrected
AndrewL 0:1dafafe7d505 337 stack--;
AndrewL 0:1dafafe7d505 338 *stack = 0; // LR
AndrewL 0:1dafafe7d505 339 stack -= 5; // R12, R3, R2, R1, R0
AndrewL 0:1dafafe7d505 340 stack -= 8; // R11, R10, R9, R8, R7, R6, R5, R4
AndrewL 0:1dafafe7d505 341 return stack;
AndrewL 0:1dafafe7d505 342 }
AndrewL 4:e740e08cbea9 343 // Stack Error function ----------------------------------------------------------------
AndrewL 0:1dafafe7d505 344 void _stackerror(uint task)
AndrewL 0:1dafafe7d505 345 {
AndrewL 0:1dafafe7d505 346 error("Stack Overflow on Task %i\n", task);
AndrewL 0:1dafafe7d505 347 }