;******************************************************************************
;*
;* Copyright (c) Microsoft Corporation.  All rights reserved.
;*
;* Use of this source code is subject to the terms of the Microsoft end-user
;* license agreement (EULA) under which you licensed this SOFTWARE PRODUCT.
;* If you did not accept the terms of the EULA, you are not authorized to use
;* this source code. For a copy of the EULA, please see the LICENSE.RTF on your
;* install media.
;*
;******************************************************************************
;*
;* Copyright (C) 2004-2007, Freescale Semiconductor, Inc. All Rights Reserved.
;* THIS SOURCE CODE, AND ITS USE AND DISTRIBUTION, IS SUBJECT TO THE TERMS
;* AND CONDITIONS OF THE APPLICABLE LICENSE AGREEMENT
;*
;******************************************************************************
;*
;* FILE:    startup.s
;*
;* PURPOSE: Before control is transferred to the kernel, the boot loader
;*          calls this StartUp code to put the CPU into an initialized state.
;*
;******************************************************************************

;!!!!! TODO:  move constants to include file

    INCLUDE mxarm11_base_regs.inc
    INCLUDE mxarm11_base_mem.inc
    INCLUDE mx31_base_regs.inc
    INCLUDE mx31_base_mem.inc
    INCLUDE image_cfg.inc

;
; ARM constants
;
ARM_CPSR_PRECISE            EQU     (1 << 8)
ARM_CPSR_IRQDISABLE         EQU     (1 << 7)
ARM_CPSR_FIQDISABLE         EQU     (1 << 6)
ARM_CPSR_MODE_SVC           EQU     0x13
ARM_CTRL_ICACHE             EQU     (1 << 12)
ARM_CTRL_DCACHE             EQU     (1 << 2)
ARM_CTRL_MMU                EQU     (1 << 0)
ARM_CTRL_A                  EQU     (1 << 1)
ARM_CTRL_FLOW               EQU     (1 << 11)
ARM_CTRL_VECTORS            EQU     (1 << 13)
ARM_CTRL_L4                 EQU     (1 << 15)
ARM_CTRL_FI                 EQU     (1 << 21)
ARM_CTRL_U                  EQU     (1 << 22)
ARM_CTRL_XP                 EQU     (1 << 23)
ARM_CACR_FULL               EQU     0x3
ARM_AUXCR_SB                EQU     (1 << 2)
ARM_AUXCR_DB                EQU     (1 << 1)
ARM_AUXCR_HUM               EQU     (1 << 31)

; VFP uses coproc 10 for single-precision instructions
ARM_VFP_SP_COP              EQU     10
ARM_VFP_SP_ACCESS           EQU     (ARM_CACR_FULL << (ARM_VFP_SP_COP*2))

; VFP uses coproc 11 for double-precision instructions
ARM_VFP_DP_COP              EQU     11
ARM_VFP_DP_ACCESS           EQU     (ARM_CACR_FULL << (ARM_VFP_DP_COP*2))

; Configure coprocessor access control
ARM_CACR_CONFIG             EQU     (ARM_VFP_SP_ACCESS | ARM_VFP_DP_ACCESS)

; Peripheral Port Memory Remap Register (PPMRR)
ARM_PPMRR_SIZE_LSH          EQU     0
ARM_PPMRR_SIZE_WID          EQU     5
ARM_PPMRR_SIZE_256MB        EQU     0x13
ARM_PPMRR_SIZE_512MB        EQU     0x14
ARM_PPMRR_SIZE_1GB          EQU     0x15
ARM_PPMRR_BASEADDR_LSH      EQU     12
ARM_PPMRR_BASEADDR_WID      EQU     20

ARM_PPMRR_SIZE              EQU     (ARM_PPMRR_SIZE_1GB << ARM_PPMRR_SIZE_LSH)
ARM_PPMRR_BASEADDR          EQU     (CSP_BASE_MEM_PA_AIPSA)

ARM_PPMRR_CONFIG            EQU     (ARM_PPMRR_BASEADDR | ARM_PPMRR_SIZE)

;
; L2CC constants
;
L2CC_CR_OFFSET              EQU     0x100
L2CC_AUXCR_OFFSET           EQU     0x104
L2CC_INVWAY_OFFSET          EQU     0x77C
L2CC_DBGCR_OFFSET           EQU     0xF40

;
; AVIC constants
;
AVIC_INTCNTL_OFFSET         EQU     0x0000
AVIC_NIMASK_OFFSET          EQU     0x0004
AVIC_INTENNUM_OFFSET        EQU     0x0008
AVIC_INTDISNUM_OFFSET       EQU     0x000C
AVIC_INTENABLEH_OFFSET      EQU     0x0010
AVIC_INTENABLEL_OFFSET      EQU     0x0014
AVIC_INTTYPEH_OFFSET        EQU     0x0018
AVIC_INTTYPEL_OFFSET        EQU     0x001C
AVIC_NIPRIORITY7_OFFSET     EQU     0x0020
AVIC_NIPRIORITY6_OFFSET     EQU     0x0024
AVIC_NIPRIORITY5_OFFSET     EQU     0x0028
AVIC_NIPRIORITY4_OFFSET     EQU     0x002C
AVIC_NIPRIORITY3_OFFSET     EQU     0x0030
AVIC_NIPRIORITY2_OFFSET     EQU     0x0034
AVIC_NIPRIORITY1_OFFSET     EQU     0x0038
AVIC_NIPRIORITY0_OFFSET     EQU     0x003C
AVIC_NIVECSR_OFFSET         EQU     0x0040
AVIC_FIVECSR_OFFSET         EQU     0x0044
AVIC_INTSRCH_OFFSET         EQU     0x0048
AVIC_INTSRCL_OFFSET         EQU     0x004C
AVIC_INTFRCH_OFFSET         EQU     0x0050
AVIC_INTFRCL_OFFSET         EQU     0x0054
AVIC_NIPNDH_OFFSET          EQU     0x0058
AVIC_NIPNDL_OFFSET          EQU     0x005C
AVIC_FIPNDH_OFFSET          EQU     0x0060
AVIC_FIPNDL_OFFSET          EQU     0x0064
AVIC_VECTOR_OFFSET          EQU     0x0100

AVIC_INTCNTL_ABFFLAG        EQU     (1<<25)
AVIC_INTCNTL_ABFEN          EQU     (1<<24)
AVIC_INTCNTL_NIDIS          EQU     (1<<22)
AVIC_INTCNTL_FIDIS          EQU     (1<<21)
AVIC_INTCNTL_NIAD           EQU     (1<<20)
AVIC_INTCNTL_FIAD           EQU     (1<<19)
AVIC_INTCNTL_NM             EQU     (1<<18)


;
; RTC Constants
;
RTC_HOUR_MIN_COUNTER       EQU     0x0000
RTC_SEC_COUNTER            EQU     0x0004
RTC_HOUR_MIN_ALARM         EQU     0x0008
RTC_SEC_ALARM              EQU     0x000C
RTC_CONTROL                EQU     0x0010
RTC_INT_STAT               EQU     0x0014
RTC_INT_ENABLE             EQU     0x0018
RTC_STOPWATCH              EQU     0x001C
RTC_DAY_COUNTER            EQU     0x0020
RTC_DAY_ALARM              EQU     0x0024

;
; AIPS Constants
;
AIPSREG_MPR0_OFFSET         EQU     0x0000
AIPSREG_MPR1_OFFSET         EQU     0x0004
AIPSREG_PACR0_OFFSET        EQU     0x0020
AIPSREG_PACR1_OFFSET        EQU     0x0024
AIPSREG_PACR2_OFFSET        EQU     0x0028
AIPSREG_PACR3_OFFSET        EQU     0x002C
AIPSREG_OPACR0_OFFSET       EQU     0x0040
AIPSREG_OPACR1_OFFSET       EQU     0x0044
AIPSREG_OPACR2_OFFSET       EQU     0x0048
AIPSREG_OPACR3_OFFSET       EQU     0x004C
AIPSREG_OPACR4_OFFSET       EQU     0x0050

;
; MAX Constants
;
MAX_MPR0_OFFSET             EQU     0x0000
MAX_SGPCR0_OFFSET           EQU     0x0010
MAX_MPR1_OFFSET             EQU     0x0100
MAX_SGPCR1_OFFSET           EQU     0x0110
MAX_MPR2_OFFSET             EQU     0x0200
MAX_SGPCR2_OFFSET           EQU     0x0210
MAX_MPR3_OFFSET             EQU     0x0300
MAX_SGPCR3_OFFSET           EQU     0x0310
MAX_MPR4_OFFSET             EQU     0x0400
MAX_SGPCR4_OFFSET           EQU     0x0410
MAX_MGPCR0_OFFSET           EQU     0x0800
MAX_MGPCR1_OFFSET           EQU     0x0900
MAX_MGPCR2_OFFSET           EQU     0x0A00
MAX_MGPCR3_OFFSET           EQU     0x0B00
MAX_MGPCR4_OFFSET           EQU     0x0C00
MAX_MGPCR5_OFFSET           EQU     0x0D00


;
; M3IF Constants
;
M3IF_M3IFCTL_OFFSET         EQU     0x0000

;
; SPBA Constants
;
SPBA_PRR_OFFSET             EQU     0x0000

SPBA_PRR_RAR_ARM_ACCESS     EQU     (1 << 0)
SPBA_PRR_RAR_DSP_ACCESS     EQU     (1 << 1)
SPBA_PRR_RAR_DMA_ACCESS     EQU     (1 << 2)

SPBA_PRR_ROI_UNOWNED        EQU     (0 << 16)
SPBA_PRR_ROI_ARM_OWNED      EQU     (1 << 16)
SPBA_PRR_ROI_DSP_OWNED      EQU     (2 << 16)
SPBA_PRR_ROI_DMA_OWNED      EQU     (3 << 16)

SPBA_PRR_RMO_UNOWNED        EQU     (0 << 30)
SPBA_PRR_RMO_ALT_OWNED      EQU     (2 << 30)
SPBA_PRR_RMO_REQ_OWNED      EQU     (3 << 30)

SPBA_NUM_SHARED_PERIPH      EQU     (14)

;
; CCM constants
;
CCM_CCMR_OFFSET             EQU     0x0000
CCM_PDR0_OFFSET             EQU     0x0004
CCM_RCSR_OFFSET             EQU     0x000C
CCM_MPCTL_OFFSET            EQU     0x0010
CCM_UPCTL_OFFSET            EQU     0x0014
CCM_SPCTL_OFFSET            EQU     0x0018

;
; WEIM constants
;
WEIM_CSCR0U_OFFSET          EQU     0x0000
WEIM_CSCR4U_OFFSET          EQU     0x0040
WEIM_CSCR5U_OFFSET          EQU     0x0050
WEIM_CSCR0L_OFFSET          EQU     0x0004
WEIM_CSCR4L_OFFSET          EQU     0x0044
WEIM_CSCR5L_OFFSET          EQU     0x0054
WEIM_CSCR0A_OFFSET          EQU     0x0008
WEIM_CSCR4A_OFFSET          EQU     0x0048
WEIM_CSCR5A_OFFSET          EQU     0x0058

;
; ESDCTL constants
;
ESDCTL_ESDCTL0_OFFSET       EQU     0x0000
ESDCTL_ESDCFG0_OFFSET       EQU     0x0004
ESDCTL_ESDCTL1_OFFSET       EQU     0x0008
ESDCTL_ESDCFG1_OFFSET       EQU     0x000C
ESDCTL_ESDMISC_OFFSET       EQU     0x0010
ESDCTL_ESDDLY1_OFFSET       EQU     0x0020
ESDCTL_ESDDLY2_OFFSET       EQU     0x0024
ESDCTL_ESDDLY3_OFFSET       EQU     0x0028
ESDCTL_ESDDLY4_OFFSET       EQU     0x002C
ESDCTL_ESDDLY5_OFFSET       EQU     0x0030

;
; IIM constants
;
IIM_SREV_OFFSET    EQU  0x0024
IIM_SCS0_OFFSET    EQU  0x002C
IIM_SCS1_OFFSET    EQU  0x0030
IIM_SCS2_OFFSET    EQU  0x0034
IIM_SCS3_OFFSET    EQU  0x0038

; BASTAT2 constants
;
ARG_BSTAT2_OFFSET           EQU     0x0000

BSP_PBC_DSW_KITL            EQU     (1 << 0)    ; SW3_8:  ON = ACTIVE, OFF = PASSIVE
BSP_PBC_DSW_L2              EQU     (1 << 1)    ; SW3_7:  ON = L2 enabled, OFF = L2 disabled
BSP_PBC_DSW_AHB_CLK         EQU     (1 << 2)    ; SW3_6:  Refer to table below
BSP_PBC_DSW_ARM_CLK         EQU     (1 << 3)    ; SW3_5:  Refer to table below
BSP_PBC_DSW_ALT_CLK         EQU     (1 << 4)    ; SW3_4:  ON = SW3[5:6] determine clocking
                                                ;         OFF = Force alternalte clocking
BSP_PBC_DSW_L2_WT           EQU     (1 << 7)    ; SW3_1   ON = L2 write-through, OFF = L2 write-back
BSP_PBC_DSW_CLK_MSK         EQU     (BSP_PBC_DSW_AHB_CLK | BSP_PBC_DSW_ARM_CLK)
BSP_PBC_DSW_AHB_133         EQU     (0 << 2)
BSP_PBC_DSW_AHB_66          EQU     (1 << 2)
BSP_PBC_DSW_CLK_532_133     EQU     (0 << 2)
BSP_PBC_DSW_CLK_532_66      EQU     (1 << 2)
BSP_PBC_DSW_CLK_266_133     EQU     (2 << 2)
BSP_PBC_DSW_CLK_266_66      EQU     (3 << 2)

ARG_RAM_START                     EQU    (IMAGE_SHARE_ARGS_RAM_PA_START)
CLK_IRAM_START                    EQU    (IMAGE_WINCE_IRAM_PA_START)
TEMP_VAL                              EQU    (IMAGE_WINCE_IRAM_PA_START + IMAGE_WINCE_IRAM_SIZE - 64)
;
;PBC BSTAT2
;
;SW3_8: ON/OFF   KITL    Active/Passive
;SW3_7: ON/OFF   L2       Enable/Disable
;SW3_6: ON/OFF  AHB     133M/66M
;SW3_5: ON/OFF  ARM     532M/266M
;SW3_4: ON/OFF  CLKH    26M/27M
;SW3_3: ON/OFF  DPTC   Enable/Disable
;SW3_2: ON/OFF  DVFC   Enable/Disable
;SW3_1: ON/OFF  L2        write-through/write-back
;DMA_REQ:    0
;RSV(15..9):  0

ARG_BSTAT2_DEFAULT          EQU    (0x0060)


    IF :LNOT: :DEF: BOOTLOADER
        GBLL        BOOTLOADER
BOOTLOADER      SETL    {FALSE}
    ENDIF

    IF :LNOT: :DEF: SKIP_INIT_FOR_LOLO
        GBLL        SKIP_INIT_FOR_LOLO
SKIP_INIT_FOR_LOLO  SETL    {TRUE}
    ENDIF

    OPT 2                                       ; disable listing
    INCLUDE kxarm.h
    OPT 1                                       ; reenable listing


    IF :LNOT: BOOTLOADER
        IMPORT  KernelStart
    ENDIF

    IMPORT  OALClearUTLB
    IMPORT OALCopyBootArgs


;******************************************************************************
;*
;* FUNCTION:    StartUp
;*
;* DESCRIPTION: System bootstrap function
;*
;* PARAMETERS:  None
;*
;* RETURNS:     None
;*
;******************************************************************************

    STARTUPTEXT
    LEAF_ENTRY StartUp

    ;--------------------------------------------------------------------------
    ; For LogicPD Loader, The pointer to the physical location of the null-terminated boot-string is passed to the operating system via R2.
    ; Save the pointer to register ip
    mov     ip, r2
    
    ;--------------------------------------------------------------------------
    ; MS RECOMMENDATION:
    ; Put the processor in supervisor mode
    ; Disable the interrupt request (IRQ) and fast interrupt request (FIQ)
    ; inputs
    ;--------------------------------------------------------------------------
    mrs     r0, cpsr                            ; r0 = CPSR
    mov     r0, #ARM_CPSR_MODE_SVC              ; enter supervisor mode
    orr     r0, r0, #ARM_CPSR_IRQDISABLE        ; disable normal IRQ
    orr     r0, r0, #ARM_CPSR_FIQDISABLE        ; disable fast IRQ
    ;orr     r0, r0, #ARM_CPSR_PRECISE           ; enable precise data aborts
    msr     cpsr_xc, r0                         ; update CPSR control bits

    ;
    ; Configure auxiliary control register 
    ;
    mrc     p15, 0, r0, c1, c0, 1               ; r0 = auxiliary control reg
    orr     r0, r0, #ARM_AUXCR_DB              ; enable dynamic branch pred
    orr     r0, r0, #ARM_AUXCR_SB              ; enable static branch pred
    orr     r0, r0, #ARM_AUXCR_HUM              ; disable hit under miss (Errata 364296)
    mcr     p15, 0, r0, c1, c0, 1               ; update auxiliary control reg

    ;--------------------------------------------------------------------------
    ; MS RECOMMENDATION:
    ; Disable memory management unit (MMU) and both the instruction and data
    ; caches
    ;--------------------------------------------------------------------------
    mrc     p15, 0, r0, c1, c0, 0               ; r0 = system control reg
    bic     r0, r0, #ARM_CTRL_ICACHE            ; disable ICache
    bic     r0, r0, #ARM_CTRL_DCACHE            ; disable DCache
    bic     r0, r0, #ARM_CTRL_MMU               ; disable MMU
    bic     r0, r0, #ARM_CTRL_VECTORS           ; set vector base to 0x00000000

    ; Copied the following 3 configuration settings from the H4Sample version
    ; of startup.s. These settings appear to fix the problem with a Data Abort
    ; error when processing dirty memory pages.
    ;
    ; Note that the only seemingly stable configuration that has been tested
    ; so far has L2 write-through mode enabled (SW3-1 = ON) and L2 cache
    ; enabled (SW3-7 = ON).
    bic     r0, r0, #ARM_CTRL_XP                ; Clear the XP bit to enable supage AP bit
    bic     r0, r0, #ARM_CTRL_L4                ; Clear the L4 bit to enable arm v6 behavior
    orr     r0, r0, #ARM_CTRL_A                 ; Set the A bit to enable alignment checking

    orr     r0, r0, #ARM_CTRL_FLOW              ; program flow prediction enabled
    orr     r0, r0, #ARM_CTRL_FI                ; enable low interrupt latency (Errata 364296)
    orr     r0, r0, #ARM_CTRL_U                 ; allow unaligned accesses (codec optimization)
    mcr     p15, 0, r0, c1, c0, 0               ; update system control reg

    ;
    ; Branch predicition is now enabled.  Flush the BTAC to ensure a valid 
    ; starting point.  Don't flush BTAC while it is disabled to avoid
    ; ARM1136 erratum 408023.
    ;
    mov     r0, #0
    mcr     p15, 0, r0, c7, c5, 6               ; flush entire BTAC
    
    ;
    ; Configure ARM Peripheral Port Memory Remap Register (PPMRR)
    ;
    ldr     r0, =ARM_PPMRR_CONFIG               ; r0 = PPMRR configuration
    mcr     p15, 0, r0, c15, c2, 4              ; update PPMRR

    ;
    ; Disable L2 cache
    ;
    ldr     r1, =CSP_BASE_REG_PA_L2CC
    ldr     r0, =0x0
    str     r0, [r1, #L2CC_CR_OFFSET]

    ;
    ; Configure ARM coprocessor access control register
    ;
    ldr     r0, =ARM_CACR_CONFIG                ; r0 = CACR configuration
    mcr     p15, 0, r0, c1, c0, 2               ; update CACR

    ; 
    ; configure AHB<->IP-bus interface (AIPS) registers
    ;
    ldr     r1, =CSP_BASE_REG_PA_AIPSAREG
    ldr     r2, =CSP_BASE_REG_PA_AIPSBREG

    ; except for AIPS regs, configure all peripherals as follows:
    ;   unbuffered writes (BW=0)
    ;   disable supervisor protect (SP=0)
    ;   disable write protect (WP=0)
    ;   disable trusted protect (TP=0)
    mov     r0, #0
    str     r0, [r1, #AIPSREG_PACR1_OFFSET]
    str     r0, [r1, #AIPSREG_PACR2_OFFSET]
    str     r0, [r1, #AIPSREG_PACR3_OFFSET]
    str     r0, [r1, #AIPSREG_OPACR0_OFFSET]
    str     r0, [r1, #AIPSREG_OPACR1_OFFSET]
    str     r0, [r1, #AIPSREG_OPACR2_OFFSET]
    str     r0, [r1, #AIPSREG_OPACR3_OFFSET]
    str     r0, [r1, #AIPSREG_OPACR4_OFFSET]
    str     r0, [r2, #AIPSREG_PACR1_OFFSET]
    str     r0, [r2, #AIPSREG_PACR2_OFFSET]
    str     r0, [r2, #AIPSREG_PACR3_OFFSET]
    str     r0, [r2, #AIPSREG_OPACR0_OFFSET]
    str     r0, [r2, #AIPSREG_OPACR1_OFFSET]
    str     r0, [r2, #AIPSREG_OPACR2_OFFSET]
    str     r0, [r2, #AIPSREG_OPACR3_OFFSET]
    str     r0, [r2, #AIPSREG_OPACR4_OFFSET]

    ; AIPS regs (PACR0) are configured as follows:
    ;   unbuffered writes (BW=0)
    ;   enable supervisor protect (SP=1)
    ;   disable write protect (WP=0)
    ;   disable trusted protect (TP=0)
    orr     r0, r0, #(1 << (28+2))
    str     r0, [r1, #AIPSREG_PACR0_OFFSET]
    str     r0, [r2, #AIPSREG_PACR0_OFFSET]

    ; Set all MPRx to be non-bufferable, trusted for R/W,
    ; not forced to user-mode.
    ldr     r0, =(0x77777777)
    str     r0, [r1, #AIPSREG_MPR0_OFFSET]
    str     r0, [r1, #AIPSREG_MPR1_OFFSET]
    str     r0, [r2, #AIPSREG_MPR0_OFFSET]
    str     r0, [r2, #AIPSREG_MPR1_OFFSET]

    ;
    ; configure AHB crossbar switch (MAX) registers
    ;
    ;
    ; M0, M1 - L2 Cache, M0 is for linefills only, M1 is linefills and non-cached accesses
    ; M2 - Peripheral AHB
    ; M3 - RTIC
    ; M4 - SDMA
    ; M5 - USB
    ; S0 - GA/MBX
    ; S1 - EIM
    ; S2 - IPU
    ; S3 - AIPS1/ROM
    ; S4 - AIPS2/RAM
    ldr     r1, =CSP_BASE_REG_PA_MAX

    ; Master Priority (0 = highest priority)
    ;   M4   > M2   > M3   > M5  > M0   > M1
    ;   DMA > PAHB > RTIC > USB > L2M0 > L2M1
    ldr r0, =(0x00302154)

    ; Master priority configured the same for all slaves
    str     r0, [r1, #MAX_MPR0_OFFSET]
    str     r0, [r1, #MAX_MPR1_OFFSET]
    str     r0, [r1, #MAX_MPR2_OFFSET]
    str     r0, [r1, #MAX_MPR3_OFFSET]
    str     r0, [r1, #MAX_MPR4_OFFSET]

    ; Slave control
    ;   PARK - Ignored since PCTL not (0 << 0)
    ;   PCTL - Park on last master (1 << 4)
    ;   ARB - Fixed priority (0 << 8)
    ;   HPE - High priority input disabled (0 << 16)
    ;   HLP - Halt request has lowest priority (0 << 30)
    ;   RO - Slave port registers can be written (0 << 31)
    ldr     r0, =(1 << 4)

    ; Slave control configured the same for all slaves
    str     r0, [r1, #MAX_SGPCR0_OFFSET]
    str     r0, [r1, #MAX_SGPCR1_OFFSET]
    str     r0, [r1, #MAX_SGPCR2_OFFSET]
    str     r0, [r1, #MAX_SGPCR3_OFFSET]
    str     r0, [r1, #MAX_SGPCR4_OFFSET]

    ; Master control
    ;   AULB - Arbitration any time during undefined burst length (1 << 0)
    ldr     r0, =(1 << 0)
    str     r0, [r1, #MAX_MGPCR0_OFFSET]
    str     r0, [r1, #MAX_MGPCR1_OFFSET]
    str     r0, [r1, #MAX_MGPCR2_OFFSET]
    str     r0, [r1, #MAX_MGPCR3_OFFSET]
    str     r0, [r1, #MAX_MGPCR4_OFFSET]
    str     r0, [r1, #MAX_MGPCR5_OFFSET]

    ; Configure M3IF registers
    ldr     r1, =CSP_BASE_REG_PA_M3IF

    ; M3IF Control Register (M3IFCTL)
    ;   MRRP[0] = TMAX not on priority list (0 << 0)        = 0x00000000
    ;   MRRP[1] = SMIF not on priority list (0 << 0)        = 0x00000000
    ;   MRRP[2] = MAX0 not on priority list (0 << 0)        = 0x00000000
    ;   MRRP[3] = MAX1 not on priority list (0 << 0)        = 0x00000000
    ;   MRRP[4] = SDMA not on priority list (0 << 0)        = 0x00000000
    ;   MRRP[5] = MPEG4 not on priority list (0 << 0)       = 0x00000000
    ;   MRRP[6] = IPU on priority list (1 << 6)             = 0x00000040
    ;   MRRP[7] = IPU 2nd Channel on priority list (0 << 0) = 0x00000080
    ;                                                       ------------
    ;                                                         0x000000C0
    ;
    ldr     r0, =0x000000C0
    str     r0, [r1, #M3IF_M3IFCTL_OFFSET]

    ; *** SPBA Bus Init ***
    ldr     r0, =CSP_BASE_REG_PA_SPBA
    add     r4, r0, #(SPBA_NUM_SHARED_PERIPH * 4)

    ; Peripheral rights (write value)
    ;   RAR0 = ARM has access (1 << 0)
    ;   RAR1 = DSP has access (1 << 1)
    ;   RAR2 = DMA has access (1 << 2)
    ldr     r1, =7

    ; Peripheral rights (read value)
    ;   RAR0 = ARM has access (1 << 0)
    ;   RAR1 = DSP has access (1 << 1)
    ;   RAR2 = DMA has access (1 << 2)
    ;   ROI = ARM owns access (1 << 16)
    ;   RMO = Requesting master has ownership (3 << 30)
    ldr     r2, =0xC0010007
spba_continue
    str     r1, [r0, #SPBA_PRR_OFFSET]
spba_check_loop
    ldr     r3, [r0, #SPBA_PRR_OFFSET]
    cmp     r2, r3
    bne     spba_check_loop
    add     r0, r0, #4
    cmp     r0, r4
    ble     spba_continue

    ;-------------------------------------------------------------------------
    ; Calculate cache_init address in RAM, store in IRAM top 64 bytes
    ;-------------------------------------------------------------------------
    adr   r0, cache_init
    adr   r2, Cal_base
    sub   r0, r0, r2
    
    ldr    r1, =TEMP_VAL
    add  r0, r0, pc
    str    r0, [r1]
    
Cal_base

    ;--------------------------------------------------------------------------
    ;Copy SETCLKSDRAM function to IRAM
    ;--------------------------------------------------------------------------

    adr     r2, CLKSDRAMSET_BEGIN
    mov    r1, r2
    ldr      r0, =CLK_IRAM_START

    ;-------------------------------------------------------------------------
    ;copy followed by CLKSDRAMSET_BEGIN 8K code IRAM first 8K space
    ;-------------------------------------------------------------------------
    
    ldr r3, =0x2000
        add     sp, r0, r3

copy_fun
    ldmia   r1!, {r4 - r11}
    stmia   r0!, {r4 - r11}
    subs    r3, r3, #32
    bne     copy_fun

   ldr     r0, =CLK_IRAM_START

    ; Call SETCLKSDRAM function to Initiailzation clock and SDRAM
    ;
    
    mov pc, r0

    nop
    nop
    nop
        LTORG


CLKSDRAMSET_BEGIN

    nop
    nop
    nop
    nop
    nop
    nop
    nop
    nop
  
    ldr     r1, =CSP_BASE_REG_PA_CCM

    ; Read GPF bits in  RCSR to determine if clocks and memory controller 
    ; already initialized
    ldr     r0, [r1, #CCM_RCSR_OFFSET]
    mov     r2, r0
    ands    r0, r0, #(1 << 4)
    bne     load_pbc_config

    ; Set flag for next Startup entry to skip initialization
    orr     r2, r2, #(1 << 4)
    str     r2, [r1, #CCM_RCSR_OFFSET]

    ;---------------------------------------------------------------------------
    ; load default config
    ; save default config to BSTAT2
    ; r7  BSTAT2 config
    ;---------------------------------------------------------------------------
    
    ldr    r7, =ARG_BSTAT2_DEFAULT
    ldr    r0, =ARG_RAM_START
    str    r7, [r0, #ARG_BSTAT2_OFFSET]

    b load_pbc_done
    
load_pbc_config
    ldr    r0, =ARG_RAM_START
    ldrh   r7, [r0, #ARG_BSTAT2_OFFSET]

load_pbc_done

    mov   r5, r7

    ;--------------------------------------------------------------------------
    ; MS RECOMMENDATION:
    ; Configure the GPIO lines per the requirements of the board. GPIO lines
    ; must be enabled for on-board features like LED.
    ;--------------------------------------------------------------------------


    ;--------------------------------------------------------------------------
    ; MS RECOMMENDATION:
    ; Configure the memory controller, set refresh frequency, and enable
    ; clocks.  Program data width and memory timing values and power up the
    ; banks.
    ;--------------------------------------------------------------------------

        ;==========================================================================================
        IF      :LNOT: SKIP_INIT_FOR_LOLO
        ;==========================================================================================
    ; Configure CS5 to debug board
    ldr     r1, =CSP_BASE_REG_PA_WEIM
    ldr     r0, =0x0000D843
    ;ldr     r0, =0x0000DCF6
    str     r0, [r1, #WEIM_CSCR5U_OFFSET]
    ldr     r0, =0x22252521
    ;ldr     r0, =0x444A4541
    str     r0, [r1, #WEIM_CSCR5L_OFFSET]
    ldr     r0, =0x22220A00
    ;ldr     r0, =0x44443302
    str     r0, [r1, #WEIM_CSCR5A_OFFSET]

        ;==========================================================================================
        ENDIF   ; IF      :LNOT: SKIP_INIT_FOR_LOLO
        ;==========================================================================================

    ; Enable IPU DI to get acknowledge for max_podf value change
    ldr     r1, =CSP_BASE_REG_PA_IPU

        ;==========================================================================================
        IF      :LNOT: SKIP_INIT_FOR_LOLO
        ;==========================================================================================
    ; IPU configuration register (IPU_CONF):
    ;   DI_EN = Display interface enabled = (1 << 6)    = 0x00000040
    ;                                                   ------------
    ;                                                     0x00000040
    ldr     r0, =0x00000040
    str     r0, [r1]
        ;==========================================================================================
        ENDIF   ; IF      :LNOT: SKIP_INIT_FOR_LOLO
        ;==========================================================================================


    ldr     r1, =CSP_BASE_REG_PA_CCM

    ; Determine PLL reference selected by external CKSS signal (reflected
    ; in PRCS bits of CCM CCMR)
    ;ldr     r0, [r1, #CCM_CCMR_OFFSET]
    ;ands    r0, r0, #(1 << 1)

    b     CKIH_PLL_REF

    ;
    ; PLL reference is CKIH
    ;

CKIH_PLL_REF
    ; Control register (CCMR):
    ;
    ;   FPME = FPM disabled = (1 << 0)              = 0x00000000
    ;   PRCS = CKIH is PLL ref = (2 << 1)           = 0x00000004 
    ;   MPE = MCU PLL enabled = (1 << 3)            = 0x00000008
    ;   SBYCS = Enabled in standby = (1 << 4)       = 0x00000010
    ;   ROMW = All masters 1 WS = (3 << 5)          = 0x00000060
    ;   MDS = MCU PLL is source = (0 << 7)          = 0x00000000
    ;   SPE = serial PLL enabled = (1 << 8)         = 0x00000100
    ;   UPE = USB PLL enabled = (1 << 9)            = 0x00000200
    ;   WAMO = masked all but DSM = (0 << 10)       = 0x00000000
    ;   FIRS = FIRI source usb_clk = (1 << 11)      = 0x00000800
    ;   LPM = wait mode = (0 << 14)                 = 0x00000000
    ;   RAMW = All masters 1 WS = (3 << 16)         = 0x00030000
    ;   SSI1S = SSI1 source serial_clk = (2 << 18)  = 0x00080000
    ;   SSI1S = SSI2 source serial_clk = (2 << 21)  = 0x00400000
    ;   PERCS = per_clk source ipg_clk = (1 << 24)  = 0x01000000
    ;   CSCS = CSI source SRPLL = (1 << 25)         = 0x02000000
    ;   FPMF = FPM factor 1024 = (1 << 26)          = 0x04000000
    ;   WBEN = well bias disabled = (0 << 27)       = 0x00000000
    ;   VSTBY = no standby = (0 << 28)              = 0x00000000
    ;   L2PG = no L2 power gating (0 << 29)         = 0x00000000
    ;                                               ------------
    ;                                                 0x074B0B7C
    ldr     r0, =0x074B0B7C
    str     r0, [r1, #CCM_CCMR_OFFSET]

        ;==========================================================================================
        IF      :LNOT: SKIP_INIT_FOR_LOLO
        ;==========================================================================================
    ; Check for forced alternate clocking
    ands    r0, r5, #BSP_PBC_DSW_ALT_CLK
    bne     CLK_ALT
        ;==========================================================================================
        ENDIF   ; IF      :LNOT: SKIP_INIT_FOR_LOLO
        ;==========================================================================================

        ;==========================================================================================
        IF      :LNOT: SKIP_INIT_FOR_LOLO
        ;==========================================================================================

    ; Configure the Serial PLL
    ;
    ;   SERPLL  = (CKIH) * 2 * (MFI + MFN/(MFD+1)) / (PDF+1)
    ;           = (26 MHz) * 2 * (8 + 292/(624+1)) / (1+1)
    ;           = 220.1472 MHz
    ;
    ;   MFN = 292 = (292 << 0)                      = 0x00000124
    ;   MFI = 8 = (8 << 10)                         = 0x00002000
    ;   MFD = 624 = (624 << 16)                     = 0x02700000
    ;   PD = 1 = (1 << 26)                          = 0x04000000
    ;   BRMO = first order = (0 << 31)              = 0x00000000
    ;                                               ------------
    ;                                                 0x06702124
    ldr     r0, =0x06702124
    str     r0, [r1, #CCM_SPCTL_OFFSET]

    ; Configure the USB PLL
    ;
    ;   USBPLL = (CKIH) * 2 * (MFI + MFN/(MFD+1)) / (PDF+1)
    ;           = (26 MHz) * 2 * (9 + 12/(51+1)) / (1+1)
    ;           = 240 MHz
    ;
    ;   MFN = 12 = (12 << 0)                        = 0x0000000C
    ;   MFI = 9 = (9 << 10)                         = 0x00002400
    ;   MFD = 51 = (51 << 16)                       = 0x00330000
    ;   PD = 1 = (1 << 26)                          = 0x04000000
    ;   BRMO = first order = (0 << 31)              = 0x00000000
    ;                                               ------------
    ;                                                 0x0433240C
    ldr     r0, =0x0433240C
    str     r0, [r1, #CCM_UPCTL_OFFSET]

        ;==========================================================================================
        ENDIF   ; IF      :LNOT: SKIP_INIT_FOR_LOLO
        ;==========================================================================================

        ;==========================================================================================
        IF      :LNOT: SKIP_INIT_FOR_LOLO
        ;==========================================================================================
    ; Determine AHB clock configuration
    and     r0, r5, #BSP_PBC_DSW_AHB_CLK
    cmp     r0, #BSP_PBC_DSW_AHB_66
    beq     CLK_AHB_66

    ; Fall though to CLK_AHB_133

CLK_AHB_133
    ; Post-divider Register 0 (PDR0):
    ;
    ;   MCU_MAIN_CLK = 532 MHz (see MPCLT programming below)
    ;
    ;   MCU_CLK = MCU_MAIN_CLK / 2 = 266 MHz
    ;   HSP_CLK = MCU_MAIN_CLK / 4 = 133 MHz
    ;   MAX_CLK = HCLK = MCU_MAIN_CLK / 4 = 133 MHz
    ;   IPG_CLK = HCLK / 2 = 66.5 MHz
    ;   NFC_CLK = HCLK / 5 = 26.6 MHz
    ;   PER_CLK = USB_CLK / 4
    ;   CSI_CLK = USB_CLK / 512
    ;
    ;   MCU_PODF = /1  = (1 << 0)                   = 0x00000001
    ;   MAX_PODF = /4 = (3 << 3)                    = 0x00000018
    ;   IPG_PODF = /2 = (1 << 6)                    = 0x00000040
    ;   NFC_PODF = /5 = (4 << 8)                    = 0x00000400
    ;   HSP_PODF = /4 = (3 << 11)                   = 0x00001800
    ;   PER_PODF = /4 = (3 << 16)                   = 0x00030000
    ;   CSI_PODF = /512 = (511 << 23)               = 0xFF800000
    ;                                               ------------
    ;                                                 0xFF831C59
    ldr     r0, =0xFF831C59
    str     r0, [r1, #CCM_PDR0_OFFSET]
    b       CLK_MCUPLL_532

CLK_AHB_66
    ; Post-divider Register 0 (PDR0):
    ;
    ;   MCU_MAIN_CLK = 532 MHz (see MPCLT programming below)
    ;
    ;   MCU_CLK = MCU_MAIN_CLK / 2 = 266 MHz
    ;   HSP_CLK = MCU_MAIN_CLK / 4 = 133 MHz
    ;   MAX_CLK = HCLK = MCU_MAIN_CLK / 8 = 66.5 MHz
    ;   IPG_CLK = HCLK / 1 = 66.5 MHz
    ;   NFC_CLK = HCLK / 3 = 22.167 MHz
    ;   PER_CLK = USB_CLK / 4
    ;   CSI_CLK = USB_CLK / 512
    ;
    ;   MCU_PODF = /2  = (1 << 0)                   = 0x00000001
    ;   MAX_PODF = /8 = (7 << 3)                    = 0x00000038
    ;   IPG_PODF = /1 = (0 << 6)                    = 0x00000000
    ;   NFC_PODF = /3 = (2 << 8)                    = 0x00000200
    ;   HSP_PODF = /4 = (3 << 11)                   = 0x00001800
    ;   PER_PODF = /4 = (3 << 16)                   = 0x00030000
    ;   CSI_PODF = /512 = (511 << 23)               = 0xFF800000
    ;                                               ------------
    ;                                                 0xFF831A39
    ldr     r0, =0xFF831A39
    str     r0, [r1, #CCM_PDR0_OFFSET]

CLK_MCUPLL_532
    ; MCU PLL control (MPCTL)
    ;
    ;   MCU_CLK = (CKIH) * 2 * (MFI + MFN/(MFD+1)) / (PDF+1)
    ;           = (26 MHz) * 2 * (10 + 12/(51+1)) / (0+1)
    ;           = 532 MHz
    ;
    ;   MFN = 12 = (12 << 0)                        = 0x0000000C
    ;   MFI = 10 = (10 << 10)                       = 0x00002800
    ;   MFD = 51 = (51 << 16)                       = 0x00330000
    ;   PD = 0 = (0 << 26)                          = 0x00000000
    ;   BRMO = first order = (0 << 31)              = 0x00000000
    ;                                               ------------
    ;                                                 0x0033280C
    ldr     r0, =0x0033280C
    str     r0, [r1, #CCM_MPCTL_OFFSET]
        ;==========================================================================================
        ENDIF   ; IF      :LNOT: SKIP_INIT_FOR_LOLO
        ;==========================================================================================
    b       WEIM_CONFIG

        ;==========================================================================================
        IF      :LNOT: SKIP_INIT_FOR_LOLO
        ;==========================================================================================
CLK_ALT
    ; Configure the Serial PLL
    ;
    ;   SERPLL  = (CKIH) * 2 * (MFI + MFN/(MFD+1)) / (PDF+1)
    ;           = (27 MHz) * 2 * (8 + 96/(624+1)) / (1+1)
    ;           = 220.1472 MHz
    ;
    ;   MFN = 96 = (96 << 0)                        = 0x00000060
    ;   MFI = 8 = (8 << 10)                         = 0x00002000
    ;   MFD = 624 = (624 << 16)                     = 0x02700000
    ;   PD = 1 = (1 << 26)                          = 0x04000000
    ;   BRMO = first order = (0 << 31)              = 0x00000000
    ;                                               ------------
    ;                                                 0x06702060
    ldr     r0, =0x06702060
    str     r0, [r1, #CCM_SPCTL_OFFSET]

    ; Configure the USB PLL
    ;
    ;   USBPLL = (CKIH) * 2 * (MFI + MFN/(MFD+1)) / (PDF+1)
    ;           = (27 MHz) * 2 * (8 + 48/(53+1)) / (1+1)
    ;           = 240 MHz
    ;
    ;   MFN = 48 = (48 << 0)                        = 0x00000030
    ;   MFI = 8 = (8 << 10)                         = 0x00002000
    ;   MFD = 53 = (53 << 16)                       = 0x00350000
    ;   PD = 1 = (1 << 26)                          = 0x04000000
    ;   BRMO = first order = (0 << 31)              = 0x00000000
    ;                                               ------------
    ;                                                 0x04352030
    ldr     r0, =0x04352030
    str     r0, [r1, #CCM_UPCTL_OFFSET]

    ; Post-divider Register 0 (PDR0):
    ;
    ;   MCU_MAIN_CLK = 528 MHz (see MPCLT programming below)
    ;
    ;   MCU_CLK = MCU_MAIN_CLK / 2 = 264 MHz
    ;   HSP_CLK = MCU_MAIN_CLK / 4 = 132 MHz
    ;   MAX_CLK = HCLK = MCU_MAIN_CLK / 4 = 132 MHz
    ;   IPG_CLK = HCLK / 2 = 66 MHz
    ;   NFC_CLK = HCLK / 5 = 26.4 MHz
    ;   PER_CLK = USB_CLK / 4
    ;   CSI_CLK = USB_CLK / 512
    ;
    ;   MCU_PODF = /2  = (1 << 0)                   = 0x00000001
    ;   MAX_PODF = /4 = (3 << 3)                    = 0x00000018
    ;   IPG_PODF = /2 = (1 << 6)                    = 0x00000040
    ;   NFC_PODF = /5 = (4 << 8)                    = 0x00000400
    ;   HSP_PODF = /4 = (3 << 11)                   = 0x00001800
    ;   PER_PODF = /4 = (3 << 16)                   = 0x00030000
    ;   CSI_PODF = /512 = (511 << 23)               = 0xFF800000
    ;                                               ------------
    ;                                                 0xFF831C59
    ldr     r0, =0xFF831C59
    str     r0, [r1, #CCM_PDR0_OFFSET]

CLK_MCUPLL_ALT
    ; MCU PLL control (MPCTL)
    ;
    ;   MCU_CLK = (CKIH) * 2 * (MFI + MFN/(MFD+1)) / (PDF+1)
    ;           = (27 MHz) * 2 * (9 + 42/(53+1)) / (0+1)
    ;           = 528 MHz
    ;
    ;   MFN = 42 = (42 << 0)                        = 0x0000002A
    ;   MFI = 9 =  (9 << 10)                        = 0x00002400
    ;   MFD = 53 = (53 << 16)                       = 0x00350000
    ;   PD = 0 = (0 << 26)                          = 0x00000000
    ;   BRMO = first order = (0 << 31)              = 0x00000000
    ;                                               ------------
    ;                                                 0x0035242A
    ldr     r0, =0x0035242A
    str     r0, [r1, #CCM_MPCTL_OFFSET]
        ;==========================================================================================
        ENDIF   ; IF      :LNOT: SKIP_INIT_FOR_LOLO
        ;==========================================================================================


WEIM_CONFIG
    ;
    ; Configure wireless external interface module (WEIM)
    ;
        ;==========================================================================================
        IF      :LNOT: SKIP_INIT_FOR_LOLO
        ;==========================================================================================
    ;
    ; Sync mode (AHB Clk = 133MHz ; BCLK = 44.3MHz):
    ;
    ldr     r1, =CSP_BASE_MEM_PA_CS0
    ldr     r0, =0xF0F0
    strh    r0, [r1]

    ; 1st command
    ldr     r2, =0xAAA
    add     r2, r2, r1
    ldr     r0, =0xAAAA
    strh    r0, [r2]

    ; 2nd command
    ldr     r2, =0x554
    add     r2, r2, r1
    ldr     r0, =0x5555
    strh    r0, [r2]

    ; 3rd command
    ldr     r2, =0xAAA
    add     r2, r2, r1
    ldr     r0, =0xD0D0
    strh    r0, [r2]

    ; Write flash config register
    ldr     r0, =0x56CA
    strh    r0, [r2]

    ; Flash reset command
    ldr     r0, =0xF0F0
    strh    r0, [r1]

    ldr     r1, =CSP_BASE_REG_PA_WEIM

    ; CS0 control (upper)
    ;   EDC - 3 extra dead cycles (3 << 0)          = 0x00000003
    ;   WWS - 0 extra write wait states (0 << 4)    = 0x00000000
    ;   EW - Posedge DTACK (0 << 7)                 = 0x00000000
    ;   WSC - 12 wait states (12 << 8)              = 0x00000C00
    ;   CNC - 3 CS negation cycles (3 << 14)        = 0x0000C000
    ;   DOL - 0 clock burst latency (0 << 16)       = 0x00000000
    ;   SYNC - Disable sync burst (0 << 20)         = 0x00000000
    ;   PME - Disable page mode (0 << 21)           = 0x00000000
    ;   PSZ - 4 word burst/page (0 << 22)           = 0x00000000
    ;   BCS - 1 BCLK delay (0 << 24)                = 0x00000000
    ;   BCD - /1 burst clock (0 << 28)              = 0x00000000
    ;   WP - No write protect (0 << 30)             = 0x00000000
    ;   SP - All user mode access (0 << 31)         = 0x00000000
    ;                                               ------------
    ;                                                 0x0000CC03
    ;ldr     r0, =0x0000CC03
    ldr     r0, =0x23524E80
    str     r0, [r1, #WEIM_CSCR0U_OFFSET]

    ; CS0 control (lower)
    ;   CSEN - Enable chip select (1 << 0)          = 0x00000001
    ;   WRAP - No wrap (0 << 1)                     = 0x00000000
    ;   CRE -  CRE pin 0 (0 << 2)                   = 0x00000000
    ;   PSR - PSRAM mode disabled (0 << 3)          = 0x00000000
    ;   CSN -  2 AHB clocks (2 << 4)                = 0x00000000
    ;   DSZ - 16-bit DATA[15:0] (5 << 8)            = 0x00000500
    ;   EBC - Only write asserts EB (1 << 11)       = 0x00000800
    ;   CSA - 2 half AHB clocks (0 << 12)           = 0x00000000
    ;   EBWN - 3 half AHB clocks (3 << 16)          = 0x00030000
    ;   EBWA - 3 half AHB clocks (3 << 20)          = 0x00300000
    ;   OEN - 2 half AHB clocks (0 << 24)           = 0x00000000
    ;   OEA - 10 half AHB clocks (10 << 28)         = 0xA0000000
    ;                                               ------------
    ;                                                 0xA0330D01
    ;ldr     r0, =0xA0330D01
    ldr     r0, =0x10000D03
    str     r0, [r1, #WEIM_CSCR0L_OFFSET]

    ; CS0 control (additional)
    ;   FCE - Data capture by AHB clk (0 << 0)      = 0x00000000
    ;   CNC2 - No CNC increase (0 << 1)             = 0x00000000
    ;   AGE - Disable ack glue logic (0 << 2)       = 0x00000000
    ;   WWU - Forbit wrap on write (0 << 3)         = 0x00000000
    ;   DCT - 2 AHB clocks (0 << 4)                 = 0x00000000
    ;   DWW - wait states same as read (0 << 6)     = 0x00000000
    ;   LBA - 0 half AHB clock (0 << 8)             = 0x00000000
    ;   LBN - 2 half AHB clocks (2 << 10)           = 0x00000800
    ;   LAH - 3 half AHB clocks (2 << 13)           = 0x00000000
    ;   MUM - Non-muxed mode (0 << 14)              = 0x00000000
    ;   RWN - 2 half AHB clocks (2 << 16)           = 0x00020000
    ;   RWA - 2 half AHB clocks (2 << 20)           = 0x00200000
    ;   EBRN - 0 half AHB clocks (0 << 24)          = 0x00000000
    ;   EBRA - 0 half AHB clocks (0 << 28)          = 0x00000000
    ;                                               ------------
    ;                                                 0x00220800
    ;ldr     r0, =0x00220800
    ldr     r0, =0x00720900
    str     r0, [r1, #WEIM_CSCR0A_OFFSET]

        ;==========================================================================================
        ENDIF   ; IF      :LNOT: SKIP_INIT_FOR_LOLO
        ;==========================================================================================

	; CAN'T SET UP RAM IF LCD CONTROLLER IS RUNNING
        ;==========================================================================================
        IF      :LNOT: SKIP_INIT_FOR_LOLO
        ;==========================================================================================
    ; Only configure ESDCTL if we are not currently running from SDRAM
    ldr     r1, =CSP_BASE_MEM_PA_CSD0
    cmp     pc, r1
    blt     esdctl_init  ; if (pc < CSP_BASE_MEM_PA_CSD0) goto esdctl_init

    ldr     r1, =CSP_BASE_MEM_PA_CS0
    cmp     pc, r1
    blt     cache_init  ; if (pc < CSP_BASE_MEM_PA_CS0) goto cache init

esdctl_init
    ;
    ; Disable maximum drive strength SDRAM/DDR lines by clearing DSE1 bits
    ; in SW_PAD_CTL registers
    ;

    ; SDCLK
    ldr     r1, =(CSP_BASE_REG_PA_IOMUXC+0x26C)
    ldr     r0, [r1]
    bic     r0, r0, #(1 << 12)
    str     r0, [r1]

    ; Set DSE0 (standard strength) for silicon 2.0 
    ldr     r1, =(CSP_BASE_REG_PA_IIM+IIM_SCS1_OFFSET)
    ldrb   r0, [r1]    
    orr    r0, r0, #(1 << 1)
    strb   r0, [r1]

    
    ; CAS
    ldr     r1, =(CSP_BASE_REG_PA_IOMUXC+0x270)
    ldr     r0, [r1]
    bic     r0, r0, #(1 << 22)
    str     r0, [r1]

    ; RAS
    ldr     r1, =(CSP_BASE_REG_PA_IOMUXC+0x274)
    ldr     r0, [r1]
    bic     r0, r0, #(1 << 2)
    str     r0, [r1]

    ; CS2 (CSD0)
    ldr     r1, =(CSP_BASE_REG_PA_IOMUXC+0x27C)
    ldr     r0, [r1]
    bic     r0, r0, #(1 << 22)
    str     r0, [r1]

    ; DQM3
    ldr     r1, =(CSP_BASE_REG_PA_IOMUXC+0x284)
    ldr     r0, [r1]
    bic     r0, r0, #(1 << 22)
    str     r0, [r1]

    ; DQS
    ldr     r1, =(CSP_BASE_REG_PA_IIM+IIM_SCS2_OFFSET)
    ldrb   r0, [r1]    
    orr    r0, r0, #(1 << 1)
    strb   r0, [r1]

    ; DQM2, DQM1, DQM0, SD31-SD0, A25-A0, MA10 (0x288..0x2DC)
    ldr     r1, =(CSP_BASE_REG_PA_IOMUXC+0x288)
    ldr     r2, =22     ; (0x2E0 - 0x288) / 4 = 22

    
pad_loop
    ldr     r0, [r1]
    bic     r0, r0, #(1 << 22)
    bic     r0, r0, #(1 << 12)
    bic     r0, r0, #(1 << 2)
    str     r0, [r1]
    add     r1, r1, #4
    subs    r2, r2, #0x1
    bne     pad_loop

    ;
    ; Configure enhanced SDRAM/DDR contoller (ESDCTL)
    ;
    ldr     r1, =CSP_BASE_REG_PA_ESDCTL
    ldr     r2, =CSP_BASE_MEM_PA_CSD0

    ; Configure SDRAM timing parameters
    ldr     r0, =0x006AC73A
    str     r0, [r1, #ESDCTL_ESDCFG0_OFFSET]

    ; Reset 
    ldr     r0, =0x2
    str     r0, [r1, #ESDCTL_ESDMISC_OFFSET]

    ; Configure for DDR
    ldr     r0, =0x4
    str     r0, [r1, #ESDCTL_ESDMISC_OFFSET]

    ; Hold for more than 200ns
    ldr     r0, =0x10000
hold
    subs    r0, r0, #0x1
    bne     hold

    ; Set precharge command
    ;
    ;   COL - 9 column addresses (1 << 20)              = 0x00100000
    ;   ROW - 13 Row addresses (2 << 24)                = 0x02000000
    ;   SP - User mode access (0 << 27)                 = 0x00000000
    ;   SMODE - Precharge command (1 << 28)             = 0x10000000
    ;   SDE - Enable controller (1 << 31)               = 0x80000000
    ;                                                   ------------
    ;                                                     0x92100000
    ldr     r0, =0x92100000
    str     r0, [r1, #ESDCTL_ESDCTL0_OFFSET]

    ; Access SDRAM with A10 high to precharge all banks
    ldr     r0, =0x0
    str     r0, [r2, #0xF00]

    ; Set autorefresh command
    ;
    ;   COL - 9 column addresses (1 << 20)              = 0x00100000
    ;   ROW - 13 Row addresses (2 << 24)                = 0x02000000
    ;   SP - User mode access (0 << 27)                 = 0x00000000
    ;   SMODE - Autorefresh command (2 << 28)           = 0x20000000
    ;   SDE - Enable controller (1 << 31)               = 0x80000000
    ;                                                   ------------
    ;                                                     0xA2100000
    ldr     r0, =0xA2100000
    str     r0, [r1, #ESDCTL_ESDCTL0_OFFSET]

    ; Use writes to refresh all banks of SDRAM
    ldr     r0, =0x0
    str     r0, [r2]
    str     r0, [r2]

    ; Set load mode command
    ;
    ;   COL - 9 column addresses (1 << 20)              = 0x00100000
    ;   ROW - 13 Row addresses (2 << 24)                = 0x02000000
    ;   SP - User mode access (0 << 27)                 = 0x00000000
    ;   SMODE - Load mode command (3 << 28)             = 0x30000000
    ;   SDE - Enable controller (1 << 31)               = 0x80000000
    ;                                                   ------------
    ;                                                     0xB2100000
    ldr r0, =0xB2100000
    str r0, [r1, #ESDCTL_ESDCTL0_OFFSET]

    ; Use SDRAM write to load SDRAM mode register
    ldr     r0, =0x0
    strb    r0, [r2, #0x33]         ; address used for mode, data ignored

    ldr     r3, =(CSP_BASE_MEM_PA_CSD0+0x01000000)
    strb    r0, [r3]                ; address used for mode, data ignored
    

    ; Set normal mode command
    ;
    ;   PRCT - Precharge timer disabled (0 << 0)        = 0x00000000
    ;   BL - Burst of 8 for SDR/DDR (1 << 7)            = 0x00000080
    ;   FP - No full page mode (0 << 8)                 = 0x00000000
    ;   PWDT - Power down timeout 128 clocks (3 << 10)  = 0x00000C00
    ;   SREFR - 4 rows refreshed each clock (3 << 13)   = 0x00006000
    ;   DSIZ - 32-bit memory width (2 << 16)            = 0x00020000
    ;   COL - 10 column addresses (2 << 20)             = 0x00200000
    ;   ROW - 13 Row addresses (2 << 24)                = 0x02000000
    ;   SP - User mode access (0 << 27)                 = 0x00000000
    ;   SMODE - Normal mode command (0 << 28)           = 0x00000000
    ;   SDE - Enable controller (1 << 31)               = 0x80000000
    ;                                                   ------------
    ;                                                     0x82226C80
    ldr r0, =0x82226C80
    str r0, [r1, #ESDCTL_ESDCTL0_OFFSET]

    ; Configure misc SDRAM parameters
    ldr     r0, =0xC
    str     r0, [r1, #ESDCTL_ESDMISC_OFFSET]

    ; Increase hold time on DDR write delay lines 
    ldr     r0, =0x80000013
    str     r0, [r1, #ESDCTL_ESDDLY1_OFFSET]
    
    ldr     r0, =0x80000013
    str     r0, [r1, #ESDCTL_ESDDLY2_OFFSET]

    ldr     r0, =0x80000013
    str     r0, [r1, #ESDCTL_ESDDLY3_OFFSET]

    ldr     r0, =0x80000013
    str     r0, [r1, #ESDCTL_ESDDLY4_OFFSET]

    ldr     r0, =0x80000013
    str     r0, [r1, #ESDCTL_ESDDLY5_OFFSET]

        ;==========================================================================================
        ENDIF   ; IF      :LNOT: SKIP_INIT_FOR_LOLO
        ;==========================================================================================
    ;-----------------------------------------------------------------
    ;Get cache_init start address in SDRAM , Jump back to SDRAM
    ;
    ;-----------------------------------------------------------------
    
    ldr  r0, =TEMP_VAL
    ldr  r1, [r0]
    mov pc, r1
    
CLKSDRAMSET_END

    nop
    nop
    nop
    nop

cache_init

    nop
    nop
    nop
    nop
    nop

        ; RAM and WEIM(CS4) is ready, store boot arguments
        mov     a1, ip
        bl      OALCopyBootArgs
        
    ;--------------------------------------------------------------------------
    ; MS RECOMMENDATION:
    ; Flush or invalidate the instruction and data caches and the translation
    ; look-aside buffer (TLB) and empty the write buffers
    ;--------------------------------------------------------------------------
    bl      OALClearUTLB
    mov     r0, #0
    mcr     p15, 0, r0, c7, c7, 0               ; flush the unified cache


    IF :LNOT: BOOTLOADER
    ; Read PBC user switches to L2 configuration
    ldr     r5, =ARG_RAM_START
    ldrh    r5, [r5, #ARG_BSTAT2_OFFSET]

    ;mov    r5, r7
    ; Check if we want to keep L2 disabled
    ands    r0, r5, #BSP_PBC_DSW_L2
    bne     AVIC_CONFIG

    ldr     r1, =CSP_BASE_REG_PA_L2CC

L2_532
    ;
    ; Configure L2 cache attributes
    ;
    ; L2 Auxiliary Control Register (AUXCR)
    ;   Data RAM Read = 4 cycle latency = (3 << 0)  = 0x00000003
    ;   Data RAM Write = 4 cycle latency = (3 << 3) = 0x00000018
    ;   Associativity = 8-way = (8 << 13)           = 0x00010000
    ;   Way Size = 16 KB = (1 << 17)                = 0x00020000
    ;                                               ------------
    ;                                                 0x0003001B
    ldr     r0, =0x0003001B
    str     r0, [r1, #L2CC_AUXCR_OFFSET]


L2_INVALIDATE
    ; 
    ; Invalidate L2 cache memory
    ;
    ; 
    ldr     r0, =0x000000FF
    str     r0, [r1, #L2CC_INVWAY_OFFSET]

    ;
    ; Wait for invalidation to complete
    ;
L2_LOOP
    ldr     r0, [r1, #L2CC_INVWAY_OFFSET]
    cmp     r0, #0
    bne     L2_LOOP

    ; Check if we want force L2 in write-through
    ands    r0, r5, #BSP_PBC_DSW_L2_WT
    bne     L2_ENABLE

    ldr     r1, =CSP_BASE_REG_PA_L2CC

    ;
    ; Force L2 write-through
    ;
    ; L2 Debug Control Register (DBGCR)
    ;   DCL = Enable cache line fills = (0 << 0)    = 0x00000000
    ;   DWB = Disable write-back = (1 << 1)         = 0x00000002
    ;                                               ------------
    ;                                                 0x00000002
    ldr     r0, =0x00000002
    str     r0, [r1, #L2CC_DBGCR_OFFSET]

L2_ENABLE
    ; 
    ; Enable L2 cache
    ;
    ldr     r0, =0x1
    str     r0, [r1, #L2CC_CR_OFFSET]
    ENDIF

AVIC_CONFIG
    ;--------------------------------------------------------------------------
    ; MS RECOMMENDATION:
    ; Configure the interrupt controller. Mask and clear any pending
    ; interrupts.
    ;--------------------------------------------------------------------------
    mov     r0, #0
    ldr     r1, =CSP_BASE_REG_PA_AVIC
    str     r0, [r1, #AVIC_INTENABLEH_OFFSET]   ; disable all interrupt sources
    str     r0, [r1, #AVIC_INTENABLEL_OFFSET]   ; disable all interrupt sources
    str     r0, [r1, #AVIC_INTTYPEH_OFFSET]     ; all sources generate norm IRQ
    str     r0, [r1, #AVIC_INTTYPEL_OFFSET]     ; all sources generate norm IRQ
    str     r0, [r1, #AVIC_INTFRCH_OFFSET]      ; no sources forced
    str     r0, [r1, #AVIC_INTFRCL_OFFSET]      ; no sources forced
    str     r0, [r1, #AVIC_NIPRIORITY7_OFFSET]  ; all sources lowest priority
    str     r0, [r1, #AVIC_NIPRIORITY6_OFFSET]  ; all sources lowest priority
    str     r0, [r1, #AVIC_NIPRIORITY5_OFFSET]  ; all sources lowest priority
    str     r0, [r1, #AVIC_NIPRIORITY4_OFFSET]  ; all sources lowest priority
    str     r0, [r1, #AVIC_NIPRIORITY3_OFFSET]  ; all sources lowest priority
    str     r0, [r1, #AVIC_NIPRIORITY2_OFFSET]  ; all sources lowest priority
    str     r0, [r1, #AVIC_NIPRIORITY1_OFFSET]  ; all sources lowest priority
    str     r0, [r1, #AVIC_NIPRIORITY0_OFFSET]  ; all sources lowest priority

    ;--------------------------------------------------------------------------
    ; MS RECOMMENDATION:
    ; Initialize the real-time clock count register to 0 and enable the
    ; real-time clock. 
    ;--------------------------------------------------------------------------
    mov     r0, #0x80
    ldr     r1, =CSP_BASE_REG_PA_RTC
    str     r0, [r1, #RTC_CONTROL]          ; Enable RTC, Select 32768Hz

    ;--------------------------------------------------------------------------
    ; MS RECOMMENDATION:
    ; Set up the power management/monitoring registers. Set conditions during
    ; sleep modes. 
    ;--------------------------------------------------------------------------

    ; Initialize setting in the CRM COM module

    ; Initialize the ADPLL and set the different frequencies

    ; Set the high frequency value of the ADPLL

    ; Initialize the UDPLL

    ; Set the high frequency value of the UDPLL

    ;--------------------------------------------------------------------------
    ; MS RECOMMENDATION:
    ; Turn on all board-level clocks and on chip peripheral clocks.
    ;--------------------------------------------------------------------------


    
    ;--------------------------------------------------------------------------
    ; MS RECOMMENDATION:
    ; Get the physical base address of the OEMAddressTable and store in r0.
    ;--------------------------------------------------------------------------
    adr     r0, g_oalAddressTable

    ;--------------------------------------------------------------------------
    ; MS RECOMMENDATION:
    ; Jump to KernelStart to boot WindowsCE or BootloaderMain for bootloader
    ;--------------------------------------------------------------------------
    b       KernelStart

spin
    b       spin

    ; Include memory configuration file with g_oalAddressTable

    INCLUDE oemaddrtab_cfg.inc

    ENTRY_END StartUp


;-------------------------------------------------------------------------------
;
;  Function:  GetCp15ControlRegister
;
;  Returns value of CP15 control register, used in OEMInit()
;
;

        EXPORT GetCp15ControlRegister
        LEAF_ENTRY GetCp15ControlRegister

	mrc	p15, 0, R0, c1, c0, 0 ; Read Control Register
	mov	pc, lr

        ENTRY_END GetCp15ControlRegister

;-------------------------------------------------------------------------------
;
;  Function:  GetCp15AuxiliaryControlRegister
;
;  Returns value of CP15 control register, used in OEMInit()

        EXPORT GetCp15AuxiliaryControlRegister
        LEAF_ENTRY GetCp15AuxiliaryControlRegister

	mrc p15, 0, r0, c1, c0, 1 ; Read Auxiliary Control Register
	mov	pc, lr

        ENTRY_END GetCp15AuxiliaryControlRegister
;-------------------------------------------------------------------------------
;
;  Function:  GetCp15MainIDRegister
;
;  Returns value of CP15 Main ID register, used in OEMInit()
;
        EXPORT GetCp15MainIDRegister
        LEAF_ENTRY GetCp15MainIDRegister

	mrc p15, 0, r0, c0, c0, 0 ; Read Main ID Register
	mov	pc, lr

        ENTRY_END GetCp15MainIDRegister
