/*
 * arm_start.S
 *
 * Copyright (c) 2020 Semidrive Semiconductor.
 * All rights reserved.
 *
 * Description: ARM start function.
 *
 * Revision History:
 * -----------------
 */

#include <config.h>
#include <armv7-r/arm.h>
#include <armv7-r/cache.h>
#include <compiler.h>

.globl __vector
.weak Undefined_Handler
.weak SWI_Handler
.weak Prefetch_Handler
.weak Abort_Handler
.weak IRQ_Handler
.weak FIQ_Handler

    .section .intvec, "ax", %progbits

    .arm

__vector:
    LDR     PC,Reset_Addr
    LDR     PC,Undefined_Addr
    LDR     PC,SWI_Addr
    LDR     PC,Prefetch_Addr
    LDR     PC,Abort_Addr
    .word   0
    LDR     PC,IRQ_Addr
    LDR     PC,FIQ_Addr

Reset_Addr:                 .word   Reset_Handler
Undefined_Addr:             .word   Undefined_Handler
SWI_Addr:                   .word   SWI_Handler
Prefetch_Addr:              .word   Prefetch_Handler
Abort_Addr:                 .word   Abort_Handler
IRQ_Addr:                   .word   IRQ_Handler
FIQ_Addr:                   .word   FIQ_Handler

    .section .boot, "ax", %progbits

FUNCTION(Reset_Handler)
    /* do some early cpu setup */
    mov     r0, #(PSR_MODE_SVC | PSR_I_BIT | PSR_F_BIT)
    msr     cpsr_c, r0

    mrc     p15, 0, r12, c1, c0, 0
    bic     r12, r12, #(SCTLR_M | SCTLR_A | SCTLR_C)
    bic     r12, r12, #(SCTLR_SW | SCTLR_I | SCTLR_RR)
    bic     r12, r12, #(SCTLR_EE | SCTLR_TE)
    orr     r12, r12, #(SCTLR_Z)
#if CONFIG_ARM_HIGHVECTORS
    orr     r12, r12, #(SCTLR_V)
#else
    bic     r12, r12, #(SCTLR_V)
#endif
    mcr     p15, 0, r12, c1, c0, 0

    /* Configure peripheral ports */
    mrc     p15, 0, r0, c15, c0, 1
    tst     r0, #(0x1F << 2)
    beq     .Lno_normal_axi_pp
    /* Enable LLPP normal AXI interface */
    orr     r0, r0, #1
    mcr     p15, 0, r0, c15, c0, 1
.Lno_normal_axi_pp:
    mrc     p15, 0, r0, c15, c0, 2
    tst     r0, #(0x1F << 2)
    beq     .Lno_virtual_axi_pp
    /* Enable LLPP virtual AXI interface */
    orr     r0, r0, #1
    mcr     p15, 0, r0, c15, c0, 2
.Lno_virtual_axi_pp:
    mrc     p15, 0, r0, c15, c0, 3
    tst     r0, #(0x1F << 2)
    beq     .Lno_ahb_pp
    /* Enable AHB peripheral interface */
    orr     r0, r0, #1
    mcr     p15, 0, r0, c15, c0, 3
.Lno_ahb_pp:

    /* enable tcm before use stacks */
#if CONFIG_ARMV7R_USE_TCMA
    ldr     r0, =CONFIG_ARMV7R_TCMA_BASE
    mov     r1, #0
#if CONFIG_ARMV7R_TCMA_ECC
    add     r1, #1
#endif
    bl      tcma_enable_early
#endif

#if CONFIG_ARMV7R_USE_TCMB
    ldr     r0, =CONFIG_ARMV7R_TCMB_BASE
    mov     r1, #0
#if CONFIG_ARMV7R_TCMB_ECC
    add     r1, #1
#endif
    bl      tcmb_enable_early
#endif

.Lstack_setup:
    /* set up the stack for irq, fiq, abort, undefined, system/user, and lastly supervisor mode */
    mov     r12, #0

    /* Setup normal interrupt stack */
    cpsid   i, #MODE_IRQ
    ldr     r12, =__irq_stack_end
    mov     sp, r12

    /* Setup fast interrupt stack */
    cpsid   i, #MODE_FIQ
    ldr     r12, =__fiq_stack_end
    mov     sp, r12

    /* Setup data abort stack */
    cpsid   i, #MODE_ABT
    ldr     r12, =__abt_stack_end
    mov     sp, r12

    /* Setup undefined instruction stack */
    cpsid   i, #MODE_UND
    ldr     r12, =__und_stack_end
    mov     sp, r12

    /* Setup system/user stack */
    cpsid   i, #MODE_SYS
    ldr     r12, =__cstack_end
    mov     sp, r12

    /* Setup supervisor stack */
    cpsid   i, #MODE_SVC
    ldr     r12, =__svc_stack_end
    mov     sp, r12

#if CONFIG_PM
    ldr     r0, arm_context_restore_const
    blx     r0
#endif

    /* enable cache, use stack enable after stack init */
#if CONFIG_ARCH_EARLY_ENABLE_ICACHE
    ldr     r0, =ICACHE
    bl      arch_enable_cache
#endif

#if CONFIG_ARCH_EARLY_ENABLE_DCACHE
    ldr     r0, =DCACHE
    bl      arch_enable_cache
#endif

#if CONFIG_EARLYCOPY
    /* Copy sections from their load address to link address. */
    ldr     r4, =__earlycopy_start
    ldr     r5, =__earlycopy_end
.Lsection_loop:
    cmp     r4, r5
    beq     .Lbss_init
    ldmia   r4!, {r0, r1, r2}   /* vma, size, lma */
    mov     r6, r0
    mov     r7, r1
    add     r7, r6

.Lcopy_loop:
    cmp     r6, r7
    ldrlt   r3, [r2], #4
    strlt   r3, [r6], #4
    blt     .Lcopy_loop

    /* Flush cache. */
    bl      arch_clean_cache_range

    b       .Lsection_loop
#endif

.Lbss_init:
    /* clear out the bss */
    ldr     r4, =__bss_start
    ldr     r5, =__bss_end
    mov     r6, #0
.Lbss_loop:
    cmp     r4, r5
    strlt   r6, [r4], #4
    blt     .Lbss_loop

    /* change to system mode */
    cpsid   i, #MODE_SYS

#if CONFIG_ARCH_WITH_FPU
    bl      arm_fpu_enable
#endif

    bl      copy_intvec

    bl      device_init

    bl      main
    b       .

FUNCTION(Undefined_Handler)
    b       Arm_Undefined_Handler

FUNCTION(SWI_Handler)
    b       Arm_SWI_Handler

FUNCTION(Prefetch_Handler)
    b       Arm_Prefetch_Handler

FUNCTION(Abort_Handler)
    b       Arm_Abort_Handler

FUNCTION(IRQ_Handler)
    b       Arm_IRQ_Handler

FUNCTION(FIQ_Handler)
    b       Arm_FIQ_Handler

#if CONFIG_PM
arm_context_restore_const:  .word arm_context_restore
#endif