/**
 ****************************************************************************************
 *
 * @file boot_handlers.s
 *
 * @brief ARM Exception Vector handler functions.
 *
 * Copyright (C) RivieraWaves 2011-2016
 *
 ****************************************************************************************
 */
    .section ".rom1", "ax"

    .globl entry_main
    .globl intc_irq
    .globl intc_fiq
    .globl boot_reset
    .globl boot_swi
    .globl boot_undefined
    .globl boot_pabort
    .globl boot_dabort
    .globl boot_reserved
    .globl irq_handler
    .globl fiq_handler
	.globl do_irq
	.globl do_fiq
	.globl do_swi
	.globl do_undefined
	.globl do_pabort
	.globl do_dabort
	.globl do_reserved

#include "sys_config.h"
#include "arm_mcu_pub.h"
/* ========================================================================
 *                                Macros
 * ======================================================================== */

#define BOOT_MODE_MASK                    0x1F
#define BOOT_MODE_USR                     0x10
#define BOOT_MODE_FIQ                     0x11
#define BOOT_MODE_IRQ                     0x12
#define BOOT_MODE_SVC                     0x13
#define BOOT_MODE_ABT                     0x17
#define BOOT_MODE_UND                     0x1B
#define BOOT_MODE_SYS                     0x1F
#define BOOT_FIQ_IRQ_MASK                 0xC0
#define BOOT_IRQ_MASK                     0x80

#define BOOT_COLOR_UNUSED                 0xAAAAAAAA      //Pattern to fill UNUSED stack
#define BOOT_COLOR_SVC                    0xBBBBBBBB      //Pattern to fill SVC stack
#define BOOT_COLOR_IRQ                    0xCCCCCCCC      //Pattern to fill IRQ stack
#define BOOT_COLOR_FIQ                    0xDDDDDDDD      //Pattern to fill FIQ stack
#define BOOT_COLOR_SYS                    0xEEEEEEEE      //Pattern to fill SYS stack

/* ========================================================================
                    restore macro definitions
 * ======================================================================== */


    .macro PUSH_ALL_ARM_REG
        BOOT_CHANGE_MODE  BOOT_MODE_SYS BOOT_MODE_MASK
        LDR R1, =MCU_REG_BACKUP_TOP_SYS // get backup top.
        STMFD   R1!, {R8-R14}       // backup R8-R14
        MRS   R0, SPSR
        STMFD   R1!, {R0}           // backup SPSR
        MRS   R0, CPSR
        STMFD   R1!, {R0}           // backup CPSR

        BOOT_CHANGE_MODE  BOOT_MODE_IRQ BOOT_MODE_MASK
        LDR R1, =MCU_REG_BACKUP_TOP_IRQ // get backup top.
        STMFD   R1!, {R8-R14}       // backup R8-R14
        MRS   R0, SPSR
        STMFD   R1!, {R0}           // backup SPSR
        MRS   R0, CPSR
        STMFD   R1!, {R0}           // backup CPSR

        BOOT_CHANGE_MODE  BOOT_MODE_FIQ BOOT_MODE_MASK
        LDR R1, =MCU_REG_BACKUP_TOP_FIQ // get backup top.
        STMFD   R1!, {R8-R14}       // backup R8-R14
        MRS   R0, SPSR
        STMFD   R1!, {R0}           // backup SPSR
        MRS   R0, CPSR
        STMFD   R1!, {R0}           // backup CPSR


        BOOT_CHANGE_MODE  BOOT_MODE_ABT BOOT_MODE_MASK
        LDR R1, =MCU_REG_BACKUP_TOP_ABT // get backup top.
        STMFD   R1!, {R8-R14}       // backup R8-R14
        MRS   R0, SPSR
        STMFD   R1!, {R0}           // backup SPSR
        MRS   R0, CPSR
        STMFD   R1!, {R0}           // backup CPSR


        BOOT_CHANGE_MODE  BOOT_MODE_UND BOOT_MODE_MASK
        LDR R1, =MCU_REG_BACKUP_TOP_UND // get backup top.
        STMFD   R1!, {R8-R14}       // backup R8-R14
        MRS   R0, SPSR
        STMFD   R1!, {R0}           // backup SPSR
        MRS   R0, CPSR
        STMFD   R1!, {R0}           // backup CPSR


        BOOT_CHANGE_MODE  BOOT_MODE_SVC BOOT_MODE_MASK
        LDR R1, =MCU_REG_BACKUP_TOP_SVC // get backup top.
        STMFD   R1!, {R8-R14}       // backup R8-R14
        MRS   R0, SPSR
        STMFD   R1!, {R0}           // backup SPSR
        MRS   R0, CPSR
        STMFD   R1!, {R0}           // backup CPSR

    .endm

/* ========================================================================
/**
 * Macro for switching ARM mode
 */
    .macro  BOOT_CHANGE_MODE, mode, mode_mask
    MRS   R0, CPSR
    BIC   R0, R0, #\mode_mask
    ORR   R0, R0, #\mode
    MSR   CPSR_c, R0
    .endm

/* ========================================================================
/**
 * Macro for setting the stack
 */
    .macro  BOOT_SET_STACK, stackStart, stackLen, color
    LDR   R1, \stackStart
    LDR   R0, \stackLen

    SUB   R0, R1, R0
    MOV   SP, R1        //Set stack pointer

    LDR   R2, =\color

3:
    CMP   R0, R1        //End of stack?
    STRLT R2, [r0]      //Colorize stack word
    ADDLT R0, R0, #4
    BLT   3b           //branch to previous local label
    .endm

/* ========================================================================
/**
 * Push SVC reg
 */
    .macro PUSH_SVC_REG
        SUB     SP, SP, #18 * 4
        STMIA   SP, {R0 - R12}
        MOV     R0, SP
        MRS     R6, SPSR
        STR     LR, [R0, #15*4]
        STR     R6, [R0, #16*4]
        STR     SP, [R0, #13*4]
        STR     LR, [R0, #14*4]
        MRS     R6, CPSR
        STR     R6, [R0, #17*4]
    .endm

/* ========================================================================
 *                      Stack and Heap Definitions
 * ========================================================================
 */
    .section .bss
    .space UND_STACK_SIZE
    .align 3
    .global und_stack_start
und_stack_start:

    .space ABT_STACK_SIZE
    .align 3
    .global abt_stack_start
abt_stack_start:

    .space FIQ_STACK_SIZE
    .align 3
    .global fiq_stack_start
fiq_stack_start:

    .space IRQ_STACK_SIZE
    .align 3
    .global irq_stack_start
irq_stack_start:

    .skip SYS_STACK_SIZE
    .align 3
    .global sys_stack_start
sys_stack_start:

    .space SVC_STACK_SIZE
    .align 3
    .global svc_stack_start
svc_stack_start:

/* ========================================================================
 *                                Functions
 * ========================================================================

/* ========================================================================
 * Function to handle reset vector
 */
.section ".rom1", "ax"

boot_reset:
    //Disable IRQ and FIQ before starting anything
    MRS   R0, CPSR
    ORR   R0, R0, #0xC0
    MSR   CPSR_c, R0

    //Setup all stacks //Note: Abt and Usr mode are not used
    BOOT_CHANGE_MODE BOOT_MODE_SYS BOOT_MODE_MASK
    BOOT_SET_STACK   boot_stack_base_SYS boot_stack_len_SYS BOOT_COLOR_SYS

    BOOT_CHANGE_MODE BOOT_MODE_ABT BOOT_MODE_MASK
    BOOT_SET_STACK   boot_stack_base_UNUSED boot_stack_len_UNUSED BOOT_COLOR_UNUSED

    BOOT_CHANGE_MODE BOOT_MODE_UND BOOT_MODE_MASK
    BOOT_SET_STACK   boot_stack_base_UNUSED boot_stack_len_UNUSED BOOT_COLOR_UNUSED
#if (CFG_SOC_NAME == SOC_BK7231N)
    B AFTER_FLAG

	.org  0xc0 //0x100 - sizeof(section.vector) = 0x100 - 0x40
	.word 0x32374B42
	.word 0x00003133

AFTER_FLAG:
#elif (CFG_SOC_NAME == SOC_BK7238) || (CFG_SOC_NAME == SOC_BK7252N)
    B AFTER_FLAG

	.org  0xc0 //0x100 - sizeof(section.vector) = 0x100 - 0x40
	.word 0x32374B42
	.word 0x00003833

AFTER_FLAG:
#endif
    BOOT_CHANGE_MODE BOOT_MODE_IRQ BOOT_MODE_MASK
    BOOT_SET_STACK   boot_stack_base_IRQ boot_stack_len_IRQ BOOT_COLOR_IRQ

    BOOT_CHANGE_MODE BOOT_MODE_FIQ BOOT_MODE_MASK
    BOOT_SET_STACK   boot_stack_base_FIQ boot_stack_len_FIQ BOOT_COLOR_FIQ

    //Clear FIQ banked registers while in FIQ mode
    MOV     R8,  #0
    MOV     R9,  #0
    MOV     R10, #0
    MOV     R11, #0
    MOV     R12, #0

    BOOT_CHANGE_MODE BOOT_MODE_SVC BOOT_MODE_MASK
    BOOT_SET_STACK   boot_stack_base_SVC boot_stack_len_SVC BOOT_COLOR_SVC

    //Stay in Supervisor Mode
    //copy data from binary to ram
    BL _sysboot_copy_data_to_ram
#if (CFG_SOC_NAME == SOC_BK7231N) || (CFG_SOC_NAME == SOC_BK7238) || (CFG_SOC_NAME == SOC_BK7252N)
    BL _sysboot_copy_code_to_itcm
#endif

    ///*Init the BSS section*/
    BL _sysboot_zi_init
#if (CFG_SOC_NAME == SOC_BK7231N)
    BL _sysboot_tcmbss_init
#endif

    //==================
    //Clear Registers
    MOV R0,  #0
    MOV R1,  #0
    MOV R2,  #0
    MOV R3,  #0
    MOV R4,  #0
    MOV R5,  #0
    MOV R6,  #0
    MOV R7,  #0
    MOV R8,  #0
    MOV R9,  #0
    MOV R10, #0
    MOV R11, #0
    MOV R12, #0

	/* start RT-Thread Kernel */
    ldr     pc, _rtthread_startup
_rtthread_startup:
    .word  rtthread_startup

/*FUNCTION:     _sysboot_copy_data_to_ram*/
/*DESCRIPTION:  copy main stack code from FLASH/ROM to SRAM*/
_sysboot_copy_data_to_ram:
    LDR     R0, =_data_flash_begin
    LDR     R1, =_data_ram_begin
    LDR     R2, =_data_ram_end

4: CMP R1, R2
    LDRLO   R4, [R0], #4
    STRLO   R4, [R1], #4
    BLO     4b
    BX LR

/*FUNCTION:     _sysboot_zi_init*/
/*DESCRIPTION:  Initialise Zero-Init Data Segment*/
_sysboot_zi_init:
    LDR     R0, =_bss_start
    LDR     R1, =_bss_end

    MOV R3, R1
    MOV R4, R0
    MOV R2, #0
5: CMP R4, R3
    STRLO R2, [R4], #4
    BLO 5b
    BX LR

#if (CFG_SOC_NAME == SOC_BK7231N) || (CFG_SOC_NAME == SOC_BK7238) || (CFG_SOC_NAME == SOC_BK7252N)
/*FUNCTION:     _sysboot_copy_code_to_itcm*/
/*DESCRIPTION:  copy itcm code from FLASH/ROM to SRAM*/
_sysboot_copy_code_to_itcm:
    LDR     R0, =_itcmcode_flash_begin
    LDR     R1, =_itcmcode_ram_begin
    LDR     R2, =_itcmcode_ram_end

6: CMP R1, R2
    LDRLO   R4, [R0], #4
    STRLO   R4, [R1], #4
    BLO     6b
    BX LR
#endif

#if (CFG_SOC_NAME == SOC_BK7231N)
/*FUNCTION:     _sysboot_sdbss_init*/
/*DESCRIPTION:  Initialise Zero-Init Data Segment of SDRAM */
_sysboot_tcmbss_init:
    LDR     R0, =_tcmbss_start
    LDR     R1, =_tcmbss_end

    MOV R3, R1
    MOV R4, R0
    MOV R2, #0
8: CMP R4, R3
    STRLO R2, [R4], #4
    BLO 8b
    BX LR
#endif

/* ========================================================================
 *                       exception handlers
 * ========================================================================
 */
    .global rt_hw_trap_udef
    .global rt_hw_trap_swi
    .global rt_hw_trap_pabt
    .global rt_hw_trap_dabt
    .global rt_hw_trap_resv
    .global rt_hw_trap_irq
    .global rt_hw_trap_fiq

    .global rt_interrupt_enter
    .global rt_interrupt_leave
    .global rt_thread_switch_interrupt_flag
    .global rt_interrupt_from_thread
    .global rt_interrupt_to_thread

    /* Interrupt */
        .align  5
do_undefined:
    LDMFD   SP!, {R0-R1}
	PUSH_SVC_REG
    STMFD  sp!,{r0-r1}
    PUSH_ALL_ARM_REG
    BOOT_CHANGE_MODE  BOOT_MODE_UND BOOT_MODE_MASK
    LDMFD   SP!, {R0-R1}
    BL rt_hw_trap_udef
	B       .

        .align  5
do_swi:
    LDMFD   SP!, {R0-R1}
	PUSH_SVC_REG
    BL rt_hw_trap_swi
	B       .

        .align  5
do_pabort:
    LDMFD   SP!, {R0-R1}
	PUSH_SVC_REG
    STMFD  sp!,{r0-r1}
    PUSH_ALL_ARM_REG
    BOOT_CHANGE_MODE  BOOT_MODE_ABT BOOT_MODE_MASK
    LDMFD   SP!, {R0-R1}
    BL rt_hw_trap_pabt
	B       .

        .align  5
do_dabort:
    LDMFD   SP!, {R0-R1}
	PUSH_SVC_REG

    STMFD  sp!,{r0-r1}
    PUSH_ALL_ARM_REG
    BOOT_CHANGE_MODE  BOOT_MODE_ABT BOOT_MODE_MASK
    LDMFD   SP!, {R0-R1}
    BL rt_hw_trap_dabt
	B       .

        .align  5
do_reserved:
    LDMFD   SP!, {R0-R1}
	PUSH_SVC_REG
    BL rt_hw_trap_resv
	B       .

        .align  5
boot_undefined:
	STMFD  sp!,{r0-r1}
	LDR    R1, =0x40000c
	LDR    r0, [R1]
	BX     r0

        .align  5
boot_pabort:
	STMFD  sp!,{r0-r1}
	LDR    R1, =0x400010
	LDR    r0, [R1]
	BX     r0

        .align  5
boot_dabort:
	STMFD  sp!,{r0-r1}
	LDR    R1, =0x400014
	LDR    r0, [R1]
	BX     r0

        .align  5
boot_reserved:
	STMFD  sp!,{r0-r1}
	LDR    R1, =0x400018
	LDR    r0, [R1]
	BX     r0


		.align	5
boot_swi:
	STMFD  sp!,{r0-r1}
	LDR    R1, =0x400008
	LDR    r0, [R1]
	BX     r0

		.align	5
irq_handler:
	STMFD  sp!,{r0-r1}
	LDR    R1, =0x400000
	LDR    r0, [R1]
	BX     r0


		.align	5
fiq_handler:
	STMFD  sp!,{r0-r1}
	LDR    R1, =0x400004
	LDR    r0, [R1]
	BX     r0


        .align  5
do_irq:
    LDMFD   SP!, {R0-R1}
    STMFD   SP!, {R0-R12,LR}

    mrs r4, cpsr
    orr r1, r4, #0xC0          @; disable interrupt
    msr cpsr_c, r1

    BL      rt_interrupt_enter
    BL      rt_irq_dispatch
    BL      rt_interrupt_leave

    LDR     R0, =rt_thread_switch_interrupt_flag
    LDR     R1, [R0]
    CMP     R1, #1
    BEQ     rt_hw_context_switch_interrupt_do

    LDMFD   SP!, {R0-R12,LR}
    SUBS    PC,  LR, #4

rt_hw_context_switch_interrupt_do:
    MOV     R1,  #0
    STR     R1,  [R0]

    MOV     R1, SP
    ADD     SP, SP, #4*4
    LDMFD   SP!, {R4-R12,LR}

    MRS     R0,  SPSR
    SUB     R2,  LR, #4

    MSR     CPSR_c, #BOOT_FIQ_IRQ_MASK|BOOT_MODE_SVC

    STMFD   SP!, {R2}
    STMFD   SP!, {R4-R12,LR}
    LDMFD   R1,  {R1-R4}
    STMFD   SP!, {R1-R4}
    STMFD   SP!, {R0}

    LDR     R4,  =rt_interrupt_from_thread
    LDR     R5,  [R4]
    STR     SP,  [R5]

    LDR     R6,  =rt_interrupt_to_thread
    LDR     R6,  [R6]
    LDR     SP,  [R6]

    LDMFD   SP!, {R4}
    MSR     SPSR_cxsf, R4

    LDMFD   SP!, {R0-R12,LR,PC}^

        .align  5
do_fiq:
    LDMFD   SP!, {R0-R1}
    STMFD   SP!,{R0-R7,LR}

    BL      rt_interrupt_enter
    BL      rt_fiq_dispatch
    BL      rt_interrupt_leave

    MRS R3, spsr
    AND R2, R3, #0x1F
    CMP R2, #0x12            @; fiq from irq(0x12)
    BEQ fiq_handler_return

    LDR     R0, =rt_thread_switch_interrupt_flag
    LDR     R1, [R0]
    CMP     R1, #1
    BEQ     rt_hw_context_switch_interrupt_fiq_do

fiq_handler_return:
    LDMFD   SP!,{R0-R7,LR}
    SUBS    PC, LR, #4

rt_hw_context_switch_interrupt_fiq_do:
    MOV     R1,  #0
    STR     R1,  [R0]

    MOV     R1, SP             @; pop {R0-R7,LR} but skip R0-R3
    ADD     SP, SP, #4*4
    LDMFD   SP!, {R4-R7,LR}

    MRS     R0,  SPSR
    SUB     R2,  LR, #4        @; Save old task's PC to R2

    MSR     CPSR_c, #BOOT_FIQ_IRQ_MASK|BOOT_MODE_SVC

    STMFD   SP!, {R2}          @; Push old task's PC
    STMFD   SP!, {R4-R12,LR}   @; Push old task's LR,R12-R4
    LDMFD   R1,  {R1-R4}       @; pop old thread R0-R3 to R1-R4
    STMFD   SP!, {R1-R4}       @; Push old thread R0-R3
    STMFD   SP!, {R0}          @; Push old task's CPSR

    LDR     R4,  =rt_interrupt_from_thread
    LDR     R5,  [R4]
    STR     SP,  [R5]

    LDR     R6,  =rt_interrupt_to_thread
    LDR     R6,  [R6]
    LDR     SP,  [R6]

    LDMFD   SP!, {R4}
    MSR     SPSR_cxsf, R4

    LDMFD   SP!, {R0-R12,LR,PC}^


/* ========================================================================
 *                                Globals
 * ======================================================================== */
boot_stack_base_UNUSED:
    .word und_stack_start

boot_stack_len_UNUSED:
    .word UND_STACK_SIZE

boot_stack_base_IRQ:
    .word irq_stack_start

boot_stack_len_IRQ:
    .word IRQ_STACK_SIZE

boot_stack_base_SVC:
    .word svc_stack_start

boot_stack_len_SVC:
     .word SVC_STACK_SIZE

boot_stack_base_FIQ:
     .word fiq_stack_start

boot_stack_len_FIQ:
    .word FIQ_STACK_SIZE

boot_stack_base_SYS:
     .word sys_stack_start

boot_stack_len_SYS:
     .word SYS_STACK_SIZE


/*EOF*/