// ---------------------------------------------------------
//   ATMEL Microcontroller Software Support
// ---------------------------------------------------------
// The software is delivered "AS IS" without warranty or
// condition of any  kind, either express, implied or
// statutory. This includes without limitation any warranty
// or condition with respect to merchantability or fitness
// for any particular purpose, or against the infringements of
// intellectual property rights of others.
// ---------------------------------------------------------
//  File: sama5d2-xplained_ddram.mac
//  User setup file for CSPY debugger.
//
// ---------------------------------------------------------

__var __tempo_var;
__var __tempo_reg;
__var __dummy_read;
__var __ba_offset;
__var __data_test;
__var __mac_i;

__var REG_CKGR_MOR;
__var CKGR_MOR_MOSCXTEN;
__var CKGR_MOR_MOSCXTBY;
__var CKGR_MOR_MOSCRCEN;
__var CKGR_MOR_MOSCSEL;
__var REG_CKGR_MCFR;
__var CKGR_MCFR_MAINFRDY;
__var REG_PMC_SR;
__var PMC_SR_MCKRDY;
__var PMC_SR_LOCKA;
__var PMC_PCK_CSS_MAIN_CLK;
__var REG_CKGR_PLLAR;
__var REG_PMC_PLLICPR;
__var REG_PMC_MCKR;
__var PMC_MCKR_PLLADIV2_DIV2;
__var PMC_MCKR_PRES_Msk;
__var PMC_MCKR_PRES_CLOCK;
__var PMC_MCKR_MDIV_Msk;
__var PMC_MCKR_MDIV_PCK_DIV3;
__var PMC_MCKR_CSS_PLLA_CLK;
__var PMC_SR_MOSCSELS;

__var REG_MPDDRC_RTR;

/*********************************************************************
* pmc_select_external_osc()
*
* Function description
*   Select external 12MHz oscillator
*********************************************************************/
pmc_select_external_osc()
{
	REG_CKGR_MOR = 0xF0014020;
	CKGR_MOR_MOSCXTEN = (0x1 << 0);     /*(CKGR_MOR) Main Crystal Oscillator Enable */
	CKGR_MOR_MOSCXTBY = (0x1 << 1);     /*(CKGR_MOR) Main Crystal Oscillator Bypass */
	CKGR_MOR_MOSCRCEN = (0x1 << 3);     /*(CKGR_MOR) Main On-Chip RC Oscillator Enable */
	CKGR_MOR_MOSCSEL = (0x1 << 24);     /*(CKGR_MOR) Main Oscillator Selection */

	REG_CKGR_MCFR = 0xF0014024;         /*(PMC) Main Clock Frequency Register */
	CKGR_MCFR_MAINFRDY = (0x1 << 16);   /*(CKGR_MCFR) Main Clock Ready */

	REG_PMC_SR = 0xF0014068;            /*(PMC) Status Register */
	PMC_SR_MOSCSELS = (0x1 << 16);      /*(PMC_SR) Main Oscillator Selection Status */
	PMC_SR_MCKRDY = (0x1 << 3);         /*(PMC_SR) Master Clock Status */

	/* enable external OSC 12 MHz  */
	__tempo_var = __readMemory32(REG_CKGR_MOR,"Memory");
	__tempo_var |= CKGR_MOR_MOSCXTEN | (0x37 << 16);
	__writeMemory32(__tempo_var,REG_CKGR_MOR,"Memory");

	/* wait Main CLK Ready */
	while(!((__readMemory32(REG_CKGR_MCFR,"Memory")) & CKGR_MCFR_MAINFRDY));

	/* disable external OSC 12 MHz bypass */
	__tempo_var = __readMemory32(REG_CKGR_MOR,"Memory");
	__tempo_var = (__tempo_var & ~CKGR_MOR_MOSCXTBY) | (0x37 << 16);
	__writeMemory32(__tempo_var,REG_CKGR_MOR,"Memory");

	/* switch MAIN clock to external OSC 12 MHz*/
	__tempo_var = __readMemory32(REG_CKGR_MOR,"Memory");
	__tempo_var |= CKGR_MOR_MOSCSEL | (0x37 << 16);
	__writeMemory32(__tempo_var,REG_CKGR_MOR,"Memory");

	/* wait MAIN clock status change for external OSC 12 MHz selection*/
	while(!((__readMemory32(REG_PMC_SR,"Memory")) & PMC_SR_MOSCSELS));

	/* in case when MCK is running on MAIN CLK */
	while(!((__readMemory32(REG_PMC_SR,"Memory")) & PMC_SR_MCKRDY));

}

/*********************************************************************
* pmc_switch_mck_to_main()
*
* Function description
*   Switch PMC from MCK to main clock.
*********************************************************************/
pmc_switch_mck_to_main()
{
	REG_PMC_MCKR = 0xF0014030;          /*(PMC) Master Clock Register */
	PMC_PCK_CSS_MAIN_CLK = (0x1 << 0);  /*(PMC_PCK[3]) Main Clock is selected */

	REG_PMC_SR = 0xF0018068;            /*(PMC) Status Register */
	PMC_SR_MCKRDY = (0x1 << 3);         /*(PMC_SR) Master Clock Status */

	/* Select Main Oscillator as input clock for PCK and MCK */
	__tempo_var = __readMemory32(REG_PMC_MCKR,"Memory");
	__tempo_var = (__tempo_var & ~0x03)| PMC_PCK_CSS_MAIN_CLK ;
	__writeMemory32(__tempo_var, REG_PMC_MCKR,"Memory");
	while(!((__readMemory32(REG_PMC_SR,"Memory")) & PMC_SR_MCKRDY));
	__mac_i=__readMemory32(REG_PMC_MCKR,"Memory");
	__message " --- Master clock switched to main --- REG_PMC_MCKR  0x",__mac_i:%X;
}

/*********************************************************************
* pmc_set_plla()
*
* Function description
*   Configure PLLA Registe.
*********************************************************************/
pmc_set_plla(pllmul)
{
	REG_CKGR_PLLAR = 0xF0014028;        /*(PMC) PLLA Register */
	REG_PMC_PLLICPR = 0xF0014080;       /*(PMC) PLL Charge Pump Current Register */
	REG_PMC_SR = 0xF0014068;            /*(PMC) Status Register */
	PMC_SR_LOCKA = (0x1 << 1);          /*(PMC_SR) PLLA Lock Status */

	__writeMemory32(((0x1 << 29) | (0x3F << 8) | ( 0 << 14) | ((pllmul) << 18) | 1 ), REG_CKGR_PLLAR,"Memory");
	//__writeMemory32((0x03<<8), REG_PMC_PLLICPR,"Memory");
	while(!((__readMemory32(REG_PMC_SR,"Memory")) & PMC_SR_LOCKA));
	__mac_i=__readMemory32(REG_CKGR_PLLAR,"Memory");
	__message " --- PLL A set up -------------------- REG_CKGR_PLLAR  0x",__mac_i:%X;
}

/*********************************************************************
*       pmc_set_mck_plla_div()
*
* Function description
*   Configure MCK PLLA divider.
*********************************************************************/
pmc_set_mck_plla_div()
{
	REG_PMC_MCKR = 0xF0014030;             /*(PMC) Master Clock Register */
	PMC_MCKR_PLLADIV2_DIV2 = (0x1 << 12);  /*(PMC_MCKR) PLLA clock frequency is divided by 2. */
	__tempo_var = __readMemory32(REG_PMC_MCKR,"Memory");
	if ((__tempo_var & PMC_MCKR_PLLADIV2_DIV2) != PMC_MCKR_PLLADIV2_DIV2)
	{
	__tempo_var |= PMC_MCKR_PLLADIV2_DIV2;
	__writeMemory32(__tempo_var, REG_PMC_MCKR,"Memory");
	while(!((__readMemory32(REG_PMC_SR,"Memory")) & PMC_SR_MCKRDY));
	}
}

/*********************************************************************
* pmc_set_mck_prescaler()
*
* Function description
*   Configure MCK Prescaler.
*********************************************************************/
pmc_set_mck_prescaler()
{
	REG_PMC_MCKR = 0xF0014030;             /*(PMC) Master Clock Register */
	PMC_MCKR_PRES_Msk = (0x7 << 4);        /*(PMC_MCKR) Master/Processor Clock Prescaler */
	PMC_MCKR_PRES_CLOCK = (0x0 << 4);      /*(PMC_MCKR) Selected clock */

	/* Change MCK Prescaler divider in PMC_MCKR register */
	__tempo_var = __readMemory32(REG_PMC_MCKR,"Memory");
	__tempo_var = (__tempo_var & ~PMC_MCKR_PRES_Msk) | PMC_MCKR_PRES_CLOCK;
	__writeMemory32(__tempo_var, REG_PMC_MCKR,"Memory");
	while(!((__readMemory32(REG_PMC_SR,"Memory")) & PMC_SR_MCKRDY));
	__mac_i=__readMemory32(REG_PMC_MCKR,"Memory");
	__message " --- Master clock prescaler set ------ REG_PMC_MCKR  0x",__mac_i:%X;
}

/*********************************************************************
* pmc_set_mck_divider()
*
* Function description
*   Configure MCK Divider.
*********************************************************************/
pmc_set_mck_divider()
{
	REG_PMC_MCKR = 0xF0014030;             /*(PMC) Master Clock Register */
	PMC_MCKR_MDIV_Msk = (0x3 << 8);        /*(PMC_MCKR) Master Clock Division */
	PMC_MCKR_MDIV_PCK_DIV3 = (0x3 << 8);   /*(PMC_MCKR) Master Clock is Prescaler Output Clock divided by 3.SysClk DDR is equal to 2 x MCK. DDRCK is equal to MCK. */

	/* change MCK Prescaler divider in PMC_MCKR register */
	__tempo_var = __readMemory32(REG_PMC_MCKR,"Memory");
	__tempo_var = (__tempo_var & ~PMC_MCKR_MDIV_Msk) | PMC_MCKR_MDIV_PCK_DIV3;
	__writeMemory32(__tempo_var, REG_PMC_MCKR,"Memory");
	while(!((__readMemory32(REG_PMC_SR,"Memory")) & PMC_SR_MCKRDY));
	__mac_i=__readMemory32(REG_PMC_MCKR,"Memory");
	__message " --- Master clock divider set -------- REG_PMC_MCKR  0x",__mac_i:%X;
}

/*********************************************************************
* pmc_switch_mck_to_pll()
*
* Function description
*   Switch PMC from MCK to PLL clock.
*********************************************************************/
pmc_switch_mck_to_pll()
{
	REG_PMC_MCKR = 0xF0014030;             /*(PMC) Master Clock Register */
	PMC_MCKR_CSS_PLLA_CLK = (0x2 << 0);    /*(PMC_MCKR) PLLACK/PLLADIV2 is selected */

	/* Select PLL as input clock for PCK and MCK */
	__tempo_var = __readMemory32(REG_PMC_MCKR,"Memory");
	__tempo_var = (__tempo_var & ~0x03) | PMC_MCKR_CSS_PLLA_CLK;
	__writeMemory32(__tempo_var, REG_PMC_MCKR,"Memory");
	while(!((__readMemory32(REG_PMC_SR,"Memory")) & PMC_SR_MCKRDY));
	__mac_i=__readMemory32(REG_PMC_MCKR,"Memory");
	__message " --- Master clock is on PLL ---------- REG_PMC_MCKR  0x",__mac_i:%X;
}



send_nop()
{
	__writeMemory32(0x00000001,0xF000C000,"Memory");
	/* Write to memory to acknoledge the command */
	__writeMemory32(0x00000000,0x20000000,"Memory");
}

send_ext_lmr(opcode, offset)
{
	__writeMemory32(0x00000005,0xF000C000,"Memory");
	/* Write to memory to acknoledge the command */
	__writeMemory32(0x00000000,0x20000000 + (opcode << offset),"Memory");
}

send_lmr()
{
	__writeMemory32(0x00000003,0xF000C000,"Memory");
	/* Write to memory to acknoledge the command */
	__writeMemory32(0x00000000,0x20000000,"Memory");
}

send_calib()
{
	__writeMemory32(0x00000006,0xF000C000,"Memory");
	/* Write to memory to acknoledge the command */
	__writeMemory32(0x00000000,0x20000000,"Memory");
}

send_normal()
{
	__writeMemory32(0x00000000,0xF000C000,"Memory");
	/* Write to memory to acknoledge the command */
	__writeMemory32(0x00000000,0x20000000,"Memory");
}


matrix_configure_slave_ddr()
{

	/* matrix_remove_write_protection(MATRIX0);*/
	__tempo_reg = 0xF00181E4;
	__tempo_var = (0x4D4154u << 8);
	__writeMemory32(__tempo_var, __tempo_reg,"Memory");

	for (__tempo_var = 3 ; __tempo_var < 10 ; __tempo_var = __tempo_var + 1)
	{
		__tempo_reg = 0xF0018200 + 4 * __tempo_var;
		/* matrix_configure_slave_sec(MATRIX0, i, 0xFF, 0xFF, 0xFF); */
		__writeMemory32(0xFFFFFF, __tempo_reg,"Memory");

		/* matrix_set_slave_split_addr(MATRIX0, i, MATRIX_AREA_128M, 0xF);*/
		__tempo_reg = 0xF0018240 + 4 * __tempo_var;
		__writeMemory32(0xFFFF, __tempo_reg,"Memory");

		/* matrix_set_slave_region_size(MATRIX0, i, MATRIX_AREA_128M, 0x1); */
		__tempo_reg = 0xF0018280 + 4 * __tempo_var;
		__writeMemory32(0xF, __tempo_reg,"Memory");
	}
}

initialize_ddr()
{
	REG_MPDDRC_RTR = 0xF000C004;
	matrix_configure_slave_ddr();

	/* Enable DDR and MPDDRC clocks */
	__writeMemory32((1<<2), 0xF0014000, "Memory");

	__writeMemory32(13, 0xF001410C, "Memory");
	__writeMemory32((13 | (1<<28) | (1<<12)) , 0xF001410C, "Memory");

	/* Step 1: Program memory device type */
	__writeMemory32(0x00000004, 0xF000C020, "Memory");

	/* set driver impedance */
	__writeMemory32(0x00000004, 0xF000C034, "Memory");
	__writeMemory32(0x00000002, 0xF000C05C, "Memory");

	/* Step 2: Program features of the DDR3-SDRAM device in the
	 * configuration register and timing parameter registers (TPR0
	 * TPR1 and TPR2) */
	__writeMemory32(0x00d0025d, 0xF000C008, "Memory");

	/* Timings */
	/* tp0 */
	__writeMemory32(0x44439425, 0xF000C00C, "Memory"); //0x32139336
	/* tp1 */
	__writeMemory32(0x0f001d1b, 0xF000C010, "Memory"); //0x03001d1b
	/* tp2 */
	__writeMemory32(0x00072000, 0xF000C014, "Memory"); //0x00072328

	__tempo_var = __readMemory32(0xF000C008,"Memory");
	__ba_offset = (__tempo_var & 0x3) + 9;

	if (!(__tempo_var & (1<<0x22))== (1<<0x22) )
	{
		__ba_offset += ((__tempo_var & (0x3<<2))>>2) + 11;
	}

	__tempo_var = __readMemory32(0xF000C020,"Memory");

	if (__tempo_var & 0x10)
	{
		__ba_offset += 1;
	}
	else
	{
		__ba_offset += 2;
	}

	/*
	 * Step 3: Issue a NOP command to the memory controller using
	 * its mode register (MPDDRC_MR).
	 */
	send_nop();

	/*
	 * Step 4: A pause of at least 500us must be observed before a
	 * single toggle.
	 */
	__delay(50);

	/*
	 * Step 5: Issue a NOP command to the memory controller using
	 * its mode register (MPDDRC_MR). CKE is now driven high.
	 */
	send_nop();
	__delay(10);

	/*
	 * Step 6: Issue Extended Mode Register Set 2 (EMRS2) cycle to
	 * choose between commercial or high temperature
	 * operations.
	 */
	send_ext_lmr(0x2, __ba_offset);
	__delay(10);

	/*
	 * Step 7: Issue Extended Mode Register Set 3 (EMRS3) cycle to set
	 * the Extended Mode Register to 0.
	 */
	send_ext_lmr(0x3, __ba_offset);
	__delay(10);

	/*
	 * Step 8: Issue Extended Mode Register Set 1 (EMRS1) cycle to
	 * disable and to program O.D.S. (Output Driver Strength).
	 */
	send_ext_lmr(0x1, __ba_offset);
	__delay(10);

	/*
	 * Step 9: Write a one to the DLL bit (enable DLL reset) in the MPDDRC
	 * Configuration Register (MPDDRC_CR)
	 */
	/* Not done for DDR3 */

	/*
	 * Step 10: Issue a Mode Register Set (MRS) cycle to reset DLL.
	 */
	send_lmr();
	__delay(10);

	/*
	 * Step 11: Issue a Calibration command (MRS) cycle to calibrate RTT and
	 * RON values for the Process Voltage Temperature (PVT).
	 */
	send_calib();
	__delay(10);

	/*
	 * Step 12: A Normal Mode command is provided.
	 * Program the Normal mode in the MPDDRC_MR and perform a write access
	 * to any DDR3-SDRAM address to acknowledge this command.
	 */
	send_normal();
	__delay(10);
	/*
	 * Step 13: Perform a write access to any DDR3-SDRAM address.
	 */
	__writeMemory32(0x00000000,0x20000000,"Memory");

	/* Last step: Write the refresh rate */
	/* Refresh Timer is (64ms / (bank_size)) * master_clock */
	__writeMemory32(0x511, REG_MPDDRC_RTR,"Memory");
	__delay(10);

}

initialize_clocks()
{
	pmc_select_external_osc();
	pmc_switch_mck_to_main();
	pmc_set_mck_plla_div();
	pmc_set_plla(82);
	pmc_set_mck_prescaler();
	pmc_set_mck_divider();
	pmc_switch_mck_to_pll();
}

/*********************************************************************
* execUserReset()
*********************************************************************/
execUserReset()
{
	__message "--- execUserReset -------------------";

	/* Reset peripherals (using RSTC_CR) */
	__writeMemory32(0xA5000004, 0xF8048000, "Memory");

	/* Disable Watchdog (using WDT_MR) */
	__writeMemory32(0x00008000, 0xF8048044, "Memory");

	/* Disable D-Cache, I-Cache and MMU */
	__jtagCP15WriteReg(1, 0, 0, 0, 0x00C50078);

	/* Disable all interrupts and go to supervisor mode */
	#CPSR = 0xD3;

	/* Zero registers (cannot reset core because it will disable JTAG) */
	#R8_fiq = 0;
	#R9_fiq = 0;
	#R10_fiq = 0;
	#R11_fiq = 0;
	#R12_fiq = 0;
	#SP_fiq = 0;
	#LR_fiq = 0;
	#SPSR_fiq = 0;
	#SP_irq = 0;
	#LR_irq = 0;
	#SPSR_irq = 0;
	#SP_abt = 0;
	#LR_abt = 0;
	#SPSR_abt = 0;
	#SP_und = 0;
	#LR_und = 0;
	#SPSR_und = 0;
	#SP_svc = 0;
	#LR_svc = 0;
	#SPSR_svc = 0;
	#R0 = 0;
	#R1 = 0;
	#R2 = 0;
	#R3 = 0;
	#R4 = 0;
	#R5 = 0;
	#R6 = 0;
	#R7 = 0;
	#R8_usr = 0;
	#R9_usr = 0;
	#R10_usr = 0;
	#R11_usr = 0;
	#R12_usr = 0;
	#SP_usr = 0;
	#LR_usr = 0;
}

/*********************************************************************
* execUserPreload()
*********************************************************************/
execUserPreload()
{
	__message "------------------------------ execUserPreload ---------------------------------";
	/* Reset peripherals (using RSTC_CR) */
	__writeMemory32(0xA5000004, 0xF8048000, "Memory");

	/* Disable Watchdog (using WDT_MR) */
	__writeMemory32(0x00008000, 0xF8048044, "Memory");

	/* Disable D-Cache, I-Cache and MMU */
	__jtagCP15WriteReg(1, 0, 0, 0, 0x00C50078);

	/* Reset L2 Cache controller */
	__writeMemory32(0x0, 0x00A00100, "Memory");


	initialize_clocks();

	initialize_ddr();
}