mbos.cpp

00001 /***********************************************************************************
00002  *  m b o s   R T O S   F O R    m b e d  (ARM CORTEX M3) 
00003  *
00004  * Copyright (c) 2010 - 2011 Andrew Levido
00005  *
00006  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR "AS IS" AND ANY EXPRESS OR IMPLIED 
00007  * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 
00008  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 
00009  * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
00010  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
00011  * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
00012  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 
00013  * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 
00014  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00015  */
00016 #include "mbos.h" 
00017 #include "mbed.h"
00018 
00019 #define MAX_PRIO                    99                  // maximum priority
00020 #define READY                       MAX_PRIO + 1        // always > MAX_PRIO
00021 #define WAITING                     0                   // aways 0
00022 #define DEFAULT_IDLE_STACK_SZ       32                  // in words
00023 #define MAX_TASKS                   99                  // tasks 0.. 99 including idle
00024 #define MAX_TIMERS                  99                  // timers 0..99
00025 #define MAX_RESOURCES               99                  // resources 0..99
00026 #define MIN_STACKSZ                 32                  // enough to run an empty function
00027 #define TICKS_PER_SECOND            1000                // 1ms Ticks
00028 
00029 typedef struct _tcb{                                    // task control block  
00030     uint id;                               
00031     uint *stack;
00032     uint priostate;
00033     uint eventlist;
00034     uint eventmask;
00035     uint *stacklimit;
00036 }_tcb_t;
00037 
00038 typedef struct _timer{                                  // timer cotrol block
00039     uint timer;
00040     uint reload;
00041     uint event;
00042     uint task;
00043 }_timer_t;    
00044 
00045 typedef struct _rcb{                                    // resource control block
00046     uint lock;
00047     uint priority;
00048     uint taskprio;
00049 }_resource_t;    
00050 
00051 extern "C" void _startos(void);                         // Assembly routine to start the scheduler
00052 extern "C" void _swap(void);                            // Assembly routine to set PendSV exception
00053 extern "C" void SysTick_Handler(void);                  // Override weak declaration in startup file
00054 extern "C" void _stackerror(uint task);                 // Stack error function called by assembler
00055 
00056 extern void __attribute__((__weak__)) mbosIdleTask(void); // Allow user to write their own idle task
00057 
00058 // Static Variables
00059 uint _hardstacklimit;                                   // stack limit
00060 _tcb_t **_tasklist;                                     // pointer to task list
00061 _tcb_t *_tasks;                                         // pointer to task control block 0        
00062 _timer_t *_timers;                                      // pointer to timer control block 0
00063 _resource_t *_resources;                                // pointer to resource control block 0
00064 uint _numtasks;                                         // number of tasks (including idle)
00065 uint _numtimers;                                        // number of timers
00066 uint _numresources;                                     // number of resources
00067 uint _tasklistsize;                                     // task list length in bytes
00068 
00069 // Default Idle Task -------------------------------------------------------------------
00070 void _idletask(void) { while(1); }                      // Default idle task
00071 
00072 // Constructor  -------------------------------------------------------------------------
00073 mbos::mbos(uint ntasks, uint ntimers, uint nresources)
00074 {
00075     uint i;
00076     
00077     ntasks += 1;
00078     // Create the tcblist, zero initialised
00079     if(ntasks > MAX_TASKS || ntasks < 1) error("mbos::mbos - %i is an invalid number of tasks\n", ntasks);
00080     _tasklist = (_tcb_t**)calloc(sizeof(_tcb_t*), ntasks);
00081     if(_tasklist == 0) error("mbos::mbos - Insufficient memory to create Tasklist\n");
00082     
00083     // Create the tcbs
00084     _tasklist[0] = (_tcb_t*)calloc(sizeof(_tcb_t), ntasks);
00085     if(_tasklist[0] == 0) error("mbos::mbos - Insufficient memory to create TCBs\n");
00086     for(i = 1; i < ntasks; i++){
00087         _tasklist[i] = _tasklist[i - 1]    + 1;
00088     }
00089     // Create the timers and clear them      
00090     if(ntimers > MAX_TIMERS + 1) error("mbos::mbos - %i is an invalid number of Timers\n", ntimers);    
00091     _timers = (_timer_t*)calloc(sizeof(_timer_t), ntimers);
00092     if(_timers == 0) error("mbos::mbos - Insufficient memory to create Timers");
00093 
00094     // create the resources & clear them
00095     if(nresources > MAX_RESOURCES + 1) error("mbos::mbos - %i is an invalid number of Resources\n", nresources); 
00096     _resources = (_resource_t*)calloc(sizeof(_resource_t), nresources);
00097     if(_resources  == 0) error("mbos::mbos - Insufficient memory to create Resources");
00098     _hardstacklimit = (uint)malloc(1);
00099     if(_hardstacklimit == 0) error("mbos::mbos - Insufficient memory to create HardStackLimit");
00100     _numtasks = ntasks;
00101     _numtimers = ntimers;
00102     _numresources = nresources;
00103     _tasks =  _tasklist[0];
00104 }
00105 // Create Tasks Function  --------------------------------------------------------------
00106 void mbos::CreateTask(uint id, uint priority, uint stacksz, void (*fun)(void))
00107 {
00108     uint* stackbase;
00109 
00110     // check bounds 
00111     if(id >= _numtasks || id < 1) error("mbos::CreateTask - %i is an invalid task id\n", id); 
00112     if(priority > MAX_PRIO) error("mbos::CreateTask - %i is an invalid priority for Task %i\n", priority, id);
00113     if(stacksz < MIN_STACKSZ) stacksz = MIN_STACKSZ;
00114         
00115     // fill tcb 
00116     if(_tasks[id].id == 0) _tasks[id].id = id;
00117     else error("mbos::CreateTask - Task %i already created\n", id); 
00118     _tasks[id].priostate = READY + priority;
00119     _tasks[id].eventlist = 0;
00120     _tasks[id].eventmask = 0;
00121     stackbase = (uint*)malloc(stacksz * 4);
00122     if(stackbase == 0) error("mbos::CreateTask - Insufficient memory to create Task %i\n", id);
00123     _tasks[id].stacklimit = stackbase;
00124     _tasks[id].stack = _initstack(stackbase + stacksz, fun);
00125 }
00126 // Start OS function  -------------------------------------------------------------------
00127 void mbos::Start(uint stacksize)
00128 {
00129     uint * stackbase;
00130 
00131     // Fill idle tcb and initialise idle stack 
00132     _tasks[0].priostate = READY;
00133     _tasks[0].id = 0;
00134     _tasks[0].eventlist = 0;
00135     _tasks[0].eventmask = 0;
00136     if(mbosIdleTask){
00137         if(stacksize < MIN_STACKSZ) stacksize = MIN_STACKSZ;
00138         stackbase = (uint*)malloc(stacksize * 4);
00139         if(stackbase == 0) error("mbos::Start - Insufficient memory to create idle task\n");    
00140         _tasks[0].stacklimit = stackbase;
00141         _tasks[0].stack = _initstack(stackbase + stacksize, mbosIdleTask);
00142     }
00143     else {
00144         stackbase = (uint*)malloc(DEFAULT_IDLE_STACK_SZ * 4);
00145         if(stackbase == 0) error("mbos::Start - Insufficient memory to create idle task\n");
00146         _tasks[0].stacklimit = stackbase;
00147         _tasks[0].stack = _initstack(stackbase + DEFAULT_IDLE_STACK_SZ, _idletask);
00148     }        
00149     _tasklistsize = 4 * (_numtasks - 1);
00150     SysTick_Config(SystemCoreClock / TICKS_PER_SECOND);
00151     NVIC_SetPriority(SysTick_IRQn, 0);
00152     _startos();
00153     
00154     while(1);                            
00155 }
00156 // OS Tick Function   -------------------------------------------------------------------
00157 void SysTick_Handler(void)
00158 {
00159     uint i;
00160     
00161     __disable_irq();
00162     
00163     for(i = 0; i < _numtimers; i++){
00164         if(_timers[i].timer){
00165             _timers[i].timer--;    
00166             if(_timers[i].timer == 0){
00167                 _timers[i].timer = _timers[i].reload;    
00168                 if(_tasks[_timers[i].task].eventmask & _timers[i].event){
00169                     _tasks[_timers[i].task].eventlist |= _timers[i].event;
00170                     _tasks[_timers[i].task].priostate += READY;
00171                 }     
00172             }
00173         }
00174     } 
00175     if((__return_address() & 0x08) == 0) {            // called from Handler, so swap later
00176     __enable_irq();
00177     return;
00178     }
00179     _swap();
00180     __enable_irq();
00181 }
00182 // Get Task id Function -----------------------------------------------------------------
00183 uint mbos::GetTask(void)
00184 { 
00185     return _tasklist[0]->id; 
00186 }
00187 // Set Priority Function ----------------------------------------------------------------
00188 void mbos::SetPriority(uint priority) 
00189 {  
00190     if(_tasklist[0]->id == 0) return;
00191     if(priority > MAX_PRIO) error("mbos::SetPriority - %i is an invalid priority\n", priority);
00192     _tasklist[0]->priostate = priority + READY;
00193 }
00194 // Get Priority Function ----------------------------------------------------------------
00195 uint mbos::GetPriority(void) 
00196 { 
00197     return _tasklist[0]->priostate - READY; 
00198 }
00199 // Wait Event Function  ------------------------------------------------------------------
00200 void mbos::WaitEvent(uint event)
00201 {   
00202     if(_tasklist[0]->id == 0) return;
00203     if(event == 0) return;
00204     __disable_irq();
00205     _tasklist[0]->eventlist = 0;
00206     _tasklist[0]->eventmask = event;
00207     _tasklist[0]->priostate -= READY;
00208     _swap();
00209     __enable_irq();
00210 }
00211 // Set Event Function --------------------------------------------------------------------
00212 void mbos::SetEvent(uint event, uint task)
00213 {    
00214     // check bounds
00215     if(task >= _numtasks || (task < 1)) return; 
00216 
00217     __disable_irq();
00218     if(_tasks[task].eventmask & event){            
00219         _tasks[task].eventlist |= event;
00220         _tasks[task].priostate += READY;
00221     }
00222     else{
00223         __enable_irq();
00224         return;
00225     } 
00226     _swap();
00227     __enable_irq();
00228 }         
00229 // Get event Function   -----------------------------------------------------------------
00230 uint mbos::GetEvent(void)
00231 {
00232     return _tasklist[0]->eventlist;
00233 }
00234 // Create Timer Function  --------------------------------------------------------------
00235 void mbos::CreateTimer(uint id, uint task, uint event)
00236 {
00237     // check bounds 
00238     if(id >= _numtimers) error("mbos::CreateTimer - %i is an invalid timer id\n", id); 
00239     if(task < 1|| task >= _numtasks) error("mbos::CreateTimer - %i is an invalid task id for Timer %i\n", task, id); 
00240     if(event == 0) error("mbos::CreateTimer - Can't use null event for Timer %i\n", id); 
00241   
00242     // fill tcb 
00243     _timers[id].timer = 0;
00244     _timers[id].reload = 0;
00245     _timers[id].task = task;
00246     _timers[id].event = event;
00247 }
00248 // Set Timer Function  -------------------------------------------------------------------
00249 void mbos::SetTimer(uint id, uint time, uint reload)
00250 {
00251     // check bounds
00252     if(id >= _numtimers) error("mbos::SetTimer - %i is an invalid timer id\n", id); 
00253    
00254     __disable_irq();
00255     _timers[id].timer = time;
00256     _timers[id].reload = reload;
00257     __enable_irq();
00258 } 
00259 // Redirect Timer Function -----------------------------------------------------------------
00260 void mbos::RedirectTimer(uint id, uint task, uint event)
00261 {
00262     // check bounds 
00263     if(id >= _numtimers) error("mbos::RedirectTimer - %i is an invalid timer id\n", id); 
00264     if(task < 1|| task >= _numtasks) error("mbos::RedirectTimer - %i is an invalid task id for Timer %i\n", task, id); 
00265     if(event == 0) error("mbos::RedirectTimer - Can't use null event for Timer %i\n", id); 
00266     
00267     __disable_irq();
00268     if( _timers[id].timer == 0){
00269         _timers[id].task = task;
00270         _timers[id].event = event;
00271     }
00272     __enable_irq();
00273 }
00274 // Clear Timer Function  -------------------------------------------------------------------
00275 void mbos::ClearTimer(uint id)
00276 {
00277     // check bounds
00278     if(id >= _numtimers) error("mbos::ClearTimer - %i is an invalid timer id\n", id); 
00279     
00280     __disable_irq();
00281     _timers[id].timer = 0;
00282     _timers[id].reload = 0;
00283     __enable_irq();
00284 }    
00285 // Create resources Function  --------------------------------------------------------------
00286 void mbos::CreateResource(uint id, uint priority)
00287 {
00288     // check bounds 
00289     if(id >= _numresources) error("mbos::CreateResource - %i is an invalid resource id\n", id);  
00290     if(priority > MAX_PRIO) error("mbos::CreateResource - %i is an invalid priority for Resource %i\n", priority, id);
00291   
00292     // fill rcb 
00293        _resources[id].priority = priority;
00294        _resources[id].lock = 0;
00295 }
00296 // Lock Resource Function  --------------------------------------------------------------
00297 uint mbos::LockResource(uint id)
00298 {
00299     // check bounds
00300     if(id >= _numresources) error("mbos::LockResource - %i is an invalid resource id\n", id);
00301     if(_tasklist[0]->id == 0 ||_resources[id].lock != 0) return _resources[id].lock;
00302     
00303     __disable_irq();
00304     _resources[id].lock = _tasklist[0]->id;
00305     _resources[id].taskprio = _tasklist[0]->priostate;
00306     _tasklist[0]->priostate = _resources[id].priority + READY;
00307     __enable_irq();
00308     return 0;
00309 }
00310 // Test Resource Function  --------------------------------------------------------------
00311 uint mbos::TestResource(uint id)
00312 {
00313     // check bounds
00314     if(id >= _numresources) error("mbos::TestResource - %i is an invalid resource id\n", id);
00315     return(_resources[id].lock);
00316 }
00317 // Free Resource Function  --------------------------------------------------------------
00318 uint mbos::FreeResource(uint id)
00319 {
00320     // check bounds
00321     if(id >= _numresources) error("mbos::FreeResource - %i is an invalid resource id\n", id);
00322     if(_tasklist[0]->id == 0 || _tasklist[0]->id != _resources[id].lock) return _resources[id].lock;
00323     
00324     __disable_irq();
00325     _resources[id].lock = 0;
00326     _tasklist[0]->priostate = _resources[id].taskprio;
00327     __enable_irq();
00328     return 0;
00329 }
00330 // Initialise stack function  -----------------------------------------------------------
00331 uint* mbos::_initstack(uint *stack, void (*fun)())
00332 {
00333     stack -= 3;                                // leave a spare word
00334     *stack = 0x01000000;                        // Initial xPSR    (reset value)
00335     stack--;
00336     *stack = (uint)fun - 1;                    // Start address of the task corrected
00337     stack--;
00338     *stack = 0;                                // LR
00339     stack -= 5;                                // R12, R3, R2, R1, R0 
00340     stack -= 8;                                // R11, R10, R9, R8, R7, R6, R5, R4
00341     return stack;    
00342 } 
00343 // Stack Error function  ----------------------------------------------------------------
00344 void _stackerror(uint task)
00345 {
00346     error("Stack Overflow on Task %i\n", task);
00347 }