/*
 *  Memory sub-system initialization code for x210-ATRD board.
 *
 *  Copyright (c) 2003	Wolfgang Denk <wd@denx.de>
 *
 * See file CREDITS for list of people who contributed to this
 * project.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of
 * the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.	 See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 * MA 02111-1307 USA
 */
 /*********************************************************************
  *
  *  Copyright 2000,2001,2002,2003
  *  Broadcom Corporation. All rights reserved.
  *
  *  This software is furnished under license and may be used and
  *  copied only in accordance with the following terms and
  *  conditions.  Subject to these conditions, you may download,
  *  copy, install, use, modify and distribute modified or unmodified
  *  copies of this software in source and/or binary form.  No title
  *  or ownership is transferred hereby.
  *
  *  1) Any source code used, modified or distributed must reproduce
  *     and retain this copyright notice and list of conditions
  *     as they appear in the source file.
  *
  *  2) No right is granted to use any trade name, trademark, or
  *     logo of Broadcom Corporation.  The "Broadcom Corporation"
  *     name may not be used to endorse or promote products derived
  *     from this software without the prior written permission of
  *     Broadcom Corporation.
  *
  *  3) THIS SOFTWARE IS PROVIDED "AS-IS" AND ANY EXPRESS OR
  *     IMPLIED WARRANTIES, INCLUDING BUT NOT LIMITED TO, ANY IMPLIED
  *     WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
  *     PURPOSE, OR NON-INFRINGEMENT ARE DISCLAIMED. IN NO EVENT
  *     SHALL BROADCOM BE LIABLE FOR ANY DAMAGES WHATSOEVER, AND IN
  *     PARTICULAR, BROADCOM SHALL NOT BE LIABLE FOR DIRECT, INDIRECT,
  *     INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  *     (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
  *     GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
  *     BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
  *     OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
  *     TORT (INCLUDING NEGLIGENCE OR OTHERWISE), EVEN IF ADVISED OF
  *     THE POSSIBILITY OF SUCH DAMAGE.
  ********************************************************************* */

#include <config.h>
#include <version.h>
#include <asm/regdef.h>
#include <asm/mipsregs.h>
#include <asm/addrspace.h>
#include <asm/cacheops.h>

#define FLAGS_UNCACHED		(0x17)
#define FLAGS_CACHED_WT		(0x07)
#define FLAGS_CACHED_WB		(0x1f)
#define FLAGS_INVALID		(0x00)

/* #define HAZARD  ssnop; ssnop; ssnop; ssnop; ssnop; ssnop; ssnop; ssnop; */
#define HAZARD  ehb

#define PVT_MAX_RETRY		(120)
#define PVT_MATCHED_COUNT	(3)
#define DELAY_COUNT		(600)

/* Define it to support large flash region */
#define PHYS_TO_FLASH_UNCADDR(x)  \
    ((x) + (TLB_FLASH_VIRT_UNCACHED - TLB_FLASH_PHYS))

#define BCM53000_REG_MEMC		0x18006000 /* MEMC core registers */
#define BCM53000_REG_DDRPHY		0x1800f000 /* DDR PHY core registers */

#define DDR23PHY_PLL_STATUS		0x010
#define DDR23PHY_PLL_CONFIG		0x014
#define DDR23PHY_PLL_PRE_DIVIDER	0x018
#define DDR23PHY_PLL_DIVIDER		0x01c
#define DDR23PHY_STATIC_VDL_OVERRIDE	0x030
#define DDR23PHY_ZQ_PVT_COMP_CTL	0x03c
#define DDR23PHY_DRIVE_PAD_CTL		0x040
#define DDR23PHY_BL3_VDL_CALIBRATE	0x104
#define DDR23PHY_BL3_VDL_STATUS		0x108
#define DDR23PHY_BL2_VDL_CALIBRATE	0x204
#define DDR23PHY_BL2_VDL_STATUS		0x208
#define DDR23PHY_BL1_VDL_CALIBRATE	0x304
#define DDR23PHY_BL1_VDL_STATUS		0x308
#define DDR23PHY_BL0_VDL_CALIBRATE	0x404
#define DDR23PHY_BL0_VDL_STATUS		0x408

#define PL341_memc_status		0x000
#define PL341_memc_cmd			0x004
#define PL341_direct_cmd		0x008
#define PL341_memory_cfg		0x00c
#define PL341_refresh_prd		0x010
#define PL341_cas_latency		0x014
#define PL341_write_latency		0x018
#define PL341_t_mrd			0x01c
#define PL341_t_ras			0x020
#define PL341_t_rc			0x024
#define PL341_t_rcd			0x028
#define PL341_t_rfc			0x02c
#define PL341_t_rp			0x030
#define PL341_t_rrd			0x034
#define PL341_t_wr			0x038
#define PL341_t_wtr			0x03c
#define PL341_t_xp			0x040
#define PL341_t_xsr			0x044
#define PL341_t_esr			0x048
#define PL341_memory_cfg2		0x04c
#define PL341_memory_cfg3		0x050
#define PL341_t_faw			0x054
#define PL341_chip_0_cfg		0x200
#define PL341_user_config0		0x304

/*
 * Values for PL341 Direct Command Register
 */
#define MCHIP_CMD_PRECHARGE_ALL         (0x0 << 18)
#define MCHIP_CMD_AUTO_REFRESH          (0x1 << 18)
#define MCHIP_CMD_MODE_REG              (0x2 << 18)
#define MCHIP_CMD_NOP                   (0x3 << 18)
#define MCHIP_MODEREG_SEL(x)            ((x) << 16)
#define MCHIP_MR_WRITE_RECOVERY(x)      (((x) - 1) << 9)
#define MCHIP_MR_DLL_RESET(x)           ((x) << 8)
#define MCHIP_MR_CAS_LATENCY(x)         ((x) << 4)
#define MCHIP_MR_BURST_LENGTH           (2)
#define MCHIP_EMR1_DLL_DISABLE(x)       ((x) << 0)
#define MCHIP_EMR1_RTT_ODT_DISABLED     (0)
#define MCHIP_EMR1_RTT_75_OHM           (1 << 2)
#define MCHIP_EMR1_RTT_150_OHM          (1 << 6)
#define MCHIP_EMR1_RTT_50_OHM           ((1 << 6) | (1 << 2))
#define MCHIP_EMR1_OCD_CALI_EXIT        (0x0 << 7)
#define MCHIP_EMR1_OCD_CALI_DEFAULT     (0x3 << 7)

#define CC_CHIPID		0x00
#define CC_CAPABILITIES		0x04
#define CC_CORECTL		0x08
#define CC_CHIPST		0x2c
#define CC_JTAGCMD		0x30
#define CC_JTAGIR		0x34
#define CC_JTAGDR		0x38
#define CC_JTAGCTRL		0x3c
#define CC_WATCHDOG		0x80
#define CC_CLKDIV		0xa4
#define CC_EROMPTR		0xfc
#define PMU_CTL			0x600
#define PMU_CAP			0x604
#define PMU_ST			0x608
#define PMU_TIMER		0x614

#define MEMC_SLAVE_BASE			0x18106000
#define	AI_RESETCTRL			0x800


#define X510_ENUM_BASE		0x18000000 /* Beginning of core enum space */
#define X510_NTLBENTRIES	64
#define X510_GPIO_BASE		0x18000060
#define X510_CHIPID_REG		0x18000000
#define X510_CAP_REG		0x18000004

#define M_SR_SOFT_RESET		0x00100000 /* Soft reset */
#define M_SR_BOOT_EXCEPT_VEC	0x00400000 /* Boot exception vectors */

#define K_CFG_K0COH_CACHEABLE	3

#ifdef __ASSEMBLER__
#define _ACAST32_
#else
#define _ACAST32_	_ATYPE_ _ATYPE32_	/* widen if necessary */
#endif
#define K0BASE 		(_ACAST32_ 0x80000000)	/* kernel unmapped cached */


/* DDR2 setup */

/* (required) PLL clock */
#define CFG_DDR_PLL_CLOCK	(300000)  /* KHz */
#define PLL_NDIV_INT_VAL	(16 * (CFG_DDR_PLL_CLOCK) / 100000)
#define MEMCYCLES(psec)		(((psec) * (CFG_DDR_PLL_CLOCK) + 999999999) / 1000000000)
#define MEMCYCLES_MIN(psec)	((psec) * (CFG_DDR_PLL_CLOCK) / 1000000000)

/* Samsung K4T51163QJ-HCE6 512Mb 667Mhz */
/* (required) Row bits: 0=11bits, 1=12bits, ..., 5=16bits */
/* #define CFG_DDR_ROW_BITS		2	*//* 13 bits */

/* (required) Column bits: 1=9bits, 2=10bits, 3=11bits */
/* #define CFG_DDR_COLUMN_BITS		2	*//* 10 bits */

/* (required) Bank bits: 0=2bits, 3=3bits */
/* #define CFG_DDR_BANK_BITS		0 */

/* (required) Memory width: 0=16bits, 1=32bits, 2=64bits */
/* #define CFG_DDR_MEM_WIDTH		1 */

/* (required) CAS Latency (NOTE: could be affected by PLL clock) */
/* #define CFG_DDR_CAS_LATENCY		5 */

/* (required) t_wr (picoseconds) */
/* #define CFG_DDR_T_WR			15000 */

/* (optional) Refresh period (picoseconds) - tREFi */
/* #define CFG_DDR_REFRESH_PRD		7800000 */

/* (optional) t_rfc (picoseconds) */
/* #define CFG_DDR_T_RFC			105000 */


/* Micron MT47H128M16RT-25E 2Gb 800Mhz (running at 600MHz) */
/* (required) Row bits: 0=11bits, 1=12bits, ..., 5=16bits */
#define CFG_DDR_ROW_BITS		3	/* 14 bits */

/* (required) Column bits: 1=9bits, 2=10bits, 3=11bits */
#define CFG_DDR_COLUMN_BITS		2	/* 10 bits */

/* (required) Bank bits: 0=2bits, 3=3bits */
#define CFG_DDR_BANK_BITS		3

/* (required) Memory width: 0=16bits, 1=32bits, 2=64bits */
#define CFG_DDR_MEM_WIDTH		1

/* (required) CAS Latency (NOTE: could be affected by PLL clock) */
#define CFG_DDR_CAS_LATENCY		5

/* (required) t_wr (picoseconds) */
#define CFG_DDR_T_WR			15000

/* (optional) Refresh period (picoseconds) - tREFi */
#define CFG_DDR_REFRESH_PRD		7800000

/* (optional) t_rfc (picoseconds) */
#define CFG_DDR_T_RFC			195000

/* (optional) t_rrd (picoseconds) */
#define CFG_DDR_T_RRD			10000

/* (optional) t_wtr (picoseconds) */
#define CFG_DDR_T_WTR			7500

/* (optional) t_faw (picoseconds) */
#define CFG_DDR_T_FAW			45000



#define PHYS_TO_K1(pa)	(K1BASE | (pa))
#define K0_TO_PHYS(va)	((va) & (K0SIZE-1))
#define K1_TO_PHYS(va)	((va) & (K1SIZE-1))
#define K0_TO_K1(va)	((va) | K1SIZE)
#define K1_TO_K0(va)	((va) & ~K1SIZE)

#define ddr_write(offset, value)	\
	li	a0, (value);		\
	sw	a0, offset(t0);






	.set	noreorder

	.align	2;
	.globl	lowlevel_init
	.ent	lowlevel_init
lowlevel_init:
	move	t9, ra


	# Correct the CPU frequency if it's for this bonding

	li	k0, 0xb8000000
	lw	k1, 0(k0)
	and	k1, k1, 0x00f00000
	srl	k1, k1, 20
	bne	k1, 4, 1f
	nop

	li	k0, 0xb8000660
	li	k1, 4
	sw	k1, 0(k0)           # Set pllcontrol_addr to 4

	li	k0, 0xb8000664
	li	k1, 0xc0011100
	sw	k1, 0(k0)           # Change n_divider to 0x20

	li	k0, 0xb8000600
	lw	k1, 0(k0)
	ori	k1, k1, (1 << 10)
	sw	k1, 0(k0)           # Write pmucontrol's pllCntlUpdate to 1

	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop

	b	99f

	nop
1:
	#
	# If we're running slower due to flash waitcount issue on A0,
	# let's switch to full speed.
	#

	li	k0, 0xb8000000
	lw	k1, 0(k0)
	and	k1, k1, 0x000f0000
	bne	k1, 0, 99f          # Do nothing if it is not A0
	nop

	li	k0, 0xb800012c
	li	k1, 0x0404141a
	sw	k1, 0(k0)           # Correct boot flash waitcount

	li	k0, 0xb800002c
	lw	k1, 0(k0)
	and	k1, k1, 1
	bne	k1, 0,99f           # Do nothing if it's low cost package
	nop

	li	k0, 0xb800002c
	lw	k1, 0(k0)
	srl	k1, k1, 26
	and	k1, k1, 0x07
	beq	k1, 0,99f           # Do nothing if it's already running at 600MHz
	nop

	li	k0, 0xb8000660
	li	k1, 4
	sw	k1, 0(k0)           # Set pllcontrol_addr to 4

	li	k0, 0xb8000664
	li	k1, 0xc0011180
	sw	k1, 0(k0)           # Change n_divider to 0x30

	li	k0, 0xb8000600
	lw	k1, 0(k0)
	ori	k1, k1, (1 << 10)
	sw	k1, 0(k0)           # Write pmucontrol's pllCntlUpdate to 1

	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
99:

	/* Set Uart clock as ALP clock */
	li	k0, 0xb8000008
	lw	k1, 0(k0)
	or	k1, k1, 1
	sw	k1, 0(k0)

	li	k0, 0xb800012c
	li	k1, 0x010A050A
	sw	k1, 0(k0)           # Correct boot flash waitcount


	li	k0, 0xb800018c
	li	k1, 0x52
	sw	k1, 0(k0)           # Setup the size and type of FLASH

	li	k0, 0xb80002f4
	li	k1, 0x01
	sw	k1, 0(k0)           # enable serial io for i2c0

	/* setup a 74K MIPs processor */
	mtc0	zero, CP0_WATCHLO
	mtc0	zero, CP0_WATCHHI
	mtc0	zero, CP0_CAUSE
	HAZARD

	mfc0	v0, CP0_STATUS		# Get status register
	and	v0, M_SR_SOFT_RESET
#ifdef DEBUG_ENV_ICE
	and	v0, ~M_SR_BOOT_EXCEPT_VEC
#else
	or	v0, M_SR_BOOT_EXCEPT_VEC
#endif
	mtc0	v0, CP0_STATUS
	HAZARD

	/* get current CONFIG register */
	mfc0	v0, CP0_CONFIG
	/* strip out K0 bits */
	srl	v0, v0,3
	sll	v0, v0,3
	/* Make K0 cacheable */
	or	v0, v0, K_CFG_K0COH_CACHEABLE
	mtc0	v0, CP0_CONFIG
	HAZARD

	mtc0	zero, CP0_WATCHLO
	mtc0	zero, CP0_WATCHHI
	/* Clear the EntryHi register */
	mtc0	zero, CP0_ENTRYHI
	HAZARD

	li	v0,-1
	mtc0	v0,CP0_COMPARE
	nop

	/* Set Uart clock as ALP clock */
	li	t0, 0xb8000008
	lw	t1, 0(t0)
	or	t1, t1, 1
	sw	t1, 0(t0)
	li	t0, 0xb8000300
	lb	t1, 3(t0)
	or	t1, t1,0x80
	sb	t1, 3(t0)
	li	t1, 0xa3
	sb	t1, 0(t0)
	lb	t1, 3(t0)
	and	t1, t1,0x7f
	sb	t1, 3(t0)
	li	t1, 0x35
	sb	t1, 0(t0)


	/* clear all the TLB entries to an invalid value */
	mtc0	zero,CP0_ENTRYLO0
	HAZARD
	mtc0	zero,CP0_ENTRYLO1
	HAZARD
	/* sets the pagemask to 4k page size */
	mtc0	zero,CP0_PAGEMASK
	HAZARD
	/* set to an impossible vpn */
	li	t0, KSEG1
	li	t1,(X510_NTLBENTRIES-1)

	nop
1:	mtc0	t0, CP0_ENTRYHI
	HAZARD
	mtc0	t1, CP0_INDEX
	HAZARD
	addu	t0, 0x2000		/* inc vpn */
	tlbwi
	HAZARD
	bnez	t1, 1b
	subu	t1, 1

	/* now initialize the caches */

	/*
	* Size the icache
	*/

	mfc0    t0,CP0_CONFIG,1

	/*
	 * Sets Per Way = 64 << CFG_IS
	 */
	li	t2, 0x01c00000
	and	t2, t0
	srl	t2, 22
	li	t1, 64
	sll	t1, t2

	/*
	* Associativity = 1 + CFG_IA
	*/
	li	t2, 0x00070000
	and	t2, t0
	srl	t2, 16
	addiu	t2, 1

	/*
	* Associativity * Sets Per Way
	*/
	multu	t1, t2
	mflo	t1

	/*
	 * Line Size = 2 << CFG_IL
	 */
	 li	t2, 0x00380000
	 and	t2, t0
	 srl	t2, 19
	 li	t0, 2
	 sll	t2, t0,t2

	/*
	 * Instruction Cache Size =
	 * Associativity * Line Size * Sets Per Way
	 */
	 multu	t1,t2
	 mflo	t0

	/*
	 * Zero the icache
	*/
	mtc0	zero, CP0_TAGLO
	mtc0	zero, CP0_TAGHI
	HAZARD
	li	t1, K0BASE

1:	sub t0,t0,t2
	cache   Index_Store_Tag_I,0(t1)
	add t1,t2
	bne t0,zero,1b
	HAZARD
	nop
	nop
	nop

	/*
	 * Size the dcache
	 */
	mfc0	t0, CP0_CONFIG,1

	/*
	 * Sets Per Way = 64 << CFG_DS
	*/
	li	t2, 0x0000e000
	and	t2, t0
	srl	t2, 13
	li	t1, 64
	sll	t1, t2

	/*
	 * Associativity = 1 + CFG_DA
	 */
	 li	t2, 0x00000380
	 and	t2, t0
	 srl	t2, 7
	 addiu	t2, 1

	/*
	* Associativity * Sets Per Way
	*/
	multu	t1,t2
	mflo	t1

	/*
	 * Line Size = 2 << CFG_DL
	*/
	li	t2, 0x00001c00
	and	t2, t0
	srl	t2, 10
	li	t0, 2
	sll	t2, t0,t2


	/*
	 * Data Cache Size =
	 * Associativity * Line Size * Sets Per Way
	 */
	 multu	t1,t2
	 mflo	t0

	/*
	* Zero the dcache
	*/
	mtc0	zero, CP0_TAGLO,2
	mtc0	zero, CP0_TAGHI,2

	HAZARD

	li	t1, K0BASE

1:	sub	t0,t0,t2
	cache	Index_Store_Tag_D,0(t1)
	add	t1,t2
	bne	t0,zero,1b
	HAZARD
	nop
	nop
	nop


	/* cache initialization complete */

	/* initialize the TLB */
	la	t0, tlb_map_init
	jalr	t0
	nop
	nop
	move	v0,zero     # auto configure DRAM

	/* initialize the DRAM */
	la	t0, x510_draminit
	jalr	t0
	nop
	nop

	jr	t9

	.set	reorder

	.end lowlevel_init





/*
 * Registers used: t0-t6
 */

	.align	2;
	.globl	tlb_map_init
	.ent	tlb_map_init
tlb_map_init:
	.set	noreorder

	move	t6, ra

	/* Index */
	li	t1, 0

	/*
	* PCIE1
	*/
	li	t2, TLB_PCIE1_PAGEMASK
	li	t3, TLB_PCIE1_VIRT_UNCACHED
	li	t4, ((TLB_PCIE1_PHYS >> 6) | FLAGS_UNCACHED)
	li	t5, (((TLB_PCIE1_PHYS + TLB_PCIE1_PAGESIZE) >> 6) | FLAGS_UNCACHED)
	la	t0, tlb_map
	jalr	t0
	nop
	li	t3, TLB_PCIE1_VIRT_CACHED
	li	t4, ((TLB_PCIE1_PHYS >> 6) | FLAGS_CACHED_WT)
	li	t5, (((TLB_PCIE1_PHYS + TLB_PCIE1_PAGESIZE) >> 6) | FLAGS_CACHED_WT)
	la	t0, tlb_map
	jalr	t0
	nop

	/*
	* Flash
	*/
	li	t2, TLB_FLASH_PAGEMASK
	li	t3, TLB_FLASH_VIRT_UNCACHED - TLB_FLASH_PAGESIZE
	li	t4, ((0x10000000 >> 6) | FLAGS_INVALID)
	li	t5, ((TLB_FLASH_PHYS >> 6) | FLAGS_UNCACHED)
	la	t0, tlb_map
	jalr	t0
	nop
	li	t3, TLB_FLASH_VIRT_CACHED - TLB_FLASH_PAGESIZE
	li	t4, ((0x10000000 >> 6) | FLAGS_INVALID)
	li	t5, ((TLB_FLASH_PHYS >> 6) | FLAGS_CACHED_WT)
	la	t0, tlb_map
	jalr	t0
	nop

	/*
	* DDR2
	*/
	li	t2, TLB_SDRAM_PAGEMASK
	li	t3, TLB_SDRAM_VIRT_UNCACHED
	li	t4, ((TLB_SDRAM_PHYS >> 6) | FLAGS_UNCACHED)
	li	t5, ((0x90000000 >> 6) | FLAGS_UNCACHED)
	la	t0, tlb_map
	jalr	t0
	nop
	li	t3, TLB_SDRAM_VIRT_CACHED
	li	t4, ((TLB_SDRAM_PHYS >> 6) | FLAGS_CACHED_WT)
	li	t5, ((0x90000000 >>6) | FLAGS_CACHED_WT)
	la	t0, tlb_map
	jalr	t0
	nop

	jr	t6
	nop

tlb_map:

	/* page size */
	move	t0, t2
	mtc0	t0, CP0_PAGEMASK
	HAZARD

	/* EntryHi: VFN2 */
	move	t0, t3
	mtc0	t0, CP0_ENTRYHI
	HAZARD

	/* EntryL0: even PFN + flags */
	move	t0, t4
	mtc0	t0, CP0_ENTRYLO0
	HAZARD

	/* EntryL1: odd PFN + flags */
	move	t0, t5
	mtc0	t0, CP0_ENTRYLO1
	HAZARD

	/* Write Index */
	mtc0	t1, CP0_INDEX
	HAZARD

	/* Write to TLB entry */
	tlbwi
	HAZARD

	/* Increase index */
	add	t1, t1, 1

	/* Set wired entry count */
	mtc0	t1, CP0_WIRED
	HAZARD

	jr	ra
	nop

	.end	tlb_map_init
	.set	reorder



	.set	noreorder

	.align	2;
	.globl	x510_draminit
	.ent	x510_draminit
x510_draminit:

	move	t6,ra

	/*
	* ddr23_phy_init
	*/

	li	t0, KSEG1ADDR(BCM53000_REG_DDRPHY)


	/*
	 * Recalibrate dn/pd PVT compensation
	 *
	 *   Issue:  The calibration may not finish when
	 *           sample_done bit has been set.
	 *
	 *   Workaround: Assume it is not finished until we get the same result
	 *               continuously for 3 times.
	 *
	 */

	li	t2, 0x10000000
	li	t3, 0
	li	t5, 0
2:
	/* Do recalibration */
	beq	t5, PVT_MAX_RETRY, 9f
	lw	a1, DDR23PHY_ZQ_PVT_COMP_CTL(t0)
	ddr_write(DDR23PHY_ZQ_PVT_COMP_CTL, 0x04000000);
1:
	/* Wait until sample_done == 1 */
	lw	t1, DDR23PHY_ZQ_PVT_COMP_CTL(t0)
	and	t1, t1, t2;
	beq	t1, zero, 1b
	nop

	/* Increase total retry count */
	add	t5, t5, 1

	/* Check if the result is the same as previous one */
	lw	t1, DDR23PHY_ZQ_PVT_COMP_CTL(t0)
	beq	t1, a1, 3f
	nop

	/* If not, clear matched count */
	b	2b
	li	t3, 0
3:
	/* If so, increase matched count; continue if matched count == 3 */
	add	t3, t3, 1
	bne	t3, PVT_MATCHED_COUNT, 2b
	nop
9:
	/* Workaround end: PVT compensation done */

	/* setup PLL */
	ddr_write(DDR23PHY_PLL_CONFIG, 0x8000000c);
	ddr_write(DDR23PHY_PLL_PRE_DIVIDER, 0x00000011 + (PLL_NDIV_INT_VAL << 8));
	ddr_write(DDR23PHY_PLL_DIVIDER, 0x02000000);
	ddr_write(DDR23PHY_PLL_CONFIG, 0x80000008);

	/* Wait for PLL locked */
	li	t2, 1;
1:	/* Wait until lock == 1 */
	lw	t1, DDR23PHY_PLL_STATUS(t0)
	and	t1, t1, t2;
	beq	t1, zero, 1b
	nop

	/* De-assert PLL reset */
	ddr_write(DDR23PHY_PLL_CONFIG, 0x80000000);
	ddr_write(DDR23PHY_PLL_CONFIG, 0x00000000);

	/*
	 * Calibrate VDL
	 */

	/* calib_once + calib_fast (for all BL) */
	ddr_write(DDR23PHY_BL3_VDL_CALIBRATE, 0x00000003);
	ddr_write(DDR23PHY_BL2_VDL_CALIBRATE, 0x00000003);
	ddr_write(DDR23PHY_BL1_VDL_CALIBRATE, 0x00000003);
	ddr_write(DDR23PHY_BL0_VDL_CALIBRATE, 0x00000003);

	li	t2, 0x0000003;
	li	t0, KSEG1ADDR(BCM53000_REG_DDRPHY)
1:	/* Wait until calib_idle == 1 and locked for all BL */
	lw	t1, DDR23PHY_BL3_VDL_STATUS(t0)
	and	t1, t1, t2;
	beq	t1, zero, 1b
	nop
	lw	t1, DDR23PHY_BL2_VDL_STATUS(t0)
	and	t1, t1, t2;
	beq	t1, zero, 1b
	nop
	lw	t1, DDR23PHY_BL1_VDL_STATUS(t0)
	and	t1, t1, t2;
	beq	t1, zero, 1b
	nop
	lw	t1, DDR23PHY_BL0_VDL_STATUS(t0)
	and	t1, t1, t2;
	beq	t1, zero, 1b
	nop

	/* clear the BLn_VDL_CALIBRATE registers */
	ddr_write(DDR23PHY_BL3_VDL_CALIBRATE, 0x00000000);
	ddr_write(DDR23PHY_BL2_VDL_CALIBRATE, 0x00000000);
	ddr_write(DDR23PHY_BL1_VDL_CALIBRATE, 0x00000000);
	ddr_write(DDR23PHY_BL0_VDL_CALIBRATE, 0x00000000);

	/* VDL override */
	lw	t1, DDR23PHY_BL0_VDL_STATUS(t0)
	srl	t1, t1, 8
	andi	t2, t1, 0x3f                        /* VDL step size */
	li	t1, 1
	sll	t1, 16
	or	t2, t2, t1                          /* ovr_en */
	li	t1, 1
	sll	t1, 20
	or	t2, t2, t1                          /* ovr_force */
	sw	t2, DDR23PHY_STATIC_VDL_OVERRIDE(t0)

	/* set drive level for address and control lines */
	ddr_write(DDR23PHY_DRIVE_PAD_CTL, 0x0000000F);

	/*
	 * Memory controller PL341 initialization
	 */

	/* De-assert core reset */
	li	t0, KSEG1ADDR(MEMC_SLAVE_BASE);
	ddr_write(AI_RESETCTRL, 0x00000000)

	li	t0, KSEG1ADDR(BCM53000_REG_MEMC);

#ifdef CFG_DDR_REFRESH_PRD
	/* refresh_prd */
	ddr_write(PL341_refresh_prd, MEMCYCLES_MIN(CFG_DDR_REFRESH_PRD));
#endif

#ifdef CFG_DDR_CAS_LATENCY
	/* cas_latency */
	ddr_write(PL341_cas_latency, (CFG_DDR_CAS_LATENCY << 1));
#endif

#ifdef CFG_DDR_WRITE_LATENCY
	/* write_latency */
	ddr_write(PL341_write_latency, CFG_DDR_WRITE_LATENCY);
#endif

#ifdef CFG_DDR_T_MRD
	/* MRD time [6:0] */
	ddr_write(PL341_t_mrd, MEMCYCLES(CFG_DDR_T_MRD));
#endif

#ifdef CFG_DDR_T_RAS
	/* ras time [4:0] */
	ddr_write(PL341_t_ras, MEMCYCLES(CFG_DDR_T_RAS));
#endif

#ifdef CFG_DDR_T_RC
	/* t_rc [4:0] */
	ddr_write(PL341_t_rc, MEMCYCLES(CFG_DDR_T_RC));
#endif

#ifdef CFG_DDR_T_RCD
	/* t_rcd [4:0] */
	ddr_write(PL341_t_rcd,
	    ((MEMCYCLES(CFG_DDR_T_RCD) - 3) << 8) | MEMCYCLES(CFG_DDR_T_RCD));
#endif

#ifdef CFG_DDR_T_RFC
	/* t_rfc [6:0]  schedule_rfc[14:8] */
	ddr_write(PL341_t_rfc,
	    ((MEMCYCLES(CFG_DDR_T_RFC) - 3) << 8) | MEMCYCLES(CFG_DDR_T_RFC));
#endif

#ifdef CFG_DDR_T_RP
	/* t_rp [3:0] */
	ddr_write(PL341_t_rp,
	    ((MEMCYCLES(CFG_DDR_T_RP) - 3) << 8) | MEMCYCLES(CFG_DDR_T_RP));
#endif

#ifdef CFG_DDR_T_RRD
	/* t_rrd [2:0] */
	ddr_write(PL341_t_rrd, MEMCYCLES(CFG_DDR_T_RRD));
#endif

#ifdef CFG_DDR_T_WR
	/* t_wr */
	ddr_write(PL341_t_wr, MEMCYCLES(CFG_DDR_T_WR));
#endif

#ifdef CFG_DDR_T_WTR
	/* t_wtr */
	ddr_write(PL341_t_wtr, MEMCYCLES(CFG_DDR_T_WTR));
#endif


#ifdef CFG_DDR_T_XP
	/* t_xp[7:0] */
	ddr_write(PL341_t_xp, MEMCYCLES(CFG_DDR_T_XP));
#endif

#ifdef CFG_DDR_T_XSR
	/* t_xsr[7:0] */
	ddr_write(PL341_t_xsr, MEMCYCLES(CFG_DDR_T_XSR));
#endif

#ifdef CFG_DDR_T_ESR
	/* t_esr[7:0] */
	ddr_write(PL341_t_esr, MEMCYCLES(CFG_DDR_T_ESR));
#endif

#ifdef CFG_DDR_T_FAW
	/* t_faw */
	ddr_write(PL341_t_faw,
	    ((MEMCYCLES(CFG_DDR_T_FAW) - 3) << 8) | MEMCYCLES(CFG_DDR_T_FAW));
#endif

	/* memory_cfg register */
	ddr_write(PL341_memory_cfg,
	    (2 << 15) | (CFG_DDR_ROW_BITS << 3) | CFG_DDR_COLUMN_BITS
	    );

	/* Figure out if it is a low cost package */
	li	s2,KSEG1ADDR(X510_ENUM_BASE)		# s2 = X510_ENUM_BASE
	lw	t1,CC_CHIPID(s2)
	li	s3,0x00300000
	and	t1,t1,s3

	/* memory_cfg2 register */
	li	t2, (CFG_DDR_MEM_WIDTH << 6) | (CFG_DDR_BANK_BITS << 4)
	beq	t1, 0, 1f
	nop
	and	t2, t2, 0xffffff3f /* Set to 16bit for low cost package */
1:
	sw	t2, PL341_memory_cfg2(t0);

	/* chip0 configuration */
	ddr_write(PL341_chip_0_cfg, 0x00000000);

	/* user_config0 */
	ddr_write(PL341_user_config0, 0x00000003);


	li	t3, 0
2:
	/* wait for 200us */
	add	t3, t3, 1
	bne	t3, DELAY_COUNT, 2b
	nop

	/*
	 * DDR2 chip initialization
	 */

	/* Issue NOP */
	ddr_write(PL341_direct_cmd, MCHIP_CMD_NOP);

	/* issue precharge all */
	ddr_write(PL341_direct_cmd, MCHIP_CMD_PRECHARGE_ALL);

	/* Set EMR2 */
	ddr_write(PL341_direct_cmd, MCHIP_CMD_MODE_REG | MCHIP_MODEREG_SEL(2));

	/* Set EMR3 */
	ddr_write(PL341_direct_cmd, MCHIP_CMD_MODE_REG | MCHIP_MODEREG_SEL(3));

	/* DLL Enable */
	ddr_write(PL341_direct_cmd,
	    MCHIP_CMD_MODE_REG | MCHIP_MODEREG_SEL(1) |
	    MCHIP_EMR1_DLL_DISABLE(0)
	    );

	/* DLL Reset */
	ddr_write(PL341_direct_cmd,
	    MCHIP_CMD_MODE_REG | MCHIP_MODEREG_SEL(0) |
	    MCHIP_MR_WRITE_RECOVERY(MEMCYCLES(CFG_DDR_T_WR)) |
	    MCHIP_MR_DLL_RESET(1) |
	    MCHIP_MR_CAS_LATENCY(CFG_DDR_CAS_LATENCY) |
	    MCHIP_MR_BURST_LENGTH
	    );

	/* issue precharge all */
	ddr_write(PL341_direct_cmd, MCHIP_CMD_PRECHARGE_ALL);

	/* auto-refresh 2 times */
	ddr_write(PL341_direct_cmd, MCHIP_CMD_AUTO_REFRESH);
	ddr_write(PL341_direct_cmd, MCHIP_CMD_AUTO_REFRESH);

	/* DLL Reset=0 (Un-Reset DLL) */
	ddr_write(PL341_direct_cmd,
	    MCHIP_CMD_MODE_REG | MCHIP_MODEREG_SEL(0) |
	    MCHIP_MR_WRITE_RECOVERY(MEMCYCLES(CFG_DDR_T_WR)) |
	    MCHIP_MR_DLL_RESET(0) |
	    MCHIP_MR_CAS_LATENCY(CFG_DDR_CAS_LATENCY) |
	    MCHIP_MR_BURST_LENGTH
	    );

	/* DLL Enable & RTT 75ohm */
	ddr_write(PL341_direct_cmd,
	    MCHIP_CMD_MODE_REG | MCHIP_MODEREG_SEL(1) |
	    MCHIP_EMR1_DLL_DISABLE(0) | MCHIP_EMR1_RTT_75_OHM |
	    MCHIP_EMR1_OCD_CALI_EXIT
	    );

	/* OCD Defaults */
	ddr_write(PL341_direct_cmd,
	    MCHIP_CMD_MODE_REG | MCHIP_MODEREG_SEL(1) |
	    MCHIP_EMR1_DLL_DISABLE(0) | MCHIP_EMR1_RTT_75_OHM |
	    MCHIP_EMR1_OCD_CALI_DEFAULT
	    );

	/* OCD Exit */
	ddr_write(PL341_direct_cmd,
	    MCHIP_CMD_MODE_REG | MCHIP_MODEREG_SEL(1) |
	    MCHIP_EMR1_DLL_DISABLE(0) | MCHIP_EMR1_RTT_75_OHM |
	    MCHIP_EMR1_OCD_CALI_EXIT
	    );

	/* set MEMC to GO */
	ddr_write(PL341_memc_cmd, 0);
	nop
	nop

	jr	t6
	nop
	.end	x510_draminit


.globl	x510_get_cpu_cap
.ent	x510_get_cpu_cap
x510_get_cpu_cap:
	li	t0, KSEG1ADDR(X510_CAP_REG)
	lw	v0, 0(t0)
	j	ra

	.end	x510_get_cpu_cap


	.globl	x510_get_chipid
	.ent	x510_get_chipid
x510_get_chipid:
	li	t0, KSEG1ADDR(X510_CHIPID_REG)
	lw	v0, 0(t0)
	j	ra
	.end	x510_get_chipid

