#include "bsp/bsp_driver.h"

#if defined (GD32F30X_HD) || defined (GD32F30X_XD) || defined (GD32F30X_CL)
#define FMC_FLAG_PGERR  FMC_FLAG_BANK0_PGERR
#define FMC_FLAG_PGAERR  FMC_FLAG_BANK0_PGERR
#define FMC_FLAG_WPERR  FMC_FLAG_BANK0_WPERR
#define FMC_FLAG_END  FMC_FLAG_BANK0_END
#endif


static void _fmc_write_data(uint32_t addr, uint8_t *data, int len);
static void _fmc_read_data(uint32_t addr, uint8_t *data, int len);
static void _fmc_erase_addr(uint32_t addr, int len);
static void _fmc_read_data(uint32_t addr, uint8_t *data, int len);
static void _fmc_erase_write_data(uint32_t addr, uint8_t *data, int len);

static uint32_t _sn_addr(void);
static uint32_t _data_addr(int index);
static uint32_t _maigc_addr(void);

static uint32_t _nv_tbl_write_addr = 0;

static u8 _nv_tbl_write_cache[4];
static u8 _nv_tbl_write_remain;

void fmc_write_sn(uint8_t *sn, int len){
	_fmc_erase_write_data(_sn_addr(), sn, len);
}

void fmc_read_sn(uint8_t *sn, int len){
	_fmc_read_data(_sn_addr(), sn, len);
}

void fmc_write_data(int index, uint8_t *data, int len){
	_fmc_erase_write_data(_data_addr(index), data, len);
}

void fmc_read_data(int index, uint8_t *data, int len){
	_fmc_read_data(_data_addr(index), data, len);
}

void fmc_read_data_with_offset(int index, u32 off, uint8_t *data, int len) {
	uint32_t addr = _data_addr(index) + off;
	_fmc_read_data(addr, data, len);
}

static __inline__ void _fmc_flag_clear(void) {
	fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_WPERR | FMC_FLAG_END);
}


void fmc_write_magic(uint32_t magic){
	uint32_t address = _maigc_addr();
	uint32_t length, checksum, value;

	value = REG32(address + 8);
	if (magic == value) {
		return;
	}

	length = REG32(address);
	checksum = REG32(address + 4);

	fmc_unlock();

	if (value != 0xFFFFFFFF) {
		_fmc_flag_clear();
		fmc_page_erase(address);

		_fmc_flag_clear();
		fmc_word_program(address, length);

		_fmc_flag_clear();
		fmc_word_program(address + 4, checksum);
	}

	if (magic != 0xFFFFFFFF) {
		_fmc_flag_clear();
		fmc_word_program(address + 8, magic);
	}

	fmc_lock();
}

uint32_t fmc_read_magic(void){
	uint32_t magic = 0x5555aaaa;
	_fmc_read_data(_maigc_addr(), (uint8_t *)&magic, sizeof(magic));
	return magic;
}

//if flash is lager than 256k, we just use the 256k
static uint32_t __inline__ _flash_capatity(void){
	uint32_t capacity;
	capacity = (REG32(0x1FFFF7E0) & 0xFFFF) << 10;
	if (capacity > (256 * 1024)){
		capacity =  256 * 1024;
	}
	return capacity;
}

static uint32_t _sn_addr(void){
	return 0x08000000 + (_flash_capatity() - one_page_size * sn_page_index);
}

static uint32_t _data_addr(int index){
	return 0x08000000 + (_flash_capatity() - one_page_size * index);
}

static uint32_t _maigc_addr(void){
	return 0x08000000 + (_flash_capatity() - one_page_size * magic_page_index);
}

static void _fmc_read_data(uint32_t addr, uint8_t *data, int len){
	int i = 0;
	for (i = 0; i < len; i++){
		data[i] = REG8(addr + i);
	}
}

uint32_t fmc_get_addr(int page) {
	return 0x08000000 + (_flash_capatity() - one_page_size * page);
}

void fmc_erase_trq_table(int addr, int len){
	_fmc_erase_addr(addr, len);
	_nv_tbl_write_addr = 0;
}
void fmc_write_trq_table(int addr, uint8_t *data, int len){
	_fmc_write_data(addr + _nv_tbl_write_addr, data, len);
	_nv_tbl_write_addr += len;
}

void fmc_write_trq_table_begin(int addr)
{
	_nv_tbl_write_addr = addr;
	_nv_tbl_write_remain = 0;
}

static void fmc_write_trq_table_flush(void)
{
	u32 value = *(u32 *) _nv_tbl_write_cache;

	if ((_nv_tbl_write_addr % one_page_size) == 0) {
		fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_WPERR | FMC_FLAG_END);
		fmc_page_erase(_nv_tbl_write_addr);
	}

	fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_WPERR | FMC_FLAG_END);
	fmc_word_program(_nv_tbl_write_addr, value);

	_nv_tbl_write_addr += _nv_tbl_write_remain;
	_nv_tbl_write_remain = 0;
}

void fmc_write_trq_table_continue(const u8 *data, int len)
{
	const u8 *data_end;

	fmc_unlock();

	for (data_end = data + len; data < data_end; data++) {
		if (_nv_tbl_write_remain >= sizeof(_nv_tbl_write_cache)) {
			fmc_write_trq_table_flush();
		}

		_nv_tbl_write_cache[_nv_tbl_write_remain] = *data;
		_nv_tbl_write_remain++;
	}

	fmc_lock();
}

void fmc_write_trq_table_end(void)
{
	if (_nv_tbl_write_remain > 0) {
		fmc_unlock();
		fmc_write_trq_table_flush();
		fmc_lock();
	}
}

extern void wdog_reload(void);
static void _fmc_erase_addr(uint32_t addr, int len){
	fmc_unlock();
	uint32_t pages = len/one_page_size + (((len % one_page_size) > 0)?1:0);
	for (int i = 0; i < pages; i++){
		fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_WPERR | FMC_FLAG_END);
		fmc_page_erase(addr + i * one_page_size);
		wdog_reload();
	}
	fmc_lock();
}

static void _fmc_write_data(uint32_t addr, uint8_t *data, int len){
	fmc_unlock();
	int total_words = len / 4;
	uint32_t *p_u32_data = (uint32_t *)data;
	int i;
	for (i = 0; i < total_words; i++){
		fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_WPERR | FMC_FLAG_END);
		fmc_word_program(addr, p_u32_data[i]);
		data += 4;
		addr += 4;
	}

	int remain_len = len - total_words * 4;
	if (remain_len > 0){
		uint32_t words = 0;
		for (int i = 0; i < remain_len; i++){
			words |= data[i] << (8*i);
		}
		fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_WPERR | FMC_FLAG_END);
		fmc_word_program(addr, words);
	}
	fmc_lock();
}


static void _fmc_erase_write_data(uint32_t addr, uint8_t *data, int len){
	_fmc_erase_addr(addr, len);
	_fmc_write_data(addr, data, len);
}