/*---------------------------------------------------------------------------*
  Project:  CtrBrom - library - os
  File:     os_exception.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/os.h>
#include <brom/swi.h>


void i_osUndefInstHandler( void );

/*---------------------------------------------------------------------------*
  Name:         osInitException

  Description:  Initialize Exceptions for Application

  Arguments:    None

  Returns:      None
 *---------------------------------------------------------------------------*/
void osInitException( void )
{
#ifdef SDK_ARM11
    static BOOL isInit;

    if ( isInit == FALSE )
    {
        isInit = TRUE;

        *(OSExcpHandler*)HW_UNDEF_VECTOR_BUF = i_osUndefInstHandler;
    }
#endif // SDK_ARM11
}

#ifdef SDK_ARM11

#include <brom/code32.h>

/*---------------------------------------------------------------------------*
  Name:         i_osUndefInstHandler

  Description:  Undefine Instruction handler

  Arguments:    None

  Returns:      None
 *---------------------------------------------------------------------------*/
asm void i_osUndefInstHandler( void )
{
#ifdef SDK_ARM11
        // 不正確データアボート有効化（clear HW_PSR_IMPR_ABORT_DISABLE）
        msr     cpsr_fsxc, #(HW_PSR_IRQ_MODE | HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE)
#endif // SDK_ARM11

        // UNDEFモード
        mrs     sp, spsr                        // ※SP_und
        and     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}
        add     r1, sp, #24                     // 呼び出し元のスタック内へ確保したSPSR_und、LR_und用スタックアドレスをコピー
        msr     cpsr_fsxc, #(HW_PSR_UNDEF_MODE | HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE)

        // UNDEFモード
        mov     sp, r1                          // ※SP_und
        mov     r1, lr
        mrs     r0, spsr
        stmfd   sp!, {r0, lr}                   // SPSR_und、LR_undを退避

        tst     r0, #HW_PSR_THUMB_STATE
        bne     i_osUndefInstTerminate

        tst     r0, #HW_PSR_JAVA_STATE
        bne     i_osUndefInstTerminate

        // ユーザモードもしくはシステムモードのみVFP使用を許可
        and     lr, r0, #HW_PSR_CPU_MODE_MASK
        // ユーザモードならシステムモードへ
        cmp     lr, #HW_PSR_USR_MODE
        moveq   lr, #HW_PSR_SYS_MODE
        cmp     lr, #HW_PSR_SYS_MODE
        bne     i_osUndefInstTerminate

        // 割り込み禁止
        orr     lr, lr, #(HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE)
        msr     cpsr_fsxc, lr

        // アボート発生元のモード
        tst     r0, #HW_PSR_THUMB_STATE
        subne   r1, r1, #2                 // Thumb code return address
        subeq   r1, r1, #4                 // ARM code return address

        ldr     r0,  [r1, #0]              // called from ARM
        mov     r1, r0, lsl #4
        sub     r1, r1, #0xC0000000
        cmp     r1, #0x30000000
        bhs     i_osUndefInstTerminate

        adr     r1, i_osUndefCPHandlerTable
        and     r0, r0, #0x00000F00
        ldr     r0, [r1, r0, lsr #8-2]

        blx     r0

i_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_UNDEF_MODE | HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE)
        // UNDEFモード
        ldmfd   sp!, {lr}                      // ※SP_und
        msr     spsr_fsxc, lr
        ldmfd   sp!, {lr}
        movs    pc, lr

        INASM_EXTERN( _VFP_Computation_Engine )

i_osUndefCPHandlerTable
        DCD     i_osUndefInstTerminate   // CP 0
        DCD     i_osUndefInstTerminate   // CP 1
        DCD     i_osUndefInstTerminate   // CP 2
        DCD     i_osUndefInstTerminate   // CP 3
        DCD     i_osUndefInstTerminate   // CP 4
        DCD     i_osUndefInstTerminate   // CP 5
        DCD     i_osUndefInstTerminate   // CP 6
        DCD     i_osUndefInstTerminate   // CP 7
        DCD     i_osUndefInstTerminate   // CP 8
        DCD     i_osUndefInstTerminate   // CP 9
        DCD     i_osUndefInstHandlerVFP  // CP 10
        DCD     i_osUndefInstHandlerVFP  // CP 11
        DCD     i_osUndefInstTerminate   // CP 12
        DCD     i_osUndefInstTerminate   // CP 13
        DCD     i_osUndefInstTerminate   // CP 14
        DCD     i_osUndefInstTerminate   // CP 15
}

asm void i_osUndefInstHandlerVFP( void )
{
        // VFP enable
        fmrx    r1, fpexc
        tst     r1, #HW_FPEXC_VFP_ENABLE
LSYM(1)
        bne     BSYM(1)                       // Error

        mov     r0,  #(HW_FPEXC_EXCEPTION_STATE | HW_FPEXC_FPINST2_VALID)
        bic     r1, r1, r0
        orr     r1, r1, #HW_FPEXC_VFP_ENABLE
        fmxr    fpexc, r1
        ldr     r1, =(HW_FPSCR_ROUND_NEAR_MODE << HW_FPSCR_ROUND_MODE_SFT \
                    | HW_FPSCR_VEC_LENGTH_1 << HW_FPSCR_VEC_LENGTH_SFT \
                    | HW_FPSCR_VEC_STRIDE_1 << HW_FPSCR_VEC_STRIDE_SFT \
                    | HW_FPSCR_DZE_ENABLE \
                    | HW_FPSCR_DEFAULT_NAN_MODE \
                    | HW_FPSCR_FLASH_TO_ZERO_MODE \
                    )
        fmxr    fpscr, r1

        bx      lr
}

asm void i_osUndefInstTerminate( void )
{
LSYM(1)
        b       BSYM(1)
        bx      lr
}

#include <brom/codereset.h>

#endif // SDK_ARM11