//
// Created by cfif on 02.11.22.
//
#include CMSIS_device_header
#include "fc7xxx_driver_dma.h"
#include "fc7xxx_driver_fcuart.h"
#include "fc7240_fcuart_regs.h"
#include "fc7xxx_driver_pcc.h"


typedef void (*pFunction)(void);

#define EnablePrivilegedMode() __asm("SVC #0")

void BootJumpToAddress(const uint32_t address) {
    // 1. ГЛОБАЛЬНО отключаем прерывания (первое, что делаем)
    __disable_irq();

    // 2. Отключаем ВСЕ прерывания в NVIC
    for (int i = 0; i < 8; i++) {  // Обычно достаточно 8 банков для всех прерываний
        NVIC->ICER[i] = 0xFFFFFFFF;
        NVIC->ICPR[i] = 0xFFFFFFFF;
    }

    // 3. Останавливаем SysTick и сбрасываем
    SysTick->CTRL = 0;
    SysTick->LOAD = 0;
    SysTick->VAL = 0;

    // 4. Сбрасываем все pending-флаги
    SCB->ICSR |= SCB_ICSR_PENDSVCLR_Msk | SCB_ICSR_PENDSTCLR_Msk;

    // 5. Отключаем периферию, которая может генерировать прерывания
    // 5.1. DMA
    DMA_DeInit(DMA_INSTANCE_0);

    // 5.2. UART
    for (uint8_t i = 0; i < FCUART_INSTANCE_COUNT; ++i) {
        FCUART_DeInit(i);
    }

    PCC_GenPeripheralReset(PCC_CLK_DMA0);
    PCC_GenPeripheralReset(PCC_CLK_DMAMUX0);
    PCC_GenPeripheralReset(PCC_CLK_FCUART0);
    PCC_GenPeripheralReset(PCC_CLK_FCUART1);
    PCC_GenPeripheralReset(PCC_CLK_FCUART2);
    PCC_GenPeripheralReset(PCC_CLK_FCUART3);
    PCC_GenPeripheralReset(PCC_CLK_FCUART4);
    PCC_GenPeripheralReset(PCC_CLK_FCUART5);
    PCC_GenPeripheralReset(PCC_CLK_FCUART6);
    PCC_GenPeripheralReset(PCC_CLK_FCUART7);


    // 6. Подготовка процессора
    if (CONTROL_nPRIV_Msk & __get_CONTROL()) {
        EnablePrivilegedMode();
    }

    if (CONTROL_SPSEL_Msk & __get_CONTROL()) {
        __set_MSP(__get_PSP());
        __set_CONTROL(__get_CONTROL() & ~CONTROL_SPSEL_Msk);
    }

    // 7. Получаем стек и точку входа приложения
    uint32_t appStack = *((__IO uint32_t *) address);
    pFunction appEntry = (pFunction) *((__IO uint32_t *) (address + 4));

    // 8. Устанавливаем VTOR и стек
    SCB->VTOR = address;
    __set_MSP(appStack);

    // 9. Очищаем регистры, которые могут содержать данные загрузчика
    __ISB();  // Instruction Synchronization Barrier
    __DSB();  // Data Synchronization Barrier

    // 10. Переход БЕЗ включения прерываний!
    //     Приложение само должно включить прерывания после инициализации
    appEntry();

    while (1);
}