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

FUNC void restoreFlexRAM(void)
{
    unsigned int value;

    value = _RDWORD(0x400E4044);
    value &= ~(0xFFFF);
    value |= 0xFFAA;
    _WDWORD(0x400E4044, value);

    value = _RDWORD(0x400E4048);
    value &= ~(0xFFFF);
    value |= 0xFFAA;
    _WDWORD(0x400E4048, value);

    value = _RDWORD(0x400E4040);
    value &= ~(0xFF << 8);
    value |= 0x7 | (0xAA << 8);
    _WDWORD(0x400E4040, value);
}

FUNC void SDRAM_WaitIpCmdDone(void)
{
  unsigned long reg;
  do
  {
    reg = _RDWORD(0x400D403C);
  }while((reg & 0x3) == 0);

  _WDWORD(0x400D403C,0x00000003); // clear IPCMDERR and IPCMDDONE bits
}

FUNC void EnableOSC400M(void)
{
  unsigned int reg;
  // CTRL1: power down
  reg = _RDWORD(0x40C84050);
  reg &= ~0x1;
  _WDWORD(0x40C84050,reg);
  // CTRL2: enable clock
  reg = _RDWORD(0x40C84060);
  reg |= 0x1;
  _WDWORD(0x40C84060,reg);
}

FUNC void EnableOSC24M(void)
{
  unsigned int reg;
  reg = _RDWORD(0x40C84020);
  if(0 == (reg & 0x10))
  {
    reg = 0x14; // OSC_EN and LP_EN
    _WDWORD(0x40C84020,reg);
    reg = _RDWORD(0x40C84020);
    while (0 == (reg & 0x80000000));
  }
}

FUNC void EnablePllLdo(void)
{
  unsigned int reg;
  // CTRL_AI_CTRL
  _WDWORD(0x40C84820,0x00000000);
  // CTRL_AI_WDATA
  _WDWORD(0x40C84830,0x00000105);
  // PMU_LDO_PLL
  reg = _RDWORD(0x40C84500);
  reg |= 0x10000;
  _WDWORD(0x40C84500,reg);
  _Sleep_(100);

  // PMU_POWER_DETECT_CTRL
  _WDWORD(0x40C84580,0x00000100);
  _Sleep_(1);
  // PMU_REF_CTRL
  _WDWORD(0x40C84570,0x00000010);
}

FUNC void InitSysPll2Pfd1(void)
{
  unsigned int reg;
  unsigned int stable;
  // ANADIG_PLL_PLL_528_PFD
  reg = _RDWORD(0x40C84270);
  if (((reg & 0x8000) != 0) || (((reg & 0x3F00) >> 8) != 16))
  {
    stable = reg & 0x4000;

    reg |= 0x8000;
    _WDWORD(0x40C84270,reg);

    reg = _RDWORD(0x40C84270);
    reg &= ~0x3F00;
    reg |= 16 << 8;
    _WDWORD(0x40C84270,reg);

    reg = _RDWORD(0x40C84250);
    reg ^= 0x4;
    _WDWORD(0x40C84250,reg);

    reg = _RDWORD(0x40C84270);
    reg &= ~0x8000;
    _WDWORD(0x40C84270,reg);
    do
    {
        reg = _RDWORD(0x40C84270) & 0x4000;
    } while (reg == stable);
  }
  else
  {
    //syspll2 pfd1 has been initialized already
    reg &= ~0x8000;
    _WDWORD(0x40C84270,reg);
  }
}

FUNC void InitSysPll2(void)
{
  unsigned int reg;

  // ANADIG_PLL_PLL_528_CTRL
  reg = _RDWORD(0x40C84240);
  if (reg & 0x800000)
  {
      // SysPll2 has been initialized
      reg &= ~0x40000000;
      _WDWORD(0x40C84240, reg);
      return;
  }

  reg = _RDWORD(0x40C84270);
  reg |= 0x80808080;
  _WDWORD(0x40C84270, reg);

  reg = _RDWORD(0x40C84240);
  reg &= ~(0x802000);
  reg |= 0x40000000;
  _WDWORD(0x40C84240, reg);

  // ANADIG_PLL_PLL_528_MFN
  _WDWORD(0x40C84280, 0);
  // ANADIG_PLL_PLL_528_MFI
  _WDWORD(0x40C84290, 22);
  // ANADIG_PLL_PLL_528_MFD
  _WDWORD(0x40C842A0, 0x0FFFFFFF);

  // ANADIG_PLL_PLL_528_CTRL
  _WDWORD(0x40C84240, 0x8 | 0x40000000);
  _Sleep_(30);

  // ANADIG_PLL_PLL_528_CTRL
  reg = _RDWORD(0x40C84240);
  reg |= 0x800000 | 0x800;
  _WDWORD(0x40C84240, reg);
  _Sleep_(250);

  reg = _RDWORD(0x40C84240);
  reg &= ~0x800;
  _WDWORD(0x40C84240, reg);

  do
  {
      reg = _RDWORD(0x40C84240);
  } while ((reg & 0x20000000) == 0);

  reg |= 0x2000;
  _WDWORD(0x40C84240, reg);

  reg &= ~0x40000000;
  _WDWORD(0x40C84240, reg);
}

FUNC void SetSemcClock(void)
{
  //EnableOSC400M();
  EnablePllLdo();
  InitSysPll2();
  InitSysPll2Pfd1();

  // Set SEMC root clock
  // Use sys pll2 pfd1 divided by 3: 198Mhz
  _WDWORD(0x40CC0200,0x00000602);
}

FUNC void _clock_init(void)
{
  SetSemcClock();
}

FUNC void _sdr_Init(void)
{
  // Config IOMUX
  _WDWORD(0x400E8010, 0x00000000);
  _WDWORD(0x400E8014, 0x00000000);
  _WDWORD(0x400E8018, 0x00000000);
  _WDWORD(0x400E801C, 0x00000000);
  _WDWORD(0x400E8020, 0x00000000);
  _WDWORD(0x400E8024, 0x00000000);
  _WDWORD(0x400E8028, 0x00000000);
  _WDWORD(0x400E802C, 0x00000000);
  _WDWORD(0x400E8030, 0x00000000);
  _WDWORD(0x400E8034, 0x00000000);
  _WDWORD(0x400E8038, 0x00000000);
  _WDWORD(0x400E803C, 0x00000000);
  _WDWORD(0x400E8040, 0x00000000);
  _WDWORD(0x400E8044, 0x00000000);
  _WDWORD(0x400E8048, 0x00000000);
  _WDWORD(0x400E804C, 0x00000000);
  _WDWORD(0x400E8050, 0x00000000);
  _WDWORD(0x400E8054, 0x00000000);
  _WDWORD(0x400E8058, 0x00000000);
  _WDWORD(0x400E805C, 0x00000000);
  _WDWORD(0x400E8060, 0x00000000);
  _WDWORD(0x400E8064, 0x00000000);
  _WDWORD(0x400E8068, 0x00000000);
  _WDWORD(0x400E806C, 0x00000000);
  _WDWORD(0x400E8070, 0x00000000);
  _WDWORD(0x400E8074, 0x00000000);
  _WDWORD(0x400E8078, 0x00000000);
  _WDWORD(0x400E807C, 0x00000000);
  _WDWORD(0x400E8080, 0x00000000);
  _WDWORD(0x400E8084, 0x00000000);
  _WDWORD(0x400E8088, 0x00000000);
  _WDWORD(0x400E808C, 0x00000000);
  _WDWORD(0x400E8090, 0x00000000);
  _WDWORD(0x400E8094, 0x00000000);
  _WDWORD(0x400E8098, 0x00000000);
  _WDWORD(0x400E809C, 0x00000000);
  _WDWORD(0x400E80A0, 0x00000000);
  _WDWORD(0x400E80A4, 0x00000000);
  _WDWORD(0x400E80A8, 0x00000000);
  _WDWORD(0x400E80AC, 0x00000010); // EMC_39, DQS PIN, enable SION
  _WDWORD(0x400E80B0, 0x00000000);
  _WDWORD(0x400E80B4, 0x00000000);
  _WDWORD(0x400E80B8, 0x00000000);
  _WDWORD(0x400E80BC, 0x00000000);
  _WDWORD(0x400E80C0, 0x00000000);
  _WDWORD(0x400E80C4, 0x00000000);
  _WDWORD(0x400E80C8, 0x00000000);
  _WDWORD(0x400E80CC, 0x00000000);
  _WDWORD(0x400E80D0, 0x00000000);
  _WDWORD(0x400E80D4, 0x00000000);
  _WDWORD(0x400E80D8, 0x00000000);
  _WDWORD(0x400E80DC, 0x00000000);
  _WDWORD(0x400E80E0, 0x00000000);
  _WDWORD(0x400E80E4, 0x00000000);
  _WDWORD(0x400E80E8, 0x00000000);
  _WDWORD(0x400E80EC, 0x00000000);
  _WDWORD(0x400E80F0, 0x00000000);
  _WDWORD(0x400E80F4, 0x00000000);
  _WDWORD(0x400E80F8, 0x00000000);
  _WDWORD(0x400E80FC, 0x00000000);
  _WDWORD(0x400E8100, 0x00000000);
  _WDWORD(0x400E8104, 0x00000000);
  _WDWORD(0x400E8108, 0x00000000);

  // PAD ctrl
  // PDRV = 1b (normal); PULL = 10b (PD)
  _WDWORD(0x400E8254, 0x00000008);
  _WDWORD(0x400E8258, 0x00000008);
  _WDWORD(0x400E825C, 0x00000008);
  _WDWORD(0x400E8260, 0x00000008);
  _WDWORD(0x400E8264, 0x00000008);
  _WDWORD(0x400E8268, 0x00000008);
  _WDWORD(0x400E826C, 0x00000008);
  _WDWORD(0x400E8270, 0x00000008);
  _WDWORD(0x400E8274, 0x00000008);
  _WDWORD(0x400E8278, 0x00000008);
  _WDWORD(0x400E827C, 0x00000008);
  _WDWORD(0x400E8280, 0x00000008);
  _WDWORD(0x400E8284, 0x00000008);
  _WDWORD(0x400E8288, 0x00000008);
  _WDWORD(0x400E828C, 0x00000008);
  _WDWORD(0x400E8290, 0x00000008);
  _WDWORD(0x400E8294, 0x00000008);
  _WDWORD(0x400E8298, 0x00000008);
  _WDWORD(0x400E829C, 0x00000008);
  _WDWORD(0x400E82A0, 0x00000008);
  _WDWORD(0x400E82A4, 0x00000008);
  _WDWORD(0x400E82A8, 0x00000008);
  _WDWORD(0x400E82AC, 0x00000008);
  _WDWORD(0x400E82B0, 0x00000008);
  _WDWORD(0x400E82B4, 0x00000008);
  _WDWORD(0x400E82B8, 0x00000008);
  _WDWORD(0x400E82BC, 0x00000008);
  _WDWORD(0x400E82C0, 0x00000008);
  _WDWORD(0x400E82C4, 0x00000008);
  _WDWORD(0x400E82C8, 0x00000008);
  _WDWORD(0x400E82CC, 0x00000008);
  _WDWORD(0x400E82D0, 0x00000008);
  _WDWORD(0x400E82D4, 0x00000008);
  _WDWORD(0x400E82D8, 0x00000008);
  _WDWORD(0x400E82DC, 0x00000008);
  _WDWORD(0x400E82E0, 0x00000008);
  _WDWORD(0x400E82E4, 0x00000008);
  _WDWORD(0x400E82E8, 0x00000008);
  _WDWORD(0x400E82EC, 0x00000008);
  _WDWORD(0x400E82F0, 0x00000008);
  _WDWORD(0x400E82F4, 0x00000008);
  _WDWORD(0x400E82F8, 0x00000008);
  _WDWORD(0x400E82FC, 0x00000008);
  _WDWORD(0x400E8300, 0x00000008);
  _WDWORD(0x400E8304, 0x00000008);
  _WDWORD(0x400E8308, 0x00000008);
  _WDWORD(0x400E830C, 0x00000008);
  _WDWORD(0x400E8310, 0x00000008);
  _WDWORD(0x400E8314, 0x00000008);
  _WDWORD(0x400E8318, 0x00000008);
  _WDWORD(0x400E831C, 0x00000008);
  _WDWORD(0x400E8320, 0x00000008);
  _WDWORD(0x400E8324, 0x00000008);
  _WDWORD(0x400E8328, 0x00000008);
  _WDWORD(0x400E832C, 0x00000008);
  _WDWORD(0x400E8330, 0x00000008);
  _WDWORD(0x400E8334, 0x00000008);
  _WDWORD(0x400E8338, 0x00000008);
  _WDWORD(0x400E833C, 0x00000008);
  _WDWORD(0x400E8400, 0x00000008);
  _WDWORD(0x400E8404, 0x00000008);
  _WDWORD(0x400E8408, 0x00000008);
  _WDWORD(0x400E840C, 0x00000008);

  // Config SDR Controller Registers/
  _WDWORD(0x400d4000,0x10000004); // MCR
  _WDWORD(0x400d4008,0x00000081); // BMCR0
  _WDWORD(0x400d400C,0x00000081); // BMCR1
  _WDWORD(0x400d4010,0x8000001D); // BR0, 64MB

  _WDWORD(0x400d4040,0x00000F32); // SDRAMCR0, 32bit
  _WDWORD(0x400d4044,0x00772A22); // SDRAMCR1
  _WDWORD(0x400d4048,0x00010A0D); // SDRAMCR2
  _WDWORD(0x400d404C,0x21210408); // SDRAMCR3

  _WDWORD(0x400d4090,0x80000000); // IPCR0
  _WDWORD(0x400d4094,0x00000002); // IPCR1
  _WDWORD(0x400d4098,0x00000000); // IPCR2


  _WDWORD(0x400d409C,0xA55A000F); // IPCMD, SD_CC_IPREA
  SDRAM_WaitIpCmdDone();
  _WDWORD(0x400d409C,0xA55A000C); // SD_CC_IAF
  SDRAM_WaitIpCmdDone();
  _WDWORD(0x400d409C,0xA55A000C); // SD_CC_IAF
  SDRAM_WaitIpCmdDone();
  _WDWORD(0x400d40A0,0x00000033); // IPTXDAT
  _WDWORD(0x400d409C,0xA55A000A); // SD_CC_IMS
  SDRAM_WaitIpCmdDone();
  _WDWORD(0x400d404C,0x21210409 ); // enable sdram self refresh again after initialization done.
}

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

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

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

LOAD %L INCREMENTAL                  // Download

Setup();                             // Setup for Running

// g, main