/*
 * flash.c
 *
 *  Created on: Jan 3, 2023
 *      Author: Tjitte van der Ploeg
 */


#include "flash.h"


uint16_t modFlashEraseNewAppData(uint32_t new_app_size) {
	__ISB();

	HAL_StatusTypeDef status = HAL_FLASH_Unlock();
	uint32_t error = 0;


	// clear all flags before you write it to flash
	__HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGSERR);

	if (status != HAL_OK)
		return status;

	for (uint32_t addres = APP_BASE; addres < EROM_BASE; addres += 0x800){
		uint32_t page = (addres - MAIN_BASE) / 0x800;
		uint32_t Bank = FLASH_BANK_1;

		if (page >= 128){
			page -= 128;
			Bank = FLASH_BANK_2;
		}

		// make sure that this structure matches the data-sheet of your chip
		FLASH_EraseInitTypeDef FLASH_EraseInitStruct = {
			.TypeErase = FLASH_TYPEERASE_PAGES,
			.Banks = Bank,
			.Page = page,
			.NbPages = 1
		};



		// perform the erase first

		HAL_FLASHEx_Erase(&FLASH_EraseInitStruct, &error);

		if (error != -1){
			return HAL_ERROR;
		}
	}

	HAL_FLASH_Lock();
	FLASH_WaitForLastOperation(100);
	HAL_FLASH_Unlock();

	return HAL_OK;
}

uint16_t modFlashEraseMainAppData(uint32_t new_app_size) {
	__ISB();

	HAL_StatusTypeDef status = HAL_FLASH_Unlock();
	uint32_t error = 0;


	// clear all flags before you write it to flash
	__HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGSERR);

	if (status != HAL_OK)
		return status;

	for (uint32_t addres = MAIN_BASE; addres < APP_BASE; addres += 0x800){
		uint32_t page = (addres - MAIN_BASE) / 0x800;
		uint32_t Bank = FLASH_BANK_1;

		if (page >= 128){
			page -= 128;
			Bank = FLASH_BANK_2;
		}

		// make sure that this structure matches the data-sheet of your chip
		FLASH_EraseInitTypeDef FLASH_EraseInitStruct = {
			.TypeErase = FLASH_TYPEERASE_PAGES,
			.Banks = Bank,
			.Page = page,
			.NbPages = 1
		};



		// perform the erase first

		HAL_FLASHEx_Erase(&FLASH_EraseInitStruct, &error);

		if (error != -1){
			return HAL_ERROR;
		}
	}

	HAL_FLASH_Lock();
	FLASH_WaitForLastOperation(100);
	HAL_FLASH_Unlock();

	return HAL_OK;
}


int lastOffset = -1;
uint64_t bufferbyte = 0xFFFFFFFFFFFFFFFF;
int bufferindex = 0;
uint64_t* adress = (uint64_t*)APP_BASE;


uint64_t* addr;
int written = 0;

uint16_t modFlashWriteNewAppData(uint32_t offset, uint8_t *data, uint32_t len) {
	uint16_t returnVal = HAL_OK;

	//Check if length and offset are aligned with and deviseble by blocks of 8 bytes.
	if ((offset % 8 != 0) || (len % 8 != 0)){
		return HAL_ERROR;
	}

	//Detect start of write
	if (offset == 0){
		addr = (uint64_t*)APP_BASE;
		written = 0;
	}

	//Check for sequentiality of offset
	if ((offset + APP_BASE) != addr){
		return HAL_ERROR;
	}

	__HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGSERR);

	for (int i = 0; i < len; i += 8) {
		uint64_t dword = 0;
		for (int j = 0; j < 8; j++){
			dword += ((uint64_t)data[i+j]) << j*8;
		}
		if(HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, (uint32_t)addr, dword) != HAL_OK){
			return HAL_ERROR;
		}
		FLASH_WaitForLastOperation(10);
		addr++;
		written += 8;
	}

	//CLEAR_BIT(FLASH->CR, FLASH_CR_PG);
	HAL_FLASH_Lock();
	FLASH_WaitForLastOperation(100);
	HAL_FLASH_Unlock();

	return HAL_OK;
}

uint16_t modFlashCopyNewAppToMainApp(uint64_t *data, uint32_t len) {

	if (len%8 != 0){
		return HAL_ERROR;
	}

	if(len > (APP_BASE - MAIN_BASE)){
		return HAL_ERROR;
	}

	__HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGSERR);

	uint64_t* addr = (uint64_t*)MAIN_BASE;

	for (int i = 0; i < len; i += 8){
		if(HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, (uint32_t)addr, data[i/8]) != HAL_OK){
			return HAL_ERROR;
		}
		FLASH_WaitForLastOperation(10);
		addr++;
	}

	//CLEAR_BIT(FLASH->CR, FLASH_CR_PG);
	HAL_FLASH_Lock();
	FLASH_WaitForLastOperation(100);
	HAL_FLASH_Unlock();

}

void modFlashJumpToBootloader(void) {
	typedef void (*pFunction)(void);

	__HAL_RCC_USART2_FORCE_RESET();
	HAL_Delay(5);
	__HAL_RCC_USART2_RELEASE_RESET();
	HAL_Delay(5);

	HAL_RCC_DeInit();

	pFunction jump_to_bootloader;

	// Variable that will be loaded with the start address of the application
	volatile uint32_t* jump_address;
	const volatile uint32_t* bootloader_address = (volatile uint32_t*)BOOT_BASE;

	// Get jump address from application vector table
	jump_address = (volatile uint32_t*) bootloader_address[1];

	// Load this address into function pointer
	jump_to_bootloader = (pFunction) jump_address;

	// Clear pending interrupts
	SCB->ICSR = SCB_ICSR_PENDSVCLR_Msk;

	// Disable all interrupts
	for(int i = 0;i < 8;i++) {
		NVIC->ICER[i] = NVIC->IABR[i];
	}

	// Set stack pointer
	__set_MSP((uint32_t) (bootloader_address[0]));

	// Jump to the bootloader
	jump_to_bootloader();
}

void modFlashJumpToMainApplication(void) {
	NVIC_SystemReset();
}