/*---------------------------------------------------------------------------*
  Project:  CtrBrom - library - init
  File:     crt0_excpHandler.c

  Copyright 2008 Nintendo.  All rights reserved.

  These coded instructions, statements, and computer programs contain
  proprietary information of Nintendo of America Inc. and/or Nintendo
  Company Ltd., and are protected by Federal copyright law.  They may
  not be disclosed to third parties or copied or duplicated in any form,
  in whole or in part, without the prior written consent of Nintendo.

  $Date::            $
  $Rev$
  $Author$
 *---------------------------------------------------------------------------*/
#include <brom/code32.h>
#include <brom/os.h>
#include <brom/swi.h>


/*---------------------------------------------------------------------------*
  Name:         stupInitExceptions

  Description:  Initialize Exceptions

  Arguments:    None

  Returns:      None
 *---------------------------------------------------------------------------*/
asm void stupInitExceptions( void )
{
        adr     r2, i_stupExcpVennerCode
        ldr     r3, =HW_EXCP_VENEER_BUF
        add     r12, r3, #HW_EXCP_VENEER_BUF_SIZE
LSYM(0)
        ldr     r0, [r2], #4
        ldm     r0, {r0-r1}
        stm     r3!,{r0-r1}
        cmp     r3, r12
        blt     BSYM(0)

#ifdef SDK_ARM11
        // disable VFP
#ifdef SDK_MG20EMU
        fmrx    r12, fpexc
        bic     r12, r12, #HW_FPEXC_VFP_ENABLE
        fmxr    fpexc, r12
#endif // SDK_MG20EMU
#endif // SDK_ARM11

        bx      lr

i_stupExcpVennerCode
        DCD     i_stupExcpTerminateCode  // IRQ
        DCD     i_stupExcpTerminateCode  // FIQ
        DCD     i_stupSwiVeneerCode      // SWI    osInit前にprint文を出したい可能性があるためここで設定
#ifdef BROM_ENABLE_SCATLD_VFP
        INASM_EXTERN( i_osUndefInstHandler )
        DCD     i_osUndefInstHandler     // UNDEF
#else // BROM_ENABLE_SCATLD_VFP
        DCD     i_stupExcpTerminateCode  // UNDEF
#endif // BROM_ENABLE_SCATLD_VFP
        DCD     i_stupIAbtTerminateCode  // IABT   命令アボートとデータアボートを区別するため別関数に
        DCD     i_stupExcpTerminateCode  // DABT

        LTORG
}

/*---------------------------------------------------------------------------*
  Name:         i_stupExcpTerminateCode

  Description:  exception veneer code

  Arguments:    None

  Returns:      None
 *---------------------------------------------------------------------------*/
asm void i_stupExcpTerminateCode( void )
{
        ldr     pc, =i_stupExcpTerminateCode
        LTORG
}

/*---------------------------------------------------------------------------*
  Name:         i_stupIAbtTerminateCode

  Description:  Prefetch Abort veneer code

  Arguments:    None

  Returns:      None
 *---------------------------------------------------------------------------*/
asm void i_stupIAbtTerminateCode( void )
{
        ldr     pc, =i_stupIAbtTerminateCode
        LTORG
}

/*---------------------------------------------------------------------------*
  Name:         i_stupSwiVeneerCode

  Description:  SWI veneer code

  Arguments:    None

  Returns:      None
 *---------------------------------------------------------------------------*/
asm void i_stupSwiVeneerCode( void )
{
        ldr     pc, =i_stupSwiHandler
        LTORG
}

/*---------------------------------------------------------------------------*
  Name:         i_stupIrqVeneer

  Description:  IRQ veneer

  Arguments:    None

  Returns:      None.
 *---------------------------------------------------------------------------*/
asm void i_stupIrqVeneer( void )
{
        ldr     pc, =HW_INTR_VENEER_BUF

        LTORG
}

/*---------------------------------------------------------------------------*
  Name:         i_stupFiqVeneer

  Description:  FIQ veneer

  Arguments:    None

  Returns:      None.
 *---------------------------------------------------------------------------*/
asm void i_stupFiqVeneer( void )
{
        ldr     pc, =HW_FIQ_VENEER_BUF

        LTORG
}

/*---------------------------------------------------------------------------*
  Name:         i_stupSwiVeneer

  Description:  SWI veneer

  Arguments:    None

  Returns:      None.
 *---------------------------------------------------------------------------*/
asm void i_stupSwiVeneer( void )
{
        ldr     pc, =HW_SWI_VENEER_BUF

        LTORG
}

/*---------------------------------------------------------------------------*
  Name:         i_stupUndefVeneer

  Description:  Undefine Instruction veneer

  Arguments:    None

  Returns:      None.
 *---------------------------------------------------------------------------*/
asm void i_stupUndefVeneer( void )
{
        ldr     pc, =HW_UNDEF_VENEER_BUF

        LTORG
}

/*---------------------------------------------------------------------------*
  Name:         i_stupIAbtVeneer

  Description:  Prefetch Abort veneer

  Arguments:    None

  Returns:      None.
 *---------------------------------------------------------------------------*/
asm void i_stupIAbtVeneer( void )
{
        ldr     pc, =HW_IABT_VENEER_BUF

        LTORG
}

/*---------------------------------------------------------------------------*
  Name:         i_stupDAbtVeneer

  Description:  Data Abort veneer

  Arguments:    None

  Returns:      None.
 *---------------------------------------------------------------------------*/
asm void i_stupDAbtVeneer( void )
{
        ldr     pc, =HW_DABT_VENEER_BUF

        LTORG
}

/*---------------------------------------------------------------------------*
  Name:         i_stupSwiHandler

  Description:  SWI handler

  Arguments:    None

  Returns:      None.
 *---------------------------------------------------------------------------*/
asm void i_stupSwiSemihosting( void )
{
        // 割り込み禁止
        mrs     r1, cpsr
        orr     r1, r1, #(HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE)
        msr     cpsr_fsxc, r1
        ldmfd   sp!, {r0-r1, lr}
        add     sp, sp, #12
        msr     cpsr_fsxc, #(HW_PSR_SVC_MODE | HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE)
        // SVCモード
        ldmfd   sp!, {lr}                      // ※SP_svc
        msr     spsr_fsxc, lr
        ldmfd   sp!, {lr}
semi_vector
        // セミホスティングアドレスはSVCモードでないとARMデバッガがエラーを出力
        movs    pc, lr
}

asm void i_stupSwiHandler( void )
{
        INASM_EXTERN( _start )

        // 不正確データアボート有効化（clear HW_PSR_IMPR_ABORT_DISABLE）
        // FIQハンドラでのSWI使用時のSP_svc上書き防止のため最初にFIQを禁止（IRQはHWが禁止する）
        msr     cpsr_fsxc, #(HW_PSR_SVC_MODE | HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE)

        // SVCモード
        mrs     sp, spsr                        // ※SP_svc
        and     sp, sp, #HW_PSR_CPU_MODE_MASK
        // ユーザモードならシステムモードへ
        cmp     sp, #HW_PSR_USR_MODE
        moveq   sp, #HW_PSR_SYS_MODE
        // 割り込み禁止
        orr     sp, sp, #(HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE)
        msr     cpsr_fsxc, sp

        // 呼び出し元のモード
        sub     sp, sp, #12
        stmfd   sp!, {r0-r1, lr}                // このタイミングでデータアボートハンドラがSWIを使用するとSVCレジスタは復帰不能
        add     r1, sp, #24                     // 呼び出し元のスタック内へ確保したSPSR_svc、LR_svc用スタックアドレスをコピー
        msr     cpsr_fsxc, #(HW_PSR_SVC_MODE | HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE)

        // SVCモード
        mov     sp, r1                          // ※SP_svc
        mov     r1, lr
        mrs     r0, spsr
        stmfd   sp!, {r0, lr}                   // SPSR_svc、LR_svcを退避（このLR_svcは最後にPOPするので安全）
        // 割り込み状態継承
        and     r0, r0, #(HW_PSR_CPU_MODE_MASK | HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE)
        // ユーザモードなら割り込み禁止でシステムモードへ
        // （割り込み禁止にするのは別スレッドからスタックへ退避したPCを
        //  書き換えて、システムモードを乗っ取られないようにするため。
        //  但し、DMAによるハッキングへ対応するにはユーザモードとSVCモードの
        //  スタックを分ける必要がある（OSのラッパーで対応予定）。）
        tst     r0, #(HW_PSR_SYS_MODE ^ HW_PSR_USR_MODE)
        orreq   r0, r0, #(HW_PSR_SYS_MODE | HW_PSR_IRQ_DISABLE)
        msr     cpsr_fsxc, r0

        // 呼び出し元のモード
        ldrh    r0, [r1, #-2]                  // システムコールNo取得
        mov     r0, r0, lsl #1
        and     r0, r0, #SVC_ID_PREMASK<<SVC_ID_SHIFT

        cmp     r0, #(SVC_ID_SEMIHOST_ARM   & SVC_ID_PREMASK)<<SVC_ID_SHIFT  // SVC_ID_SEMIHOST_ARM   = 0x12
        cmpne   r0, #(SVC_ID_SEMIHOST_THUMB & SVC_ID_PREMASK)<<SVC_ID_SHIFT  // SVC_ID_SEMIHOST_THUMB = 0xab
        beq     i_stupSwiSemihosting

        adr     r1, SWI_Table                  // システムコールアドレス（2バイトテーブル）取得
        ldrh    r0, [r1, r0]

        ldr     r1, =HW_BROM
        orr     r0, r0, r1

        blx     r0

swi_return
        // 割り込み禁止
        mrs     r1, cpsr
        orr     r1, r1, #(HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE)
        msr     cpsr_fsxc, r1
        ldmfd   sp!, {r0-r1, lr}
        add     sp, sp, #12
        msr     cpsr_fsxc, #(HW_PSR_SVC_MODE | HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE)
        // SVCモード
        ldmfd   sp!, {lr}                      // ※SP_svc
        msr     spsr_fsxc, lr
        ldmfd   sp!, {lr}
        movs    pc, lr

        LTORG

#include <../build/libraries/swi/common/swi_table.c>
}