STM32H563 flash gets locked after programming

[Ehow]

I am trying to implement an IAP function on stm32h563 with Keil. I have a bootloader and image A and B, all running on flash. Image A and image B are running on bank 1 and 2. When i was running image A and updating image B, new image B is successfully programmed in flash and passed software CRC-16 check.

Then I jumped to image B and image B get stuck at the software CRC-16 check. Because I need to read data from flash and flash is somehow locked, and PC goes to a wrong place (like calling an uninitialized pointer). 

 

The flash keeps locked even if I download my firmware again. I can only unlock the flash by erasing the whole flash via Keil IDE and reprogram the bootloader.

 

I use the ST sample API to erase and write flash. Below is my code of flash operation. Could anyone help me?

 

/**
 * @brief Gets the bank of a given address
 * @PAram Address: Address of the FLASH Memory
 * @retval The bank of a given address
 */
static uint32_t GetBank(int32_t Address)
{
	uint32_t bank = 0;

	if (READ_BIT(FLASH->OPTSR_CUR, FLASH_OPTSR_SWAP_BANK) == 0)
	{
		/* No Bank swap */
		if (Address < (FLASH_BASE + FLASH_BANK_SIZE)) // hardfault here 
		{
		 bank = FLASH_BANK_1;
		}
		else
		{
		 bank = FLASH_BANK_2;
		}
	}
	else
	{
		/* Bank swap */
		if (Address < (FLASH_BASE + FLASH_BANK_SIZE))
		{
		 bank = FLASH_BANK_2;
		}
		else
		{
		 bank = FLASH_BANK_1;
		}
	}

 return bank;
}

/**
 * @brief Gets the sector of a given address
 * @PAram Address: Address of the FLASH Memory
 * @retval The sector of a given address
 */
static uint32_t GetSector(uint32_t Address)
{
	uint32_t sector = 0;
	
	if(Address > (FLASH_BASE + FLASH_SIZE_DEFAULT) || Address < FLASH_BASE)
	{
		sector = OUT_OF_RANGE_SECTOR; // out of range
		return sector;
	}
	
	if (Address < (FLASH_BASE + FLASH_BANK_SIZE)) // hardfault here 
	{
		sector = (Address - FLASH_BASE)/FLASH_SECTOR_SIZE;
	}
	else
	{
		sector = (Address - FLASH_BASE)/FLASH_SECTOR_SIZE;
	}

	return sector;
}


/*
 * Erase @numOfSector sectors in flash from @dstAddr.
 * The function will check in which sector @dstAddr locates and erase from the beginning of that sector
 */
HAL_StatusTypeDef flash_sector_erase_only(uint32_t dstAddr, uint8_t numOfSector)
{
 uint32_t startSector, BankNumber;
 HAL_StatusTypeDef status;
 
 /* Disable instruction cache prior to internal cacheable memory update */
 if (HAL_ICACHE_Disable() != HAL_OK)
 {
		Error_Handler();
 }
 
 /* Unlock the Flash to enable the flash control register access *************/
 HAL_FLASH_Unlock();

 startSector = GetSector(dstAddr);
 BankNumber = GetBank(dstAddr);

 if(numOfSector == 0 || startSector + numOfSector > OUT_OF_RANGE_SECTOR)
 {
 return kStatus_InvalidArgument;
 }
 else
 {
 /* Fill EraseInit structure*/
 EraseInitStruct.TypeErase = FLASH_TYPEERASE_SECTORS;
 EraseInitStruct.Banks = BankNumber;
 EraseInitStruct.Sector = startSector;
 EraseInitStruct.NbSectors = numOfSector;

 status = HAL_FLASHEx_Erase(&EraseInitStruct, &SectorError);
 }
 
 HAL_FLASH_Lock();

 /* Re-enable instruction cache */
 if (HAL_ICACHE_Enable() != HAL_OK)
 {
		Error_Handler();
 }

 return status;
}


/*
 * Write a single flash page from @srcAddr to @dstAddr without erasing it.
 * The function will check in which page @dstAddr locates and erase from the beginning of that page. 
 * Only support quad word write(128 bit)
 */
void flash_bytes_write(uint32_t dstAddr, uint8_t* srcAddr, uint32_t length)
{
	int ret=0;
 
 /* Disable instruction cache prior to internal cacheable memory update */
 if (HAL_ICACHE_Disable() != HAL_OK)
 {
		Error_Handler();
 }

 
 HAL_FLASH_Unlock();

	for(uint32_t i = 0; i<length; i+=16)
	{
	 ret = HAL_FLASH_Program(FLASH_TYPEPROGRAM_QUADWORD, dstAddr + i, (uint32_t)(srcAddr + i));
	}

	HAL_FLASH_Lock();
	
 
 /* Re-enable instruction cache */
 if (HAL_ICACHE_Enable() != HAL_OK)
 {
		Error_Handler();
 }
}