#include "eeprom.h"
#include <stm32l0xx.h>

/**
 * @brief Defined in Linkerscript as the start of the EEPROM
 */
extern uint32_t __ld_seeprom;

int data_eeprom_write_word(uint32_t word_addr_offset, uint32_t word)
{
	uint32_t *eeprom_base_addr = &__ld_seeprom;

	/* Unlock the NVM interface */
	FLASH->PEKEYR = 0x89ABCDEFUL;
	FLASH->PEKEYR = 0x02030405UL;

	__DSB();

	/* Check if we are unlocked */
	if (FLASH->PECR & FLASH_PECR_PELOCK)
		return -1;

	FLASH->PECR &= ~(FLASH_PECR_PELOCK | FLASH_PECR_ERASE);

	eeprom_base_addr += word_addr_offset;
	*eeprom_base_addr = word;

	/* Lock the flash interface again */
	FLASH->PECR |= FLASH_PECR_PELOCK;

	return 0;
}

int data_eeprom_write(uint32_t byte_offset, const uint8_t *data, uint32_t len)
{
	char *eeprom_base_addr = (char *)&__ld_seeprom;
	char *dest_ptr;
	uint32_t *word_dest_ptr;
	uint32_t full_words;
	uint32_t remain_bytes;

	/* Unlock the NVM interface */
	FLASH->PEKEYR = 0x89ABCDEFUL;
	FLASH->PEKEYR = 0x02030405UL;

	__DSB();

	/* Check if we are unlocked */
	if (FLASH->PECR & FLASH_PECR_PELOCK)
		return -1;

	FLASH->PECR &= ~(FLASH_PECR_PELOCK | FLASH_PECR_ERASE);

	dest_ptr = eeprom_base_addr + byte_offset;

	/* Do the first bytes */
	while ((uint32_t)dest_ptr & 0x3) {
		*(dest_ptr++) = *(data++);
		len -= 1;
	}

	/* Do the remianing words */
	full_words = len / 4;
	remain_bytes = len % 4;

	while (full_words) {
		word_dest_ptr = (uint32_t *)dest_ptr;
		*word_dest_ptr = (((uint32_t)data[3]) << 24) | (((uint32_t)data[2]) << 16) | (((uint32_t)data[1]) << 8) |
				(((uint32_t)data[0]) << 0);
		dest_ptr += 4;
		data += 4;
		full_words--;
	}

	while (remain_bytes) {
		*dest_ptr = *data;
		dest_ptr++;
		data++;
		remain_bytes--;
	}

	/* Lock the flash interface again */
	FLASH->PECR |= FLASH_PECR_PELOCK;

	return 0;
}