/* Config */
#include "FlashConfig.h"

/* HAL */
#include "stm32h7xx_hal.h"

/* Private */
#include "FlashOperations.h"

#define APP_FLASH_COMMAND_TIMEOUT (5000)

#define LED_G_Pin       GPIO_PIN_1
#define LED_G_GPIO_Port GPIOA

#define APP_DBG_LED_SET(x) HAL_GPIO_WritePin(LED_G_GPIO_Port, LED_G_Pin, x)

static QSPI_HandleTypeDef hqspi;

static int SystemClock_Config(void) {
    RCC_OscInitTypeDef RCC_OscInitStruct = {0};
    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

    /** Supply configuration update enable
     */
    HAL_PWREx_ConfigSupply(PWR_LDO_SUPPLY);

    /** Configure the main internal regulator output voltage
     */
    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE3);

    while (!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) {
    }

    /** Initializes the RCC Oscillators according to the specified parameters
     * in the RCC_OscInitTypeDef structure.
     */
    RCC_OscInitStruct.OscillatorType      = RCC_OSCILLATORTYPE_HSI;
    RCC_OscInitStruct.HSIState            = RCC_HSI_DIV1;
    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
    RCC_OscInitStruct.PLL.PLLState        = RCC_PLL_NONE;
    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
        return -1;
    }

    /** Initializes the CPU, AHB and APB buses clocks
     */
    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 |
                                  RCC_CLOCKTYPE_PCLK2 | RCC_CLOCKTYPE_D3PCLK1 | RCC_CLOCKTYPE_D1PCLK1;
    RCC_ClkInitStruct.SYSCLKSource   = RCC_SYSCLKSOURCE_HSI;
    RCC_ClkInitStruct.SYSCLKDivider  = RCC_SYSCLK_DIV1;
    RCC_ClkInitStruct.AHBCLKDivider  = RCC_HCLK_DIV1;
    RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV1;
    RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV1;
    RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV1;
    RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV1;

    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK) {
        return -2;
    }

    return 0;
}

static void GPIO_Init(void) {
    GPIO_InitTypeDef GPIO_InitStruct = {0};

    __HAL_RCC_GPIOA_CLK_ENABLE();

    HAL_GPIO_WritePin(LED_G_GPIO_Port, LED_G_Pin, GPIO_PIN_SET);

    GPIO_InitStruct.Pin   = LED_G_Pin;
    GPIO_InitStruct.Mode  = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull  = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(LED_G_GPIO_Port, &GPIO_InitStruct);
}

static int QSPI_Init(void) {
    hqspi.Instance                = QUADSPI;
    hqspi.Init.ClockPrescaler     = 7;
    hqspi.Init.FifoThreshold      = 4;
    hqspi.Init.SampleShifting     = QSPI_SAMPLE_SHIFTING_HALFCYCLE;
    hqspi.Init.FlashSize          = APP_FLASH_BITS;
    hqspi.Init.ChipSelectHighTime = QSPI_CS_HIGH_TIME_3_CYCLE;
    hqspi.Init.ClockMode          = QSPI_CLOCK_MODE_0;
    hqspi.Init.DualFlash          = QSPI_DUALFLASH_DISABLE;
    if (HAL_QSPI_Init(&hqspi) != HAL_OK) {
        return -1;
    }

    return 0;
}

static int Flash_Wait_Busy(uint32_t timeout) {
    QSPI_AutoPollingTypeDef poll_def = {
        .Match           = 0U,
        .Mask            = 1U, /* Status register: Bit 0 */
        .Interval        = 0xFFFFU,
        .StatusBytesSize = 1,
        .MatchMode       = QSPI_MATCH_MODE_AND,
        .AutomaticStop   = QSPI_AUTOMATIC_STOP_ENABLE,
    };

    QSPI_CommandTypeDef cmd = {
        .Instruction       = 0x05, /* SR1 */
        .InstructionMode   = QSPI_INSTRUCTION_1_LINE,
        .AddressMode       = QSPI_ADDRESS_NONE,
        .AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE,
        .DataMode          = QSPI_DATA_1_LINE,
    };

    if (HAL_QSPI_AutoPolling(&hqspi, &cmd, &poll_def, timeout) != HAL_OK) {
        return -1;
    }

    return 0;
}

static int Flash_Write_Enable(void) {
    QSPI_CommandTypeDef cmd = {
        .Instruction       = 0x06, /* Write Enable */
        .InstructionMode   = QSPI_INSTRUCTION_1_LINE,
        .AddressMode       = QSPI_ADDRESS_NONE,
        .AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE,
        .DataMode          = QSPI_DATA_NONE,
    };

    if (HAL_QSPI_Command(&hqspi, &cmd, APP_FLASH_COMMAND_TIMEOUT) != HAL_OK) {
        return -1;
    }

    return 0;
}

int Flash_Init(void) {
    int ret;

    SystemInit();

    ret = SystemClock_Config();
    if (ret != 0) {
        return ret;
    }

    GPIO_Init();

    ret = QSPI_Init();
    if (ret != 0) {
        return ret;
    }

    return 0;
}

int Flash_EraseSector(uint32_t addr) {
    APP_DBG_LED_SET(0);

    if (Flash_Write_Enable() != 0) {
        return -1;
    }

    QSPI_CommandTypeDef cmd = {
        .Instruction       = 0x20,
        .InstructionMode   = QSPI_INSTRUCTION_1_LINE,
        .Address           = addr & ~(APP_FLASH_SECTOR_SIZE - 1),
        .AddressMode       = QSPI_ADDRESS_1_LINE,
        .AddressSize       = QSPI_ADDRESS_24_BITS,
        .AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE,
        .DataMode          = QSPI_DATA_NONE,
    };

    if (HAL_QSPI_Command(&hqspi, &cmd, APP_FLASH_COMMAND_TIMEOUT) != HAL_OK) {
        return -2;
    }

    if (Flash_Wait_Busy(APP_FLASH_SECTOR_ERASE_TIMEOUT) != 0) {
        return -3;
    }

    APP_DBG_LED_SET(1);
    return 0;
}

int Flash_ProgramPage(uint32_t addr, uint8_t *data, uint32_t len) {
    APP_DBG_LED_SET(0);

    if (Flash_Write_Enable() != 0) {
        return -1;
    }

    QSPI_CommandTypeDef cmd = {
        .Instruction       = 0x32, /* Quad input page program */
        .InstructionMode   = QSPI_INSTRUCTION_1_LINE,
        .Address           = addr,
        .AddressMode       = QSPI_ADDRESS_1_LINE,
        .AddressSize       = QSPI_ADDRESS_24_BITS,
        .AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE,
        .DataMode          = QSPI_DATA_4_LINES,
        .DummyCycles       = 0U,
        .NbData            = len,
    };

    if (HAL_QSPI_Command(&hqspi, &cmd, APP_FLASH_COMMAND_TIMEOUT) != HAL_OK) {
        return -2;
    }

    if (HAL_QSPI_Transmit(&hqspi, data, APP_FLASH_COMMAND_TIMEOUT) != HAL_OK) {
        return -3;
    }

    if (Flash_Wait_Busy(APP_FLASH_PAGE_PROGRAM_TIMEOUT) != 0) {
        return -4;
    }

    APP_DBG_LED_SET(1);

    return 0;
}

int Flash_EraseChip(void) {
    APP_DBG_LED_SET(0);

    if (Flash_Write_Enable() != HAL_OK) {
        return -1;
    }

    QSPI_CommandTypeDef cmd = {
        .Instruction       = 0xC7, /* Chip erase */
        .InstructionMode   = QSPI_INSTRUCTION_1_LINE,
        .AddressMode       = QSPI_ADDRESS_NONE,
        .AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE,
        .DataMode          = QSPI_DATA_NONE,
    };

    if (HAL_QSPI_Command(&hqspi, &cmd, APP_FLASH_COMMAND_TIMEOUT) != HAL_OK) {
        return -2;
    }

    if (Flash_Wait_Busy(APP_FLASH_CHIP_ERASE_TIMEOUT) != 0) {
        return -3;
    }

    APP_DBG_LED_SET(1);
    return 0;
}

int Flash_Map(void) {
    QSPI_CommandTypeDef cmd = {
        .Instruction        = 0xEB, /* Fast read quad IO */
        .InstructionMode    = QSPI_INSTRUCTION_1_LINE,
        .AddressMode        = QSPI_ADDRESS_4_LINES,
        .AddressSize        = QSPI_ADDRESS_24_BITS,
        .AlternateByteMode  = QSPI_ALTERNATE_BYTES_4_LINES,
        .AlternateBytesSize = QSPI_ALTERNATE_BYTES_8_BITS,
        .AlternateBytes     = 0xF0,
        .DataMode           = QSPI_DATA_4_LINES,
        .DummyCycles        = 4U,
    };

    QSPI_MemoryMappedTypeDef mmap = {
        .TimeOutPeriod     = 0xFFFFU,
        .TimeOutActivation = QSPI_TIMEOUT_COUNTER_DISABLE,
    };

    if (HAL_QSPI_MemoryMapped(&hqspi, &cmd, &mmap) != HAL_OK) {
        return -1;
    }

    return 0;
}

int Flash_UnMap(void) {
    HAL_StatusTypeDef ret;
    do {
        ret = HAL_QSPI_Abort(&hqspi);
    } while (ret != HAL_OK);
    return 0;
}

int Flash_Deinit(void) {
    HAL_QSPI_DeInit(&hqspi);
    return 0;
}