/*---------------------------------------------------------------------------*
  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_UDEF_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 )
{
        // change into system mode
        msr     cpsr_fsxc, #HW_PSR_SYS_MODE | HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE
        stmfd   sp!, {r0, r12, lr, pc}      // push to previous mode stack (8byte alignment)

        // get LR and SPSR of UNDEF mode
        msr     cpsr_fsxc, #HW_PSR_UNDEF_MODE | HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE
        mov     r12, lr
        mrs     r0,  spsr

        tst     r0, #HW_PSR_THUMB_STATE
        bne     i_osUndefInstTerminate

        tst     r0, #HW_PSR_JAVA_STATE
        bne     i_osUndefInstTerminate

        // permit for only system and user mode to use VFP
        and     lr, r0, #HW_PSR_CPU_MODE_MASK
        cmp     lr, #HW_PSR_SYS_MODE
        cmpne   lr, #HW_PSR_USR_MODE
        bne     i_osUndefInstTerminate

        // change into system mode
        msr     cpsr_fsxc, #HW_PSR_SYS_MODE | HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE

        tst     r0, #HW_PSR_THUMB_STATE
        subne   r12, r12, #2                // Thumb code return address
        subeq   r12, r12, #4                // ARM code return address
        str     r12, [sp, #4*3]             // store LR_svc to return PC

        stmfd   sp!, {r0-r3}                // push spsr (8byte alignment)

        adr     lr, i_return

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

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

i_return
        ldmfd   sp!, {r0-r3}                // pop spsr
        // restore into previous mode
        msr     cpsr_fsxc, r0
        ldmfd   sp!, {r0, r12, lr, pc}      // pop from USR stack

        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    r12, fpexc
        tst     r12, #HW_FPEXC_VFP_ENABLE
LSYM(1)
        bne     BSYM(1)                       // Error

        mov     r0,  #(HW_FPEXC_EXCEPTION_STATE | HW_FPEXC_FPINST2_VALID)
        bic     r12, r12, r0
        orr     r12, r12, #HW_FPEXC_VFP_ENABLE
        fmxr    fpexc, r12
        ldr     r12, =(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, r12

        bx      lr
}

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

#include <brom/codereset.h>

#endif // SDK_ARM11