#include <mips/asm.h>
#include <boot.h>
#include <mips/m32c0.h>
#include <mips/regdef.h>

/**************************************************************************************
    R E S E T   E X C E P T I O N   H A N D L E R
**************************************************************************************/

LEAF(__reset_vector)

    la      v0,__ram_end
    li      v1,0x12345678
    sw      v1,0(v0)

    la a2,     check_nmi
    mtc0    zero, C0_COUNT          // Clear cp0 Count (Used to measure boot time.)
    jr a2

    // Note: adding code here may conflict with board ID register at 0xbfc0010.

END(__reset_vector)

/**************************************************************************************
    B O O T   E X C E P T I O N   H A N D L E R S
**************************************************************************************/

.org 0x200 /* TLB refill, 32 bit task. */
LEAF(__boot_tlb_refill)
    PTR_MFC0  k0, C0_EPC
    PTR_MFC0  k1, C0_BADVADDR
1:  wait
    b     1b  /* Stay here */
    nop
END(__boot_tlb_refill)



.org 0x280 /* XTLB refill, 64 bit task. */
LEAF(__boot_xtlb_refill)
    PTR_MFC0  k0, C0_EPC
    PTR_MFC0  k1, C0_BADVADDR    
1:  wait
    b     1b  /* Stay here */
    nop
END(__boot_xtlb_refill)


.org 0x300 /* Cache error exception. */
LEAF(__boot_cache_error)
    PTR_MFC0  k0, C0_EPC
    PTR_MFC0  k1, C0_BADVADDR    
1:  wait
    b     1b  /* Stay here */
    nop
END(__boot_cache_error)

.extern _general_exception_handler

.org 0x380 /* General exception. */
LEAF(__boot_general_exception)
    PTR_MFC0  a0, C0_EPC
    PTR_MFC0  a1, C0_BADVADDR    
1:  wait
    b     1b  /* Stay here */
    nop
END(__boot_general_exception)

.org 0x480 /* EJTAG debug exception (EJTAG Control Register[ProbTrap] == 0.) */
LEAF(__boot_debug_exception)
    PTR_MFC0  k0, C0_DEPC
    PTR_MFC0  k1, C0_BADVADDR    
1:  wait
    b     1b  /* Stay here */
END(__boot_debug_exception)

/**************************************************************************************
**************************************************************************************/
.org 0x500 /* Resume code past the boot exception vectors. */

check_nmi: // Verify we are here due to a reset (and not NMI.)
    mfc0    a0, C0_STATUS               // Read CP0 Status
    srl     a0, 19                      // Shift [NMI] into LSBs.
    andi    a0, a0, 1                   // Inspect CP0 Status[NMI]
    beqz    a0, verify_isa              // Branch if this is NOT an NMI exception.
    sdbbp                               // Failed assertion: not NMI.

verify_isa: // Verify device ISA meets code requirements (RISC32R2 or later.)
    mfc0    a0, C0_CONFIG               // Read CP0 Config
    srl     a0, 10                      // Shift [AT AR] into LSBs.
    andi    a3, a0, 0x18                // Inspect CP0 Config[AT]
    beqz    a3, is_mips32               // Branch if executing on RISC32 ISA.
    sdbbp                               // Failed assertion: RISC32R2.

is_mips32:
    andi    a3, a0, 0x07                // Inspect CP0 Config[AR]
    bnez    a3, init_common_resources // Continue if ISA is RISC32R2 or later.
    sdbbp                                       # Failed assertion RISC32R2.
/**************************************************************************************
 What is initialized on execution depends on the core executing it.
 **************************************************************************************/
init_common_resources:  // initializes resources for virtual or physical "cpu".
    la a2,init_gpr      // Fill register file with dummy value then boot info.
    jalr a2

    la a2,  set_gpr_boot_values  // Fill register file boot info.
    jalr a2

    la a2,     init_cp0    // Init CP0 Status, Count, Compare, Watch*, and Cause.
    jalr a2

    // Determine if we have a TLB
    mfc0    v1, C0_CONFIG           // read C0_Config
    ext     v1, v1, 7, 3            // extract MT field
    li      a3, 0x1                 // load a 1 to check against
    bne     v1, a3, done_tlb        // no tlb?

    // determined if this is VPE 0 so skip shared TLB check
    beqz    r9_vpe_num, do_tlb      // VPE 0?

    // Must be VPE1 determined if TLB is shared if it is skip init_tlb (already done by VPE0)
    mfc0    a0, $0, 1               // MVPControl
    ext     a0, a0, 3, 1            // MVPControl[STLB]
    beq     a0, zero, done_tlb      // TLB shared?

do_tlb:
    la a2,     init_tlb             // Generate unique EntryHi contents per entry pair.
    jalr a2
 

done_tlb:
    bnez    r9_vpe_num, init_done   // If we are not a vpe0 then we are done.

init_sys_resources:     # We are core0 vpe0.
    la a2,     init_icache // Initialize the L1 instruction cache. (Executing using I$ on return.)
    jalr a2

// The changing of Kernel mode cacheability must be done from KSEG1
// Since the code is executing from KSEG0 It needs to do a jump to KSEG1 change K0 and jump back to KSEG0
    la a2,      change_k0_cca
    li          a1, 0xf
    ins         a2, a1, 29, 1 // changed to KSEG1 address by setting bit 29
    jalr a2

    la a2,     init_dcache // Initialize the L1 data cache
    jalr a2

    la a2,     copy_c2_ram // Copy "C" code and data to RAM and zero bss (uncached.)
    jalr a2

    la a2,     init_itc    // Initialize Inter-Thread Communications unit
    jalr a2

    la a2,     init_vpe1   // Set up MT ASE vpe1 to execute this boot code also.
    jalr a2

init_done:

    // Prepare for eret to _start (sp and gp set up per vpe in set_gpr_boot_values).
    la      ra, __exit  // If main return then go to all_done:.
    la      a1, _start
    mtc0    a1, C0_ERRPC            // ErrorEPC
    ehb
    eret    # Exit reset exception handler for this vpe and start execution of main().
    nop
/**************************************************************************************
**************************************************************************************/
.global __exit

__exit:

all_done:
    # Looks like main returned. Just busy wait spin.
    b       all_done
