#include "drv_chipFlash.h"  
#include "stm32l4xx.h"

#include "stm32l4xx_hal_flash.h"
#include "drv_IAP.h"
//#define TEST 
//unsigned short ipaFlag = 0;
//BASE_CONFIG base_config;


//从指定地址开始读取多个数据
int FLASH_ReadMoreData(uint32_t startAddress, uint64_t *readData, uint32_t countToRead){
    
    //数据大小必须双字节对齐
    if(countToRead % 8 != 0 )
        return -1;
    
    uint16_t dataIndex = 0;

    for(dataIndex = 0; dataIndex < (countToRead / 8); dataIndex++){
        readData[dataIndex] = FLASH_ReadDoubleWord(startAddress + dataIndex * 8);
    }

    return 0;
}


//读取指定地址的半字(16位数据)
uint16_t FLASH_ReadHalfWord(uint32_t address){
    return *(volatile uint16_t *)address;
}

//读取指定地址的双字(64位数据)
uint64_t FLASH_ReadDoubleWord(uint32_t address){
    return *(volatile uint64_t *)address;
}


//擦除指定地址的扇区
int FLASH_ErasePage(uint32_t StartPage, uint32_t PageNumber){
    FLASH_EraseInitTypeDef eraseInit = {0};
    uint32_t PageError = 0;

    eraseInit.TypeErase = FLASH_TYPEERASE_PAGES;
    eraseInit.Page = StartPage;
    eraseInit.NbPages = PageNumber;

    //关闭中断
    __disable_irq();
    //HAL_NVIC_DisableIRQ(SysTick_IRQn);
    
    if(HAL_FLASHEx_Erase(&eraseInit, &PageError) != HAL_OK){
        //恢复中断
        __enable_irq();
        //HAL_NVIC_EnableIRQ(SysTick_IRQn);
        return -1;
    }

    //恢复中断
    __enable_irq();
    //HAL_NVIC_EnableIRQ(SysTick_IRQn);

    return 0;
}



void erasure_bin_flash(void){
    HAL_FLASH_Unlock();

    __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_PGSERR | FLASH_FLAG_PROGERR | FLASH_FLAG_BSY
                                | FLASH_FLAG_WRPERR | FLASH_FLAG_OPTVERR );

    //获取APP开始地址所在扇区
    uint32_t UserStartSector = (APPLICATION_ADDRESS - FLASH_BASE) / SECTOR_SIZE;

    //获取APP结束地址所在扇区
    uint32_t UserEndSector = UserStartSector + (iap_bin_Size + SECTOR_SIZE - 1) / SECTOR_SIZE;
    
    // 如果结束地址错误。APP地址从FLASH_Sector_4开始
    if(UserEndSector < UserStartSector){
        while(1){
            send_mavlink_bootloader_status(BOOTLOADER_STATUS_FLASH_ERASOR_ERROR);
            HAL_Delay(600);
        }
    }
    
    // 擦写到FLASH_Sector_7需要3.5s时间
    for(uint32_t i = UserStartSector; i <= UserEndSector; i++){
        //驱动电压在[2.7V to 3.6V]范围内，擦除操作将按字擦除
        if(FLASH_ErasePage(i, 1)!= 0){
            HAL_FLASH_Lock();
            while(1){
                //擦除失败
                send_mavlink_bootloader_status(BOOTLOADER_STATUS_FLASH_ERASOR_ERROR);
                HAL_Delay(600);
            }
        }
    }
}

// int FLASH_Write(volatile uint32_t FlashAddress, uint32_t *WriteData, uint32_t DataLength){
//     uint32_t i =0;

//     //解锁    
//     HAL_FLASH_Unlock();
//     for(i = 0; (i < DataLength) && (FlashAddress <= (USER_FLASH_END_ADDR - 4)); i++){
//         __disable_irq();
        
//         //if(FLASH_WriteMoreData(FlashAddress, (uint32_t *)(WriteData + i), 4) == 0) 
//         if(HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, FlashAddress, *(uint32_t*)(WriteData + i))== HAL_OK)
//         {   
//             __enable_irq();
//             if(*(uint32_t *)FlashAddress != *(uint32_t*)(WriteData + i)){
//                 //FLASH中数据与SRAM中内容不一致
//                 //写操作上锁
//                 HAL_FLASH_Lock();
//                 return -1;
//             }
//             FlashAddress += 4;
//         }
//         else{
//             __enable_irq();
//             //写操作上锁
//             HAL_FLASH_Lock();
//             //编程Flash memory时发生错误
//             return -1;
//         }
//     }
//     __enable_irq();
//     //写操作上锁
//     HAL_FLASH_Lock();

//     cur_address = FlashAddress;
    
//     return 0;
// }

// int FLASH_Write_HalfWord(volatile uint32_t FlashAddress, uint16_t *WriteData, uint16_t DataLength){
//     //解锁
//     HAL_FLASH_Unlock();

//     uint32_t i =0;
//     for(i = 0; (i < DataLength) && (FlashAddress <= (USER_FLASH_END_ADDR - 2)); i++)
//     {
//         __disable_irq();

//         if(HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, FlashAddress, *(uint16_t*)(WriteData + i)) == HAL_OK)
//         {
//             __enable_irq();
//             if(*(uint16_t *)FlashAddress!= *(uint16_t*)(WriteData + i)){
//                 //FLASH中数据与SRAM中内容不一致
//                 //写操作上锁
//                 HAL_FLASH_Lock();
//                 return -1;
//             }
//             FlashAddress += 2;
//         }
//         else{
//             __enable_irq();
//             //写操作上锁
//             HAL_FLASH_Lock();
//             //编程Flash memory时发生错误
//             return -1;
//         }
//     }

//     __enable_irq();
//     //写操作上锁
//     HAL_FLASH_Lock();

//     cur_address = FlashAddress;
    
//     return 0;
// }

int FLASH_Write_DoubleWord(volatile uint32_t FlashAddress, uint64_t *WriteData, uint16_t DataLength){
    //解锁
    HAL_FLASH_Unlock();

    uint32_t i =0;
    for(i = 0; (i < DataLength) && (FlashAddress <= (USER_FLASH_END_ADDR - 2)); i++)
    {
        __disable_irq();

        if(HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, FlashAddress, *(uint64_t*)(WriteData + i)) == HAL_OK)
        {
            __enable_irq();
            if(*(uint64_t *)FlashAddress!= *(uint64_t*)(WriteData + i)){
                //FLASH中数据与SRAM中内容不一致
                //写操作上锁
                HAL_FLASH_Lock();
                return -1;
            }
            FlashAddress += 8;
        }
        else{
            __enable_irq();
            //写操作上锁
            HAL_FLASH_Lock();
            //编程Flash memory时发生错误
            return -1;
        }
    }

    __enable_irq();
    //写操作上锁
    HAL_FLASH_Lock();

    cur_address = FlashAddress;
    
    return 0;
}