/*
 * Copyright 2018-2020 NXP
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

FUNC void _loadDcdcTrim(void)
{
    unsigned int dcdc_trim_loaded;
    unsigned long ocotp_base;
    unsigned long ocotp_fuse_bank0_base;
    unsigned long dcdc_base;
    unsigned long reg;
    unsigned long trim_value;
    unsigned int index;

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

    reg = _RDWORD(ocotp_fuse_bank0_base + 0x90);
    if (reg & (1<<10))
    {
      // DCDC: REG0->VBG_TRM
       trim_value = (reg & (0x1F << 11)) >> 11; 
       reg = (_RDWORD(dcdc_base + 0x4) & ~(0x1F << 24)) | (trim_value << 24);
       _WDWORD(dcdc_base + 0x4, reg);
       dcdc_trim_loaded = 1;
    }

    reg = _RDWORD(ocotp_fuse_bank0_base + 0x80);
    if (reg & (1<<30))
    {
        index = (reg & (3 << 28)) >> 28;
        if (index < 4)
        {
            // DCDC: REG3->TRG 
            reg = (_RDWORD(dcdc_base + 0xC) & ~(0x1F)) | ((0xF + index));
            _WDWORD(dcdc_base + 0xC, reg);
            dcdc_trim_loaded = 1;
        }
    }

    if (dcdc_trim_loaded)
    {
        // delay about 400us till dcdc is stable.
        _Sleep_(1);
    }
}

FUNC void SDRAM_WaitIpCmdDone(void)
{
  unsigned long reg;
  do
  {
    reg = _RDWORD(0x402F003C);
  }while((reg & 0x3) == 0);
  
  _WDWORD(0x402F003C,0x00000003); // clear IPCMDERR and IPCMDDONE bits
}

FUNC void _clock_init(void)
{
  unsigned int reg;
  // Enable all clocks
  _WDWORD(0x400FC068,0xffffffff);
  _WDWORD(0x400FC06C,0xffffffff);
  _WDWORD(0x400FC070,0xffffffff);
  _WDWORD(0x400FC074,0xffffffff);
  _WDWORD(0x400FC078,0xffffffff);
  _WDWORD(0x400FC07C,0xffffffff);
  _WDWORD(0x400FC080,0xffffffff);

  // PERCLK_PODF: 1 divide by 2
  _WDWORD(0x400FC01C, 0x04900001);
  // Enable SYS PLL but keep it bypassed.
  _WDWORD(0x400D8030, 0x00012001);
  do
  {
    reg = _RDWORD(0x400D8030);
  }while((reg & 0x80000000) == 0);
  // Disable bypass of SYS PLL
  _WDWORD(0x400D8030, 0x00002001);
  
  // PFD2_FRAC: 29, PLL2 PFD2=528*18/PFD2_FRAC=327
  // Ungate SYS PLL PFD2
  reg = _RDWORD(0x400D8100);
  reg &= ~0xBF0000;
  reg |= 0x1D0000;
  _WDWORD(0x400D8100,reg);
  
  // SEMC_PODF: 001, AHB_PODF: 011, IPG_PODF: 01
  // SEMC_ALT_CLK_SEL: 0 PLL2 (SYS PLL) PFD2
  // SEMC_CLK_SEL: 1 SEMC_ALT_CLK
  _WDWORD(0x400FC014, 0x00010D40);
}

FUNC void _sdr_Init(void)
{
  // Config IOMUX
  _WDWORD(0x401F8014, 0x00000000);
  _WDWORD(0x401F8018, 0x00000000);
  _WDWORD(0x401F801C, 0x00000000);
  _WDWORD(0x401F8020, 0x00000000);
  _WDWORD(0x401F8024, 0x00000000);
  _WDWORD(0x401F8028, 0x00000000);
  _WDWORD(0x401F802C, 0x00000000);
  _WDWORD(0x401F8030, 0x00000000);
  _WDWORD(0x401F8034, 0x00000000);
  _WDWORD(0x401F8038, 0x00000000);
  _WDWORD(0x401F803C, 0x00000000);
  _WDWORD(0x401F8040, 0x00000000);
  _WDWORD(0x401F8044, 0x00000000);
  _WDWORD(0x401F8048, 0x00000000);
  _WDWORD(0x401F804C, 0x00000000);
  _WDWORD(0x401F8050, 0x00000000);
  _WDWORD(0x401F8054, 0x00000000);
  _WDWORD(0x401F8058, 0x00000000);
  _WDWORD(0x401F805C, 0x00000000);
  _WDWORD(0x401F8060, 0x00000000);
  _WDWORD(0x401F8064, 0x00000000);
  _WDWORD(0x401F8068, 0x00000000);
  _WDWORD(0x401F806C, 0x00000000);
  _WDWORD(0x401F8070, 0x00000000);
  _WDWORD(0x401F8074, 0x00000000);
  _WDWORD(0x401F8078, 0x00000000);
  _WDWORD(0x401F807C, 0x00000000);
  _WDWORD(0x401F8080, 0x00000000);
  _WDWORD(0x401F8084, 0x00000000);
  _WDWORD(0x401F8088, 0x00000000);
  _WDWORD(0x401F808C, 0x00000000);
  _WDWORD(0x401F8090, 0x00000000);
  _WDWORD(0x401F8094, 0x00000000);
  _WDWORD(0x401F8098, 0x00000000);
  _WDWORD(0x401F809C, 0x00000000);
  _WDWORD(0x401F80A0, 0x00000000);
  _WDWORD(0x401F80A4, 0x00000000);
  _WDWORD(0x401F80A8, 0x00000000);
  _WDWORD(0x401F80AC, 0x00000000);
  _WDWORD(0x401F80B0, 0x00000010); // EMC_39, DQS PIN, enable SION
  
  // PAD ctrl
  // drive strength = 0x7 to increase drive strength
  // otherwise the data7 bit may fail.
  _WDWORD(0x401F8204, 0x000110F9);
  _WDWORD(0x401F8208, 0x000110F9);
  _WDWORD(0x401F820C, 0x000110F9);
  _WDWORD(0x401F8210, 0x000110F9);
  _WDWORD(0x401F8214, 0x000110F9);
  _WDWORD(0x401F8218, 0x000110F9);
  _WDWORD(0x401F821C, 0x000110F9);
  _WDWORD(0x401F8220, 0x000110F9);
  _WDWORD(0x401F8224, 0x000110F9);
  _WDWORD(0x401F8228, 0x000110F9);
  _WDWORD(0x401F822C, 0x000110F9);
  _WDWORD(0x401F8230, 0x000110F9);
  _WDWORD(0x401F8234, 0x000110F9);
  _WDWORD(0x401F8238, 0x000110F9);
  _WDWORD(0x401F823C, 0x000110F9);
  _WDWORD(0x401F8240, 0x000110F9);
  _WDWORD(0x401F8244, 0x000110F9);
  _WDWORD(0x401F8248, 0x000110F9);
  _WDWORD(0x401F824C, 0x000110F9);
  _WDWORD(0x401F8250, 0x000110F9);
  _WDWORD(0x401F8254, 0x000110F9);
  _WDWORD(0x401F8258, 0x000110F9);
  _WDWORD(0x401F825C, 0x000110F9);
  _WDWORD(0x401F8260, 0x000110F9);
  _WDWORD(0x401F8264, 0x000110F9);
  _WDWORD(0x401F8268, 0x000110F9);
  _WDWORD(0x401F826C, 0x000110F9);
  _WDWORD(0x401F8270, 0x000110F9);
  _WDWORD(0x401F8274, 0x000110F9);
  _WDWORD(0x401F8278, 0x000110F9);
  _WDWORD(0x401F827C, 0x000110F9);
  _WDWORD(0x401F8280, 0x000110F9);
  _WDWORD(0x401F8284, 0x000110F9);
  _WDWORD(0x401F8288, 0x000110F9);
  _WDWORD(0x401F828C, 0x000110F9);
  _WDWORD(0x401F8290, 0x000110F9);
  _WDWORD(0x401F8294, 0x000110F9);
  _WDWORD(0x401F8298, 0x000110F9);
  _WDWORD(0x401F829C, 0x000110F9);
  _WDWORD(0x401F82A0, 0x000110F9);

  // Config SDR Controller Registers/
  _WDWORD(0x402F0000,0x10000004); // MCR
  _WDWORD(0x402F0008,0x00000081); // BMCR0
  _WDWORD(0x402F000C,0x00000081); // BMCR1
  _WDWORD(0x402F0010,0x8000001B); // BR0, 32MB
  
  _WDWORD(0x402F0040,0x00000F31); // SDRAMCR0
  _WDWORD(0x402F0044,0x00652922); // SDRAMCR1
  _WDWORD(0x402F0048,0x00010920); // SDRAMCR2
  _WDWORD(0x402F004C,0x50210A08); // SDRAMCR3

  _WDWORD(0x402F0090,0x80000000); // IPCR0
  _WDWORD(0x402F0094,0x00000002); // IPCR1
  _WDWORD(0x402F0098,0x00000000); // IPCR2

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

FUNC void restoreFlexRAM(void)
{
    unsigned int value;
    unsigned int base;

    base = 0x400AC000;

    value = _RDWORD(base + 0x44);
    value &= ~(0xFFFFFFFF);
    value |= 0x55AFFA55;
    _WDWORD(base + 0x44, value);

    value = _RDWORD(base + 0x40);
    value |= (1 << 2);
    _WDWORD(base + 0x40, value);
}

FUNC void Setup (void) {
  _loadDcdcTrim();
  SP = _RDWORD(0x00000000);          // Setup Stack Pointer
  PC = _RDWORD(0x00000004);          // Setup Program Counter
  _WDWORD(0xE000ED08, 0x00000000);   // Setup Vector Table Offset Register
}

FUNC void OnResetExec (void)  {      // executes upon software RESET
  restoreFlexRAM();
  _clock_init();
  _sdr_Init();
  Setup();                           // Setup for Running
}

restoreFlexRAM();
_clock_init();
_sdr_Init();

LOAD %L INCREMENTAL                  // Download

Setup();                             // Setup for Running

// g, main