os_cpu_a.s

STR71X系列ARM微控制器原理与实践配套光盘

        
        LDMFD   SP!, {R0-R12,LR,PC}^    @    Pop new task's context

@*********************************************************************************************************
@                   PERFORM A CONTEXT SWITCH (From interrupt level) - OSIntCtxSw()
@
@ Note(s) : 1) OSIntCtxSw() is called in SYS mode with BOTH FIQ and IRQ interrupts DISABLED
@
@           2) The pseudo-code for OSCtxSw() is:
@              a) OSTaskSwHook();
@              b) OSPrioCur             = OSPrioHighRdy;
@              c) OSTCBCur              = OSTCBHighRdy;
@              d) SP                    = OSTCBHighRdy->OSTCBStkPtr;
@              e) Restore the new task's context from the new task's stack
@              f) Return to new task's code
@
@           3) Upon entry: 
@              OSTCBCur      points to the OS_TCB of the task to suspend
@              OSTCBHighRdy  points to the OS_TCB of the task to resume
@*********************************************************************************************************

OSIntCtxSw:
        LDR     R0, =OSTaskSwHook       @ OSTaskSwHook();
        MOV     LR, PC
        BX      R0            

        LDR     R4,=OSPrioCur           @ OSPrioCur = OSPrioHighRdy
        LDR     R5,=OSPrioHighRdy
        LDRB    R6,[R5]
        STRB    R6,[R4]
        
        LDR     R4,=OSTCBCur            @ OSTCBCur  = OSTCBHighRdy;
        LDR     R6,=OSTCBHighRdy
        LDR     R6,[R6]
        STR     R6,[R4]

        LDR     SP,[R6]                 @ SP = OSTCBHighRdy->OSTCBStkPtr;

                                        @ RESTORE NEW TASK'S CONTEXT
        LDMFD   SP!, {R4}               @    Pop new task's CPSR
        MSR     SPSR_cxsf, R4
        
        LDMFD   SP!, {R0-R12,LR,PC}^    @    Pop new task's context

@*********************************************************************************************************
@                                      IRQ Interrupt Service Routine
@*********************************************************************************************************

OS_CPU_IRQ_ISR:

        STMFD   SP!, {R1-R3}                   @ PUSH WORKING REGISTERS ONTO IRQ STACK                     
        
        MOV     R1, SP                         @ Save   IRQ stack pointer
        
        ADD     SP, SP,#12                     @ Adjust IRQ stack pointer 
        
        SUB     R2, LR,#4                      @ Adjust PC for return address to task

        MRS     R3, SPSR                       @ Copy SPSR (i.e. interrupted task's CPSR) to R3
        
        MSR     CPSR_c, #(NO_INT | SVC32_MODE) @ Change to SVC mode

                                               @ SAVE TASK'S CONTEXT ONTO TASK'S STACK
        STMFD   SP!, {R2}                      @    Push task's Return PC
        STMFD   SP!, {LR}                      @    Push task's LR
        STMFD   SP!, {R4-R12}                  @    Push task's R12-R4
        
        LDMFD   R1!, {R4-R6}                   @    Move task's R1-R3 from IRQ stack to SVC stack
        STMFD   SP!, {R4-R6}
        STMFD   SP!, {R0}                      @    Push task's R0    onto task's stack
        STMFD   SP!, {R3}                      @    Push task's CPSR (i.e. IRQ's SPSR)
                
                                               @ HANDLE NESTING COUNTER
        LDR     R0, =OSIntNesting              @ OSIntNesting++;
        LDRB    R1, [R0]
        ADD     R1, R1,#1
        STRB    R1, [R0]

        CMP     R1, #1                         @ if (OSIntNesting == 1) {
        BNE     OS_CPU_IRQ_ISR_1

        LDR     R4, =OSTCBCur                  @     OSTCBCur->OSTCBStkPtr = SP
        LDR     R5, [R4]
        STR     SP, [R5]                       @ }

OS_CPU_IRQ_ISR_1:
        MSR     CPSR_c, #(NO_INT | IRQ32_MODE) @ Change to IRQ mode (to use the IRQ stack to handle interrupt)
        
@        LDR     R0, =OS_CPU_IRQ_ISR_Handler    ; OS_CPU_IRQ_ISR_Handler();
@        MOV     LR, PC
@        BX      R0            

.equ EIC_base_addr,               0xFFFFF800@ EIC base address.
.equ CICR_off_addr,               0x04      @ Current Interrupt Channel Register.
.equ IVR_off_addr,                0x18      @ Interrupt Vector Register.
.equ IPR_off_addr,                0x40      @ Interrupt Pending Register.

        LDR     R0, =(EIC_base_addr + IVR_off_addr)
        MOV     LR, PC
        BX      R0                             @ Branch to the IRQ handler.
                                               @ Clear pending bit in EIC (using the proper IPRx)
        LDR     R0, =EIC_base_addr
        LDR     R2, [R0, #CICR_off_addr]       @ Get the IRQ channel number.
        MOV     R3, #1
        MOV     R3, R3, LSL r2
        STR     R3, [R0, #IPR_off_addr]        @ Clear the corresponding IPR bit.
        
        MSR     CPSR_c, #(NO_INT | SVC32_MODE) @ Change to SVC mode
        
        LDR     R0, =OSIntExit                 @ OSIntExit();
        MOV     LR, PC
        BX      R0            
        
                                               @ RESTORE NEW TASK'S CONTEXT
        LDMFD   SP!, {R4}                      @    Pop new task's CPSR
        MSR     SPSR_cxsf, R4
        
        LDMFD   SP!, {R0-R12,LR,PC}^           @    Pop new task's context

@*********************************************************************************************************
@                                      FIQ Interrupt Service Routine
@*********************************************************************************************************

OS_CPU_FIQ_ISR:
                                        
        STMFD   SP!, {R1-R3}                   @ PUSH WORKING REGISTERS ONTO FIQ STACK                     
        
        MOV     R1, SP                         @ Save   FIQ stack pointer
        
        ADD     SP, SP,#12                     @ Adjust FIQ stack pointer 
        
        SUB     R2, LR,#4                      @ Adjust PC for return address to task

        MRS     R3, SPSR                       @ Copy SPSR (i.e. interrupted task's CPSR) to R3
        
        MSR     CPSR_c, #(NO_INT | SVC32_MODE) @ Change to SVC mode

                                               @ SAVE TASK'S CONTEXT ONTO TASK'S STACK
        STMFD   SP!, {R2}                      @    Push task's Return PC
        STMFD   SP!, {LR}                      @    Push task's LR
        STMFD   SP!, {R4-R12}                  @    Push task's R12-R4
        
        LDMFD   R1!, {R4-R6}                   @    Move task's R1-R3 from FIQ stack to SVC stack
        STMFD   SP!, {R4-R6}
        STMFD   SP!, {R0}                      @    Push task's R0    onto task's stack
        STMFD   SP!, {R3}                      @    Push task's CPSR (i.e. FIQ's SPSR)
                
                                               @ HANDLE NESTING COUNTER
        LDR     R0, =OSIntNesting              @ OSIntNesting++;
        LDRB    R1, [R0]
        ADD     R1, R1,#1
        STRB    R1, [R0]

        CMP     R1, #1                         @ if (OSIntNesting == 1) {
        BNE     OS_CPU_FIQ_ISR_1

        LDR     R4, =OSTCBCur                  @     OSTCBCur->OSTCBStkPtr = SP
        LDR     R5, [R4]
        STR     SP, [R5]                       @ }

OS_CPU_FIQ_ISR_1:
        MSR     CPSR_c, #(NO_INT | FIQ32_MODE) @ Change to FIQ mode (to use the FIQ stack to handle interrupt)
        
        LDR     R0, =FIQ_Handler               @ FIQ_Handler();
        MOV     LR, PC
        BX      R0            
        
        MSR     CPSR_c, #(NO_INT | SVC32_MODE) @ Change to SVC mode
        
        LDR     R0, =OSIntExit                 @ OSIntExit();
        MOV     LR, PC
        BX      R0            
        
                                               @ RESTORE NEW TASK'S CONTEXT
        LDMFD   SP!, {R4}                      @    Pop new task's CPSR
        MSR     SPSR_cxsf, R4
        
        LDMFD   SP!, {R0-R12,LR,PC}^           @    Pop new task's context

        .ltorg