//
void Load_Dcdc_Trim()
{
  unsigned int ocotp_base;
  unsigned int dcdc_base;
  unsigned int ocotp_fuse_bank0_base;
  unsigned int reg;
  unsigned int index;
  unsigned int trim_value;
  unsigned int dcdc_trim_loaded;

  ocotp_base = 0x401F4000;
  ocotp_fuse_bank0_base = 0x401F4000 + 0x400;
  dcdc_base = 0x40080000;

  dcdc_trim_loaded = 0;

  reg = Target.ReadU32(ocotp_fuse_bank0_base + 0x90);
  if (reg & (1<<10))
  {
      trim_value = (reg & (0x1F << 11)) >> 11;
      reg = (Target.ReadU32(dcdc_base + 0x4) & ~(0x1F << 24)) | (trim_value << 24);
      Target.WriteU32(dcdc_base + 0x4, reg);
      dcdc_trim_loaded = 1;
  }

  // DCDC_VOLT_CHANG_EN
  reg = Target.ReadU32(ocotp_fuse_bank0_base + 0x80);
  if (reg & (1<<30))
  {
    index = (reg & (3 << 28)) >> 28;
    if (index < 4)
    {
      reg = (Target.ReadU32(dcdc_base + 0xC) & ~(0x1F)) | (0xF + index);
      Target.WriteU32(dcdc_base + 0xC, reg);
      dcdc_trim_loaded = 1;
    }
  }

  if (dcdc_trim_loaded)
  {
      // delay 1ms for dcdc to get stable
      Util.Sleep(1);
      Util.Log("DCDC trim value loaded.");
  }
}

void Clock_Init()
{
  // Enable all clocks
  Target.WriteU32(0x400FC068,0xffffffff);
  Target.WriteU32(0x400FC06C,0xffffffff);
  Target.WriteU32(0x400FC070,0xffffffff);
  Target.WriteU32(0x400FC074,0xffffffff);
  Target.WriteU32(0x400FC078,0xffffffff);
  Target.WriteU32(0x400FC07C,0xffffffff);
  Target.WriteU32(0x400FC080,0xffffffff);

  Target.WriteU32(0x400D8030,0x00002001);
  Target.WriteU32(0x400D8100,0x001d0000);
  Target.WriteU32(0x400FC014,0x00010D40);

  Util.Log("Clock Init Done");
}

void SDRAM_WaitIpCmdDone(void)
{
  unsigned int reg;
  do
  {
      reg = Target.ReadU32(0x402F003C);
  }while((reg & 0x3) == 0);

  Target.WriteU32(0x402F003C,0x00000003); // clear IPCMDERR and IPCMDDONE bits
}

void SDRAM_Init() {
  // Config IOMUX for SDRAM
  Target.WriteU32(0x401F8014,0x00000000); // EMC_00
  Target.WriteU32(0x401F8018,0x00000000); // EMC_01
  Target.WriteU32(0x401F801C,0x00000000); // EMC_02
  Target.WriteU32(0x401F8020,0x00000000); // EMC_03
  Target.WriteU32(0x401F8024,0x00000000); // EMC_04
  Target.WriteU32(0x401F8028,0x00000000); // EMC_05
  Target.WriteU32(0x401F802C,0x00000000); // EMC_06
  Target.WriteU32(0x401F8030,0x00000000); // EMC_07
  Target.WriteU32(0x401F8034,0x00000000); // EMC_08
  Target.WriteU32(0x401F8038,0x00000000); // EMC_09
  Target.WriteU32(0x401F803C,0x00000000); // EMC_10
  Target.WriteU32(0x401F8040,0x00000000); // EMC_11
  Target.WriteU32(0x401F8044,0x00000000); // EMC_12
  Target.WriteU32(0x401F8048,0x00000000); // EMC_13
  Target.WriteU32(0x401F804C,0x00000000); // EMC_14
  Target.WriteU32(0x401F8050,0x00000000); // EMC_15
  Target.WriteU32(0x401F8054,0x00000000); // EMC_16
  Target.WriteU32(0x401F8058,0x00000000); // EMC_17
  Target.WriteU32(0x401F805C,0x00000000); // EMC_18
  Target.WriteU32(0x401F8060,0x00000000); // EMC_19
  Target.WriteU32(0x401F8064,0x00000000); // EMC_20
  Target.WriteU32(0x401F8068,0x00000000); // EMC_21
  Target.WriteU32(0x401F806C,0x00000000); // EMC_22
  Target.WriteU32(0x401F8070,0x00000000); // EMC_23
  Target.WriteU32(0x401F8074,0x00000000); // EMC_24
  Target.WriteU32(0x401F8078,0x00000000); // EMC_25
  Target.WriteU32(0x401F807C,0x00000000); // EMC_26
  Target.WriteU32(0x401F8080,0x00000000); // EMC_27
  Target.WriteU32(0x401F8084,0x00000000); // EMC_28
  Target.WriteU32(0x401F8088,0x00000000); // EMC_29
  Target.WriteU32(0x401F808C,0x00000000); // EMC_30
  Target.WriteU32(0x401F8090,0x00000000); // EMC_31
  Target.WriteU32(0x401F8094,0x00000000); // EMC_32
  Target.WriteU32(0x401F8098,0x00000000); // EMC_33
  Target.WriteU32(0x401F809C,0x00000000); // EMC_34
  Target.WriteU32(0x401F80A0,0x00000000); // EMC_35
  Target.WriteU32(0x401F80A4,0x00000000); // EMC_36
  Target.WriteU32(0x401F80A8,0x00000000); // EMC_37
  Target.WriteU32(0x401F80AC,0x00000000); // EMC_38
  Target.WriteU32(0x401F80B0,0x00000010); // EMC_39, DQS PIN, enable SION
  Target.WriteU32(0x401F80B4,0x00000000); // EMC_40
  Target.WriteU32(0x401F80B8,0x00000000); // EMC_41

  // PAD ctrl
  // drive strength = 0x7 to increase drive strength
  // otherwise the data7 bit may fail.
  Target.WriteU32(0x401F8204,0x000110F9); // EMC_00
  Target.WriteU32(0x401F8208,0x000110F9); // EMC_01
  Target.WriteU32(0x401F820C,0x000110F9); // EMC_02
  Target.WriteU32(0x401F8210,0x000110F9); // EMC_03
  Target.WriteU32(0x401F8214,0x000110F9); // EMC_04
  Target.WriteU32(0x401F8218,0x000110F9); // EMC_05
  Target.WriteU32(0x401F821C,0x000110F9); // EMC_06
  Target.WriteU32(0x401F8220,0x000110F9); // EMC_07
  Target.WriteU32(0x401F8224,0x000110F9); // EMC_08
  Target.WriteU32(0x401F8228,0x000110F9); // EMC_09
  Target.WriteU32(0x401F822C,0x000110F9); // EMC_10
  Target.WriteU32(0x401F8230,0x000110F9); // EMC_11
  Target.WriteU32(0x401F8234,0x000110F9); // EMC_12
  Target.WriteU32(0x401F8238,0x000110F9); // EMC_13
  Target.WriteU32(0x401F823C,0x000110F9); // EMC_14
  Target.WriteU32(0x401F8240,0x000110F9); // EMC_15
  Target.WriteU32(0x401F8244,0x000110F9); // EMC_16
  Target.WriteU32(0x401F8248,0x000110F9); // EMC_17
  Target.WriteU32(0x401F824C,0x000110F9); // EMC_18
  Target.WriteU32(0x401F8250,0x000110F9); // EMC_19
  Target.WriteU32(0x401F8254,0x000110F9); // EMC_20
  Target.WriteU32(0x401F8258,0x000110F9); // EMC_21
  Target.WriteU32(0x401F825C,0x000110F9); // EMC_22
  Target.WriteU32(0x401F8260,0x000110F9); // EMC_23
  Target.WriteU32(0x401F8264,0x000110F9); // EMC_24
  Target.WriteU32(0x401F8268,0x000110F9); // EMC_25
  Target.WriteU32(0x401F826C,0x000110F9); // EMC_26
  Target.WriteU32(0x401F8270,0x000110F9); // EMC_27
  Target.WriteU32(0x401F8274,0x000110F9); // EMC_28
  Target.WriteU32(0x401F8278,0x000110F9); // EMC_29
  Target.WriteU32(0x401F827C,0x000110F9); // EMC_30
  Target.WriteU32(0x401F8280,0x000110F9); // EMC_31
  Target.WriteU32(0x401F8284,0x000110F9); // EMC_32
  Target.WriteU32(0x401F8288,0x000110F9); // EMC_33
  Target.WriteU32(0x401F828C,0x000110F9); // EMC_34
  Target.WriteU32(0x401F8290,0x000110F9); // EMC_35
  Target.WriteU32(0x401F8294,0x000110F9); // EMC_36
  Target.WriteU32(0x401F8298,0x000110F9); // EMC_37
  Target.WriteU32(0x401F829C,0x000110F9); // EMC_38
  Target.WriteU32(0x401F82A0,0x000110F9); // EMC_39
  Target.WriteU32(0x401F82A4,0x000110F9); // EMC_40
  Target.WriteU32(0x401F82A8,0x000110F9); // EMC_41

  // Config SDR Controller Registers/
  Target.WriteU32(0x402F0000,0x10000004); // MCR
  Target.WriteU32(0x402F0008,0x00030524); // BMCR0
  Target.WriteU32(0x402F000C,0x06030524); // BMCR1
  Target.WriteU32(0x402F0010,0x80000019); // BR0, 16MB

  Target.WriteU32(0x402F0040,0x00000F31); // SDRAMCR0
  Target.WriteU32(0x402F0044,0x00652922); // SDRAMCR1
  Target.WriteU32(0x402F0048,0x000a0b0d); // SDRAMCR2
  Target.WriteU32(0x402F004C,0x0f0f0a00); // SDRAMCR3

  Target.WriteU32(0x402F0090,0x80000000); // IPCR0
  Target.WriteU32(0x402F0094,0x00000002); // IPCR1
  Target.WriteU32(0x402F0098,0x00000000); // IPCR2

  Target.WriteU32(0x402F009C,0xA55A000F); // IPCMD, SD_CC_IPREA
  SDRAM_WaitIpCmdDone();
  Target.WriteU32(0x402F009C,0xA55A000C); // SD_CC_IAF
  SDRAM_WaitIpCmdDone();
  Target.WriteU32(0x402F009C,0xA55A000C); // SD_CC_IAF
  SDRAM_WaitIpCmdDone();
  Target.WriteU32(0x402F00A0,0x00000033); // IPTXDAT
  Target.WriteU32(0x402F009C,0xA55A000A); // SD_CC_IMS
  SDRAM_WaitIpCmdDone();
  Target.WriteU32(0x402F004C,0x08080A01 ); // enable sdram self refresh again after initialization done.

  Util.Log("SDRAM Init Done");
}


void INTRAM_Init() {

	unsigned int gpr14_addr;
	unsigned int gpr16_addr;
	unsigned int gpr17_addr;
	unsigned int ret;

	gpr14_addr = 0x400AC038;
	gpr16_addr = 0x400AC040;
	gpr17_addr = 0x400AC044;
	ret = 0;

	// 448 KBytes of DTCM
	Target.WriteU32(gpr17_addr,0x5AAAAAAA);


	ret = Target.ReadU32(gpr16_addr);

	// Turn off DTCM
	//ret &= ~0x02;
	// Turn off ITCM
	ret &= ~0x01;
	Target.WriteU32(gpr16_addr,ret);


	// Configure DTCM/ITCM size
	ret = Target.ReadU32(gpr14_addr);
	// Mask ITCM/DTCM size bits
	ret &= ~0xFF0000;
	// Set DTCM size to 512KBytes
	ret |= 0xA00000;
	Target.WriteU32(gpr14_addr,ret);

	ret = Target.ReadU32(gpr16_addr);

	// FlexRAM_BANK_CFG_SEL
	ret &= ~0x04;
	ret |= 0x04;
	Target.WriteU32(gpr16_addr,ret);

	Util.Log("INTRAM Init Done");
}

/*********************************************************************
*                                                                     
*       OnProjectLoad                                                 
*                                                                     
* Function description                                                
*   Project load routine. Required.                                   
*                                                                     
**********************************************************************
*/                                                                    
void OnProjectLoad (void) {
  //
  // Dialog-generated settings
  //
  Project.SetDevice ("MCIMXRT1051");
  Project.SetHostIF ("USB", "");
  Project.SetTargetIF ("SWD");
  Project.SetTIFSpeed ("1 MHz");
  Project.AddSvdFile ("$(InstallDir)/Config/CPU/Cortex-M7F.svd");
  Project.AddPathSubstitute ("/home/mati/Software/Mudita/git/PurePhone", "$(ProjectDir)");
  Project.SetOSPlugin("FreeRTOSPlugin");
  //
  // User settings
  //
  File.Open ("$(ProjectDir)/build/PurePhone.elf");
}

/*********************************************************************
*                                                                     
*      TargetReset                                                    
*                                                                     
* Function description                                                
*   Replaces the default target device reset routine. Optional.       
*                                                                     
* Notes                                                               
*   This example demonstrates the usage when                          
*   debugging a RAM program on a Cortex-M target device               
*                                                                     
**********************************************************************
*/                                                                    
void TargetReset (void) {
  Exec.Reset();
}

/*********************************************************************
*
*       BeforeTargetReset
*
* Function description
*   Event handler routine. Optional.
*
**********************************************************************
*/
void BeforeTargetReset (void) {

}

/*********************************************************************
*
*       AfterTargetReset
*
* Function description
*   Event handler routine.
*    - Sets the PC register to program reset value.
*    - Sets the SP register to program reset value on Cortex-M.
*
**********************************************************************
*/
void AfterTargetReset (void) {

	  Load_Dcdc_Trim();
	  Clock_Init();
	  SDRAM_Init();
	  INTRAM_Init();
}

/*********************************************************************
*
*       DebugStart
*
* Function description
*   Replaces the default debug session startup routine. Optional.
*
**********************************************************************
*/
//void DebugStart (void) {
//}

/*********************************************************************
*
*       TargetConnect
*
* Function description
*   Replaces the default target IF connection routine. Optional.
*
**********************************************************************
*/
//void TargetConnect (void) {
//}

/*********************************************************************
*
*       BeforeTargetConnect
*
* Function description
*   Event handler routine. Optional.
*
**********************************************************************
*/
//void BeforeTargetConnect (void) {
//}

/*********************************************************************
*
*       AfterTargetConnect
*
* Function description
*   Event handler routine. Optional.
*
**********************************************************************
*/
//void AfterTargetConnect (void) {
//}

/*********************************************************************
*
*       TargetDownload
*
* Function description
*   Replaces the default program download routine. Optional.
*
**********************************************************************
*/
//void TargetDownload (void) {
//}

/*********************************************************************
*
*       BeforeTargetDownload
*
* Function description
*   Event handler routine. Optional.
*
**********************************************************************
*/
//void BeforeTargetDownload (void) {
//}

/*********************************************************************
*
*      AfterTargetDownload
*
* Function description
*   Event handler routine.
*    - Sets the PC register to program reset value.
*    - Sets the SP register to program reset value on Cortex-M.
*
**********************************************************************
*/
void AfterTargetDownload (void) {
  unsigned int SP;
  unsigned int PC;
  unsigned int VectorTableAddr;

  VectorTableAddr = Elf.GetExprValue( "g_pfnVectors" );

  if (VectorTableAddr == 0xFFFFFFFF) {
    Util.Log("Project file error: failed to get program base");
  } else {
    SP = Target.ReadU32(VectorTableAddr);
    Target.SetReg("SP", SP);

    PC = Target.ReadU32(VectorTableAddr + 4);
    Target.SetReg("PC", PC);
  }
}

/*********************************************************************
*
*       BeforeTargetDisconnect
*
* Function description
*   Event handler routine. Optional.
*
**********************************************************************
*/
//void BeforeTargetDisconnect (void) {
//}

/*********************************************************************
*
*       AfterTargetDisconnect
*
* Function description
*   Event handler routine. Optional.
*
**********************************************************************
*/
//void AfterTargetDisconnect (void) {
//}

/*********************************************************************
*
*       AfterTargetHalt
*
* Function description
*   Event handler routine. Optional.
*
**********************************************************************
*/
//void AfterTargetHalt (void) {
//}