#include "utils.h"

/* Defines */
#define WRITE_KEY1  0x45670123
#define WRITE_KEY2  0xCDEF89AB

/* Functions */
flash_result_t Flash_Write(uint64_t * data) {
  nt64_t val;
  flash_result_t res = Flash_Ok;
  uint32_t address = FLASH_PAGE_START;

  val.u64 = *data;

  // search first free cell
  while ((address < FLASH_PAGE_END) && ((*(__IO uint32_t*)address) != 0xffffffff)) {
    address += 8;
  }

  /* Authorize the FLASH Registers access */
  FLASH->KEYR = WRITE_KEY1;
  FLASH->KEYR = WRITE_KEY2;

  /* verify Flash is unlock */
  if (READ_BIT(FLASH->CR, FLASH_CR_LOCK) != 0x00U) {
    res = Flash_Error;
  } else {

    if (address > FLASH_PAGE_END) {
      // no space, format flash page
      uint32_t tmp;

      /* Get configuration register, then clear page number */
      tmp = (FLASH->CR & ~FLASH_CR_PNB);

      /* Set page number, Page Erase bit & Start bit */
      FLASH->CR = (tmp | (FLASH_CR_STRT | (FLASH_PAGE_NMB <<  FLASH_CR_PNB_Pos) | FLASH_CR_PER));

      /* Wait for end of page erase */
      while ((FLASH->CR & FLASH_CR_STRT) != 0) { __NOP(); }

      address = FLASH_PAGE_START;
    }

    // write data from address
    /* Set PG bit */
    FLASH->CR |= FLASH_CR_PG;

    /* Program first word */
    *(uint32_t *)address = val.u32[0];

    /* Barrier to ensure programming is performed in 2 steps, in right order
      (independently of compiler optimization behavior) */
    __ISB();

    /* Program second word */
    *(uint32_t *)(address + 4U) = val.u32[1];

    /* Set the LOCK Bit to lock the FLASH Registers access */
    SET_BIT(FLASH->CR, FLASH_CR_LOCK);

  }

  return res;
}

/**
 * @brief Read fom flash latest actual data.
 * @param data 
 * @return flash_result_t 
 */
flash_result_t Flash_Read(uint64_t * data) {
  nt64_t val;
  flash_result_t res = Flash_Ok;
  uint32_t address = FLASH_PAGE_START;

  while ((address < FLASH_PAGE_END) && ((*(__IO uint32_t*)address) != 0xffffffff)) {
    val.u32[0] = (*(__IO uint32_t*)address);
    address += 4;
    val.u32[1] = (*(__IO uint32_t*)address);
    address += 4;
  }

  *data = val.u64;

  if (address == FLASH_PAGE_START) {
    res = Flash_PG_Clear;
  } else if (address > FLASH_PAGE_END) {
    res = Flash_PG_End;
  }

  return res;
}