path: root/cfe/cfe/arch/mips/board/bcm63xx_rom/src/bcm63xx_impl1_rom_boot.S

#include "sbmips.h"
#include "bsp_config.h"

#include "bcm_cpu.h"
#include "bcm_common.h"

#include "bcm_hwdefs.h"
#include "boardparms.h"
#include "mipsmacros.h"

#define DDR_TEST            1
#define UBUS_SYNC_ENABLE    1
#define LMB_SYNC_ENABLE     1
#define MIPS_SYNC_ENABLE    1
#define LMB_ENABLE          1

/* Memory mapping table for different size DRAMs (256Mb, 512Mb, 1Gb, 2Gb) */
      .globl  dram_map_table_x8
dram_map_table_x8:
    // 128Mb 64B Interleaving (x8 Mode) 32MB
    // This is just a place holder. This memory does not exist
    _LONG_  0x0F0E0D0C, 0x13121110, 0x17161514, 0x00000018  // Row00_0 Row00_1 Row01_0 Row01_1
    _LONG_  0x04000000, 0x0A090805, 0x0000000B, 0x00000000  // Col00_0 Col00_1 Col01_0 Col01_1
    _LONG_  0x00080706, 0x00000002, 0x00000055              // Bank CS_End Dramsize

    // 256Mb 64B Interleaving (x8 Mode) 64MB
    _LONG_  0x100F0E0D, 0x14131211, 0x18171615, 0x00000019  // Row00_0 Row00_1 Row01_0 Row01_1
    _LONG_  0x04000000, 0x0A090805, 0x00000C0B, 0x00000000  // Col00_0 Col00_1 Col01_0 Col01_1
    _LONG_  0x00000706, 0x00000004, 0x00000066              // Bank CS_End Dramsize
    
    // 512Mb 64B Interleaving (x8 Mode) 128MB
    _LONG_  0x100F0E0D, 0x14131211, 0x18171615, 0x00001A19  // Row00_0 Row00_1 Row01_0 Row01_1
    _LONG_  0x04000000, 0x0A090805, 0x00000C0B, 0x00000000  // Col00_0 Col00_1 Col01_0 Col01_1
    _LONG_  0x00000706, 0x00000008, 0x00000077              // Bank CS_End Dramsize

    // 1Gb 64B Interleaving (x8 Mode) 256MB
    _LONG_  0x11100F0E, 0x15141312, 0x19181716, 0x00001B1A  // Row00_0 Row00_1 Row01_0 Row01_1
    _LONG_  0x04000000, 0x0B0A0905, 0x00000D0C, 0x00000000  // Col00_0 Col00_1 Col01_0 Col01_1
    _LONG_  0x00080706, 0x00000010, 0x00000088              // Bank CS_End Dramsize

      .globl  dram_map_table_x16
dram_map_table_x16:
    // 256Mb 64B Interleaving (x16 Mode) 32MB
    _LONG_  0x0F0E0D0C, 0x13121110, 0x17161514, 0x00000018  // Row00_0 Row00_1 Row01_0 Row01_1
    _LONG_  0x04000000, 0x0A090805, 0x0000000B, 0x00000000  // Col00_0 Col00_1 Col01_0 Col01_1
    _LONG_  0x00000706, 0x00000002, 0x00000055              // Bank CS_End Dramsize
    
    // 512Mb 64B Interleaving (x16 Mode) 64MB
    _LONG_  0x100F0E0D, 0x14131211, 0x18171615, 0x00000019  // Row00_0 Row00_1 Row01_0 Row01_1
    _LONG_  0x04000000, 0x0A090805, 0x00000C0B, 0x00000000  // Col00_0 Col00_1 Col01_0 Col01_1
    _LONG_  0x00000706, 0x00000004, 0x00000066              // Bank CS_End Dramsize

    // 1Gb 64B Interleaving (x16 Mode) 128MB
    _LONG_  0x11100F0E, 0x15141312, 0x19181716, 0x0000001A  // Row00_0 Row00_1 Row01_0 Row01_1
    _LONG_  0x04000000, 0x0B0A0905, 0x00000D0C, 0x00000000  // Col00_0 Col00_1 Col01_0 Col01_1
    _LONG_  0x00080706, 0x00000008, 0x00000077              // Bank CS_End Dramsize

    // 2Gb 64B Interleaving (x16 Mode) 256MB
    _LONG_  0x11100F0E, 0x15141312, 0x19181716, 0x00001B1A  // Row00_0 Row00_1 Row01_0 Row01_1
    _LONG_  0x04000000, 0x0B0A0905, 0x00000D0C, 0x00000000  // Col00_0 Col00_1 Col01_0 Col01_1
    _LONG_  0x00080706, 0x00000010, 0x00000088              // Bank CS_End Dramsize

    .globl  dram_tRefi_table
dram_tRefi_table:       /* Refresh Interval table for different Speed DRAMs (100-200, 300-333, 400) */
    _LONG_  0x16,       0x49,       0x5D    // 100-200 300-333 400 (MHz)
    
    .globl  dram_tRFC_table
dram_tRFC_table:        /* tRFC table for different size & Speed DRAMs (256Mb, 512Mb, 1Gb)/(200, 333, 400) */
//          200         333         400 (MHz)       
    _LONG_  0xF,        0x19,       0x1E    // 256Mb 
    _LONG_  0x15,       0x23,       0x2A    // 512Mb        
    _LONG_  0x1A,       0x2B,       0x33    // 1Gb      
    _LONG_  0x28,       0x42,       0x4F    // 2Gb  

    .globl  dram_timing_table
dram_timing_table:      /*  */
//          tRCD  tCL  tWR  tWL  tRP tRRD  tRC tFAW tW2R tR2W tR2R  tAL tRTP tW2W
//          ---------------------------------------------------------------------
    .byte   0x03,0x04,0x03,0x03,0x03,0x02,0x11,0x00,0x01,0x01,0x00,0x02,0x02,0x00,0x00,0x00 // 200MHz
    .byte   0x05,0x05,0x05,0x04,0x05,0x04,0x13,0x00,0x02,0x01,0x00,0x04,0x03,0x00,0x00,0x00 // 300MHz   
    .byte   0x05,0x05,0x06,0x04,0x06,0x05,0x18,0x00,0x02,0x01,0x00,0x04,0x03,0x00,0x00,0x00 // 400MHz
    
    .globl  dram_sync_table
dram_sync_table:        /* Bit vector table for Ubus sync modes and Lmb sync modes */
#if defined(_BCM96328_)
    _LONG_  0x8E10FFFF, 0x8A10FFFF, 0xEDC2FFFF  // UBUS Sync , LMB Sync, Mips Sync
#elif defined(_BCM96362_)
    _LONG_  0xFFBFBFBF, 0x6341C101, 0xE7E5E5E5  // UBUS Sync , LMB Sync, Mips Sync
#elif defined(_BCM96816_)
    _LONG_  0xC03033B5, 0x80303095, 0xFF90FFFF  // UBUS Sync , LMB Sync, Mips Sync
#endif  
    .globl  dram_speed_table
dram_speed_table:       /* Memory Speed Table for different clock strap values */
                        /* 0=200Mhz 1=333MHz 2=400MHz */
#if defined(_BCM96328_)
    .byte   0   // 0x0
    .byte   0   // 0x1
    .byte   0   // 0x2
    .byte   0   // 0x3
    .byte   0   // 0x4
    .byte   0   // 0x5
    .byte   0   // 0x6
    .byte   0   // 0x7
    .byte   0   // 0x8
    .byte   0   // 0x9
    .byte   0   // 0xa
    .byte   0   // 0xb
    .byte   0   // 0xc
    .byte   0   // 0xd
    .byte   0   // 0xe
    .byte   0   // 0xf
    .byte   0   // 0x10
    .byte   0   // 0x11
    .byte   0   // 0x12
    .byte   0   // 0x13
    .byte   0   // 0x14
    .byte   0   // 0x15
    .byte   2   // 0x16
    .byte   2   // 0x17
    .byte   0   // 0x18
    .byte   0   // 0x19
    .byte   0   // 0x1a
    .byte   0   // 0x1b
    .byte   0   // 0x1c
    .byte   0   // 0x1d
    .byte   0   // 0x1e
    .byte   1   // 0x1f
#elif defined(_BCM96362_)
    .byte   1   // 0x0
    .byte   2   // 0x1
    .byte   0   // 0x2
    .byte   1   // 0x3
    .byte   0   // 0x4
    .byte   2   // 0x5
    .byte   2   // 0x6
    .byte   1   // 0x7
    .byte   1   // 0x8
    .byte   2   // 0x9
    .byte   0   // 0xa
    .byte   1   // 0xb
    .byte   0   // 0xc
    .byte   2   // 0xd
    .byte   2   // 0xe
    .byte   1   // 0xf
    .byte   1   // 0x10
    .byte   2   // 0x11
    .byte   0   // 0x12
    .byte   1   // 0x13
    .byte   0   // 0x14
    .byte   1   // 0x15              // 267MHz. Need to change tREFI if this is used.
    .byte   2   // 0x16
    .byte   1   // 0x17
    .byte   1   // 0x18
    .byte   1   // 0x19
    .byte   0   // 0x1a
    .byte   1   // 0x1b
    .byte   0   // 0x1c
    .byte   0   // 0x1d
    .byte   2   // 0x1e
    .byte   1   // 0x1f
#elif defined(_BCM96816_)
    .byte   0   // 0x0
    .byte   1   // 0x1
    .byte   0   // 0x2
    .byte   0   // 0x3
    .byte   0   // 0x4
    .byte   1   // 0x5
    .byte   1   // 0x6
    .byte   1   // 0x7
    .byte   1   // 0x8
    .byte   1   // 0x9
    .byte   1   // 0xa
    .byte   2   // 0xb
    .byte   2   // 0xc
    .byte   2   // 0xd
    .byte   2   // 0xe
    .byte   2   // 0xf
    .byte   2   // 0x10
    .byte   2   // 0x11
    .byte   2   // 0x12
    .byte   2   // 0x13
    .byte   2   // 0x14
    .byte   0   // 0x15
    .byte   0   // 0x16
    .byte   1   // 0x17
    .byte   1   // 0x18
    .byte   1   // 0x19
    .byte   1   // 0x1a
    .byte   1   // 0x1b
    .byte   2   // 0x1c
    .byte   2   // 0x1d
    .byte   2   // 0x1e
    .byte   2   // 0x1f
#endif

    .globl  memc_ubus_ratio_table
memc_ubus_ratio_table:        /* Memory Speed Table for different clock strap values */
#if defined(_BCM96328_)
// 6328 Clock Speed Table
    .byte   3   // 0x0
    .byte   3   // 0x1
    .byte   3   // 0x2
    .byte   3   // 0x3
    .byte   3   // 0x4
    .byte   3   // 0x5
    .byte   3   // 0x6
    .byte   3   // 0x7
    .byte   3   // 0x8
    .byte   3   // 0x9
    .byte   3   // 0xa
    .byte   3   // 0xb
    .byte   3   // 0xc
    .byte   3   // 0xd
    .byte   3   // 0xe
    .byte   3   // 0xf
    .byte   3   // 0x10
    .byte   3   // 0x11
    .byte   0xf // 0x12
    .byte   0xf // 0x13
    .byte   3   // 0x14    
    .byte   9   // 0x15
    .byte   9   // 0x16
    .byte   9   // 0x17
    .byte   3   // 0x18
    .byte   3   // 0x19
    .byte   3   // 0x1a
    .byte   3   // 0x1b
    .byte   0xb // 0x1c
    .byte   0xb // 0x1d
    .byte   0xb // 0x1e
    .byte   3   // 0x1f
#elif defined(_BCM96362_)
// 6362 Clock Speed Table
    .byte    3  // 0x0
    .byte    3  // 0x1
    .byte    3  // 0x2
    .byte    3  // 0x3
    .byte    3  // 0x4
    .byte    3  // 0x5
    .byte    3  // 0x6
    .byte    3  // 0x7
    .byte    3  // 0x8
    .byte    3  // 0x9
    .byte    3  // 0xa
    .byte    3  // 0xb
    .byte    3  // 0xc
    .byte    3  // 0xd
    .byte    3  // 0xe
    .byte    3  // 0xf
    .byte    3  // 0x10
    .byte    3  // 0x11
    .byte    3  // 0x12
    .byte    3  // 0x13
    .byte    4  // 0x14
    .byte    4  // 0x15
    .byte    15 // 0x16
    .byte    3  // 0x17
    .byte    3  // 0x18
    .byte    3  // 0x19
    .byte    3  // 0x1a
    .byte    3  // 0x1b
    .byte    3  // 0x1c
    .byte    3  // 0x1d
    .byte    3  // 0x1e
    .byte    3  // 0x1f
#elif defined(_BCM96816_)
// 6816 Clock Speed Table    
    .byte    3  // 0x0
    .byte    9  // 0x1
    .byte    3  // 0x2
    .byte    4  // 0x3
    .byte    3  // 0x4
    .byte    3  // 0x5
    .byte    9  // 0x6
    .byte    3  // 0x7
    .byte    3  // 0x8
    .byte    3  // 0x9
    .byte    2  // 0xa
    .byte    4  // 0xb
    .byte    2  // 0xc
    .byte    3  // 0xd
    .byte    4  // 0xe
    .byte    9  // 0xf
    .byte    1  // 0x10
    .byte    1  // 0x11
    .byte    1  // 0x12
    .byte    1  // 0x13
    .byte    3  // 0x14
    .byte    3  // 0x15
    .byte    4  // 0x16
    .byte    9  // 0x17
    .byte    6  // 0x18
    .byte    2  // 0x19
    .byte    6  // 0x1a
    .byte    2  // 0x1b
    .byte    4  // 0x1c
    .byte    3  // 0x1d
    .byte    4  // 0x1e
    .byte    3  // 0x1f
#endif

#if defined(_BCM96816_)
    .globl  periph_fix_table  
periph_fix_table:       // Periph Clock fix vector for different clock strap values
    _LONG_  0xAA30A047
#endif

#define SETLEDS1(a,b,c,d)                    \
        li      a0,(((a)<<24)|((b)<<16)|((c)<<8)|(d)) ; \
        bal     board_setleds; \
        nop
        
/*  *********************************************************************
    *  BOARD_EARLYINIT()
    *
    *  Initialize board registers.  This is the earliest
    *  time the BSP gets control.  This routine cannot assume that
    *  memory is operational, and therefore all code in this routine
    *  must run from registers only.  The $ra register must not
    *  be modified, as it contains the return address.
    *
    *  This routine will be called from uncached space, before
    *  the caches are initialized.  If you want to make
    *  subroutine calls from here, you must use the CALLKSEG1 macro.
    *
    *  Among other things, this is where the GPIO registers get
    *  programmed to make on-board LEDs function, or other startup
    *  that has to be done before anything will work.
    *
    *  Input parameters:
    *       a0 - Flash base address (address of MIPS reset)
    *
    *  Return value:
    *       nothing
    ********************************************************************* */

LEAF(board_earlyinit)

        .set    noreorder

        mfc0    t1, C0_BCM_CONFIG, 3
        li      t2, CP0_CMT_TPID
        and     t1, t2
        bnez    t1, 3f                  # if we are running on thread 1, skip init
        nop

        /**-------------------------------------------------------------**/
        /** platform specific code                                      **/
        /**-------------------------------------------------------------**/

#if defined(_BCM96816_)
        /**----- Offset UBUS Clock 180 degrees -------------------------**/
        li      t2, MISC_BASE
        lw      t1, MISC_STRAP_BUS(t2)

        srl     t1, MISC_STRAP_BUS_MIPS_PLL_FVCO_SHIFT
        andi    t1, 0x1f                      // Mask out strap bits

        move    s0,ra
        LOADREL(t2, periph_fix_table)
        lw      t2, 0(t2)
        srl     t2, t1
        andi    t2, 1
        move    ra,s0

        beq     t2, zero, 2f
        nop
        
        li      t0, DDR_BASE
        li      t1, 0x00070000                // send new phase value to PLL every 8 cycles
        sw      t1, DDR_CTL_PI_UBUS_CTL(t0)
        ori     t1, 0x4080                    // shift 128 steps in the positive direction to be 180 degree's offset VCO 1.6GHz
        sw      t1, DDR_CTL_PI_UBUS_CTL(t0)
        li      t1, 0x00000001
        sw      t1, DDR_CTL_PI_GCF(t0)
        
        li      t1, 0x10000                   // delay
1:
        bnez    t1, 1b
        addi    t1, -1

2:
        /**----- Set 1.2V and 2.5V Voltage regulators ------------------**/
        li      t1, GPIO_BASE
        lw      t2, GPIO_SWREG_CONFIG(t1)
        and     t2, ~GPIO_SW_VREG_SEL_MASK
        or      t2, (0x0 << GPIO_SW_VREG_SEL_SHIFT)
        and     t2, ~GPIO_LIN_VREG_ADJ_MASK
        or      t2, (0x0 << GPIO_LIN_VREG_ADJ_SHIFT)
        sw      t2, GPIO_SWREG_CONFIG(t1)

        li      t1, 0x10000
1:
        bnez    t1, 1b
        addi    t1, -1
#endif

#if defined (_BCM96328_) || defined (_BCM96362_)
        /* slow down mips clk (div 4) to unlock memory */
        mfc0    t1, C0_BCM_CONFIG, 5
        or      t2, t1, 0x40000000
        mtc0    t2, C0_BCM_CONFIG, 5
        nop

        mtc0    t1, C0_BCM_CONFIG, 5
        nop
#endif

#if defined (_BCM96362_)
        /* Adjust VREG frequency up by 50% to improve DSL performance */
        li      t2, MISC_BASE

        /* First set ramp control */
        lw      t1, MISC_VREG_CONTROL0(t2)
        or      t1, (0x2 << MISC_VREG_CONTROL0_VREG_RAMP1P8_SHIFT) | (0x2 << MISC_VREG_CONTROL0_VREG_RAMP1P2_SHIFT)
        sw      t1, MISC_VREG_CONTROL0(t2)

        /* wait 10ms for the setting to take effect */
        li      t8, -2000000
        mtc0    t8, C0_COUNT
1:
        bltz    t8, 1b
        mfc0    t8, C0_COUNT

        and     t1, ~((0x7 << MISC_VREG_CONTROL0_VREG_RAMP1P8_SHIFT) | (0x7 << MISC_VREG_CONTROL0_VREG_RAMP1P2_SHIFT))
        or      t1, (0x3 << MISC_VREG_CONTROL0_VREG_ADJ_SHIFT)
        sw      t1, MISC_VREG_CONTROL0(t2)
        
        /* Increase 2.5V regulator to provide increased range for 1.8V */
        lw      t1, MISC_VREG_CONTROL1(t2)
        and     t1, ~MISC_VREG_CONTROL1_VREG_ISEL2P5_MASK
        or      t1, (MISC_VREG_LDO_2P61 << MISC_VREG_CONTROL1_VREG_ISEL2P5_SHIFT)
        sw      t1, MISC_VREG_CONTROL1(t2)
#endif

#if 0
        /* wait for a while to allow catch by jtag debugger */
        li      t8, -(200000000*3)      /* we will count up to 0 to delay a couple of seconds */
                                        /* and give the emulator a chance to catch us */
        mtc0    t8, C0_COUNT
catchloop:
        bltz    t8, catchloop
        mfc0    t8, C0_COUNT
#endif

        /**----- Enable I Cache -----------------------------------------**/
        mfc0    t1, C0_BCM_CONFIG
        or      t1, (CP0_BCM_CFG_ICSHEN | CP0_BCM_CFG_DCSHEN)
        mtc0    t1, C0_BCM_CONFIG       # Enable I Cache

        // In the begining MIPS core registers are mapped to 0xbfax_xxxx
        li      t1, 0x1FA0000C          # Set up CBR to 1FAx_xxxx
        mtc0    t1, C0_BCM_CONFIG, 6

        li      t1, MIPS_BASE_BOOT
        lw      t2, MIPS_LMB_CR(t1)
        or      t2, 0xC0000000          # enable ffxx_xxxx space
        sw      t2, MIPS_LMB_CR(t1)
        li      t2, 0xFFF80001          # SBR FFF8_xxxx and enable
        sw      t2, MIPS_SBR(t1)

        // Now map MIPS core registers to 0xFF4x_xxxx space
        li      t1, 0xFF40000C          # CBR FF4x_xxxx (and reserved bits 0xc).
        mtc0    t1, C0_BCM_CONFIG, 6

#if defined(_BCM96816_)
        /**----- Initialize EBI -----------------------------------------**/
        li      t1, MPI_BASE
        li      t2, EBI_SIZE_32M
        or      t2, a0
        sw      t2, CS0BASE(t1)         # CS[0] Base
        li      t2, THREEWT|EBI_WORD_WIDE|EBI_ENABLE
        sw      t2, CS0CNTL(t1)         # CS[0] Control
#endif

        /**----- Initialize Serial --------------------------------------**/
        li      t3, ((FPERIPH / 115200) / 16)
        /*
        #  Baudword = (FPeriph)/Baud/32-1.  We have to perform rounding
        #  and subtraction.  Above we divided by 16 (instead of 32).  If
        #  bit0 is set, we round up.  However, we then subtract 1, so final
        #  result should be t3/2.  If bit0 is 0, then we truncate and subtract
        #  1, t3=t3/2-1.
        */
        andi    t0, t3, 0x1
        bne     t0,zero,1f              # do shift only (in delay slot)
                                        # and jump to apply
        srl     t3,1                    # do divide by 2
        addiu   t3, -1                  # subtract 1
1:

        // t3 contains the UART BAUDWORD
        li      t0, UART_BASE
        sw      t3, UART0BAUD(t0)       # Store BaudRate
        li      t1, BITS8SYM|ONESTOP
        sb      t1, UART0CONFIG(t0)     # 8 Bits/1 Stop
        li      t1, TXEN|RXEN|BRGEN
        sb      t1, UART0CONTROL(t0)    # Enable, No Parity
        move    t1, zero
        sh      t1, UART0INTMASK(t0)

        .set    reorder
3:
        j       ra
END(board_earlyinit)

/*  *********************************************************************
    *  BOARD_DRAMINFO
    *
    *  Return the address of the DRAM information table
    *
    *  Input parameters:
    *       nothing
    *
    *  Return value:
    *       v0 - DRAM info table, return 0 to use default table
    ********************************************************************* */
LEAF(board_draminfo)
        j       ra
END(board_draminfo)
        
/*  *********************************************************************
    *  BOARD_DRAMINIT
    *
    *  This routine should activate memory.
    *
    *  Input parameters:
    *       None
    *
    *  Return value:
    *       None
    *
    *  Registers used:
    *       can use all registers.
    ********************************************************************* */
LEAF(board_draminit)
        .set    noreorder

        move    s0,ra

/***** Load DDR Base *************************************/
        li      t0, DDR_BASE

/***** Disable Auto refresh ******************************/     
        li      t1, 0x10000
        sw      t1, DDR_CTL_CLKS(t0)
        SETLEDS1('-','-','-', '-')
        SETLEDS1('P','H','Y','S')

        li      t1, 0x10                // Loop count value
        li      t3, 1

zq_loop:
        sub     t1, t3                  // Decrement count by 1
        beq     t1, zero, zq_error
        nop

        sw      zero, PHY_CONTROL_REGS_ZQ_PVT_COMP_CTL(t0)      // Reset the calibration engine
        li      t4, (1 << 26)
        sw      t4, PHY_CONTROL_REGS_ZQ_PVT_COMP_CTL(t0)        // Start the calibration

        li      t2, 0x100               // Timeout value

wait_zq1:
        beq     t2, zero, zq_timeout
        nop

        lw      t4, PHY_CONTROL_REGS_ZQ_PVT_COMP_CTL(t0)        // Read the calibration result
        move    t5, t4                                          // Save calibration result1
        and     t4, (1 << 28)
        beq     t4, zero, wait_zq1
        sub     t2, t3

        sw      zero, PHY_CONTROL_REGS_ZQ_PVT_COMP_CTL(t0)      // Reset the calibration engine
        li      t4, (1 << 26)
        sw      t4, PHY_CONTROL_REGS_ZQ_PVT_COMP_CTL(t0)        // Start the calibration

        li      t2, 0x100               // Timeout value

wait_zq2:
        beq     t2, zero, zq_timeout
        nop

        lw      t4, PHY_CONTROL_REGS_ZQ_PVT_COMP_CTL(t0)        // Read the calibration result
        move    t6, t4                                          // Save calibration result1
        and     t4, (1 << 28)
        beq     t4, zero, wait_zq2
        sub     t2, t3

        sw      zero, PHY_CONTROL_REGS_ZQ_PVT_COMP_CTL(t0)      // Reset the calibration engine
        li      t4, (1 << 26)
        sw      t4, PHY_CONTROL_REGS_ZQ_PVT_COMP_CTL(t0)        // Start the calibration

        li      t2, 0x100               // Timeout value

wait_zq3:
        beq     t2, zero, zq_timeout
        nop

        lw      t4, PHY_CONTROL_REGS_ZQ_PVT_COMP_CTL(t0)        // Read the calibration result
        move    t7, t4                                          // Save calibration result1
        and     t4, (1 << 28)
        beq     t4, zero, wait_zq3
        sub     t2, t3

        bne     t5, t6, zq_loop
        nop
        bne     t5, t7, zq_loop
        nop
        bne     t6, t7, zq_loop
        nop

        b       zq_done;
        nop

zq_error:
        SETLEDS1('Z','Q','E','R')
        b       zq_done
        nop

zq_timeout:
        SETLEDS1('Z','Q','T','O')
        nop
zq_done:
        sw      zero, PHY_CONTROL_REGS_ZQ_PVT_COMP_CTL(t0)      // Reset the calibration engine

        SETLEDS1('Z','Q','D','N')
        nop

/****** Set control pad strength to half ********/
        lw      t2, PHY_CONTROL_REGS_DRIVE_PAD_CTL(t0)
        or      t2, 0x4
        sw      t2, PHY_CONTROL_REGS_DRIVE_PAD_CTL(t0)

#if defined(_BCM96816_)
/****** Disable byte lanes 2 and 3 ********/
        li        t1, 0x800fffff
        sw        t1, PHY_BYTE_LANE_2_IDLE_PAD_CONTROL(t0)
        sw        t1, PHY_BYTE_LANE_3_IDLE_PAD_CONTROL(t0)
#endif
        
/****** Disable byte lane 1 clock ********/
        li      t1, 1
        sw      t1, PHY_BYTE_LANE_1_CLOCK_PAD_DISABLE(t0)

ddr_x8:

/****** Change the slew and receiver power to full strength in the byte lanes ********/
        li      t1, 0xFFFFFFFC
        lw      t2, PHY_BYTE_LANE_0_DRIVE_PAD_CTL(t0)
        and     t2, t2, t1
        sw      t2, PHY_BYTE_LANE_0_DRIVE_PAD_CTL(t0)
        lw      t2, PHY_BYTE_LANE_1_DRIVE_PAD_CTL(t0)
        and     t2, t2, t1
        sw      t2, PHY_BYTE_LANE_1_DRIVE_PAD_CTL(t0)
    
/****** Hardware calibrate VDL *******/     
        li      t1, 3
        sw      t1, PHY_BYTE_LANE_0_VDL_CALIBRATE(t0)
        sw      t1, PHY_BYTE_LANE_1_VDL_CALIBRATE(t0)

        li      t1, 0x1000
1:  
        bnez    t1, 1b
        addi    t1, -1

/****** Check strap value to figure out 400MHz or 200Mhz DDR ******/
        li      t2, MISC_BASE
        lw      t1, MISC_STRAP_BUS(t2)

        srl     t1, MISC_STRAP_BUS_MIPS_PLL_FVCO_SHIFT
        andi    t1, 0x1f        // Mask out strap bits
    
        LOADREL(t2, dram_speed_table)
  
        add     t2, t1
        lb      t1, 0x00(t2)
        beq     t1, zero, ddr_clk_200
        li      t2, 1
        beq     t1, t2, ddr_clk_300
        nop
    
/****** Software override rd_en VDL and ADDRESS VDL ********/
ddr_clk_450:
ddr_clk_400:
        li      a1, 2       // Set speed to 400MHz (2)
        b       1f
        nop
ddr_clk_300:    
        li      a1, 1       // Set speed to 300MHz (1)

/* At higher frequencies set Read_en VDL value to calibrated VDL value + 10 */      
1:      li      t2, 0x10000 
        lw      t1, PHY_BYTE_LANE_0_VDL_STATUS(t0)
        srl     t1, 8
        addi    t1, 0xA
        or      t1, t1, t2
        sw      t1, PHY_BYTE_LANE_0_VDL_OVERRIDE_2(t0)
        lw      t1, PHY_BYTE_LANE_1_VDL_STATUS(t0)
        srl     t1, 8
        addi    t1, 0xA
        or      t1, t1, t2
        sw      t1, PHY_BYTE_LANE_1_VDL_OVERRIDE_2(t0)
        b       vdl_override_cont
        nop
    
ddr_clk_200:        
        li      a1, 0       // Set speed to 200MHz (0)
        li      t3, 0x1f    // Set maximum VDL step size for 200MHz
        b       1f
        nop

vdl_override_cont:  
        lw      t2, PHY_BYTE_LANE_0_VDL_STATUS(t0)
        andi    t3, t2, 0x1f00
        srl     t3, 8
        addi    t3, t3, 0x4 // Add Stepsize 4

1:      li      t1, 0x110000    // Enable & Force Override
        or      t1, t3      // Fine rise and fine fall are set to 0
    
        sw      t1, PHY_CONTROL_REGS_STATIC_VDL_OVERRIDE(t0)
        li      t1, 0x0c
        sw      t1, DDR_CTL_DCMD(t0)    // Set VDL

        SETLEDS1('P','H','Y','E')
    
/* Program MC Timing Registers

 Read each timing parameter based on the speed and then create the
 timing registers and program them. 
*/  
        LOADREL(t2, dram_timing_table)
  
        li      t1, 0x10    // size of dram_timing_table element
        mult    t1, a1
        mflo    t1          // dram_timing_table offset
        add     t2, t1

        move    t3, zero
        lb      t1, 0x00(t2)    // tRCD
        andi    t1, 0xf
        move    t3, t1
        lb      t1, 0x01(t2)    // tCL
        andi    t1, 0xf
    
        move    t4, t1
        and     t4, 0x7         // Make sure that only 3 bits are written to DRAM's tCL field
        sll     t4, 20
        sw      t4, DDR_CTL_DMODE_0 (t0)   // Write tCL to the MRS register holder

        sll     t1, 4
        or      t3, t1
        lb      t1, 0x02(t2)    // tWR
        andi    t1, 0xf
    
        // Here we create the MRS register values
        move    t4, t1
        li      t5, 1
        subu    t4, t5            // tWR written to DRAM is 1 less than real tWR value
        andi    t4, 0x7
        sll     t4, 25
        li      t5, 0x01030000    // Sequential burst mode, burst of 8, reset DLL
        or      t4, t5
        lw      t5, DDR_CTL_DMODE_0 (t0)  
        or      t5, t4
        sw      t5, DDR_CTL_DMODE_0 (t0)  // Add tWR to the MRS register holder
    
        sll     t1, 9
        or      t3, t1
        lb      t1, 0x03(t2)    // tWL
        andi    t1, 0xf
        sll     t1, 12
        or      t3, t1
        lb      t1, 0x04(t2)    // tRP
        andi    t1, 0xf
        sll     t1, 16
        or      t3, t1
        lb      t1, 0x05(t2)    // tRRD
        andi    t1, 0xf
        sll     t1, 20
        or      t3, t1
        lb      t1, 0x06(t2)    // tRC
        andi    t1, 0x1f
        sll     t1, 24      // tRCw
        or      t3, t1
        sw      t3, DDR_CTL_TIM1_0(t0)  // Program TIM1_0 register

        move    t3, zero
        lb      t1, 0x06(t2)    // tRC
        andi    t1, 0x1f
        or      t3, t1      // tRCr
        lb      t1, 0x07(t2)    // tFAW
        andi    t1, 0x3f
        sll     t1, 8
        or      t3, t1
        li      t1, 0xff    // tRFC = 0xff (Set to max value first.
        sll     t1, 16      // We'll fix it after we determine dram size)
        or      t3, t1

        // We skip tFIFO since it needs to be 0

        lb      t1, 0x08(t2)    // tW2R
        andi    t1, 0x3
        sll     t1, 26
        or      t3, t1
        lb      t1, 0x09(t2)    // tR2W
        andi    t1, 0x3
        sll     t1, 28
        or      t3, t1
        lb      t1, 0x0a(t2)    // tR2R
        andi    t1, 0x1
        sll     t1, 30
        or      t3, t1
        sw      t3, DDR_CTL_TIM1_1(t0)  // Program TIM1_1 register
    
        move    t3, zero
        lb      t1, 0x0b(t2)    // tAL
        andi    t1, 0xf
    
        // Here we create the EMRS register values
        move    t4, t1
        andi    t4, 0x7
        sll     t4, 3
        li      t5, 0x384   // RTT=75ohm, OCD Enter
        or      t4, t5
        lw      t5, DDR_CTL_DMODE_0 (t0)  
        or      t5, t4
        sw      t5, DDR_CTL_DMODE_0 (t0)  // Store required values in EMRS holding register

        or      t3, t1
        lb      t1, 0x0c(t2)    // tRTP
        andi    t1, 0x7
        sll     t1, 4
        or      t3, t1
        lb      t1, 0x0d(t2)    // tW2W
        andi    t1, 0x3
        sll     t1, 8
        or      t3, t1
        sw      t3, DDR_CTL_TIM2(t0)    // Program TIM2 register
    
/*
// (tRRD is incremented by 1 due to tFAW bug for >=1Gb devices)
        li      t1, 0x18564c55  // tRCD=5,tWR=6,tCL=5,tWL=4,tRP=6,tRRD=5,tRCw=0x18
//      li      t1, 0x13554a55  // tRCD=5,tWR=5,tCL=5,tWL=4,tRP=5,tRRD=5,tRCw=0x13
        sw      t1, DDR_CTL_TIM1_0(t0)
        li      t1, 0x18330018  // tR2W=1,tR2R=0,tW2R=2,tFIFO=0,tRFC=0x33,tFAW=0,tRCr=0x18
//      li      t1, 0x182b0013  // tR2W=1,tR2R=0,tW2R=2,tFIFO=0,tRFC=0x2b,tFAW=0,tRCr=0x13
        sw      t1, DDR_CTL_TIM1_1(t0)
        li      t1, 0x00000034  // tAL=4 (tRCD-1), tRTP=3, tW2W=0
        sw      t1, DDR_CTL_TIM2(t0)
*/

// Set x16 mode and Page policy
        li      t1, 0x100
        sw      t1, DDR_CTL_DMODE_1(t0)
        
// Enable ODT for writes
        li      t1, 0x104
        sw      t1, DDR_CTL_ODT(t0)
        
/***** Turn on CKE ***************/
        li      t1, 0x35
        sw      t1, DDR_CTL_DCMD(t0)

        li      t1, 0x200
2:  
        bnez    t1, 2b
        addi    t1, -1
        
/***** Set arbitor for Burst Round Robin Mode ***/
        lw      t1, DDR_CTL_ARB(t0)
        or      t1, 4 << 16
        sw      t1, DDR_CTL_ARB(t0)

/***** Issue Precharge All Banks Command ***/
        li      t1, 0x32
        sw      t1, DDR_CTL_DCMD(t0)
        
/***** Issue EMRS2 Command ***/
        li      t1, 0x0
        lw      t2, DDR_CTL_DMODE_0 (t0)   // Load previous value to t2 to preserve it
        nop
        sw      t1, DDR_CTL_DMODE_0 (t0)
        li      t1, 0x38
        sw      t1, DDR_CTL_DCMD(t0)
        
/***** Issue EMRS3 Command***/
        li      t1, 0x0
        sw      t1, DDR_CTL_DMODE_0 (t0)
        li      t1, 0x39
        sw      t1, DDR_CTL_DCMD(t0)
        
// Enable DLL by issuing EMRS Command
//      li      t1, 0x0
//      sw      t1, DDR_CTL_DMODE_0 (t0)
        sw      t2, DDR_CTL_DMODE_0 (t0)  // Use the saved value back to DMODE_0 register
        li      t1, 0x30
        sw      t1, DDR_CTL_DCMD(t0)
        
/*  
// Issue MRS Command tCL=5 tWL=4
        li      t1, 0x0b530000
//      li      t1, 0x07530000          // tCL=5 tWR=5, Reset DLL, Sequential Mode, Burst Length = 8
        sw      t1, DDR_CTL_DMODE_0 (t0)
        li      t1, 0x31
        sw      t1, DDR_CTL_DCMD(t0)
*/
// Issue MRS Command. Set Reset DLL bit
        li      t1, 0x31
        sw      t1, DDR_CTL_DCMD(t0)
        
// Issue Precharge All Banks Command
        li      t1, 0x32
        sw      t1, DDR_CTL_DCMD(t0)
        
// Issue Autorefresh Command
        li      t1, 0x33
        sw      t1, DDR_CTL_DCMD(t0)
        li      t1, 0x33
        sw      t1, DDR_CTL_DCMD(t0)

        li      t1, 0x200
3:  
        bnez    t1, 3b
        addi    t1, -1
        
/*
// Clear DLL Reset by Issuing MRS Command, tCL=5 tWL=4
        li      t1, 0x0a530000
        sw      t1, DDR_CTL_DMODE_0 (t0)
        li      t1, 0x31
        sw      t1, DDR_CTL_DCMD(t0)
*/
    
// Issue MRS Command w/ DLL Reset bit set to 0
        lw      t1, DDR_CTL_DMODE_0 (t0)
        li      t2, 0xFEFFFFFF          // Reset DLL reset bit
        and     t1, t2
        sw      t1, DDR_CTL_DMODE_0 (t0)
        li      t1, 0x31
        sw      t1, DDR_CTL_DCMD(t0)
    
/*  
// Issue EMRS Command (Enter OCD Calibration)
        li      t1, 0x380
        sw      t1, DDR_CTL_DMODE_0 (t0)
        li      t1, 0x30
        sw      t1, DDR_CTL_DCMD(t0)
*/
    
// Issue EMRS Command (Enter OCD Calibration)  75 Ohm, Full strength Drive, tAL=tRCD-1
        li      t1, 0x30
        sw      t1, DDR_CTL_DCMD(t0)
        
// Issue EMRS Command (Exit OCD Calibration)
        lw      t1, DDR_CTL_DMODE_0 (t0)
        li      t2, 0xFC7F          // Reset OCD field for exit mode
        and     t1, t2
        sw      t1, DDR_CTL_DMODE_0 (t0)
        li      t1, 0x30
        sw      t1, DDR_CTL_DCMD(t0)
        
// Check x8 or x16 DDR
        li      t1, 0x12345678
        li      t3, 0xA0000000
        sw      zero, 0(t3)
        sw      t1, 0(t3)
        lw      t2, 0(t3)
        bne     t1, t2, 3f  // Failed
        nop
// Do the test twice. Just in case random values match...
        li      t1, 0x87654321
        sw      t1, 0(t3)
        lw      t2, 0(t3)
        beq     t1, t2, 1f  // Clock lines are enabled as needed
        nop
        
3:
// Memory test failed. Need to re-enable byte lane 1 clock
        sw      zero, PHY_BYTE_LANE_1_CLOCK_PAD_DISABLE(t0)
        li      t1, 0x200
2:  
        bnez    t1, 2b      // Delay after enabling clocks
        addi    t1, -1
        b       ddr_x8
        nop        

1:
// Find memory size. a3 keeps the size: 0=256Mb, 1=512Mb, 2=1Gb, 3=2Gb
// Start from 2Gb device
        LOADREL(t7, dram_map_table_x8)
        lw      t1, PHY_BYTE_LANE_1_CLOCK_PAD_DISABLE(t0)
        beqz    t1, 1f
        nop
        LOADREL(t7, dram_map_table_x16)
        
1:
        li      a3, 4
ddr_size:
        addi    a3, -1
  
        li      t1, 0x2c        // size of dram_map_table element
        mult    t1, a3
        mflo    t2              // dram_map_table offset
        add     t2, t7

        lw      t1, 0x00(t2)        // Row00_0
        sw      t1, DDR_CTL_ROW00_0(t0)

        lw      t1, 0x04(t2)        // Row00_1
        sw      t1, DDR_CTL_ROW00_1(t0)

        lw      t1, 0x08(t2)        // Row01_0
        sw      t1, DDR_CTL_ROW01_0(t0)

        lw      t1, 0x0C(t2)        // Row01_1
        sw      t1, DDR_CTL_ROW01_1(t0)

        lw      t1, 0x10(t2)        // Col00_0
        sw      t1, DDR_CTL_COL00_0(t0)

        lw      t1, 0x14(t2)        // Col00_1
        sw      t1, DDR_CTL_COL00_1(t0)

        lw      t1, 0x18(t2)        // Col01_0
        sw      t1, DDR_CTL_COL01_0(t0)

        lw      t1, 0x1C(t2)        // Col01_1
        sw      t1, DDR_CTL_COL01_1(t0)

        lw      t1, 0x20(t2)        // Bank
        sw      t1, DDR_CTL_BNK10(t0)

        li      t1, 0x0         // CS_Start
        sw      t1, DDR_CTL_CSST(t0)

        lw      t1, 0x24(t2)        // CS_End
        sw      t1, DDR_CTL_CSEND(t0)

        li      t1, 0x0         // CS Interleaving CFG
        sw      t1, DDR_CTL_CNFG(t0)

        lw      t3, 0x28(t2)        // Dram Size
        lw      t1, DDR_CTL_GCFG(t0)    // GCFG
        li      t2, 0xFFFFFF00
        and     t1, t1, t2      // Mask out Dram Size Fields
        or      t1, t3, t1      // insert new DRAM Size value
        sw      t1, DDR_CTL_GCFG(t0)
        beqz    a3, ddr_speed
        nop

// Check for memory aliasing
// This assumes that the bank interleaving is below address bits 8.
        li      t1, 8
        li      t2, 24
        add     t2, a3      // Scan up to maximum memory size
        li      t3, 0xA0000000

check_alias:        
        li      t4, 1
        sll     t4, t1
        add     t4, t3
        
        sw      zero, 0(t3)
        li      t5, -1
        sw      t5, 0(t4)
        lw      t6, 0(t3)
        
        beq     t5, t6, ddr_size
        nop
        
        bne     t1, t2, check_alias
        addi    t1, 1
    
ddr_speed:
        li      t1, 3
        lw      t2, PHY_BYTE_LANE_1_CLOCK_PAD_DISABLE(t0)
        beqz    t2, 1f
        nop
        li      t1, 2
1:
        blt     a3, t1, tRefi_update    // If smaller than 1Gb device, keep tRRD same
        nop
        
        lw      t1, DDR_CTL_TIM1_0(t0)  // Load DDR_CTL_TIM1_0 register
        move    t2, t1
        srl     t2, 20
        andi    t2, 0xf
        addi    t2, 1           // Increment ttRRD by one (software work around for a bug)
        sll     t2, 20
        li      t3, 0xFF0FFFFF
        and     t1, t3          // Clear tRRD field
        or      t1, t2          // Insert the new tRRD value
        sw      t1, DDR_CTL_TIM1_0(t0)  // Store DDR_CTL_TIM1_0 register
        nop

tRefi_update:   
        LOADREL(t2, dram_tRefi_table)

        li      t1, 0x4         // size of dram_tRefi_table entry
        mult    t1, a1
        mflo    t1              // dram_tRefi_table offset
        add     t2, t1

        lw      t1, 0x0(t2)     // tRefi
        sll     t1, 8
        sw      t1, DDR_CTL_CLKS(t0)
    
        LOADREL(t2, dram_tRFC_table)
  
        li      t1, 0x4         // size of dram_tRFC_table entry
        mult    t1, a1
        mflo    t1              // dram_tRefi_table offset
        add     t2, t1          // Calculate address in the row

        li      t1, 0xc         // size of dram_tRFC_table row
        mult    t1, a3
        mflo    t1              // dram_tRefi_table offset
        add     t2, t1          // Calculate address in the column

        lw      t3, 0x0(t2)     // Load tRFC value
        andi    t3, 0xFF        // Mask tRFC to 8-bits
        sll     t3, 16          // Move it to bit location [23:16]

        lw      t1, DDR_CTL_TIM1_1(t0)  // Load DDR_CTL_TIM1_1 register
        li      t2, 0xFF00FFFF
        and     t1, t1, t2      // Mask out tRFC Field
        or      t1, t3, t1      // insert new tRFC value
        sw      t1, DDR_CTL_TIM1_1(t0)  // Write to TIM1_1 register

        SETLEDS1('D','I','N','T')
    
/***** UBUS align to MEMC *****/
align_memc: 

/*** check MEMC clk ratio to set sampling freq *****/
        li      t2, MISC_BASE
        lw      t1, MISC_STRAP_BUS(t2)

        srl     t1, MISC_STRAP_BUS_MIPS_PLL_FVCO_SHIFT
        andi    t1, 0x1f        // Mask out strap bits

#if defined(_BCM96816_)
        LOADREL(t2, periph_fix_table)
        lw      t2, 0(t2)
        srl     t2, t1
        andi    t2, 1
        bne     t2, zero, 2f
#endif

        LOADREL(t2, memc_ubus_ratio_table)

        add     t2, t1
        lb      t1, 0x00(t2)
        sll     t4,t1,28               // Update Sampling Period Field

pi_ubus:    
        li      t2, 4048
        li      t3, 1

        sw      zero, DDR_CTL_PI_GCF(t0)
        li      t1, 0x00001c0b          // send feedback command every 11 cycles 
        or      t1, t1, t4
        sw      t1, DDR_CTL_PI_UBUS_SMPL(t0)
        ori     t1, 0x80            // trigger stoke signal to latch in new counter value
        sw      t1, DDR_CTL_PI_UBUS_SMPL(t0)
        li      t1, 0x00040000          // send new phase value to PLL every 5 cycles
        sw      t1, DDR_CTL_PI_UBUS_CTL(t0)
        li      t1, 0x00000001          // enable enable counter that change PLL phase
        sw      t1, DDR_CTL_PI_GCF(t0)
        nop

#if defined (_BCM96328_) || defined(_BCM96362_)
// Enable PHY MIPS PI
        li      t1, 0x00130000
        sw      t1, DDR_CTL_PI_DSL_MIPS_CTL(t0)
        nop
#endif
        
1:
        lw      t4, DDR_CTL_PI_UBUS_SMPL(t0)    // Read a sample value.
        srl     t4, 16                  // The sample is in the upper 16 bits.

        andi    t4, t4, 0x22            // Look at the 2 outermost bits; if the LSB is 0 and the MSB is 1,
        beq     t4, 0x20, 2f            // then there is an edge somewhere in the sample.
        nop
        lw      t5, DDR_CTL_PI_UBUS_CTL(t0)
        and     t5, 0xffff0000
        or      t5, t3
        or      t5, (1<<14)         // move phase in positive direction
        sw      t5, DDR_CTL_PI_UBUS_CTL(t0)
        nop
        lw      t5, DDR_CTL_PI_UBUS_CTL(t0) // Delay before reading another sample.
        add     t3, 1
        bne     t2, t3, 1b
        nop

        SETLEDS1('U','A','S','Y')
        SETLEDS1('L','A','S','Y')
        SETLEDS1('M','A','S','Y')
        b       run_async               // failed to set sync mode
        nop 

2:
/*** check MIPS clk ratio *****/
        li      t4, 0xff410010
        li      t2, MISC_BASE
        lw      t1, MISC_STRAP_BUS(t2)

        srl     t1, MISC_STRAP_BUS_MIPS_PLL_FVCO_SHIFT
        andi    t1, 0x1f                // Mask out strap bits

        b       _mips_align_2_ubus      // none of the above means it is either 1/1 or 2/1
        nop


_mips_align_2_ubus:
        
/***** MIPS align to UBUS *****/
        li      t1, 0xff410000

        li      t2, 0x00040000      // update PLL phase value every 5 MC cycles
        sw      t2, DDR_CTL_PI_MIPS_CTL(t0)

        li      t2, 0xc0001c03      // force update on mclk_period
        sw      t2, 0x40(t1)
        li      t2, 0x80001c83
        sw      t2, 0x40(t1)

        lw      t2, DDR_CTL_PI_MIPS_CTL(t0)     // add delay
        li      t3, 1
    
1:      lw      t4, 0x40(t1)    // Read a sample value.
        srl     t4, 16          // The sample is in the upper 16 bits.

        andi    t4, t4, 0x22    // Look at the 2 outermost bits, if the LSB is 0 and the MSB is 1,
        beq     t4, 0x20, 2f    // then there is an edge somewhere in the sample.
        nop
        lw      t5, DDR_CTL_PI_MIPS_CTL(t0)
        and     t5, 0xffff0000
        or      t5, t3
        or      t5, (1<<14) // move phase in positive direction
        sw      t5, DDR_CTL_PI_MIPS_CTL(t0)
        nop
        lw      t5, DDR_CTL_PI_MIPS_CTL(t0) // Delay before reading another sample.
        add     t3, 1
        bne     t2, t3, 1b
        nop
        
        SETLEDS1('U','A','S','Y')
        SETLEDS1('L','A','S','Y')
        SETLEDS1('M','A','S','Y')
        b       run_async           // failed to set sync mode
        nop 
2:  
        // Success
        lw      t2, DDR_CTL_PI_MIPS_CTL(t0) // Turn on auto-PI mode.
        or      t2, (1 << 20)       // PI_UBUS_CTL_Hw_Cntr_En = 1
        sw      t2, DDR_CTL_PI_MIPS_CTL(t0)

        /**----- Enable DDR/UBUS and DDR/LMB sync mode ------------------**/
        /*** only MIPS in sync mode for these strap options */
    
        li      t2, MISC_BASE
        lw      t1, MISC_STRAP_BUS(t2)

        srl     t1, MISC_STRAP_BUS_MIPS_PLL_FVCO_SHIFT
        andi    t1, 0x1f        // Mask out strap bits
    
        li      t3, 0           // Used to keep MC/MIPS and MC/UBUS sync/async mode
    
        LOADREL(t2, dram_sync_table)

        lw      t4, 0x0(t2)     // Ubus Sync vector
        lw      t5, 0x4(t2)     // Lmb Sync vector

#if defined (_96816_)
        move    t4, t6
#endif
        lw      t6, 0x8(t2)     // Mips Sync vector

        srl     t4, t1          // Get the ubus sync flag
        srl     t5, t1          // Get the lmb sync flag
        srl     t6, t1          // Get the mips sync flag
        andi    t4, 1           // Mask out other bits
        andi    t5, 1           // Mask out other bits
        andi    t6, 1           // Mask out other bits
    
        li      t2, 1
        bne     t4, t2, ubus_async  // 1f below. If ubus is not sync, lmb can't be in sync
        nop                 // Go to ubus_async (1f)

        // Ubus Sync Mode. Turn on Ubus Clock tracking
        lw      t2, DDR_CTL_PI_UBUS_CTL(t0) // Turn on auto-PI mode.
        or      t2, (1 << 20)       // PI_UBUS_CTL_Hw_Cntr_En = 1
        sw      t2, DDR_CTL_PI_UBUS_CTL(t0)

        // Is LMB Sync as well?
        li      t2, 1
        beq     t5, t2, memc_all_sync   // If both ubus and lmb are sync, go to memc_all_sync
        nop
    
memc_ubus_sync: 
#if UBUS_SYNC_ENABLE
// set MC/UBUS to SYNC          
        li      t3, 0x4
        SETLEDS1('L','A','S','Y')
        SETLEDS1('U','S','Y','N')
#else
// set MC/UBUS to ASYNC
        li      t3, 0xc
        SETLEDS1('L','A','S','Y')
        SETLEDS1('U','A','S','Y')
#endif

        b       update_sync_mode
        nop

memc_all_sync:  
#if LMB_SYNC_ENABLE
// set MC/MIPS to SYNC
        nop
        SETLEDS1('L','S','Y','N')
#else
// set MC/MIPS to ASYNC
        li      t1, 0x4
        or      t3, t1
        SETLEDS1('L','A','S','Y')
#endif
#if UBUS_SYNC_ENABLE
// set MC/UBUS to SYNC          
        nop
        SETLEDS1('U','S','Y','N')
#else
// set MC/UBUS to ASYNC
        li      t1, 0x8
        or      t3, t1
        SETLEDS1('U','A','S','Y')
#endif

update_sync_mode:
        li      t1, MISC_BASE
        sw      t3, MISC_MEMC_CONTROL(t1)
        b       2f  
        nop
1:  
ubus_async: 
        SETLEDS1('L','A','S','Y')
        SETLEDS1('U','A','S','Y')

2:
        // Can Mips run in sync mode? If not, skip all below
        beq     t6, zero, run_async
        nop
    
#if MIPS_SYNC_ENABLE
        /**----- Clear MIPS Async mode bit ------------------------------**/
        mfc0    t1, C0_BCM_CONFIG, 5
        and     t1, ~(0x1 << 28)
        mtc0    t1, C0_BCM_CONFIG, 5

        SETLEDS1('M','S','Y','N')
#else
        SETLEDS1('M','A','S','Y')
#endif

3:
        /**----- Enable RAC and LMB -------------------------------------**/
        li      t0, MIPS_BASE
        lw      t2, MIPS_LMB_CR(t0)
#if LMB_ENABLE
        or      t2, LMB_EN              // Enable LMB
        SETLEDS1('L','M','B','E')
#else
        SETLEDS1('L','M','B','D')
#endif
        sw      t2, MIPS_LMB_CR(t0)

        li      t2, 0xFFF << RAC_UPB_SHFT       // Enable prefetch for RAM address range up to 256MB
        sw      t2, MIPS_RAC_ARR(t0)

        lw      t2, MIPS_RAC_CR0(t0)
        or      t2, (RAC_C_INV | RAC_I | RAC_PF_I)
        sw      t2, MIPS_RAC_CR0(t0)

        lw      t2, MIPS_RAC_CR1(t0)
        or      t2, (RAC_C_INV | RAC_I | RAC_PF_I)
        sw      t2, MIPS_RAC_CR1(t0)

run_async:
        /**----- Enable branch prediction and non-blocking data cache ---**/
        mfc0    t1, C0_BCM_CONFIG
        and     t1, ~CP0_BCM_CFG_BTHD
        or      t1, CP0_BCM_CFG_NBK
        or      t1, CP0_BCM_CFG_CLF
        mtc0    t1, C0_BCM_CONFIG

#if DDR_TEST
#######################################
# Run test on DRAM using Test Engine  #
#######################################
#define BACKGND_MODE_DATA       0x0
#define BACKGND_MODE_LFSR       0x400
#define BACKGND_MODE_PRBS       0x800
#define BACKGND_MODE_NOT_PRBS   0xc00
#define BACKGND_MODE_PAT        0x1000
#define BACKGND_MODE_NOT_PAT    0x1400

#define VICT_MODE_PRBS          0
#define VICT_MODE_NOT_PRBS      0x100
#define VICT_MODE_PAT           0x200
#define VICT_MODE_NOT_PAT       0x300

#define VICT_ENABLE             0x8000
#define VICT_SWEEP_ENABLE       0x10000
#define VICT_COUNT              0x0

#define PRBS_ORDER(x)           ((x & 0x3) << 13)

#define TEST_COUNT              0x1000
#define TEST_ADDR               0x0
#define TEST_ADDR_UPDT          0x1
#define TEST_PATTERN0           0x5555
#define TEST_PATTERN1           0xaaaa
#define TEST_PATTERN            (TEST_PATTERN1 << 16 | TEST_PATTERN0)

#define TEST_ENABLE             0x1
#define TEST_DONE               0x2
#define TEST_ERROR              0x4
#define TEST_WRITE              0x10
#define TEST_READ               0x0

#define BACKGND_DATA0           0xa5a5a5a5
#define BACKGND_DATA1           0x5a5a5a5a
#define BACKGND_DATA2           0xa5a5a5a5
#define BACKGND_DATA3           0x5a5a5a5a

#define TEST_DATA0              0x02468ace
#define TEST_DATA1              0x13579bdf
#define TEST_DATA2              0x33cccc33
#define TEST_DATA3              0x55aaaa55

/***** Load DDR Base *************************************/
        li      t0, DDR_BASE
        li      t1, BACKGND_DATA0
        sw      t1, DDR_CTL_TEST_DATA0(t0)  
        li      t1, BACKGND_DATA1
        sw      t1, DDR_CTL_TEST_DATA1(t0)  
        li      t1, BACKGND_DATA2
        sw      t1, DDR_CTL_TEST_DATA2(t0)  
        li      t1, BACKGND_DATA3
        sw      t1, DDR_CTL_TEST_DATA3(t0)  
        li      t1, TEST_COUNT
        li      t2, VICT_COUNT
        or      t1, t2              # add victim count value
        sw      t1, DDR_CTL_TEST_COUNT(t0)  
        li      t1, TEST_ADDR
        sw      t1, DDR_CTL_TEST_ADDR(t0)   
        li      t1, TEST_ADDR_UPDT
        sw      t1, DDR_CTL_TEST_ADDR_UPDT(t0)  
        li      t1, TEST_PATTERN
        sw      t1, DDR_CTL_TEST_PAT(t0)
        
# Write a background pattern first
        li      t1, BACKGND_MODE_DATA
        li      t2, TEST_WRITE
        or      t1, t2
        li      t2, TEST_ENABLE
        or      t1, t2
        sw      t1, DDR_CTL_TEST_CFG1(t0)

        li      t2, 0x2
        li      t3, MISC_BASE

        li      t5, 0x10000
2:      li      t1, 0x100
1:      addiu   t1, -1
        bnez    t1, 1b
        nop

        lw      t4, MISC_MEMC_CONTROL(t3)
        and     t4, 0x2
        beq     t4, t2, backgnd_write_done
        nop
        addiu   t5, -1
        bnez    t5, 2b
        nop

# Background write operation is finished

backgnd_write_done:
        li      t1, TEST_DATA0
        sw      t1, DDR_CTL_TEST_DATA0(t0)  
        li      t1, TEST_DATA1
        sw      t1, DDR_CTL_TEST_DATA1(t0)  
        li      t1, TEST_DATA2
        sw      t1, DDR_CTL_TEST_DATA2(t0)  
        li      t1, TEST_DATA3
        sw      t1, DDR_CTL_TEST_DATA3(t0)  
        li      t1, TEST_COUNT
        li      t2, VICT_COUNT
        or      t1, t2              # add victim count value
        sw      t1, DDR_CTL_TEST_COUNT(t0)  
        li      t1, TEST_ADDR
        sw      t1, DDR_CTL_TEST_ADDR(t0)   
        li      t1, TEST_ADDR_UPDT
        sw      t1, DDR_CTL_TEST_ADDR_UPDT(t0)  
        li      t1, TEST_PATTERN
        sw      t1, DDR_CTL_TEST_PAT(t0)

#       li      t1, BACKGND_MODE_DATA
#       li      t1, BACKGND_MODE_PAT
#       li      t2, VICT_MODE_NOT_PAT
        li      t1, BACKGND_MODE_PRBS
        li      t2, VICT_MODE_NOT_PRBS
        
        or      t1, t2
        li      t2, VICT_ENABLE
        or      t1, t2
        li      t2, VICT_SWEEP_ENABLE
        or      t1, t2
        li      t2, PRBS_ORDER(0)
        or      t1, t2
        li      t2, TEST_WRITE
        or      t1, t2
        li      t2, TEST_ENABLE
        or      t1, t2
        sw      t1, DDR_CTL_TEST_CFG1(t0)

        li      t2, 0x2
        li      t3, MISC_BASE

        li      t5, 0x10000
2:      li      t1, 0x100
1:      addiu   t1, -1
        bnez    t1, 1b
        nop

        lw      t4, MISC_MEMC_CONTROL(t3)
        and     t4, 0x2
        beq     t4, t2, test_write_done
        nop
        addiu   t5, -1
        bnez    t5, 2b
        nop

# Test write operation is finished

test_write_done:
        li      t1, TEST_DATA0
        sw      t1, DDR_CTL_TEST_DATA0(t0)  
        li      t1, TEST_DATA1
        sw      t1, DDR_CTL_TEST_DATA1(t0)  
        li      t1, TEST_DATA2
        sw      t1, DDR_CTL_TEST_DATA2(t0)  
        li      t1, TEST_DATA3
        sw      t1, DDR_CTL_TEST_DATA3(t0)  
        li      t1, TEST_COUNT
        li      t2, VICT_COUNT
        or      t1, t2              # add victim count value
        sw      t1, DDR_CTL_TEST_COUNT(t0)  
        li      t1, TEST_ADDR
        sw      t1, DDR_CTL_TEST_ADDR(t0)   
        li      t1, TEST_ADDR_UPDT
        sw      t1, DDR_CTL_TEST_ADDR_UPDT(t0)  
        li      t1, TEST_PATTERN
        sw      t1, DDR_CTL_TEST_PAT(t0)

#       li      t1, BACKGND_MODE_DATA
#       li      t1, BACKGND_MODE_PAT
#       li      t2, VICT_MODE_NOT_PAT
        li      t1, BACKGND_MODE_PRBS
        li      t2, VICT_MODE_NOT_PRBS
        
        or      t1, t2
        li      t2, VICT_ENABLE
        or      t1, t2
        li      t2, VICT_SWEEP_ENABLE
        or      t1, t2
        li      t2, PRBS_ORDER(0)
        or      t1, t2
        li      t2, TEST_READ
        or      t1, t2
        li      t2, TEST_ENABLE
        or      t1, t2
        sw      t1, DDR_CTL_TEST_CFG1(t0)

        li      t3, MISC_BASE
        li      t2, 0x2

        li      t5, 0x10000
2:      li      t1, 0x100
1:      addiu   t1, -1
        bnez    t1, 1b
        nop

        lw      t4, MISC_MEMC_CONTROL(t3)
        and     t4, 0x2
        beq     t4, t2, test_read_done
        nop
        addiu   t5, -1
        bnez    t5, 2b
        nop

# Test read operation is finished

test_read_done:
        lw      t1, DDR_CTL_TEST_CFG1(t0)
        srl     t1, 2
        and     t1, 1
        beq     t1, zero, test_passed
        nop
        
test_failed:
        SETLEDS1('F','A','I','L')
        b   1f
        nop
test_passed:
        SETLEDS1('P','A','S','S')
1:
        SETLEDS1('-','-','-', '-')
#endif

        move    ra,s0
        j       ra
        nop
    
        .set    reorder

END(board_draminit)

/*  *********************************************************************
    *  BOARD_SETLEDS(x)
    *
    *  Set LEDs for boot-time progress indication.  Not used if
    *  the board does not have progress LEDs.  This routine
    *  must not call any other routines, since it may be invoked
    *  either from KSEG0 or KSEG1 and it may be invoked
    *  whether or not the icache is operational.
    *
    *  Input parameters:
    *       a0 - LED value (8 bits per character, 4 characters)
    *
    *  Return value:
    *       nothing
    *
    *  Registers used:
    *       t7,t8,t9
    ********************************************************************* */
LEAF(board_setleds)
#if 1
        li      t7, UART_BASE
        li      t8, TXFIFOEMT

1:      lh      t9, UART0INTSTAT(t7)
        and     t9, t8
        bne     t9, t8, 1b

        srl     t8, a0, 24
        sb      t8, UART0DATA(t7)
        srl     t8, a0, 16
        sb      t8, UART0DATA(t7)
        srl     t8, a0, 8
        sb      t8, UART0DATA(t7)
        sb      a0, UART0DATA(t7)
        li      a0, '\r'
        sb      a0, UART0DATA(t7)
        li      a0, '\n'
        sb      a0, UART0DATA(t7)
#endif
        j       ra
END(board_setleds)

/*  *********************************************************************
    *  BCMCORE_TP1_SWITCH()
    *
    *  Check if the thread switch is required. If we are already
    *  running on thread 1 this function will do nothing and just return
    *  If we are running on thread 0 this function will take thread 1
    *  out of reset and put thread 0 to sleep waiting for singnal from
    *  thread 1.
    *
    *  Input parameters:
    *       nothing
    *
    *  Return value:
    *       nothing
    ********************************************************************* */
LEAF(bcmcore_tp1_switch)

#if defined(_BCM96328_)
        li      t1, OTP_BASE
        addi    t1, OTP_USER_BITS
        addi    t1, 0xc - ((OTP_TP1_DISABLE_BIT / 8) & ~3)
        lw      t0, 0(t1)
        andi    t0, 1 << (OTP_TP1_DISABLE_BIT % 32)
        beqz    t0, 1f
        j       ra
1:
#endif
        mfc0    t1, C0_BCM_CONFIG, 3
        li      t2, CP0_CMT_TPID
        and     t1, t2
        bnez    t1, tp1                 # Already running on thread 1

# Start TP1
# Set boot address for TP1
        li      t1, MIPS_BASE
        li      t2, 0x98000000 | ENABLE_ALT_BV
        sw      t2, MIPS_TP1_ALT_BV(t1)

# Set a flag so we can wait for TP1 to catch up
        li      t1, 0x0
        mtc0    t1, $31                 # CO_DESAVE

# Take TP1 out of reset
        mfc0    t1, C0_BCM_CONFIG, 2
        or      t1, CP0_CMT_RSTSE
        mtc0    t1, C0_BCM_CONFIG, 2

        /* wait until second thread catches up with the first */
waittp1:
        mfc0    t0, $31                 # CO_DESAVE
        beqz    t0, waittp1

        li      t0, THREAD_NUM_ADDRESS
        FIXUP(t0)
        lw      t0, 0(t0)
        li      t1, 1
        bne     t0, t1, return          # Linux will run on TP0, continue running bootloader

# Voice will run on TP0. Set it up and put it to sleep

        # enable interrupts and enable SW IRQ 0
        li      t0, M_SR_IE | M_SR_IBIT1
        mtc0    t0, C0_SR

        # Set up to use alternate exception vector 0x80000200
        li      t0, M_CAUSE_IV
        mtc0    t0, C0_CAUSE

        mfc0    t1, C0_BCM_CONFIG, 1
        # set all ints except IRQ1 to TP1 and cross over SW IRQ 0
        or      t1, (CP0_CMT_XIR_4 | CP0_CMT_XIR_3 | CP0_CMT_XIR_2 | CP0_CMT_XIR_0 | CP0_CMT_SIR_0 | CP0_CMT_NMIR_TP1)
        mtc0    t1, C0_BCM_CONFIG, 1

        mfc0    t1, C0_BCM_CONFIG, 2
        # Set debug on TP1, give priority to TP0, and
        # set TLB exception serialization to ignore SCNT value in CP0 reg22 sel 4
        and     t1, ~CP0_CMT_TPS_MASK;
        or      t1, (CP0_CMT_DSU_TP1 | CP0_CMT_PRIO_TP0 | (1 << CP0_CMT_TPS_SHFT))
        mtc0    t1, C0_BCM_CONFIG, 2

        # Enable Data RAC on TP0
        li      t1, MIPS_BASE
        lw      t2, MIPS_RAC_CR0(t1)
        or      t2, (RAC_D | RAC_PF_D)
        sw      t2, MIPS_RAC_CR0(t1)

2:
        b       wait_for_wake

tp1:
# Running on TP1....
# First signal to TP0 that TP1 is up
        li      t1, 0x1
        mtc0    t1, $31                 # CO_DESAVE

        li      t0, THREAD_NUM_ADDRESS
        FIXUP(t0)
        lw      t0, 0(t0)
        li      t1, 1
        beq     t0, t1, return          # Linux will run on TP1, continue running bootloader

# Voice will run on TP1. Set it up and put it to sleep

        # enable interrupts and enable SW IRQ 0
        li      t0, M_SR_IE | M_SR_IBIT1
        mtc0    t0, C0_SR

        # Set up to use alternate exception vector 0x80000200
        li      t0, M_CAUSE_IV
        mtc0    t0, C0_CAUSE

        mfc0    t1, C0_BCM_CONFIG, 1
        # set IRQ1 to TP1 and cross over SW IRQ 0
        or      t1, (CP0_CMT_XIR_1 | CP0_CMT_SIR_0 | CP0_CMT_NMIR_TP0)
        mtc0    t1, C0_BCM_CONFIG, 1

        mfc0    t1, C0_BCM_CONFIG, 2
        # Set debug on TP0, give priority to TP1, and
        # set TLB exception serialization to ignore SCNT value in CP0 reg22 sel 4
        and     t1, ~CP0_CMT_TPS_MASK;
        or      t1, (CP0_CMT_PRIO_TP1 | (1 << CP0_CMT_TPS_SHFT))
        mtc0    t1, C0_BCM_CONFIG, 2

        # Enable Data RAC on TP1
        li      t1, MIPS_BASE
        lw      t2, MIPS_RAC_CR1(t1)
        or      t2, (RAC_D | RAC_PF_D)
        sw      t2, MIPS_RAC_CR1(t1)
        b       2b

return:
        j       ra

END(bcmcore_tp1_switch)

# align this code to cache line. NAND flash is not memory mapped after system boots
# so when we are signaling to the second TP to wake we need
# jal instruction to be in cache
        .align  4
LEAF(wait_for_wake)
        sync
        wait                            # wait for interrupt
        jal     t8                      # jump to entry point
END(wait_for_wake)