/*
 * Copyright (c) 2015, Freescale Semiconductor, Inc.
 * Copyright 2016-2021 NXP
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <stdint.h>
#include <stdbool.h>

#include "boot.h"
#include "flash_partitioning.h"
#include "bootutil/bootutil_log.h"
#include "bootutil/image.h"
#include "bootutil/bootutil.h"
#include "sysflash/sysflash.h"
#include "flash_map.h"
#include "bootutil_priv.h"

#include "fsl_debug_console.h"
#include "mflash_drv.h"

#ifdef CONFIG_MCUBOOT_ENCRYPTED_XIP_SUPPORT
#include "mcuboot_enc_support.h"
#endif

#include <cr_section_macros.h>
/*******************************************************************************
 * Definitions
 ******************************************************************************/

#ifdef NDEBUG
#undef assert
#define assert(x) ((void)(x))
#endif

/*******************************************************************************
 * Prototypes
 ******************************************************************************/

#ifdef CONFIG_BOOT_SIGNATURE
status_t CRYPTO_InitHardware(void);
#endif

#ifdef CONFIG_MCUBOOT_FLASH_REMAP_ENABLE
extern void SBL_EnableRemap(uint32_t start_addr, uint32_t end_addr, uint32_t off);
extern void SBL_DisableRemap(void);
#endif

#pragma weak cleanup
void cleanup(void);

extern void SBL_DisablePeripherals(void);

/*******************************************************************************
 * Types
 ******************************************************************************/

struct arm_vector_table
{
    uint32_t msp;
    uint32_t reset;
};

static struct arm_vector_table *vt;

/*******************************************************************************
 * Code
 ******************************************************************************/
__RAMFUNC(SRAM) static void hidden_bl_flash_access(void)
{
	uint8_t glbac_hidden_bl[16] = {7, 7, 7, 4, 4, 4, 4, 4, 4, 0, 0, 0, 0, 0, 0, 0};
	uint8_t *glbac = glbac_hidden_bl;
    uint32_t val;
    uint32_t *cfg_w = (uint32_t *) &(MBC0->MBC_INDEX[0].MBC_DOM0_MEM0_BLK_CFG_W[0]);

    /* Configure the MBC to hidden the mcuboot when FLASH_ACL in IFR0 is invalid */
//    if ((*((volatile const uint32_t *)(0x1000000)) == 0xFFFFFFFFU) ||
//        ((*((volatile const uint32_t *)(0x1000000)) == 0x59630000U) &&
//         (*((volatile const uint32_t *)(0x1000040)) == 0xFFFFFFFFU) &&
//         (*((volatile const uint32_t *)(0x1000044)) == 0xFFFFFFFFU)))
    {
        /* Enable MBC register written with GLIKEY index15 */
        GLIKEY0->CTRL_0 = 0x00060000U;
        GLIKEY0->CTRL_0 = 0x0002000FU;
        GLIKEY0->CTRL_0 = 0x0001000FU;
        GLIKEY0->CTRL_1 = 0x00290000U;
        GLIKEY0->CTRL_0 = 0x0002000FU;
        GLIKEY0->CTRL_1 = 0x00280000U;
        GLIKEY0->CTRL_0 = 0x0000000FU;

//        /* Enable RWX for GLBAC0 */
//        MBC0->MBC_INDEX[0].MBC_MEMN_GLBAC[0] = 0x7700U;

        for (int i=0; i < 2; i++)
        {
            for (int block=0; block < 8; block++)
            {
                /* prepare */
                val = *cfg_w;
                val &= ~(0x7 << (block*4));
                val |= (*glbac & 0x7) << (block*4);

                /* set */
                *cfg_w = val;

                /* read back */
                val = (*cfg_w >> (block*4)) & 0x7;
                if (val != *glbac)
                {
                    PRINTF("\n! Failed to set MBACSEL%u for block %u !\n", block, i*8 + block);
                }
                glbac++;
            }
            cfg_w++;
        }

        /* Disable MBC register written */
        GLIKEY0->CTRL_0 = 0x0002000FU;
    }
}
__RAMFUNC(SRAM) static void hidden_and_jump(void)
{
	//hidden mcuboot
	hidden_bl_flash_access();
	//jump
    __set_CONTROL(0);
    __set_MSP(vt->msp);
    __ISB();
    ((void (*)(void))vt->reset)();
}

/* Starts selected application */

void do_boot(struct boot_rsp *rsp)
{
    uintptr_t flash_base;
    int rc;

    /* The beginning of the image is the ARM vector table, containing
     * the initial stack pointer address and the reset vector
     * consecutively. Manually set the stack pointer and jump into the
     * reset vector
     */
    rc = flash_device_base(rsp->br_flash_dev_id, &flash_base);
    assert(rc == 0);

#if defined(MCUBOOT_DIRECT_XIP) && defined(CONFIG_MCUBOOT_FLASH_REMAP_ENABLE)

    /* In case direct-xip mode and enabled flash remapping function check if
     * the secondary slot is chosen to boot. If so we have to modify boot_rsp
     * structure here and enable flash remapping just before the jumping to app.
     * Flash remapping function has to be disabled when bootloader starts.
     */

    if (rsp->br_image_off == (BOOT_FLASH_CAND_APP - BOOT_FLASH_BASE))
    {
        uintptr_t start, end, off;
        start = BOOT_FLASH_ACT_APP;
        end   = BOOT_FLASH_ACT_APP + (BOOT_FLASH_CAND_APP - BOOT_FLASH_ACT_APP);
        off   = BOOT_FLASH_CAND_APP - BOOT_FLASH_ACT_APP;

        SBL_EnableRemap(start, end, off);
        rsp->br_image_off = BOOT_FLASH_ACT_APP - BOOT_FLASH_BASE;
        PRINTF("Booting the secondary slot - flash remapping is enabled\n");
    }
    else
    {
        PRINTF("Booting the primary slot - flash remapping is disabled\n");
    }
#endif

    vt = (struct arm_vector_table *)(flash_base + rsp->br_image_off + rsp->br_hdr->ih_hdr_size);

    cleanup();

//    __set_CONTROL(0);
//    __set_MSP(vt->msp);
//    __ISB();
//    ((void (*)(void))vt->reset)();
    hidden_and_jump();
}


/* Calls MCUBoot and executes image selected by the bootloader */

int sbl_boot_main(void)
{
    int rc = -1;
    struct boot_rsp rsp;

#ifdef CONFIG_BOOT_SIGNATURE
    CRYPTO_InitHardware();
#endif

    mflash_drv_init();

    BOOT_LOG_INF("Bootloader Version %s", BOOTLOADER_VERSION);

    rc = boot_go(&rsp);
    if (rc != 0)
    {
        BOOT_LOG_ERR("Unable to find bootable image");
        for (;;)
            ;
    }

#ifdef CONFIG_MCUBOOT_ENCRYPTED_XIP_SUPPORT
    BOOT_LOG_INF("\nStarting post-bootloader process of encrypted image...");
    if(mcuboot_process_encryption(&rsp) != kStatus_Success){
      BOOT_LOG_ERR("Fatal error: failed to process encrypted image");
      while(1)
        ;
    }
    BOOT_LOG_INF("Post-bootloader process of encrypted image successful\n");
#endif
    BOOT_LOG_INF("Bootloader chainload address offset: 0x%x", rsp.br_image_off);
    BOOT_LOG_INF("Reset_Handler address offset: 0x%x", rsp.br_image_off + rsp.br_hdr->ih_hdr_size);
    BOOT_LOG_INF("Jumping to the image\r\n\r\n");
    do_boot(&rsp);

    BOOT_LOG_ERR("Never should get here");
    for (;;)
        ;
}

void cleanup(void)
{
    SBL_DisablePeripherals();
}

#if !defined(MCUBOOT_DIRECT_XIP) && !defined(MCUBOOT_SWAP_USING_MOVE) && !defined(MCUBOOT_OVERWRITE_ONLY)
#warning "Make sure scratch area is defined in 'boot_flash_map' array if required by defined swap mechanism"
#endif

#if defined(MCUBOOT_DIRECT_XIP) && CONFIG_UPDATEABLE_IMAGE_NUMBER > 1
#error "DIRECT_XIP (using remapping) and multiple images is not currently supported"
#endif
