/* mcxa.c
 *
 * Copyright (C) 2021 wolfSSL Inc.
 *
 * This file is part of wolfBoot.
 *
 * wolfBoot is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * wolfBoot is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA
 */

#include <stdint.h>
#include <target.h>
#include "image.h"
/* FSL includes */
#include "fsl_common.h"

/* Clock + RAM voltage settings */
#include "fsl_clock.h"
#include "fsl_spc.h"

/* Flash driver */
#include "fsl_romapi.h"

#define BOARD_BOOTCLOCKFRO96M_CORE_CLOCK           96000000U  /*!< Core clock frequency: 96000000Hz */

static flash_config_t pflash;
//static ftfx_cache_config_t pcache;
static int flash_init = 0;

#ifdef __WOLFBOOT
/* Assert hook needed by Kinetis SDK */
void __assert_func(const char *a, int b, const char *c, const char *d)
{
    while(1)
        ;
}

/* The following clock setting function is autogenerated by the MCUXpresso IDE */
void BOARD_BootClockFRO96M(void)
{
    uint32_t coreFreq;
    spc_active_mode_core_ldo_option_t ldoOption;
    spc_sram_voltage_config_t sramOption;

    /* Get the CPU Core frequency */
    coreFreq = CLOCK_GetCoreSysClkFreq();

    /* The flow of increasing voltage and frequency */
    if (coreFreq <= BOARD_BOOTCLOCKFRO96M_CORE_CLOCK) {
        /* Set the LDO_CORE VDD regulator level */
        ldoOption.CoreLDOVoltage = kSPC_CoreLDO_NormalVoltage;
        ldoOption.CoreLDODriveStrength = kSPC_CoreLDO_NormalDriveStrength;
        (void)SPC_SetActiveModeCoreLDORegulatorConfig(SPC0, &ldoOption);
        /* Configure Flash to support different voltage level and frequency */
        FMU0->FCTRL = (FMU0->FCTRL & ~((uint32_t)FMU_FCTRL_RWSC_MASK)) | (FMU_FCTRL_RWSC(0x2U));
        /* Specifies the operating voltage for the SRAM's read/write timing margin */
        sramOption.operateVoltage = kSPC_sramOperateAt1P1V;
        sramOption.requestVoltageUpdate =  true;
        (void)SPC_SetSRAMOperateVoltage(SPC0, &sramOption);
    }

    CLOCK_SetupFROHFClocking(96000000U);               /*!< Enable FRO HF(96MHz) output */

    CLOCK_SetupFRO12MClocking();                /*!< Setup FRO12M clock */

    CLOCK_AttachClk(kFRO_HF_to_MAIN_CLK);       /* !< Switch MAIN_CLK to FRO_HF */

    /* The flow of decreasing voltage and frequency */
    if (coreFreq > BOARD_BOOTCLOCKFRO96M_CORE_CLOCK) {
        /* Configure Flash to support different voltage level and frequency */
        FMU0->FCTRL = (FMU0->FCTRL & ~((uint32_t)FMU_FCTRL_RWSC_MASK)) | (FMU_FCTRL_RWSC(0x2U));
        /* Specifies the operating voltage for the SRAM's read/write timing margin */
        sramOption.operateVoltage = kSPC_sramOperateAt1P1V;
        sramOption.requestVoltageUpdate =  true;
        (void)SPC_SetSRAMOperateVoltage(SPC0, &sramOption);
        /* Set the LDO_CORE VDD regulator level */
        ldoOption.CoreLDOVoltage = kSPC_CoreLDO_NormalVoltage;
        ldoOption.CoreLDODriveStrength = kSPC_CoreLDO_NormalDriveStrength;
        (void)SPC_SetActiveModeCoreLDORegulatorConfig(SPC0, &ldoOption);
    }

    /*!< Set up clock selectors - Attach clocks to the peripheries */

    /*!< Set up dividers */
    CLOCK_SetClockDiv(kCLOCK_DivAHBCLK, 1U);               /* !< Set AHBCLKDIV divider to value 1 */
    CLOCK_SetClockDiv(kCLOCK_DivFRO_HF_DIV, 1U);           /* !< Set FROHFDIV divider to value 1 */
}


void hal_init(void)
{
    /* Clock setting  */
    BOARD_BootClockFRO96M();

    /* Clear the FLASH configuration structure */
    memset(&pflash, 0, sizeof(pflash));
    /* FLASH driver init */
    FLASH_Init(&pflash);
}

void hal_prepare_boot(void)
{
}


#endif

int RAMFUNCTION hal_flash_write(uint32_t address, const uint8_t *data, int len)
{
    int w = 0;
    int ret;
    const uint8_t empty_qword[16] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
                                     0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};

    while (len > 0) {
        if ((len < 16) || address & 0x0F) {
            uint8_t aligned_qword[16];
            uint32_t address_align = address - (address & 0x0F);
            uint32_t start_off = address - address_align;
            int i;
            memcpy(aligned_qword, (void*)address_align, 16);
            for (i = start_off; ((i < 16) && (i < len + (int)start_off)); i++)
                aligned_qword[i] = data[w++];
            if (memcmp(aligned_qword, empty_qword, 16) != 0) {
                    ret = FLASH_ProgramPhrase(&pflash, address_align, aligned_qword, 16);
                if (ret != kStatus_Success)
                    return -1;
            }
            address += i;
            len -= i;
        } else {
            uint32_t len_align = len - (len & 0x0F);
            ret = FLASH_ProgramPhrase(&pflash, address, (uint8_t*)data + w, len_align);
            if (ret != kStatus_Success)
                return -1;
            len -= len_align;
            address += len_align;
        }
    }
    return 0;
}

void RAMFUNCTION hal_flash_unlock(void)
{
}

void RAMFUNCTION hal_flash_lock(void)
{
}


int RAMFUNCTION hal_flash_erase(uint32_t address, int len)
{
    while ((address % 4) != 0)
        address --;
    if (FLASH_EraseSector(&pflash, address, len, kFLASH_ApiEraseKey) != kStatus_Success)
        return -1;
    return 0;
}