#include "s600_iap.h"
#include "s600_can.h"
#include "string.h"

static u8 s600_iap_page[FMC_PAGE_SIZE];
static u32 s600_iap_address;
static u16 s600_iap_length;

static void s600_iap_flag_clear(void)
{
#ifdef CONFIG_GD32E10X
    fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR | FMC_FLAG_END);
#else
	fmc_flag_clear(FMC_FLAG_BANK0_PGERR | FMC_FLAG_BANK0_WPERR | FMC_FLAG_BANK0_END);
#endif
}

static u32 s600_iap_capacity(void)
{
	return (REG32(0x1FFFF7E0) & 0xFFFF) << 10;
}

static u32 s600_iap_last_page(void)
{
	u32 capacity = s600_iap_capacity();
	return 0x08000000 + (capacity - FMC_PAGE_SIZE);
}

void s600_iap_write_begin(void)
{
	s600_iap_address = PART_APP;
	s600_iap_length = 0;
}

bool s600_iap_write_end(u32 length, u32 checksum)
{
	u32 buff[2];

	if (!s600_iap_flush(s600_iap_length)) {
		return false;
	}

	if (checksum != s600_iap_checksum(PART_APP, length)) {
		return false;
	}

	buff[0] = length;
	buff[1] = checksum;

	return s600_iap_write_page(s600_iap_last_page(), buff, sizeof(buff), true);
}

bool s600_iap_write_page(u32 addr, const void *buff, u16 length, bool erase)
{
	const u32 *p, *p_end;

	fmc_unlock();

	if (erase) {
		s600_iap_flag_clear();

		if (FMC_READY != fmc_page_erase(addr)) {
			return false;
		}
	}

	for (p = (const u32 *) buff, p_end = p + ((length + 3) / 4); p < p_end; p++, addr += 4) {
		s600_iap_flag_clear();

		if (FMC_READY != fmc_word_program(addr, *p)) {
			return false;
		}
	}

	fmc_lock();

	return true;
}

bool s600_iap_flush(u32 length)
{
	if (!s600_iap_write_page(s600_iap_address, s600_iap_page, length, true)) {
		return false;
	}

	s600_iap_address += length;
	s600_iap_length = 0;

	return true;
}

bool s600_iap_write(const u8 *buff, u16 length)
{
	while (1) {
		u16 remain = sizeof(s600_iap_page) - s600_iap_length;

		if (length < remain) {
			memcpy(s600_iap_page + s600_iap_length, buff, length);
			s600_iap_length += length;
			break;
		}

		memcpy(s600_iap_page + s600_iap_length, buff, remain);
		if (!s600_iap_flush(sizeof(s600_iap_page))) {
			return false;
		}

		buff += remain;
		length -= remain;
	}

	return true;
}

bool s600_iap_write_u32(u32 value)
{
	return s600_iap_write((u8 *) &value, sizeof(value));
}

#if S600_IAP_CRC32
u32 s600_iap_checksum(u32 addr, u32 length)
{
	u32 crc = 0xFFFFFFFFU;
	const u8 *p, *p_end;

	for (p = (u8 *) addr, p_end = p + length; p < p_end; p++) {
		u8 value = *p;
		u8 i;

		for (i = 0; i < 8; i++) {
			if (((crc ^ value) & 1) != 0) {
				crc = (crc >> 1) ^ 0xEDB88320U;
			} else {
				crc >>= 1;
			}

			value >>= 1;
		}
	}

	return crc ^ 0xFFFFFFFFU;
}
#else
u32 s600_iap_checksum(u32 addr, u32 length)
{
	const u8 *p, *p_end;
	u32 checksum = 0;

	for (p = (u8 *) addr, p_end = p + length; p < p_end; p++) {
		checksum += *p;
	}

	return checksum;
}
#endif

bool s600_iap_boot(bool normal)
{
	const s600_iap_info_t *info = (s600_iap_info_t *) PART_FACTORY;
	const char *board_name = (const char *) PART_NAME;

	if (info->success == 0xFFFFFFFF) {
		u32 value = APP_BOOT_OK;
		s600_iap_write_page((u32) &info->success, &value, sizeof(value), false);
		s600_iap_write_page(s600_iap_last_page(), info, sizeof(*info), true);
	}

	if (strcmp(board_name, CONFIG_BOARD_NAME) != 0) {
		return false;
	}

	info = (s600_iap_info_t *) s600_iap_last_page();

	if (normal) {
		if (info->success != APP_BOOT_OK) {
			return false;
		}
	} else if (info->length > s600_iap_capacity() - 0x2000) {
		return false;
	} else if (info->checksum != s600_iap_checksum(PART_APP, info->length)) {
		return false;
	}

#if 0
	if ((REG32(PART_APP) & 0x2FFE0000) != 0x20000000) {
		return false;
	}
#endif

#ifdef CONFIG_BOARD_PS100
	nvic_irq_disable(USART0_IRQn);
	nvic_irq_disable(USART1_IRQn);
#endif

	s600_can_device_deinit();
	SysTick->CTRL = 0;

	/* initialize user application's stack pointer */
	__set_MSP(REG32(PART_APP));
	nvic_vector_table_set(PART_APP, 0);

	/* jump to user application */
	((void (*)(void)) REG32(PART_APP + 4))();

	return false;
}