/*****************************************************
 * gpmc_init(): init gpmc bus
 * Init GPMC for x16, MuxMode (SDRAM in x32).
 * This code can only be executed from SRAM or SDRAM.
 *****************************************************/
void gpmc_init(void)
{
	u32 mux=0, mtype, mwidth, rev, tval;

	rev  = get_cpu_rev();
	if (rev == CPU_2420_2422_ES1)
		tval = 1;
	else
		tval = 0;  /* disable bit switched meaning */

	/* global settings */
	__raw_writel(0x10, GPMC_SYSCONFIG);	/* smart idle */
	__raw_writel(0x0, GPMC_IRQENABLE);	/* isr's sources masked */
	__raw_writel(tval, GPMC_TIMEOUT_CONTROL);/* timeout disable */
#ifdef CONFIG_SYS_NAND_BOOT
	__raw_writel(0x001, GPMC_CONFIG);	/* set nWP, disable limited addr */
#else
	__raw_writel(0x111, GPMC_CONFIG);	/* set nWP, disable limited addr */
#endif

	/* discover bus connection from sysboot */
	if (is_gpmc_muxed() == GPMC_MUXED)
		mux = BIT9;
	mtype = get_gpmc0_type();
	mwidth = get_gpmc0_width();

	/* setup cs0 */
	__raw_writel(0x0, GPMC_CONFIG7_0);	/* disable current map */
	sdelay(1000);

#ifdef CONFIG_SYS_NAND_BOOT
	__raw_writel(H4_24XX_GPMC_CONFIG1_0|mtype|mwidth, GPMC_CONFIG1_0);
#else
	__raw_writel(H4_24XX_GPMC_CONFIG1_0|mux|mtype|mwidth, GPMC_CONFIG1_0);
#endif

#ifdef PRCM_CONFIG_III
	__raw_writel(H4_24XX_GPMC_CONFIG2_0, GPMC_CONFIG2_0);
#endif
	__raw_writel(H4_24XX_GPMC_CONFIG3_0, GPMC_CONFIG3_0);
	__raw_writel(H4_24XX_GPMC_CONFIG4_0, GPMC_CONFIG4_0);
#ifdef PRCM_CONFIG_III
	__raw_writel(H4_24XX_GPMC_CONFIG5_0, GPMC_CONFIG5_0);
	__raw_writel(H4_24XX_GPMC_CONFIG6_0, GPMC_CONFIG6_0);
#endif
	__raw_writel(H4_24XX_GPMC_CONFIG7_0, GPMC_CONFIG7_0);/* enable new mapping */
	sdelay(2000);

	/* setup cs1 */
	__raw_writel(0, GPMC_CONFIG7_1); /* disable any mapping */
	sdelay(1000);
	__raw_writel(H4_24XX_GPMC_CONFIG1_1|mux, GPMC_CONFIG1_1);
	__raw_writel(H4_24XX_GPMC_CONFIG2_1, GPMC_CONFIG2_1);
	__raw_writel(H4_24XX_GPMC_CONFIG3_1, GPMC_CONFIG3_1);
	__raw_writel(H4_24XX_GPMC_CONFIG4_1, GPMC_CONFIG4_1);
	__raw_writel(H4_24XX_GPMC_CONFIG5_1, GPMC_CONFIG5_1);
	__raw_writel(H4_24XX_GPMC_CONFIG6_1, GPMC_CONFIG6_1);
	__raw_writel(H4_24XX_GPMC_CONFIG7_1, GPMC_CONFIG7_1); /* enable mapping */
	sdelay(2000);
}

int dmc_config_zq(struct mem_timings *mem,
		  struct exynos5_phy_control *phy0_ctrl,
		  struct exynos5_phy_control *phy1_ctrl)
{
	unsigned long val = 0;
	int i;

	/*
	 * ZQ Calibration:
	 * Select Driver Strength,
	 * long calibration for manual calibration
	 */
	val = PHY_CON16_RESET_VAL;
	val |= mem->zq_mode_dds << PHY_CON16_ZQ_MODE_DDS_SHIFT;
	val |= mem->zq_mode_term << PHY_CON16_ZQ_MODE_TERM_SHIFT;
	val |= ZQ_CLK_DIV_EN;
	writel(val, &phy0_ctrl->phy_con16);
	writel(val, &phy1_ctrl->phy_con16);

	/* Disable termination */
	if (mem->zq_mode_noterm)
		val |= PHY_CON16_ZQ_MODE_NOTERM_MASK;
	writel(val, &phy0_ctrl->phy_con16);
	writel(val, &phy1_ctrl->phy_con16);

	/* ZQ_MANUAL_START: Enable */
	val |= ZQ_MANUAL_STR;
	writel(val, &phy0_ctrl->phy_con16);
	writel(val, &phy1_ctrl->phy_con16);

	/* ZQ_MANUAL_START: Disable */
	val &= ~ZQ_MANUAL_STR;

	/*
	 * Since we are manaully calibrating the ZQ values,
	 * we are looping for the ZQ_init to complete.
	 */
	i = ZQ_INIT_TIMEOUT;
	while ((readl(&phy0_ctrl->phy_con17) & ZQ_DONE) != ZQ_DONE && i > 0) {
		sdelay(100);
		i--;
	}
	if (!i)
		return -1;
	writel(val, &phy0_ctrl->phy_con16);

	i = ZQ_INIT_TIMEOUT;
	while ((readl(&phy1_ctrl->phy_con17) & ZQ_DONE) != ZQ_DONE && i > 0) {
		sdelay(100);
		i--;
	}
	if (!i)
		return -1;
	writel(val, &phy1_ctrl->phy_con16);

	return 0;
}

void sendBT(char command[]) {
    Serial1.println();
    sdelay(250);
    Serial1.print(command);
    sdelay(250);
    Serial1.println();
    Serial1.flush();
    sdelay(250);
}

/**
 * FUNCTION_PURPOSE: Write Command to LCD
 * FUNCTION_INPUTS: dat- data to be written
 * FUNCTION_OUTPUTS: none
 */
void LCD_DataWrite( char dat)
{
	LCD_Ready();
  LCD_data=dat;	   				// Send the data to LCD
  LCD_rs=1;	   						// Select the Data Register by pulling LCD_rs HIGH
  LCD_rw=0;    	     			// Select the Write Operation by pulling RW LOW
  LCD_en=1;	   						// Send a High-to-Low Pusle at Enable Pin
  sdelay(5);
  LCD_en=0;
	sdelay(5);
}

/**
 * FUNCTION_PURPOSE: Write Command to LCD
 * FUNCTION_INPUTS: cmd- command to be written
 * FUNCTION_OUTPUTS: none
 */
void LCD_CmdWrite(char cmd)
{
	LCD_Ready();
	LCD_data=cmd;     			// Send the command to LCD
	LCD_rs=0;         	 		// Select the Command Register by pulling LCD_rs LOW
  LCD_rw=0;          			// Select the Write Operation  by pulling RW LOW
  LCD_en=1;          			// Send a High-to-Low Pusle at Enable Pin
  sdelay(5);
  LCD_en=0;
	sdelay(5);
}

static inline void dram_enable_dll(struct sunxi_dram_reg *dram, int i) {

	sr32(&dram->dllcr[i], 31, 1, DLL_DISABLE);
	sr32(&dram->dllcr[i], 30, 1, ~DLL_RESET);
	sdelay(0x100);
	sr32(&dram->dllcr[i], 31, 1, DLL_ENABLE);
	sr32(&dram->dllcr[i], 30, 1, ~DLL_RESET);
	sdelay(0x1000);
	sr32(&dram->dllcr[i], 31, 1, DLL_ENABLE);
	sr32(&dram->dllcr[i], 30, 1, DLL_RESET);
	sdelay(0x1000);
}

int  main(){
	int a, b;
	sdelay(0);
	a = 10;
	if(a> 10){
	  fdelay(3, "ms");
	}
	else
	   fdelay(4, "ms");
	sdelay(b, "ms");
	
}

int clock_init(void) {
#if 0
	struct sunxi_ccm_reg *ccm =
		(struct sunxi_ccm_reg *)SUNXI_CCM_BASE;

	/* set clock source to OSC24M */
	sr32(&ccm->cpu_ahb_apb0_cfg, 16, 2, CPU_CLK_SRC_OSC24M);		/* CPU_CLK_SRC_SEL [17:16] */

	/* set the pll1 factors, pll1 out = 24MHz*n*k/m/p */	
	sr32(&ccm->pll1_cfg, 8, 5, PLL1_FACTOR_N);		/* PLL1_FACTOR_N [12:8] */
	sr32(&ccm->pll1_cfg, 4, 2, PLL1_FACTOR_K);		/* PLL1_FACTOR_K [5:4] */
	sr32(&ccm->pll1_cfg, 0, 2, PLL1_FACTOR_M);		/* PLL1_FACTOR_M [1:0] */
	sr32(&ccm->pll1_cfg, 16, 2, PLL1_FACTOR_P);		/* PLL1_FACTOR_P [17:16] */

	/* wait for clock to be stable*/	
	sdelay(0x4000);
	/* set clock divider, cpu:axi:ahb:apb0 = 8:4:2:1 */
	sr32(&ccm->pll1_cfg, 0, 2, AXI_DIV);	/* AXI_CLK_DIV_RATIO [1:0] */
#ifdef CONFIG_SUN5I
	sr32(&ccm->pll1_cfg, 6, 2, AHB_CLK_SRC_AXI);/* AHB_CLK_SRC [7:6] */
#endif
	sr32(&ccm->pll1_cfg, 4, 2, AHB_DIV);	/* AHB_CLK_DIV_RATIO [5:4] */
	sr32(&ccm->pll1_cfg, 9, 2, APB0_DIV);	/* APB0_CLK_DIV_RATIO [9:8] */

	/* change cpu clock source to pll1 */
	sr32(&ccm->pll1_cfg, 16, 2, CPU_CLK_SRC_PLL1);/* CPU_CLK_SRC_SEL [17:16] */
	/* 
	 * if the clock source is changed,
	 * at most wait for 8 present running clock cycles
	 */
	sdelay(10);

	/* config apb1 clock */
	sr32(&ccm->apb1_clk_div_cfg, 24, 2, APB1_CLK_SRC_OSC24M);
	sr32(&ccm->apb1_clk_div_cfg, 16, 2, APB1_FACTOR_N);
	sr32(&ccm->apb1_clk_div_cfg, 0, 5, APB1_FACTOR_M);

	/* open the clock for uart0 */
	sr32((u32 *)SUNXI_CCM_APB1_GATING, 16, 1, CLK_GATE_OPEN);

	/* config nand clock */
	sr32((u32 *)SUNXI_CCM_NAND_SCLK_CFG, 24, 2, NAND_CLK_SRC_OSC24);
	sr32((u32 *)SUNXI_CCM_NAND_SCLK_CFG, 16, 2, NAND_CLK_DIV_N);
	sr32((u32 *)SUNXI_CCM_NAND_SCLK_CFG, 0, 4, NAND_CLK_DIV_M);
	sr32((u32 *)SUNXI_CCM_NAND_SCLK_CFG, 31, 1, CLK_GATE_OPEN);
	/* open clock for nand */
	sr32((u32 *)SUNXI_CCM_AHB_GATING0, 13, 1, CLK_GATE_OPEN);
#endif
	return 0;
}

static inline void dram_ddr_reset(struct sunxi_dram_reg *dram) {


	/* different cpu revision in bit [7:6] */
	if(readl(SUNXI_CPU_CFG)) {
		sr32(&dram->mcr, 12, 1, SDRAM_RST_PIN_HIGH);
		sdelay(0x100);
		sr32(&dram->mcr, 12, 1, SDRAM_RST_PIN_LOW);
	} else {
		sr32(&dram->mcr, 12, 1, SDRAM_RST_PIN_LOW);
		sdelay(0x100);
		sr32(&dram->mcr, 12, 1, SDRAM_RST_PIN_HIGH);
	}

}

/*********************************************************************************
 * prcm_init() - inits clocks for PRCM as defined in clocks.h (config II default).
 *   -- called from SRAM, or Flash (using temp SRAM stack).
 *********************************************************************************/
void prcm_init(void)
{
	u32 div;
	void (*f_lock_pll) (u32, u32, u32, u32);
	extern void *_end_vect, *_start;

	f_lock_pll = (void *)((u32)&_end_vect - (u32)&_start + SRAM_VECT_CODE);

	__raw_writel(0, CM_FCLKEN1_CORE);	   /* stop all clocks to reduce ringing */
	__raw_writel(0, CM_FCLKEN2_CORE);	   /* may not be necessary */
	__raw_writel(0, CM_ICLKEN1_CORE);
	__raw_writel(0, CM_ICLKEN2_CORE);

	__raw_writel(DPLL_OUT, CM_CLKSEL2_PLL);	/* set DPLL out */
	__raw_writel(MPU_DIV, CM_CLKSEL_MPU);	/* set MPU divider */
	__raw_writel(DSP_DIV, CM_CLKSEL_DSP);	/* set dsp and iva dividers */
	__raw_writel(GFX_DIV, CM_CLKSEL_GFX);	/* set gfx dividers */

	div = BUS_DIV;
	__raw_writel(div, CM_CLKSEL1_CORE);/* set L3/L4/USB/Display/Vlnc/SSi dividers */
	sdelay(1000);

	if(running_in_sram()){
		/* If running fully from SRAM this is OK.  The Flash bus drops out for just a little.
		* but then comes back.  If running from Flash this sequence kills you, thus you need
		* to run it using CONFIG_PARTIAL_SRAM.
		*/
		__raw_writel(MODE_BYPASS_FAST, CM_CLKEN_PLL); /* go to bypass, fast relock */
		wait_on_value(BIT0|BIT1, BIT0, CM_IDLEST_CKGEN, LDELAY); /* wait till in bypass */
		sdelay(1000);
		/* set clock selection and dpll dividers. */
		__raw_writel(DPLL_VAL, CM_CLKSEL1_PLL);	 /* set pll for target rate */
		__raw_writel(COMMIT_DIVIDERS, PRCM_CLKCFG_CTRL); /* commit dividers */
		sdelay(10000);
		__raw_writel(DPLL_LOCK, CM_CLKEN_PLL); /* enable dpll */
		sdelay(10000);
		wait_on_value(BIT0|BIT1, BIT1, CM_IDLEST_CKGEN, LDELAY);  /*wait for dpll lock */
	}else if(running_in_flash()){
		/* if running from flash, need to jump to small relocated code area in SRAM.
		 * This is the only safe spot to do configurations from.
		 */
		(*f_lock_pll)(PRCM_CLKCFG_CTRL, CM_CLKEN_PLL, DPLL_LOCK, CM_IDLEST_CKGEN);
	}

	__raw_writel(DPLL_LOCK|APLL_LOCK, CM_CLKEN_PLL);   /* enable apll */
	wait_on_value(BIT8, BIT8, CM_IDLEST_CKGEN, LDELAY);	/* wait for apll lock */
	sdelay(1000);
}

void clock_set_pll1(unsigned int clk)
{
	struct sunxi_ccm_reg * const ccm =
		(struct sunxi_ccm_reg *)SUNXI_CCM_BASE;
	const int p = 0;

	/* Switch cluster 0 to 24MHz clock while changing PLL1 */
	clrsetbits_le32(&ccm->cpu_clk_source, C0_CPUX_CLK_SRC_MASK,
			C0_CPUX_CLK_SRC_OSC24M);

	writel(CCM_PLL1_CTRL_EN | CCM_PLL1_CTRL_P(p) |
	       CCM_PLL1_CLOCK_TIME_2 |
	       CCM_PLL1_CTRL_N(clk / 24000000),
	       &ccm->pll1_c0_cfg);
	/*
	 * Don't bother with the stable-time registers, as it doesn't
	 * wait until the PLL is stable.  Note, that even Allwinner
	 * just uses a delay loop (or rather the AVS timer) for this
	 * instead of the PLL_STABLE_STATUS register.
	 */
	sdelay(2000);

	/* Switch cluster 0 back to PLL1 */
	clrsetbits_le32(&ccm->cpu_clk_source, C0_CPUX_CLK_SRC_MASK,
			C0_CPUX_CLK_SRC_PLL1);
}

void clock_init(void)
{
  /*AXI_DIV_1[1:0]  AXI_CLK_DIV_RATIO 00:/1 AXI Clock source is CPU clock
   *AHB_DIV_2[5:4]  AHP_CLK_DIV_RATIO 01:/2 AHB Clock source is AXI CLOCK
   *APB0_DIV_1[9:8] APB0_CLK_RATIO    00:/2 APB0 clock source is AHB2 clock
   *CPU_CLK_SRC_OSC24M[17:16] CPU_CLK_SRC_SEL 01:OSC24M
   */
   CPU_AHB_APB0_CFG = ((0<<0)|(0x1<<4)|(0<<8)|(1<<16));

   /*bit31:PLL1_Enable 1:Enable
    *bit25:EXG_MODE 0x0:Exchange mode
    *bit[17:16]:PLL1_OUT_EXT_DIVP 0x0:P=1
    *bit[12:8]:PLL1_FACTOR_N 0x10:Factor=16,N=16
    *bit[5:4]:PLL1_FACTOR_K 0x0:K=1
    *bit3:SIG_DELT_PAT_IN 0x0
    *bit2:SIG_DELT_PAT_EN 0x0
    *bit[1:0]PLL1_FACTOR_M 0x0:M=1
    *The PLL1 output=(24M*N*K)/(M*P)=(24M*16*1)/(1*1)=384M is for the coreclk
    */
   PLL1_CFG = 0xa1005000;

   sdelay(200);

   CPU_AHB_APB0_CFG = ((0<<0)|(0x1<<4)|(0<<8)|(2<<16));//CPU_CLK_SRC_SEL 10:PLL1

   /*uart clock source is apb1,config apb1 clock*/
   /*bit[25:24]:APB1_CLK_SRC_SEL 00:OSC24M
    *bit[17:16]:CLK_RAT_N 0X0:1 The select clock source is pre-divided by 2^1
    *bit[4:0]:CLK_RAT_M 0x0:1 The pre-devided clock is divided by(m+1)
    */
   APB1_CLK_DIV_CFG = ((0<<5)|(0<<16)|(0<<24));
   /*open the clock for uart0*/
   /*bit16:UART0_APB_GATING 1:pass 0:mask*/
   APB1_GATE = (0x1<<16);
}

void initservo() {
    int pos = 0;   
    for(pos = 0; pos < 180; pos += 1)  // goes from 0 degrees to 180 degrees 
  {                                  // in steps of 1 degree 
    myservo.write(pos);              // tell servo to go to position in variable 'pos' 
    sdelay(20);
  } 
  sdelay(100);
  for(pos = 180; pos>=1; pos-=1)     // goes from 180 degrees to 0 degrees 
  {                                
    myservo.write(pos);              // tell servo to go to position in variable 'pos' 
    sdelay(20);
    SoftwareServo::refresh();
  } 
  
}

static void config_zq(struct exynos5_phy_control *phy0_ctrl,
			struct exynos5_phy_control *phy1_ctrl)
{
	unsigned long val = 0;
	/*
	 * ZQ Calibration:
	 * Select Driver Strength,
	 * long calibration for manual calibration
	 */
	val = PHY_CON16_RESET_VAL;
	SET_ZQ_MODE_DDS_VAL(val);
	SET_ZQ_MODE_TERM_VAL(val);
	val |= ZQ_CLK_DIV_EN;
	writel(val, &phy0_ctrl->phy_con16);
	writel(val, &phy1_ctrl->phy_con16);

	/* Disable termination */
	val |= ZQ_MODE_NOTERM;
	writel(val, &phy0_ctrl->phy_con16);
	writel(val, &phy1_ctrl->phy_con16);

	/* ZQ_MANUAL_START: Enable */
	val |= ZQ_MANUAL_STR;
	writel(val, &phy0_ctrl->phy_con16);
	writel(val, &phy1_ctrl->phy_con16);
	sdelay(0x10000);

	/* ZQ_MANUAL_START: Disable */
	val &= ~ZQ_MANUAL_STR;
	writel(val, &phy0_ctrl->phy_con16);
	writel(val, &phy1_ctrl->phy_con16);
}

static void config_cdrex(void)
{
	struct exynos5_clock *clk = (struct exynos5_clock *)EXYNOS5_CLOCK_BASE;
	writel(CLK_DIV_CDREX_VAL, &clk->div_cdrex);
	writel(CLK_SRC_CDREX_VAL, &clk->src_cdrex);
	sdelay(0x30000);
}

static void reset_phy_ctrl(void)
{
	struct exynos5_clock *clk = (struct exynos5_clock *)EXYNOS5_CLOCK_BASE;

	writel(PHY_RESET_VAL, &clk->lpddr3phy_ctrl);
	sdelay(0x10000);
}

void main()
{
	brdInit();

	for (;;) // Loop forever
	{
		relayOut(0,	COM_NC);		//connect contact COM to NC
		printf("Relay_COM is connected to Relay_NC contact\n");
		sdelay(2);					//delay for 2 seconds

		relayOut(0, COM_NO);		//turn relay off
		printf("Relay_COM is connected to Relay_NO contact\n");
		sdelay(2);							//delay for 2 seconds
	}
		
}

void clock_set_pll1(unsigned int clk)
{
	struct sunxi_ccm_reg * const ccm =
		(struct sunxi_ccm_reg *)SUNXI_CCM_BASE;
	int k = 1;
	int m = 1;

	if (clk > 1152000000) {
		k = 2;
	} else if (clk > 768000000) {
		k = 3;
		m = 2;
	}

	/* Switch to 24MHz clock while changing PLL1 */
	writel(AXI_DIV_3 << AXI_DIV_SHIFT |
	       ATB_DIV_2 << ATB_DIV_SHIFT |
	       CPU_CLK_SRC_OSC24M << CPU_CLK_SRC_SHIFT,
	       &ccm->cpu_axi_cfg);

	/* PLL1 rate = 24000000 * n * k / m */
	writel(CCM_PLL1_CTRL_EN | CCM_PLL1_CTRL_MAGIC |
	       CCM_PLL1_CTRL_N(clk / (24000000 * k / m)) |
	       CCM_PLL1_CTRL_K(k) | CCM_PLL1_CTRL_M(m), &ccm->pll1_cfg);
	sdelay(200);

	/* Switch CPU to PLL1 */
	writel(AXI_DIV_3 << AXI_DIV_SHIFT |
	       ATB_DIV_2 << ATB_DIV_SHIFT |
	       CPU_CLK_SRC_PLL1 << CPU_CLK_SRC_SHIFT,
	       &ccm->cpu_axi_cfg);
}

/*An example of a single block of soft delay with close finishing time.
	Soft Delay = 1.999 sec 
	Exec Time For Code between soft delays = 2 sec 
*/
int  main(){
	long long unsigned int diff, diff_ms;
	struct timespec st, et;
	clock_gettime(CLOCK_MONOTONIC, &st);
	sdelay(0, "ms");
	printf("Soft Delay 1.999 sec \nSleep 2 sec\n");
	sleep(2);
	sdelay(1999, "ms");
	clock_gettime(CLOCK_MONOTONIC, &et);
	diff =  BILLION * (et.tv_sec - st.tv_sec) + et.tv_nsec - st.tv_nsec;
	diff_ms = diff/1000000;
	printf("elapsed time = %llu ms\n", (long long unsigned int) diff_ms);
	/*sleep(1);
	fdelay(2, "ms");*/
	return 1;
	
}

/**
 * FUNCTION_PURPOSE:LCD Initialization
 * FUNCTION_INPUTS: void
 * FUNCTION_OUTPUTS: none
 */
void LCD_Init()
{
  sdelay(100);
  LCD_CmdWrite(0x38);   	// LCD 2lines, 5*7 matrix
  LCD_CmdWrite(0x0E);			// Display ON cursor ON  Blinking off
  LCD_CmdWrite(0x01);			// Clear the LCD
  LCD_CmdWrite(0x80);			// Cursor to First line First Position
}