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?
User | Revision | Line number | New 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 | } |