/*
 * 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 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();
  Setup();                           // Setup for Running
}

restoreFlexRAM();

LOAD %L INCREMENTAL                  // Download

Setup();                             // Setup for Running

// g, main