0384 90200217  LDS	R2,_OSRunning
    0386 2022      TST	R2
    0387 F009      BEQ	0x0389
    0388 C038      RJMP	0x03C1
(0220)         y             = OSUnMapTbl[OSRdyGrp];        /* Find highest priority's task priority number   */
    0389 E384      LDI	R24,0x34
    038A E090      LDI	R25,0
    038B 91E0021A  LDS	R30,_OSRdyGrp
    038D 27FF      CLR	R31
    038E 0FE8      ADD	R30,R24
    038F 1FF9      ADC	R31,R25
    0390 90A4      LPM	R10,0(Z)
(0221)         x             = OSUnMapTbl[OSRdyTbl[y]];
    0391 E188      LDI	R24,0x18
    0392 E092      LDI	R25,2
    0393 2DEA      MOV	R30,R10
    0394 27FF      CLR	R31
    0395 0FE8      ADD	R30,R24
    0396 1FF9      ADC	R31,R25
    0397 81E0      LDD	R30,0+Z
    0398 27FF      CLR	R31
    0399 E384      LDI	R24,0x34
    039A E090      LDI	R25,0
    039B 0FE8      ADD	R30,R24
    039C 1FF9      ADC	R31,R25
    039D 90C4      LPM	R12,0(Z)
(0222)         OSPrioHighRdy = (INT8U)((y << 3) + x);
    039E 2C2C      MOV	R2,R12
    039F 2433      CLR	R3
    03A0 2C4A      MOV	R4,R10
    03A1 2455      CLR	R5
    03A2 0C44      LSL	R4
    03A3 1C55      ROL	R5
    03A4 0C44      LSL	R4
    03A5 1C55      ROL	R5
    03A6 0C44      LSL	R4
    03A7 1C55      ROL	R5
    03A8 0C42      ADD	R4,R2
    03A9 1C53      ADC	R5,R3
    03AA 9240021B  STS	_OSPrioHighRdy,R4
(0223)         OSPrioCur     = OSPrioHighRdy;
    03AC 2C24      MOV	R2,R4
    03AD 9220021C  STS	_OSPrioCur,R2
(0224)         OSTCBHighRdy  = OSTCBPrioTbl[OSPrioHighRdy]; /* Point to highest priority task ready to run    */
    03AF E082      LDI	R24,2
    03B0 9D82      MUL	R24,R2
    03B1 01F0      MOVW	R30,R0
    03B2 EE8E      LDI	R24,0xEE
    03B3 E091      LDI	R25,1
    03B4 0FE8      ADD	R30,R24
    03B5 1FF9      ADC	R31,R25
    03B6 8020      LDD	R2,0+Z
    03B7 8031      LDD	R3,1+Z
    03B8 92300211  STS	_OSTCBHighRdy+1,R3
    03BA 92200210  STS	_OSTCBHighRdy,R2
(0225)         OSTCBCur      = OSTCBHighRdy;
    03BC 92300215  STS	_OSTCBCur+1,R3
    03BE 92200214  STS	_OSTCBCur,R2
(0226)         OSStartHighRdy();                            /* Execute target specific code to start task     */
    03C0 D2DF      RCALL	_OSStartHighRdy
(0227)     }
(0228) }
    03C1 D427      RCALL	pop_gset4x
    03C2 9508      RET
(0229) /*$PAGE*/
(0230) /*
(0231) *********************************************************************************************************
(0232) *                                              IDLE TASK
(0233) *
(0234) * Description: This task is internal to uC/OS-II and executes whenever no other higher priority tasks
(0235) *              executes because they are waiting for event(s) to occur.
(0236) *
(0237) * Arguments  : none
(0238) *
(0239) * Returns    : none
(0240) *********************************************************************************************************
(0241) */
(0242) 
(0243) void OSTaskIdle (void *pdata)
(0244) {
(0245)     pdata = pdata;                               /* Prevent compiler warning for not using 'pdata'     */
(0246)     for (;;) {
(0247)         OS_ENTER_CRITICAL();
_OSTaskIdle:
  pdata                --> R16
    03C3 930A      ST	R16,-Y
    03C4 B70F      IN	R16,P3F
    03C5 94F8      BCLR	7
    03C6 930F      PUSH	R16
    03C7 9109      LD	R16,Y+
(0248)         OSIdleCtr++;
    03C8 E081      LDI	R24,1
    03C9 E090      LDI	R25,0
    03CA E0A0      LDI	R26,0
    03CB E0B0      LDI	R27,0
    03CC 90400221  LDS	R4,_OSIdleCtr+2
    03CE 90500222  LDS	R5,_OSIdleCtr+3
    03D0 9020021F  LDS	R2,_OSIdleCtr
    03D2 90300220  LDS	R3,_OSIdleCtr+1
    03D4 0E28      ADD	R2,R24
    03D5 1E39      ADC	R3,R25
    03D6 1E4A      ADC	R4,R26
    03D7 1E5B      ADC	R5,R27
    03D8 92300220  STS	_OSIdleCtr+1,R3
    03DA 9220021F  STS	_OSIdleCtr,R2
    03DC 92500222  STS	_OSIdleCtr+3,R5
    03DE 92400221  STS	_OSIdleCtr+2,R4
(0249)         OS_EXIT_CRITICAL();
    03E0 930A      ST	R16,-Y
    03E1 910F      POP	R16
    03E2 BF0F      OUT	P3F,R16
    03E3 9109      LD	R16,Y+
    03E4 CFDE      RJMP	_OSTaskIdle
    03E5 9508      RET
_OSTCBInit:
  ptcb                 --> R10
  opt                  --> Y+10
  pext                 --> Y+8
  stk_size             --> Y+6
  id                   --> Y+4
  pbos                 --> Y+2
  ptos                 --> R18
  prio                 --> R16
    03E6 D406      RCALL	push_gset3x
(0250)     }
(0251) }
(0252) /*$PAGE*/
(0253) /*
(0254) *********************************************************************************************************
(0255) *                                            INITIALIZE TCB
(0256) *
(0257) * Description: This function is internal to uC/OS-II and is used to initialize a Task Control Block when
(0258) *              a task is created (see OSTaskCreate() and OSTaskCreateExt()).
(0259) *
(0260) * Arguments  : prio          is the priority of the task being created
(0261) *
(0262) *              ptos          is a pointer to the task's top-of-stack assuming that the CPU registers
(0263) *                            have been placed on the stack.  Note that the top-of-stack corresponds to a 
(0264) *                            'high' memory location is OS_STK_GROWTH is set to 1 and a 'low' memory
(0265) *                            location if OS_STK_GROWTH is set to 0.  Note that stack growth is CPU
(0266) *                            specific.
(0267) *
(0268) *              pbos          is a pointer to the bottom of stack.  A NULL pointer is passed if called by
(0269) *                            'OSTaskCreate()'.
(0270) *
(0271) *              id            is the task's ID (0..65535)
(0272) *
(0273) *              stk_size      is the size of the stack (in 'stack units').  If the stack units are INT8Us
(0274) *                            then, 'stk_size' contains the number of bytes for the stack.  If the stack
(0275) *                            units are INT32Us then, the stack contains '4 * stk_size' bytes.  The stack
(0276) *                            units are established by the #define constant OS_STK which is CPU
(0277) *                            specific.  'stk_size' is 0 if called by 'OSTaskCreate()'.
(0278) *
(0279) *              pext          is a pointer to a user supplied memory area that is used to extend the task
(0280) *                            control block.  This allows you to store the contents of floating-point
(0281) *                            registers, MMU registers or anything else you could find useful during a 
(0282) *                            context switch.  You can even assign a name to each task and store this name
(0283) *                            in this TCB extension.  A NULL pointer is passed if called by OSTaskCreate().
(0284) *
(0285) *              opt           options as passed to 'OSTaskCreateExt()' or, 
(0286) *                            0 if called from 'OSTaskCreate()'.
(0287) *
(0288) * Returns    : OS_NO_ERR         if the call was successful
(0289) *              OS_NO_MORE_TCB    if there are no more free TCBs to be allocated and thus, the task cannot
(0290) *                                be created.
(0291) *
(0292) * Note       : This function is INTERNAL to uC/OS-II and your application should not call it.
(0293) *********************************************************************************************************
(0294) */
(0295) 
(0296) INT8U OSTCBInit (INT8U prio, OS_STK *ptos, OS_STK *pbos, INT16U id, INT16U stk_size, void *pext, INT16U opt)
(0297) {
(0298)     OS_TCB *ptcb;
(0299) 
(0300) 
(0301)     OS_ENTER_CRITICAL();
    03E7 930A      ST	R16,-Y
    03E8 B70F      IN	R16,P3F
    03E9 94F8      BCLR	7
    03EA 930F      PUSH	R16
    03EB 9109      LD	R16,Y+
(0302)     ptcb = OSTCBFreeList;                                  /* Get a free TCB from the free TCB list    */
    03EC 90A00212  LDS	R10,_OSTCBFreeList
    03EE 90B00213  LDS	R11,_OSTCBFreeList+1
(0303)     if (ptcb != (OS_TCB *)0) {
    03F0 20AA      TST	R10
    03F1 F419      BNE	0x03F5
    03F2 20BB      TST	R11
    03F3 F409      BNE	0x03F5
    03F4 C073      RJMP	0x0468
(0304)         OSTCBFreeList        = ptcb->OSTCBNext;            /* Update pointer to free TCB list          */
    03F5 01F5      MOVW	R30,R10
    03F6 8022      LDD	R2,2+Z
    03F7 8033      LDD	R3,3+Z
    03F8 92300213  STS	_OSTCBFreeList+1,R3
    03FA 92200212  STS	_OSTCBFreeList,R2
(0305)         OS_EXIT_CRITICAL();
    03FC 930A      ST	R16,-Y
    03FD 910F      POP	R16
    03FE BF0F      OUT	P3F,R16
    03FF 9109      LD	R16,Y+
(0306)         ptcb->OSTCBStkPtr    = ptos;                       /* Load Stack pointer in TCB                */
    0400 8320      STD	R18,0+Z
    0401 8331      STD	R19,1+Z
(0307)         ptcb->OSTCBPrio      = (INT8U)prio;                /* Load task priority into TCB              */
    0402 8701      STD	R16,9+Z
(0308)         ptcb->OSTCBStat      = OS_STAT_RDY;                /* Task is ready to run                     */
    0403 2422      CLR	R2
    0404 8620      STD	R2,8+Z
(0309)         ptcb->OSTCBDly       = 0;                          /* Task is not delayed                      */
    0405 2433      CLR	R3
    0406 8226      STD	R2,6+Z
    0407 8237      STD	R3,7+Z
(0310) 
(0311)         pext                 = pext;                       /* Prevent compiler warning if not used     */
(0312)         stk_size             = stk_size;
(0313)         pbos                 = pbos;
(0314)         opt                  = opt;
(0315)         id                   = id;
(0316) 
(0317)         ptcb->OSTCBY         = prio >> 3;                  /* Pre-compute X, Y, BitX and BitY          */
    0408 2E20      MOV	R2,R16
    0409 9426      LSR	R2
    040A 9426      LSR	R2
    040B 9426      LSR	R2
    040C 8623      STD	R2,11+Z
(0318)         ptcb->OSTCBBitY      = OSMapTbl[ptcb->OSTCBY];
    040D E28C      LDI	R24,0x2C
    040E E090      LDI	R25,0
    040F 85E3      LDD	R30,11+Z
    0410 27FF      CLR	R31
    0411 0FE8      ADD	R30,R24
    0412 1FF9      ADC	R31,R25
    0413 91E4      LPM	R30,0(Z)
    0414 01D5      MOVW	R26,R10
    0415 961D      ADIW	R26,0xD
    0416 93EC      ST	R30,0(X)
(0319)         ptcb->OSTCBX         = prio & 0x07;
    0417 2F80      MOV	R24,R16
    0418 7087      ANDI	R24,7
    0419 01F5      MOVW	R30,R10
    041A 8782      STD	R24,10+Z
(0320)         ptcb->OSTCBBitX      = OSMapTbl[ptcb->OSTCBX];
    041B E28C      LDI	R24,0x2C
    041C E090      LDI	R25,0
    041D 85E2      LDD	R30,10+Z
    041E 27FF      CLR	R31
    041F 0FE8      ADD	R30,R24
    0420 1FF9      ADC	R31,R25
    0421 91E4      LPM	R30,0(Z)
    0422 01D5      MOVW	R26,R10
    0423 961C      ADIW	R26,0xC
    0424 93EC      ST	R30,0(X)
(0321) 
(0322)         OS_ENTER_CRITICAL();
    0425 930A      ST	R16,-Y
    0426 B70F      IN	R16,P3F
    0427 94F8      BCLR	7
    0428 930F      PUSH	R16
    0429 9109      LD	R16,Y+
(0323)         OSTCBPrioTbl[prio]   = ptcb;
    042A E082      LDI	R24,2
    042B 9F80      MUL	R24,R16
    042C 01F0      MOVW	R30,R0
    042D EE8E      LDI	R24,0xEE
    042E E091      LDI	R25,1
    042F 0FE8      ADD	R30,R24
    0430 1FF9      ADC	R31,R25
    0431 82A0      STD	R10,0+Z
    0432 82B1      STD	R11,1+Z
(0324)         ptcb->OSTCBNext      = OSTCBList;                  /* Link into TCB chain                      */
    0433 9020020E  LDS	R2,_OSTCBList
    0435 9030020F  LDS	R3,_OSTCBList+1
    0437 01F5      MOVW	R30,R10
    0438 8222      STD	R2,2+Z
    0439 8233      STD	R3,3+Z
(0325)         ptcb->OSTCBPrev      = (OS_TCB *)0;
    043A 2422      CLR	R2
    043B 2433      CLR	R3
    043C 8224      STD	R2,4+Z
    043D 8235      STD	R3,5+Z
(0326)         if (OSTCBList != (OS_TCB *)0) {
    043E 9020020E  LDS	R2,_OSTCBList
    0440 9030020F  LDS	R3,_OSTCBList+1
    0442 2022      TST	R2
    0443 F411      BNE	0x0446
    0444 2033      TST	R3
    0445 F031      BEQ	0x044C
(0327)             OSTCBList->OSTCBPrev = ptcb;
    0446 91E0020E  LDS	R30,_OSTCBList
    0448 91F0020F  LDS	R31,_OSTCBList+1
    044A 82A4      STD	R10,4+Z
    044B 82B5      STD	R11,5+Z
(0328)         }
(0329)         OSTCBList               = ptcb;
    044C 92B0020F  STS	_OSTCBList+1,R11
    044E 92A0020E  STS	_OSTCBList,R10
(0330)         OSRdyGrp               |= ptcb->OSTCBBitY;         /* Make task ready to run                   */
    0450 01F5      MOVW	R30,R10
    0451 8425      LDD	R2,13+Z
    0452 9030021A  LDS	R3,_OSRdyGrp
    0454 2832      OR	R3,R2
    0455 9230021A  STS	_OSRdyGrp,R3
(0331)         OSRdyTbl[ptcb->OSTCBY] |= ptcb->OSTCBBitX;
    0457 E188      LDI	R24,0x18
    0458 E092      LDI	R25,2
    0459 8423      LDD	R2,11+Z
    045A 2433      CLR	R3
    045B 0E28      ADD	R2,R24
    045C 1E39      ADC	R3,R25
    045D 8444      LDD	R4,12+Z
    045E 01F1      MOVW	R30,R2
    045F 8050      LDD	R5,0+Z
    0460 2854      OR	R5,R4
    0461 8250      STD	R5,0+Z
(0332)         OS_EXIT_CRITICAL();
    0462 930A      ST	R16,-Y
    0463 910F      POP	R16
    0464 BF0F      OUT	P3F,R16
    0465 9109      LD	R16,Y+
(0333)         return (OS_NO_ERR);
    0466 2700      CLR	R16
    0467 C005      RJMP	0x046D
(0334)     } else {
(0335)         OS_EXIT_CRITICAL();
    0468 930A      ST	R16,-Y
    0469 910F      POP	R16
    046A BF0F      OUT	P3F,R16
    046B 9109      LD	R16,Y+
(0336)         return (OS_NO_MORE_TCB);
    046C E406      LDI	R16,0x46
    046D D36F      RCALL	pop_gset3x
    046E 9508      RET
(0337)     }
(0338) }
(0339) /*$PAGE*/
(0340) /*
(0341) *********************************************************************************************************
(0342) *                                         PROCESS SYSTEM TICK
(0343) *
(0344) * Description: This function is used to signal to uC/OS-II the occurrence of a 'system tick' (also known
(0345) *              as a 'clock tick').  This function should be called by the ticker ISR but, can also be
(0346) *              called by a high priority task.
(0347) *
(0348) * Arguments  : none
(0349) *
(0350) * Returns    : none
(0351) *********************************************************************************************************