OSEK Betriebssystem für HCS12 mit Cosmic compiler

Hallo,
nach einer langen Tortur im "HCS12+RTOS+STACK+ASSEMBLER" Lager bin ich
wieder dabei..
mittlerweile habe ich mich entschieden ein OSEK konformes
OS selber zu schreiben...und bin ein Stück weitergekommen. Ich habe
erstmal einen einfachen Taskwechsel zwischen 3 Tasks realisiert(siehe
Code unten).
Nun meine Frage wie kann ich das gesammte OSEK am bestens
organisieren (in Module aufteilen) statt ohne Planung eine Sammlung von
Funktionen zu schreiben. Auf eure Antwort wäre ich sehr dankbar..

TASK(TASK_IDLE)
{
   typ_UI_16 idx, idx2 ;
   while (TRUE)
   {
      PA1 ^= TRUE; /* led anzeige */
      for(idx=0;idx<0xFFFF;idx++){
        for(idx2=0;idx2<100;idx2++);
      }  /* Delay Test */
      Os_pre_task = TASK_IDLE;
      Os_next_task = TASK1;
       __TRAP_SWI; /* swi */
      //os_simple_schedule_bcc1(); /* Test: freiwillige CPU Abgabe */

   }

}

TASK(TASK1)
{
    typ_UI_08 simpleCnt_1 = 0; /* zaehler von 0 bis 128 */
    typ_UI_16 i, idx2;
    while (TRUE)
    {
      //  simpleCnt_1++;
//      simpleCnt_1 = ( simpleCnt_1 == 0x80 ? 0 : simpleCnt_1 );
      PA3 ^= TRUE; /* rotes led anzeige */
      for(i=0;i<0xFFFF;i++){   /* Delay Test */
        for(idx2=0;idx2<100;idx2++);
      }
      Os_pre_task = TASK1;
      Os_next_task = TASK2;
       __TRAP_SWI; /* Test: freiwillige CPU Abgabe */
    }
}

TASK(TASK2)
{
   static typ_UI_08 simpleCnt_2 = 0x81;/* zaehler von 129 bis 255 */
   typ_UI_16 i, idx2;
   while (TRUE)
   {
      /*~T*/
      PA5 ^= TRUE; /* gelbes led anzeige */
      //simpleCnt_2++;
      //simpleCnt_2 = ( simpleCnt_2 == 0xFF ? 0x81 : simpleCnt_2 );
      for(i=0;i<0xFFFF;i++){   /* Delay Test */
        for(idx2=0;idx2<100;idx2++);
      }
      Os_pre_task = TASK2;
      Os_next_task = TASK_IDLE;
      __TRAP_SWI; /* Test: freiwillige CPU Abgabe */

    }
 }
/* isr routine zum swi Befehl bzw. __TRAP_SWI */
@near @interrupt void isr_main_swi_context_switch (void)
{
//_asm ("sei\n");/* I Bit wird automatisch gesetzt=>sei ueberfluessig */

   /*~+: erstmal SP sichern*/
      __STORESP(); /* sts _tmpSP*/
   /*~+:Stackpointer im TCB des unterbrochenen Task rettern*/
    TaskList[Os_pre_task].p_TCBStackTop = (OS_STK *)tmpSP;
    p_TCBNextRun = &TaskList[Os_next_task];
#asm
   ;  Register aus dem TCB Task in Prozessor laden
   ldx _p_TCBNextRun    ;  Stack Pointer auf neue Task setzen
   lds 0,x
#endasm
   /* rti wird hier autmatisch eingefuegt */
   /* rti ==> restores CCR,D,X,Y Reg and the ReturnAdress from the Stack
*/
}