;/*----------------------------------------------------------------------------
; *      R T L  -  K e r n e l
; *----------------------------------------------------------------------------
; *      Name:    SWI_RTX.S
; *      Purpose: Pre-defined SWI Handler
; *      Rev.:    V3.00 / 25-apr-2006
; *----------------------------------------------------------------------------
; *      This code is part of the RealView Run-Time Library.
; *      Copyright (c) 2004-2006 KEIL - An ARM Company. All rights reserved. 
; *---------------------------------------------------------------------------*/

T_Bit           EQU     0x20

                IMPORT  os_runtask
;/* Offsets from the 'struct OS_TCB' */
TCB_FCTX        EQU     33
TCB_TSTACK      EQU     36

;/*-------------------------- SWI handler -----------------------------------*/
;/* SWI-0 is reserved for context task switch. 'os_switch_tasks()' prolog    */
;/* and epilog are implemented here in this module.                          */

                PRESERVE8                       ; 8-Byte aligned Stack
                AREA    SWI_RTX, CODE, READONLY
                ARM

                IMPORT  SWI_Count
                IMPORT  SWI_Table

                EXPORT  SWI_Handler
SWI_Handler
                MRS     R12,SPSR                ; Get SPSR
                TST     R12,#T_Bit              ; Check Thumb Bit
                LDRNEH  R12,[LR,#-2]            ; Thumb: Load Halfword
                BICNE   R12,R12,#0xFF00         ;        Extract SWI Number
                LDREQ   R12,[LR,#-4]            ; ARM:   Load Word
                BICEQ   R12,R12,#0xFF000000     ;        Extract SWI Number

                CMP     R12, #0
                BNE     SWI_User

;               /*------------------- os_switch_tasks ------------------------*/

                STMDB   SP,{SP}^                ; Save reduced context
                NOP                             ; After User Bank Register Access
                LDMDB   SP,{R1}                 ; User SP
                MRS     R12,SPSR                ; User CPSR
                STMDB   R1!,{R4-R11,R12,LR}     ; push PC, CPSR, R11-R4
                LDR     R4,=os_runtask          ; os_runtask->tsk_stack
                LDR     R4,[R4,#0x0]
                STR     R1,[R4,#TCB_TSTACK]
                MOV     R4,R0                   ; R4 = p_new

                LDR     R12,=SWI_Table
                LDR     R12,[R12]               ; Load SWI Function Address
                MOV     LR,PC                   ; Return Address
                BX      R12                     ; Call SWI Function 

                LDR     LR,[R4,#TCB_TSTACK]     ; p_new->tsk_stack
                LDRB    R0,[R4,#TCB_FCTX]       ; p_new->full_ctx
                CMP     R0,#0x0
                BNE     Full_ctx

                LDMIA   LR!,{R4-R11,R12}        ; Restore Reduced context
                MSR     SPSR_cxsf,R12
                LDMIA   LR!,{R12}
                STMDB   SP,{LR}                 ; Set User SP
                LDMDB   SP,{SP}^
                NOP                             ; After User Bank Register Access
                MOVS    PC,R12                  ; RETI

Full_ctx
                ADD     R0,LR,#64
                LDMDB   R0,{R1-R3}              ; LR, CPSR (R3=dummy)
                MSR     SPSR_cxsf,R2            ; Set User CPSR
                STMDB   SP,{R0-R1}
                LDMDB   SP,{SP,LR}^             ; Set User SP,LR
                NOP                             ; After User Bank Register Access
                LDMIA   LR,{R0-R12}             ; Restore R12-R0
                LDR     LR,[LR,#60]             ; PC
                MOVS    PC,LR                   ; RETI

;               /*------------------- User SWI -------------------------------*/

SWI_User        STMFD   SP!,{R8,LR}             ; Store R8, LR
                LDR     R8,SWI_Count
                CMP     R12,R8
                BHS     SWI_Dead                ; Overflow
                LDR     R8,=SWI_Table
                LDR     R12,[R8,R12,LSL #2]     ; Load SWI Function Address
                MOV     LR,PC                   ; Return Address
                BX      R12                     ; Call SWI Function 
                LDMFD   SP!,{R8,PC}^            ; Restore R8 and Return

SWI_Dead        B       SWI_Dead                ; None Existing SWI


                END

/*----------------------------------------------------------------------------
 * end of file
 *---------------------------------------------------------------------------*/