;********************************************************************************
;*                                                                              *
;*    Copyright (C) 2002 Oki Electric Industry Co., LTD.                        *
;*                                                                              *
;*    System Name    :  uPLAT series                                            *
;*    Module Name    :  SWI handler with ARM Thumb interworking                 *
;*    File   Name    :  swi_handler.s                                           *
;*    Revision       :  01.00                                                   *
;*    Date           :  2002/05/07  initial version                             *
;*                                                                              *
;********************************************************************************

        AREA    SWI_HANDLER, CODE, READONLY

        INCLUDE define.s        ; common definitions

        EXPORT  SWI
        
;********************************
;*      SWI Handler             *
;********************************
SWI
        ENTRY
        STMFD   sp!, {r1-r12,lr}        ; save registers
        MRS     r1, spsr                ; move SPSR into general purpose register
        TST     r1, #T_Bit              ; check ARM or Thumb
        LDRNEH  r1, [lr,#-2]            ; get instruction code(Thumb)
        BICNE   r1, r1, #0xffffff00     ; decode SWI number(Thumb)
        LDREQ   r1, [lr,#-4]            ; get instruction code(ARM)
        BICEQ   r1, r1, #0xff000000     ; decode SWI number(ARM)
        BIC     r2, r1, #0x0000000f     ; decode SWI_Jump_Table number
        AND     r1, r1, #0x0000000f
        ADR     r3, SWI_Jump_Table_Table; get address of jump table table
        LDR     r4, [r3,r2,LSR #2]      ; get address of jump table
        LDR     pc, [r4,r1,LSL #2]      ; refer to jump table and branch.

SWI_Jump_Table_Table
        DCD     SWI_Jump_Table
        DCD     SWI_PM_Jump_Table

SWI_Jump_Table  ; normal SWI jump table
        DCD     SWI_irq_en
        DCD     SWI_irq_dis

SWI_PM_Jump_Table       ; SWI jump table for power management
        DCD     SWI_pm_if_recov
        DCD     SWI_pm_if_dis

SWI_irq_en                   ; enable IRQ
        MRS     r0, SPSR        ; get SPSR
        BIC     r1, r0, #I_Bit  ; I_Bit clear
        AND     r0, r0, #I_Bit  ; set the return value
        MSR     SPSR_c, r1      ; set SPSR
        B       EndofSWI

SWI_irq_dis                  ; disable IRQ
        MRS     r0, SPSR        ; get SPSR
        ORR     r1, r0, #I_Bit  ; I_Bit set
        AND     r0, r0, #I_Bit  ; set the return value
        MSR     SPSR_c, r1      ; set SPSR
        B       EndofSWI

SWI_pm_if_recov         ; recover CPSR
        MSR     SPSR_c, r0      ; Set SPSR
        B       EndofSWI

SWI_pm_if_dis           ; mask IRQ and FIQ. and return pre-masked CPSR
        MRS     r0, SPSR        ; Get SPSR & Return value
        ORR     r3, r0, #I_Bit:OR:F_Bit
        MSR     SPSR_c, r3      ; Set SPSR
        B       EndofSWI

EndofSWI
        LDMFD   sp!, {r1-r12,pc}^       ; restore registers
; End of SWI

        END