template<typename props>
void stm32_flash_t<props>::wait()
{
while (FLASH->SR & FLASH_SR_BSY) ;
}

template<typename props>
void stm32_flash_t<props>::delay()
{
for (volatile int i = 0; i < 0xFF; i++) ;
}

template<typename props>
bool stm32_flash_t<props>::wait(uint32_t timeout)
{
while (FLASH->SR & FLASH_SR_BSY)
{
if (!--timeout)
return false;
delay();
}
return true;
}

template<typename props>
bool stm32_flash_t<props>::write_halfword(uint32_t addr, uint32_t data)
{
TCritSect cs;
volatile uint16_t* addr_16 = reinterpret_cast<volatile uint16_t*>(addr);

uint32_t cr = FLASH->CR;
FLASH->CR = FLASH_CR_PG;
wait();
addr_16[0] = data;
wait();
FLASH->CR = cr & ~FLASH_CR_PG;
return (*(uint16_t*)addr == data);
}

template<typename props>
bool stm32_flash_t<props>::write(uint32_t addr, void* buf, uint32_t count)
{
uint8_t* src = static_cast<uint8_t*>(buf);
uint32_t end = addr + count;

while (addr < end)
{
uint32_t w = 0xFFFFFFFF;
if ((uint32_t)addr & 1) // odd flash address - step back
--addr;
else
w = 0xFFFFFF00 | (*src++);
if (addr < end)
w = (w & 0xFFFF00FF) | (*src++ << 8);

if (!write_halfword(addr, w))
return false;
addr += 2;
}
return true;
}

template<typename props>
bool stm32_flash_t<props>::erase_page(uint32_t addr)
{
TCritSect cs;
wait();
FLASH->CR |= FLASH_CR_PER;
FLASH->AR = addr;
FLASH->CR |= FLASH_CR_STRT;
return wait(PAGE_ERASE_TIMEOUT);
}

template<typename props>
bool stm32_flash_t<props>::mass_erase()
{
TCritSect cs;
wait();
FLASH->CR |= FLASH_CR_MER;
FLASH->CR |= FLASH_CR_STRT;
return wait(MASS_ERASE_TIMEOUT);
}

int flash_page_erase(uint32_t page_address) // page_address not page_number
{
    static const uint32_t FPEC_KEY1 = 0x45670123;
    static const uint32_t FPEC_KEY2 = 0xCDEF89AB;

    int res;

    ENTER_CRITICAL_SECTION();
    {
        FPEC->KEYR = FPEC_KEY1;     // Authorize the FPEC Access
        FPEC->KEYR = FPEC_KEY2;

        while (FPEC->SR & FPEC_SR_BSY) {;}

        FPEC->SR = FPEC_SR_EOP | FPEC_SR_WRPRTERR | FPEC_SR_PGERR; // clr bits by writing 1
        FPEC->CR = FPEC_CR_PER;                // page erase operation w/o fpec interrupts
        FPEC->AR = page_address;               // page address
        FPEC->CR = FPEC_CR_STRT | FPEC_CR_PER; // start operation    
        FPEC->SR;                               // according to errata: must be dummy cycle before polling BSY after START operation

        while (FPEC->SR & FPEC_SR_BSY) {;}

        res = ((FPEC->SR & FPEC_SR_EOP)  != 0);// EOP is asserted at the end of each successful program or erase operation

        FPEC->CR = FPEC_CR_LOCK;  // Set the Lock Bit to lock the FPEC and the FCR
    }
    LEAVE_CRITICAL_SECTION();

    return (res);
}

#ifndef _ATOMIC_H_
#define _ATOMIC_H_

#ifdef __cplusplus
  extern "C" {
#endif

//=============================================================================
static __asm __inline uint32_t get_interrupt_state(void)
{
    mrs r0, primask
    bx lr
}

//=============================================================================
static __asm __inline void set_interrupt_state(uint32_t status)
{
    msr primask, r0
    bx lr
}

#define ENTER_CRITICAL_SECTION()  do {uint32_t sreg_temp = get_interrupt_state(); __disable_irq()
#define LEAVE_CRITICAL_SECTION()  set_interrupt_state(sreg_temp);} while (0)

#ifdef __cplusplus
  }
#endif

#endif