00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00035 #include <avr/interrupt.h>
00036 #include <avr/io.h>
00037 #include <string.h>
00038 #include <avr/pgmspace.h>
00039
00040 #include "otos_cfg.h"
00041 #include "otos_def.h"
00042 #include "types.h"
00043 #include "memory.h"
00044 #include "hardware.h"
00045 #include "error.h"
00046 #include "time.h"
00047 #include "task.h"
00048
00049
00050
00051
00052
00053
00054
00055 uint8_t msgErrOutOfMem[] PROGMEM = "otOS ERR: out of mem";
00056
00057
00058
00060 OtosTask* volatile g_pReadyQueue;
00061
00063 OtosTask* volatile g_pBlockedQueue;
00064
00066 OtosTask* volatile g_pRunningTask;
00067
00069
00070
00072 uint8_t volatile g_performDispatch;
00073
00075 uint8_t volatile g_timeSliceTicks;
00076
00078 uint8_t volatile g_timeSliceCounter;
00079
00080
00081 #ifdef OTOS_PROFILING
00082
00083 uint32_t g_idleEnterTicks;
00084 uint32_t g_idleTotalTicks;
00085 uint8_t g_idleActive;
00086
00087 #endif
00088
00089
00113 OtosTask* otosCreateTask(void (*pFunc)(void*), void* pArg, uint8_t prio, uint16_t stacksize, uint8_t name)
00114 {
00115 OtosTask* pTask;
00116 TaskStackLayout* pStack;
00117 uint8_t* pMem;
00118
00119
00120
00121 pMem = otosAllocate(stacksize);
00122 if (pMem == NULL)
00123 return NULL;
00124
00125
00126 pTask = otosAllocate(sizeof (OtosTask));
00127 if (pTask == NULL)
00128 return NULL;
00129
00130 #ifdef OTOS_DEBUG_STACK
00131
00132 memset(pMem, 0x42, stacksize);
00133
00134
00135 pTask->stacksize = stacksize;
00136 #endif
00137
00138 #ifdef OTOS_STACK_CHECK
00139
00140 pTask->pStack = (uint16_t*) pMem;
00141 *(pTask->pStack) = STACK_CHECK;
00142 #endif
00143
00144
00145 pStack = (TaskStackLayout*) (pMem + stacksize - sizeof (TaskStackLayout));
00146
00147
00148
00149 pStack->pcHi = ((uint16_t) pFunc) >> 8;
00150 pStack->pcLo = ((uint16_t) pFunc) & 0xff;
00151
00152 pStack->r24 = ((uint16_t) pArg) & 0xff;
00153 pStack->r25 = ((uint16_t) pArg) >> 8;
00154 #ifdef OTOS_BANKING
00155 pStack->bank = 0;
00156 #endif
00157
00158 cli();
00159
00160
00161 pTask->sp = (uint16_t) pStack - 1;
00162 pTask->events = 0;
00163 pTask->waitEvents = 0;
00164 pTask->pWaitMsgQueue = NULL;
00165 pTask->pWaitMutex = NULL;
00166 pTask->sleepTicks = 0;
00167 pTask->priority = prio;
00168 pTask->name = name;
00169
00170
00171 otosInsertTaskInQueue(&g_pReadyQueue, pTask, TASK_READY);
00172
00173
00174
00175
00176
00177
00178 otosScheduler(SCHED_NORM);
00179
00180 return pTask;
00181 }
00182
00183
00190 OtosTask* otosGetRunningTask(void)
00191 {
00192 return g_pRunningTask;
00193 }
00194
00195
00204 uint8_t otosSetPriority(uint8_t prio)
00205 {
00206 uint8_t oldPrio;
00207
00208
00209 cli();
00210 oldPrio = g_pRunningTask->priority;
00211 g_pRunningTask->priority = prio;
00212 otosScheduler(SCHED_NORM);
00213
00214 return oldPrio;
00215 }
00216
00217
00218 #ifdef OTOS_PROFILING
00219
00228 uint8_t otosGetIdleRate(void)
00229 {
00230 return 100 * g_idleTotalTicks / otosGetTicks();
00231 }
00232
00233 #endif
00234
00235
00236
00257 OtosTask* otosCreateMainTask(uint8_t prio, uint16_t stacksize)
00258 {
00259 OtosTask* pTask;
00260 uint8_t* pSpNew;
00261 uint16_t spOld;
00262 uint8_t* pMem;
00263
00264
00265
00266 pMem = otosAllocate(stacksize);
00267 if (pMem == NULL)
00268 otosError_P(msgErrOutOfMem);
00269
00270
00271 pTask = otosAllocate(sizeof (OtosTask));
00272 if (pTask == NULL)
00273 otosError_P(msgErrOutOfMem);
00274
00275 #ifdef OTOS_DEBUG_STACK
00276
00277 memset(pMem, 0x42, stacksize);
00278
00279
00280 pTask->stacksize = stacksize;
00281 #endif
00282
00283 #ifdef OTOS_STACK_CHECK
00284
00285 pTask->pStack = (uint16_t*) pMem;
00286 *(pTask->pStack) = STACK_CHECK;
00287 #endif
00288
00289
00290 spOld = GET_SP();
00291
00292
00293 pSpNew = pMem + stacksize - 1 - RAMEND + spOld;
00294
00295 cli();
00296
00297
00298
00299
00300
00301
00302
00303 memcpy(pSpNew + 1, (uint8_t*) spOld + 1, RAMEND - spOld);
00304
00305
00306 __asm__ volatile(
00307 "out %1, %A0" "\n\t"
00308 "out %2, %B0" "\n\t"
00309 :
00310 : "r" (pSpNew), "I" (_SFR_IO_ADDR(SPL)), "I" (_SFR_IO_ADDR(SPH))
00311 );
00312
00313
00314
00315 pTask->state = TASK_RUNNING;
00316 pTask->events = 0;
00317 pTask->waitEvents = 0;
00318 pTask->pWaitMsgQueue = NULL;
00319 pTask->pWaitMutex = NULL;
00320 pTask->sleepTicks = 0;
00321 pTask->priority = prio;
00322 pTask->name = NAME_MAIN;
00323
00324
00325 g_pRunningTask = pTask;
00326
00327
00328 sei();
00329
00330 return pTask;
00331 }
00332
00333
00344 void otosDispatch(OtosTask* pOldTask, OtosTask* pNewTask)
00345 {
00346
00347
00348 TASK_CONTEXT_SAVE(pOldTask);
00349
00350
00351 TASK_CONTEXT_RESTORE(pNewTask);
00352
00353
00354 sei();
00355 }
00356
00357
00368 void otosScheduler(uint8_t operation)
00369 {
00370 OtosTask* pTmp;
00371 #ifdef OTOS_STACK_CHECK
00372 static uint8_t errorMsg[] = "otOS ERR: task x stack overflow";
00373 #endif
00374
00375
00376 cli();
00377
00378 g_performDispatch = FALSE;
00379
00380 #ifdef OTOS_STACK_CHECK
00381
00382 if (*(g_pRunningTask->pStack) != STACK_CHECK)
00383 {
00384 errorMsg[15] = g_pRunningTask->name;
00385 otosError(errorMsg);
00386 }
00387 #endif
00388
00389 if (operation == SCHED_NORM)
00390 {
00391
00392
00393 if ((g_pReadyQueue == NULL) || (g_pReadyQueue->priority < g_pRunningTask->priority))
00394 {
00395 sei();
00396 return;
00397 }
00398 }
00399
00400
00401
00402 pTmp = g_pRunningTask;
00403
00404
00405 g_pRunningTask = g_pReadyQueue;
00406 g_pReadyQueue = g_pReadyQueue->pNext;
00407 g_pRunningTask->state = TASK_RUNNING;
00408
00409 if (operation == SCHED_NORM)
00410
00411 otosInsertTaskInQueue(&g_pReadyQueue, pTmp, TASK_READY);
00412
00413 else
00414
00415 otosInsertTaskInQueue(&g_pBlockedQueue, pTmp, TASK_BLOCKED);
00416
00417
00418 #ifdef OTOS_PROFILING
00419 if (!g_idleActive)
00420 {
00421 if (g_pRunningTask->priority == 0)
00422 {
00423
00424 g_idleEnterTicks = otosGetTicks();
00425 g_idleActive = TRUE;
00426 }
00427 }
00428 else
00429 {
00430
00431 {
00432
00433 g_idleActive = FALSE;
00434 g_idleTotalTicks += otosGetTickDiff(g_idleEnterTicks);
00435 }
00436 }
00437 #endif
00438
00439
00440 otosDispatch(pTmp, g_pRunningTask);
00441 }
00442
00443
00456 void otosWakeup(OtosTask* pTask)
00457 {
00458 OtosTask* pTmp;
00459 OtosTask* volatile* pTmpPrev;
00460
00461
00462
00463 pTmp = g_pBlockedQueue;
00464 pTmpPrev = &g_pBlockedQueue;
00465
00466
00467 while ((pTmp != pTask) && (pTmp != NULL))
00468 {
00469 pTmpPrev = &pTmp->pNext;
00470 pTmp = pTmp->pNext;
00471 }
00472
00473 if (pTmp == NULL)
00474 otosError_P(PSTR("otOS ERR: task not found"));
00475
00476
00477 *pTmpPrev = pTask->pNext;
00478
00479
00480 otosInsertTaskInQueue(&g_pReadyQueue, pTask, TASK_READY);
00481 }
00482
00499 void otosInsertTaskInQueue(OtosTask* volatile* pQueue, OtosTask* pTask, uint8_t state)
00500 {
00501 OtosTask* pTmp;
00502 OtosTask* volatile* pTmpPrev;
00503
00504
00505
00506 pTmp = *pQueue;
00507 pTmpPrev = pQueue;
00508
00509
00510 while ((pTmp != NULL) && (pTmp->priority >= pTask->priority))
00511 {
00512 pTmpPrev = &pTmp->pNext;
00513 pTmp = pTmp->pNext;
00514 }
00515
00516
00517 pTask->pNext = pTmp;
00518 *pTmpPrev = pTask;
00519
00520 pTask->state = state;
00521 }
00522
00533 uint8_t otosSetPreemtive(uint8_t timeSlice)
00534 {
00535 uint8_t oldSetting;
00536 uint8_t iStat;
00537
00538
00539 oldSetting = g_timeSliceTicks;
00540
00541 iStat = SREG;
00542 cli();
00543 g_timeSliceCounter = timeSlice;
00544 g_timeSliceTicks = timeSlice;
00545 SREG = iStat;
00546
00547 return oldSetting;
00548 }
00549
00560 uint8_t otosGetPreemtive(void)
00561 {
00562 return g_timeSliceTicks;
00563 }
00564
00575 OTOS_TASK_FUNCTION(idle, pArg)
00576 {
00577 while (1)
00578 {
00579 otosScheduler(SCHED_NORM);
00580 }
00581 }
00582
00583
00594 int main(void)
00595 {
00596
00597 otosInitHardware();
00598
00599 #ifdef OTOS_BANKING
00600
00601 otosSetRamBank(0);
00602 #endif
00603
00604
00605 otosGetTicks();
00606
00607
00608 otosCreateMainTask(PRIO_MAIN, STACK_MAIN);
00609
00610
00611 otosCreateTask(idle, NULL, PRIO_IDLE, STACK_IDLE, NAME_IDLE);
00612
00613
00614 otosMain();
00615 while (1);
00616 }
00617
00621 #ifdef OTOS_DEBUG_TASK_QUEUES
00622 #include "dev/uart.h"
00623
00624 void otos_dump_queues(void)
00625 {
00626 OtosTask* pTask;
00627
00628
00629 cli();
00630
00631 otosPrint_P(PSTR("\nRunning Task: "));
00632 otosPutchar(g_pRunningTask->name);
00633 otosPutchar('\n');
00634
00635 otosPrint_P(PSTR("Ready Queue: "));
00636 pTask = g_pReadyQueue;
00637 while (pTask != NULL)
00638 {
00639 otosPutchar(pTask->name);
00640 otosPutchar(' ');
00641 pTask = pTask->pNext;
00642 }
00643 otosPutchar('\n');
00644
00645 otosPrint_P(PSTR("Blocked Queue: "));
00646 pTask = g_pBlockedQueue;
00647 while (pTask != NULL)
00648 {
00649 otosPutchar(pTask->name);
00650 otosPutchar(' ');
00651 pTask = pTask->pNext;
00652 }
00653 otosPutchar('\n');
00654
00655 sei();
00656
00657
00658 }
00659 #endif
00660
00661
00662 #ifdef OTOS_DEBUG_STACK
00663 #include "dev/uart.h"
00664
00665 static unsigned char convertBuff[9];
00666
00667 unsigned char hexTab[] PROGMEM = "0123456789abcdef";
00668
00669
00671
00673
00674 {
00675 convertBuff[0] = PRG_RDB(&hexTab[ucValue >> 4]);
00676 convertBuff[1] = PRG_RDB(&hexTab[ucValue & 0x0f]);
00677 convertBuff[2] = '\0';
00678
00679 return convertBuff;
00680 }
00681
00683
00685
00686 {
00687 convertBuff[0] = PRG_RDB(&hexTab[usiValue >> 12]);
00688 convertBuff[1] = PRG_RDB(&hexTab[(usiValue >> 8) & 0x0f]);
00689 convertBuff[2] = PRG_RDB(&hexTab[(usiValue >> 4) & 0x0f]);
00690 convertBuff[3] = PRG_RDB(&hexTab[usiValue & 0x0f]);
00691 convertBuff[4] = '\0';
00692
00693 return convertBuff;
00694 }
00695
00699 void otos_dump_task_stack(OtosTask* pTask)
00700 {
00701 uint8_t* addr;
00702 uint16_t i;
00703
00704
00705 addr = (uint8_t*) pTask->pStack;
00706
00707 for (i = 0; i < pTask->stacksize; ++i)
00708 {
00709 otosPrint(shortToHex(pTask->stacksize - 1 - i));
00710 otosPrint(": ");
00711 otosPrint(byteToHex(*addr));
00712 otosPrint("\n");
00713 ++addr;
00714 }
00715 otosPrint("\n");
00716 }
00717
00718 void otos_dump_stack(void)
00719 {
00720 OtosTask* pTask;
00721
00722
00723 cli();
00724
00725 otosPrint_P(PSTR("\nRunning Task:\n"));
00726 otosPutchar(g_pRunningTask->name);
00727 otosPutchar('\n');
00728 otos_dump_task_stack(g_pRunningTask);
00729
00730 otosPrint_P(PSTR("Ready Queue:\n"));
00731 pTask = g_pReadyQueue;
00732 while (pTask != NULL)
00733 {
00734 otosPutchar(pTask->name);
00735 otosPutchar('\n');
00736 otos_dump_task_stack(pTask);
00737 pTask = pTask->pNext;
00738 }
00739 otosPutchar('\n');
00740
00741 otosPrint_P(PSTR("Blocked Queue:\n"));
00742 pTask = g_pBlockedQueue;
00743 while (pTask != NULL)
00744 {
00745 otosPutchar(pTask->name);
00746 otosPutchar('\n');
00747 otos_dump_task_stack(pTask);
00748 pTask = pTask->pNext;
00749 }
00750 otosPutchar('\n');
00751
00752 sei();
00753
00754
00755 }
00756
00757
00758 #endif
00759