os_armaux.s

NXP LPC ARMThumb port for uCOS-II

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;
; File:                 os_cpu_a.s
; 
; Purpose:              Provide interfaces to routines that must be
;                       done in native ARM mode to get at the architecture
;                       specific registers.
;
; By:                   Lee Dunbar
;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

SwiV        = (0x08)
IrqV        = (0x18)
FiqV        = (0x1C)
NoInt       = (0x80)
ThumbFlag   = (0x20)

SVC32Mode   = (0x13)
IRQ32Mode   = (0x12)
FIQ32Mode   = (0x11)

OSEnterSWI  = (0x00)

    .text
; External symbols we need the addresses of

    .extern OSTCBCur
addr_OSTCBCur:  
    .long   OSTCBCur

    .extern OSTCBHighRdy
addr_OSTCBHighRdy:  
    .long   OSTCBHighRdy

    .extern OSPrioCur 
addr_OSPrioCur:
    .long   OSPrioCur

    .extern OSPrioHighRdy    
addr_OSPrioHighRdy:
    .long   OSPrioHighRdy


        
    .global OSIrqContextSwap
    .type   OSIrqContextSwap, @function
    .align  8
OSIrqContextSwap:
    ; NOTE: The following code assumes that all threads use r13 as
    ; the stack-pointer, and that it is a APCS conformant stack.
    ; i.e. there is never any data stored beneath the current
    ; stack-pointer.
    ; The above needs to be true to enable the context switches
    ; started as a return from an interrupt to use the current
    ; threads stack as the state save area.
    ;
    ldmfd   sp!,{r12}               ; recover SPSR value from stack
    ; if NE then we need to check if we were a nested interrupt
    and r11,r12,#0x1F               ; mask out all but the mode bits
    teqne   r11,#IRQ32Mode          ; check for interrupted IRQ thread
    ; if EQ then we can return immediately
    msreq   SPSR,r12                ; restore the SPSR
    ldmeqfd sp!,{r0-r12,pc}^        ; and return to the interrupted thread

    ; We now need to perform a context switch.
    ; r12 = SPSR describing the interrupted thread.
    ; r11 = interrupted thread processor mode

    ; We need to protect the SPSR before we actually perform the
    ; return to the interrupted thread, since we don't want to
    ; lose the value by another interrupt occuring between the
    ; SPSR load and the PC+CPSR load. Similarly we need to protect
    ; the IRQ stack and threading code while we setup the state
    ; required to enter the context switch code from an interrupt
    ; routine. We rely on the interrupted thread having IRQs
    ; enabled (since we would never have reached this point
    ; otherwise).
    ; We have recovered the SPSR value, so only r0-r12,lr are on the stack.

    ldr r4,[sp,#(13 * 4)]           ; load return address: nasty use of
                                    ; a constant
    ; r11 contains the mode bits describing the interrupted thread
    mrs r0,CPSR                     ; get current mode info.
    orr r0,r0,#0x80                 ; and set IRQ disable flag
    bic r1,r0,#0x1F                 ; clear mode bits
    orr r1,r1,r11                   ; insert interrupted thread mode bits
    msr CPSR,r1                     ; and change to that mode

    ; We are now in the interrupted thread mode with IRQs
    ; disabled.
    mov r3,lr                       ; copy LR_svc
    mrs r1,SPSR                     ; copy SPSR_svc
    mov r2,r12                      ; copy SPSR_irq which is CPSR_svc
    
    tst     r12, #ThumbFlag         ; check for return state
    orrne   r4, r4, 1               ; set thumb bit for bx return 

    stmfd   sp!,{r1,r2,r3,r4}       ; and construct return stack
    msr CPSR,r0                     ; return to IRQ mode
    ; IRQ mode; IRQs disabled
    ; r12 = SPSR describing interrupted thread
    bic     r12,r12,#ThumbFlag      ; clear possible thumb mode
    orr     r12,r12,#NoInt          ; ensure IRQ disabled
    msr     SPSR,r12                ; restore SPSR_irq ready for return
    ldmfd   sp!,{r0-r12,lr}         ; restore all the registers
    subs    pc,pc,#0                ; and return to the interrupted mode
    nop                             ; flush the pipeline
    nop
    nop

    ; we are now executing in the interrupted mode with IRQs enabled
    bl  OS_TASK_SW                  ; perform the context switch

    ldr     lr,[sp,12]              ; look at PC to determine if ARM/Thumb
    tst     lr,1
    bne     thumbSwap

    ldmfd   sp!,{lr}                ; recover the SPSR when the thread 
    msr     SPSR,lr                 ; was interrupted

    ldmfd   sp!,{lr}                ; recover the CPSR when the thread 
    msr     CPSR,lr                 ; was interrupted

    ldmfd   sp!,{lr,pc}             ; return to the interrupted thread

thumbSwap:
    ldmfd   sp!,{lr}                ; recover the SPSR when the thread 
    msr     SPSR,lr                 ; was interrupted

    ldmfd   sp!,{lr}                ; recover the CPSR when the thread 
    bic     lr,lr,#ThumbFlag        ; was interrupted and remove
    msr     CPSR,lr                 ; from thumb mode

    ldmfd   sp!,{lr}                ; recover the return

    stmfd   sp!,{r4}                ; save temporary register
    add     r4,pc,1                 ; to go to thumb mode without changing
    bx      r4                      ; condition codes
    .code16
    pop     {r4}                    ; restore register
    pop     {pc}                    ; return to the interrupted thread

    .code32
;   void OS_TASK_SW(void)
;   
;   Perform a context switch - always called in the SVC mode
;
;   The following code assumes that the virtual memory is directly
;   mapped into  physical memory. If this is not true, the cache must 
;   be flushed at context switch to avoid address aliasing.

    .global OS_TASK_SW
    .type   OS_TASK_SW, @function
    .align  8
OS_TASK_SW:

    stmfd   sp!, {r0-r12, lr}       ; save register file and ret address
    mrs     r4, CPSR
    stmfd   sp!, {r4}               ; save current PSR
    mrs     r4, SPSR                ; YYY+
    stmfd   sp!, {r4}               ; YYY+ save SPSR

    ; Get current task TCB address
    ; OSPrioCur = OSPrioHighRdy
    ldr     r4, addr_OSPrioCur
    ldr     r5, addr_OSPrioHighRdy
    ldrb    r6, [r5]
    strb    r6, [r4]

    ; Get current task TCB address
    ldr     r4, addr_OSTCBCur
    ldr     r5, [r4]
    str     sp, [r5]                ; store sp in preempted tasks's TCB

    ; Get highest priority task TCB address
    ldr     r6, addr_OSTCBHighRdy
    ldr     r6, [r6]
    ldr     sp, [r6]                ; get new task's stack pointer

    ; OSTCBCur = OSTCBHighRdy
    str     r6, [r4]                ; set new current task TCB address

    ldmfd   sp!, {r4}               ; YYY+
    msr     SPSR, r4                ; YYY+
    ldmfd   sp!, {r4}               ; YYY+
    msr     CPSR, r4                ; YYY+
    ldmfd   sp!, {r0-r12,lr}        ; YYY+
    bx      lr

;       void OS_ENTER_CRITICAL()
;       void OS_EXIT_CRITICAL()
;
;       Enable and Disable IRQ and FIQ preserving current CPU mode
;
    .global OS_ENTER_CRITICAL
    .type   OS_ENTER_CRITICAL, @function
    .align  8
OS_ENTER_CRITICAL:
    mrs     r0, CPSR                ; get the CPSR
    orr     r1, r0, #NoInt          ; INT enabled - disable INT
    msr     CPSR, r1                ; INT enabled - set CPSR
    bx      lr                  

    .global OS_EXIT_CRITICAL
    .type   OS_EXIT_CRITICAL, @function
    .align  8
OS_EXIT_CRITICAL:
    msr     CPSR, r0                ; set the CPSR register
    bx      lr
    


;   void OSStartHighRdy(void)
;   
;   Start the task with the highest priority;
;
    .global OSStartHighRdy
    .type   OSStartHighRdy, @function
    .align  8
OSStartHighRdy:
    ; Get current task TCB address
    ldr     r4, addr_OSTCBCur
    ; Get highest priority task TCB address
    ldr     r5, addr_OSTCBHighRdy
    ldr     r5, [r5]                ; get stack pointer
    ldr     sp, [r5]                ; switch to the new stack

    str     r5, [r4]                ; set new current task TCB address

    ldmfd   sp!, {r4}               ; YYY
    ldmfd   sp!, {r4}               ; get new state from top of the stack
    msr     CPSR, r4                ; CPSR
    ldmfd   sp!, {r0-r12, lr}       ; pop the task default registers
    bx      lr

    .end