#include <board/simpflash.h>

#include <stm32f1xx.h>
#include <stm32f1xx_hal_flash.h>

#include <strobe_rng_init.h> /* roundup */

void
doflash(const void *dst, void *src, int bytes)
{
	FLASH_EraseInitTypeDef erase;
	const uint32_t *dstp;
	uint32_t *srcp;
	size_t i;
	uint32_t pageerr;
	uint32_t primask;

	dstp = dst;
	srcp = src;

	primask = __get_PRIMASK();
	__disable_irq();

	HAL_FLASH_Unlock();

	erase = (FLASH_EraseInitTypeDef){
		.TypeErase = FLASH_TYPEERASE_PAGES,
		.Banks = FLASH_BANK_1,
		.PageAddress = (intptr_t)&dstp[0],
		.NbPages = roundup(bytes, FLASH_PAGE_SIZE) / FLASH_PAGE_SIZE,
	};
	HAL_FLASHEx_Erase(&erase, &pageerr);

	for (i = 0; i < roundup(bytes, sizeof *srcp) / 4; i++) {
		HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, (intptr_t)&dstp[i], srcp[i]);
	}

	HAL_FLASH_Lock();

	__set_PRIMASK(primask);
}