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     for(i = 0; i < _numtimers; i++){
00163         if(_timers[i].timer){
00164             _timers[i].timer--;    
00165             if(_timers[i].timer == 0){
00166                 _timers[i].timer = _timers[i].reload; 
00167                 if( _tasks[_timers[i].task].priostate < READY){
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     } 
00176     if((__return_address() & 0x08) == 0) {            // called from Handler, so swap later
00177     __enable_irq();
00178     return;
00179     }
00180     _swap();
00181     __enable_irq();
00182 }
00183 // Get Task id Function -----------------------------------------------------------------
00184 uint mbos::GetTask(void)
00185 { 
00186     return _tasklist[0]->id; 
00187 }
00188 // Set Priority Function ----------------------------------------------------------------
00189 void mbos::SetPriority(uint priority) 
00190 {  
00191     if(_tasklist[0]->id == 0) return;
00192     if(priority > MAX_PRIO) error("mbos::SetPriority - %i is an invalid priority\n", priority);
00193     _tasklist[0]->priostate = priority + READY;
00194 }
00195 // Get Priority Function ----------------------------------------------------------------
00196 uint mbos::GetPriority(void) 
00197 { 
00198     return _tasklist[0]->priostate - READY; 
00199 }
00200 // Wait Event Function  ------------------------------------------------------------------
00201 void mbos::WaitEvent(uint event)
00202 {   
00203     if(_tasklist[0]->id == 0) return;
00204     if(event == 0) return;
00205     __disable_irq();
00206     _tasklist[0]->eventlist = 0;
00207     _tasklist[0]->eventmask = event;
00208     _tasklist[0]->priostate -= READY;
00209     _swap();
00210     __enable_irq();
00211 }
00212 // Set Event Function --------------------------------------------------------------------
00213 void mbos::SetEvent(uint event, uint task)
00214 {    
00215     // check bounds
00216     if(task >= _numtasks || (task < 1)) return; 
00217 
00218     __disable_irq();
00219     if(_tasks[task].eventmask & event){            
00220         _tasks[task].eventlist |= event;
00221         _tasks[task].priostate += READY;
00222     }
00223     else{
00224         __enable_irq();
00225         return;
00226     } 
00227     _swap();
00228     __enable_irq();
00229 }         
00230 // Get event Function   -----------------------------------------------------------------
00231 uint mbos::GetEvent(void)
00232 {
00233     return _tasklist[0]->eventlist;
00234 }
00235 // Create Timer Function  --------------------------------------------------------------
00236 void mbos::CreateTimer(uint id, uint task, uint event)
00237 {
00238     // check bounds 
00239     if(id >= _numtimers) error("mbos::CreateTimer - %i is an invalid timer id\n", id); 
00240     if(task < 1|| task >= _numtasks) error("mbos::CreateTimer - %i is an invalid task id for Timer %i\n", task, id); 
00241     if(event == 0) error("mbos::CreateTimer - Can't use null event for Timer %i\n", id); 
00242   
00243     // fill tcb 
00244     _timers[id].timer = 0;
00245     _timers[id].reload = 0;
00246     _timers[id].task = task;
00247     _timers[id].event = event;
00248 }
00249 // Set Timer Function  -------------------------------------------------------------------
00250 void mbos::SetTimer(uint id, uint time, uint reload)
00251 {
00252     // check bounds
00253     if(id >= _numtimers) error("mbos::SetTimer - %i is an invalid timer id\n", id); 
00254    
00255     __disable_irq();
00256     _timers[id].timer = time;
00257     _timers[id].reload = reload;
00258     __enable_irq();
00259 } 
00260 // Redirect Timer Function -----------------------------------------------------------------
00261 void mbos::RedirectTimer(uint id, uint task, uint event)
00262 {
00263     // check bounds 
00264     if(id >= _numtimers) error("mbos::RedirectTimer - %i is an invalid timer id\n", id); 
00265     if(task < 1|| task >= _numtasks) error("mbos::RedirectTimer - %i is an invalid task id for Timer %i\n", task, id); 
00266     if(event == 0) error("mbos::RedirectTimer - Can't use null event for Timer %i\n", id); 
00267     
00268     __disable_irq();
00269     if( _timers[id].timer == 0){
00270         _timers[id].task = task;
00271         _timers[id].event = event;
00272     }
00273     __enable_irq();
00274 }
00275 // Clear Timer Function  -------------------------------------------------------------------
00276 void mbos::ClearTimer(uint id)
00277 {
00278     // check bounds
00279     if(id >= _numtimers) error("mbos::ClearTimer - %i is an invalid timer id\n", id); 
00280     
00281     __disable_irq();
00282     _timers[id].timer = 0;
00283     _timers[id].reload = 0;
00284     __enable_irq();
00285 }    
00286 // Create resources Function  --------------------------------------------------------------
00287 void mbos::CreateResource(uint id, uint priority)
00288 {
00289     // check bounds 
00290     if(id >= _numresources) error("mbos::CreateResource - %i is an invalid resource id\n", id);  
00291     if(priority > MAX_PRIO) error("mbos::CreateResource - %i is an invalid priority for Resource %i\n", priority, id);
00292   
00293     // fill rcb 
00294        _resources[id].priority = priority;
00295        _resources[id].lock = 0;
00296 }
00297 // Lock Resource Function  --------------------------------------------------------------
00298 uint mbos::LockResource(uint id)
00299 {
00300     // check bounds
00301     if(id >= _numresources) error("mbos::LockResource - %i is an invalid resource id\n", id);
00302     if(_tasklist[0]->id == 0 ||_resources[id].lock != 0) return _resources[id].lock;
00303     
00304     __disable_irq();
00305     _resources[id].lock = _tasklist[0]->id;
00306     _resources[id].taskprio = _tasklist[0]->priostate;
00307     _tasklist[0]->priostate = _resources[id].priority + READY;
00308     __enable_irq();
00309     return 0;
00310 }
00311 // Test Resource Function  --------------------------------------------------------------
00312 uint mbos::TestResource(uint id)
00313 {
00314     // check bounds
00315     if(id >= _numresources) error("mbos::TestResource - %i is an invalid resource id\n", id);
00316     return(_resources[id].lock);
00317 }
00318 // Free Resource Function  --------------------------------------------------------------
00319 uint mbos::FreeResource(uint id)
00320 {
00321     // check bounds
00322     if(id >= _numresources) error("mbos::FreeResource - %i is an invalid resource id\n", id);
00323     if(_tasklist[0]->id == 0 || _tasklist[0]->id != _resources[id].lock) return _resources[id].lock;
00324     
00325     __disable_irq();
00326     _resources[id].lock = 0;
00327     _tasklist[0]->priostate = _resources[id].taskprio;
00328     __enable_irq();
00329     return 0;
00330 }
00331 // Initialise stack function  -----------------------------------------------------------
00332 uint* mbos::_initstack(uint *stack, void (*fun)())
00333 {
00334     stack -= 3;                                // leave a spare word
00335     *stack = 0x01000000;                        // Initial xPSR    (reset value)
00336     stack--;
00337     *stack = (uint)fun - 1;                    // Start address of the task corrected
00338     stack--;
00339     *stack = 0;                                // LR
00340     stack -= 5;                                // R12, R3, R2, R1, R0 
00341     stack -= 8;                                // R11, R10, R9, R8, R7, R6, R5, R4
00342     return stack;    
00343 } 
00344 // Stack Error function  ----------------------------------------------------------------
00345 void _stackerror(uint task)
00346 {
00347     error("Stack Overflow on Task %i\n", task);
00348 }