/*---------------------------------------------------------------------------*
  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:: 2008-11-28#$
  $Rev: 41 $
  $Author: nakasima $
 *---------------------------------------------------------------------------*/
#include <brom/code32.h>
#include <brom/os.h>
#include <brom/swi.h>

/*---------------------------------------------------------------------------*
  Name:         STUPi_DbgHandler

  Description:  Debug (FIQ, etc) handler

  Arguments:    None

  Returns:      None.
 *---------------------------------------------------------------------------*/
asm void STUPi_DbgHandler( void )
{
        mrs		sp, cpsr                        // IRQ/FIQ不許可
        orr		sp, sp, #HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE
        msr		cpsr_cxsf, sp

dbg_m
        b       dbg_m

        LTORG
}

/*---------------------------------------------------------------------------*
  Name:         STUPi_IrqHandler

  Description:  IRQ handler

  Arguments:    None

  Returns:      None.
 *---------------------------------------------------------------------------*/
asm void STUPi_IrqHandler( void )
{
#ifdef SDK_ARM9
        mrc     p15, 0, r0, c9, c1, 0           // DTCMアドレス獲得
        mov     r0, r0, lsr #HW_C9_TCMR_BASE_SHIFT
        mov     r0, r0, lsl #HW_C9_TCMR_BASE_SHIFT
        add     r0, r0, #HW_DTCM_SIZE
#endif // SDK_ARM9

irq_m
        b       irq_m

        LTORG
}

/*---------------------------------------------------------------------------*
  Name:         STUPi_SwiHandler

  Description:  SWI handler

  Arguments:    None

  Returns:      None.
 *---------------------------------------------------------------------------*/
asm void STUPi_SwiSemihosting( void )
{
        ldmfd	sp!, {r11, r12, lr}
semi_vector
        movs	pc, lr
}

asm void STUPi_SwiHandler( void )
{
        stmfd	sp!, {r11, r12, lr}             // SVCモード レジスタ退避
        ldrh	r12, [lr, #-2]                  // システムコールNo取得（ハーフワードアクセス対応）
        mov     r12, r12, lsl #1
        and		r12, r12, #SVC_ID_PREMASK<<SVC_ID_SHIFT

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

        adr		r11, SWI_Table                  // システムコールアドレス 取得
        ldrh	r12, [r11, r12]
#ifdef SDK_ARM9
        adr     r11, _start                     // = HW_BIOS
        orr     r12, r12, r11
#endif // SDK_ARM9

        mrs		r11, spsr                       // SVCモード SPSR退避（合計4ワード）
        stmfd	sp!, {r11}

        and		r11, r11, #HW_PSR_IRQ_DISABLE   // IRQ許可状態 コピー
        orr		r11, r11, #HW_PSR_SYS_MODE      // システムモードへ切り換え & FIQ許可
        msr		cpsr_cxsf,r11

        stmfd	sp!, {r2, lr}                   // システムモード レジスタ退避（2ワード）
#ifdef SDK_ARM9
        blx		r12
#else // SDK_ARM7
        adr		lr, swi_return
        bx		r12
#endif // SDK_ARM7

swi_return
        ldmfd	sp!, {r2, lr}                   // システムモード ﾚｼﾞｽﾀ復帰
        mov		r12, #0xc0 | HW_PSR_SVC_MODE    // SVCモードへ切り換え
        msr		cpsr_cxsf, r12                  //   & IRQ/FIQ不許可

        ldmfd	sp!, {r11}                      // SVCモード レジスタ復帰
        msr		spsr_cxsf, r11

semi_return
        ldmfd	sp!, {r11, r12, lr}
        movs	pc, lr

        LTORG

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