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

#ifndef CONFIG_CAN_ID
#error CONFIG_CAN_ID not set
#endif

#define S600_CAN_LOOPBACK			0

static s600_can_packet_t s600_can_tx_packets[5];
static s600_can_packet_t *s600_can_tx_front;

static s600_can_packet_t s600_can_rx_packets[10];
static s600_can_packet_t *s600_can_rx_fronts[10];
static u8 s600_can_rx_front_len;

static u8 s600_can_tx_alloc_buff[NELEM(s600_can_tx_packets) + 1];
static byte_queue_t s600_can_tx_allocator;

static u8 s600_can_rx_alloc_buff[NELEM(s600_can_rx_packets) + 1];
static byte_queue_t s600_can_rx_allocator;

static u8 s600_can_tx_buff[NELEM(s600_can_tx_packets) + 1];
static byte_queue_t s600_can_tx_queue = { s600_can_tx_buff, NELEM(s600_can_tx_buff), 0, 0 };

static u8 s600_can_rx_buff[NELEM(s600_can_rx_packets) + 1];
static byte_queue_t s600_can_rx_queue = { s600_can_rx_buff, NELEM(s600_can_rx_buff), 0, 0 };

// ================================================================================

static u32 s600_can_write_position;

__STATIC_INLINE int s600_can_process_cmd_write_begin(const u8 *buff, u16 length)
{
	s600_can_write_position = 0;
	s600_iap_write_begin();
	return 0;
}

__STATIC_INLINE int s600_can_process_cmd_write_end(const u8 *buff, u16 length)
{
	u32 size, checksum;

	if (length < 7) {
		return -1;
	}

	size = s600_decode_u24(buff);
	checksum = s600_decode_u32(buff + 3);

	if (s600_iap_write_end(size, checksum)) {
		return 0;
	}

	return -1;
}

__STATIC_INLINE int s600_can_process_cmd_write_slow(u8 can, u8 *buff, u16 length)
{
	u32 position;

	if (length < 3) {
		return -1;
	}

	position = s600_decode_u24(buff);

	if (s600_can_write_position == position) {
		length -= 3;
		s600_iap_write(buff + 3, length);
		s600_can_write_position += length;
	}

	s600_encode_u24(buff + 1, s600_can_write_position);

	return 3;
}

__STATIC_INLINE void s600_can_process_command(u8 can, u8 *command, u16 length)
{
	int rsplen;
	u8 *args;
	u16 key;

	if (length < 2) {
		return;
	}

	key = *(u16 *) command;

	if ((key & 0xFFF0) != S600_CAN_BUILD_CMD(0xF0)) {
		return;
	}

	s600_iap_100ms = 0;
	args = command + 2;
	length -= 2;

	switch (key)
	{
	case S600_CAN_BUILD_CMD(0xF0):
		rsplen = 0;
		break;

	case S600_CAN_BUILD_CMD(0xF1):
		rsplen = s600_can_process_cmd_write_begin(args, length);
		break;

	case S600_CAN_BUILD_CMD(0xF2):
		rsplen = s600_can_process_cmd_write_end(args, length);
		break;

	case S600_CAN_BUILD_CMD(0xF3):
		s600_iap_write(args, length);
		return;

	case S600_CAN_BUILD_CMD(0xF4):
		rsplen = s600_can_process_cmd_write_slow(can, args, length);
		break;

	case S600_CAN_BUILD_CMD(0xF5):
		s600_iap_boot(false);
		rsplen = -1;
		break;

	default:
		rsplen = -1;
		break;
	}

	if (rsplen < 0) {
		rsplen = 0;
		args[0] = 0x01;
	} else {
		args[0] = 0x00;
	}

	s600_can_send_response(can, command, rsplen + 3);
}

// ================================================================================

__STATIC_INLINE void s600_can_rcu_config(void)
{
	byte_queue_alloc_init(&s600_can_tx_allocator, s600_can_tx_alloc_buff, sizeof(s600_can_tx_alloc_buff));
	byte_queue_alloc_init(&s600_can_rx_allocator, s600_can_rx_alloc_buff, sizeof(s600_can_rx_alloc_buff));

#if CONFIG_CAN_PIN_MAP == CAN_PIN_MAP_PA11_PA12
	rcu_periph_clock_enable(RCU_GPIOA);
#elif CONFIG_CAN_PIN_MAP == CAN_PIN_MAP_PB8_PB9
	rcu_periph_clock_enable(RCU_GPIOB);
#elif CONFIG_CAN_PIN_MAP == CAN_PIN_MAP_PD0_PD1
	rcu_periph_clock_enable(RCU_GPIOD);
#else
#error "Invalid CONFIG_CAN_PIN_MAP"
#endif

	rcu_periph_clock_enable(RCU_AF);
    rcu_periph_clock_enable(RCU_CAN0);
}

__STATIC_INLINE void s600_can_gpio_config(void)
{
#if CONFIG_CAN_PIN_MAP == CAN_PIN_MAP_PA11_PA12
	gpio_init(GPIOA, GPIO_MODE_IPU, GPIO_OSPEED_50MHZ, GPIO_PIN_11);
	gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_12);
#elif CONFIG_CAN_PIN_MAP == CAN_PIN_MAP_PB8_PB9
	gpio_init(GPIOB, GPIO_MODE_IPU, GPIO_OSPEED_50MHZ, GPIO_PIN_8);
	gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_9);
#ifdef GPIO_CAN0_PARTIAL_REMAP
	gpio_pin_remap_config(GPIO_CAN0_PARTIAL_REMAP, ENABLE);
#else
	gpio_pin_remap_config(GPIO_CAN_PARTIAL_REMAP, ENABLE);
#endif
#elif CONFIG_CAN_PIN_MAP == CAN_PIN_MAP_PD0_PD1
	gpio_init(GPIOD, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_0);
	gpio_init(GPIOD, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_1);
	gpio_pin_remap_config(GPIO_CAN_FULL_REMAP, ENABLE);
#else
#error "Invalid CONFIG_CAN_PIN_MAP"
#endif
}

void s600_can_device_config(void)
{
	can_parameter_struct can_parameter;
	can_filter_parameter_struct can_filter;

	s600_can_rcu_config();
	s600_can_gpio_config();

	// can_struct_para_init(CAN_INIT_STRUCT, &can_parameter);
	// can_struct_para_init(CAN_FILTER_STRUCT, &can_filter);

	can_deinit(CAN0);

	/* initialize CAN parameters */
	can_parameter.time_triggered = DISABLE;
	can_parameter.auto_bus_off_recovery = DISABLE;
	can_parameter.auto_wake_up = DISABLE;
	can_parameter.auto_retrans = DISABLE;
	can_parameter.rec_fifo_overwrite = DISABLE;
	can_parameter.trans_fifo_order = ENABLE;
	can_parameter.resync_jump_width = CAN_BT_SJW_1TQ;
	can_parameter.time_segment_1 = CAN_BT_BS1_5TQ;
	can_parameter.time_segment_2 = CAN_BT_BS2_3TQ;

#if S600_CAN_LOOPBACK
	can_parameter.working_mode = CAN_LOOPBACK_MODE;
#else
	can_parameter.working_mode = CAN_NORMAL_MODE;
#endif

#if S600_CAN_BAUDRATE == 1000
		can_parameter.prescaler = 6;
#elif S600_CAN_BAUDRATE == 500
		can_parameter.prescaler = 12;
#elif S600_CAN_BAUDRATE == 250
		can_parameter.prescaler = 24;
#elif S600_CAN_BAUDRATE == 125
		can_parameter.prescaler = 48;
#elif S600_CAN_BAUDRATE == 100
		can_parameter.prescaler = 60;
#elif S600_CAN_BAUDRATE == 50
		can_parameter.prescaler = 120;
#elif S600_CAN_BAUDRATE == 20
		can_parameter.prescaler = 300;
#else
	#error "Invalid Baudrate"
#endif

	/* initialize CAN */
	can_init(CAN0, &can_parameter);

#ifdef CONFIG_GD32E10X
	can_frequency_set(CAN0, S600_CAN_BAUDRATE * 1000);
#endif

	/* initialize filter */
	can_filter.filter_number = 0;
	can_filter.filter_mode = CAN_FILTERMODE_MASK;
	can_filter.filter_bits = CAN_FILTERBITS_32BIT;
	can_filter.filter_list_high = 0x0000;
	can_filter.filter_list_low = S600_CAN_DEST_ID(CONFIG_CAN_ID) << 3 | 1 << 2;
	can_filter.filter_mask_high = 0x0000;
	can_filter.filter_mask_low = S600_CAN_DEST_ID(S600_CAN_ALL_ID) << 3 | 1 << 2;
	can_filter.filter_fifo_number = CAN_FIFO1;
	can_filter.filter_enable = ENABLE;
	can_filter_init(&can_filter);

	nvic_irq_enable(CAN0_RX1_IRQn, 0, 0);
	can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE1);
}

void s600_can_device_deinit(void)
{
	nvic_irq_disable(CAN0_RX1_IRQn);
	can_deinit(CAN0);
}

u8 s600_can_find_mailbox(void)
{
	u32 value = CAN_TSTAT(CAN0);

	if ((value & CAN_TSTAT_TME0) != 0) {
		return CAN_MAILBOX0;
	}

	if ((value & CAN_TSTAT_TME1) != 0) {
		return CAN_MAILBOX1;
	}

	if ((value & CAN_TSTAT_TME2) != 0) {
		return CAN_MAILBOX2;
	}

	return CAN_NOMAILBOX;
}

u8 s600_can_wait_mailbox(void)
{
	u32 count;

	for (count = 0; count < 1000000; count++) {
		u8 mailbox = s600_can_find_mailbox();

		if (mailbox != CAN_NOMAILBOX) {
			return mailbox;
		}
	}

	return CAN_NOMAILBOX;
}

void s600_can_send_frame(u8 mailbox, u32 efid, const u8 *buff, u8 length)
{
	/* set the data length */
	CAN_TMP(CAN0, mailbox) &= ~CAN_TMP_DLENC;
	CAN_TMP(CAN0, mailbox) |= length;

	/* set the data */
	CAN_TMDATA0(CAN0, mailbox) = ((const u32 *) buff)[0];

	if (length > 4) {
		CAN_TMDATA1(CAN0, mailbox) = ((const u32 *) buff)[1];
	}

	/* enable transmission */
	CAN_TMI(CAN0, mailbox) = TMI_EFID(efid) | CAN_FF_EXTENDED | CAN_FT_DATA | CAN_TMI_TEN;
}

bool s600_can_tx_packet(s600_can_packet_t *packet)
{
	u8 total, index;
	u8 mailbox;
	u16 length;
	u32 efid;

	mailbox = s600_can_find_mailbox();
	if (mailbox == CAN_NOMAILBOX) {
		return false;
	}

	total = (packet->length + 7) / 8;
	index = packet->offset / 8 + 1;

	efid = 6 << 26 | U32(packet->type) << 24
		| U32(total & 0x1F) << 19 | U32(index & 0x1F) << 14
		| S600_CAN_DEST_ID(packet->dest) | S600_CAN_SRC_ID(packet->src);

	length = packet->length - packet->offset;
	if (length > 8) {
		length = 8;
	}

	s600_can_send_frame(mailbox, efid, packet->data + packet->offset, length);
	packet->offset += length;

	return true;
}

__STATIC_INLINE s600_can_packet_t *s600_can_rx_front_get(u8 addr)
{
	u8 index;

	for (index = 0; index < s600_can_rx_front_len; index++) {
		s600_can_packet_t *packet = s600_can_rx_fronts[index];

		if (packet->src == addr) {
			return packet;
		}
	}

	if (s600_can_rx_front_len < NELEM(s600_can_rx_fronts)) {
		u8 index = byte_queue_alloc(&s600_can_rx_allocator);
		s600_can_packet_t *packet;

		if (index == 0xFF) {
			return NULL;
		}

		packet = s600_can_rx_packets + index;
		packet->src = addr;

#if S600_CAN_USE_MASK
		packet->mask = 0;
#else
		packet->index = 0xFF;
#endif

		s600_can_rx_fronts[s600_can_rx_front_len] = packet;
		s600_can_rx_front_len++;
		return packet;
	}

	return NULL;
}

__STATIC_INLINE bool s600_can_rx_front_put(s600_can_packet_t *packet)
{
	u8 index;

	for (index = 0; index < s600_can_rx_front_len; index++) {
		if (s600_can_rx_fronts[index] == packet) {
			s600_can_rx_front_len--;

			while (index < s600_can_rx_front_len) {
				s600_can_rx_fronts[index] = s600_can_rx_fronts[index + 1];
				index++;
			}

			return true;
		}
	}

	return false;
}

bool s600_can_rx_packet(u8 fifo)
{
	s600_can_packet_t *packet;
	u8 index, total;
	u32 *buff;
	u32 value;
	u32 efid;

	value = CAN_RFIFOMI(CAN0, fifo);
	if ((value & (CAN_RFIFOMI_FT | CAN_RFIFOMI_FF)) != (CAN_FT_DATA | CAN_FF_EXTENDED)) {
		return true;
	}

	efid = GET_RFIFOMI_EFID(value);

	value = CAN_RFIFOMP(CAN0, fifo);

#ifdef CAN_RFIFOMP_FDF
	if ((value & CAN_RFIFOMP_FDF) != CAN_FDF_CLASSIC) {
		return true;
	}
#endif

	packet = s600_can_rx_front_get((efid >> 7) & 0x7F);
	if (packet == NULL) {
		return false;
	}

	index = (efid >> 14) & 0x1F;
	total = (efid >> 19) & 0x1F;

#if S600_CAN_USE_MASK
	if (index == total) {
		packet->dest = efid & 0x7F;
		packet->type = (efid >> 24) & 0x03;
		packet->length = ((total - 1) & 0x1F) << 3;
		packet->length += GET_RFIFOMP_DLENC(value);
	}

	index = (index - 1) & 0x1F;

	if (index == 0) {
		packet->mask = 1;
	} else {
		packet->mask |= 1 << index;
	}

	buff = (u32 *) packet->datas[index];
	buff[0] = CAN_RFIFOMDATA0(CAN0, fifo);
	buff[1] = CAN_RFIFOMDATA1(CAN0, fifo);

	if (total == 0) {
		value = 0xFFFFFFFF;
	} else {
		value = (1 << total) - 1;
	}

	if ((packet->mask & value) == value) {
		byte_queue_write_byte(&s600_can_rx_queue, packet - s600_can_rx_packets);
		s600_can_rx_front_put(packet);
	}
#else
	if (index != packet->index) {
		if (index != 1) {
			return true;
		}

		packet->type = (efid >> 24) & 0x03;
		packet->dest = efid & 0x7F;
		packet->offset = 0;
		packet->index = 2;
	} else {
		packet->index = (index + 1) & 0x1F;
	}

	buff = (u32 *) (packet->data + packet->offset);
	packet->offset += GET_RFIFOMP_DLENC(value);
	buff[0] = CAN_RFIFOMDATA0(CAN0, fifo);
	buff[1] = CAN_RFIFOMDATA1(CAN0, fifo);

	if (total == index) {
		packet->length = packet->offset;
		byte_queue_write_byte(&s600_can_rx_queue, packet - s600_can_rx_packets);
		s600_can_rx_front_put(packet);
	}
#endif

	return true;
}

void s600_can_transmit(void)
{
	while (1) {
		u8 index;

		if (s600_can_tx_front) {
			if (s600_can_tx_front->offset < s600_can_tx_front->length) {
				s600_can_tx_packet(s600_can_tx_front);
				break;
			}

			byte_queue_free(&s600_can_tx_allocator, s600_can_tx_front - s600_can_tx_packets);
		}

		if (byte_queue_read(&s600_can_tx_queue, &index, 1) > 0) {
			s600_can_tx_front = s600_can_tx_packets + index;
		} else {
			s600_can_tx_front = NULL;
			break;
		}
	}
}

__STATIC_INLINE void s600_can_process_packet(s600_can_packet_t *packet)
{
	s600_can_process_command(packet->src, packet->data, packet->length);
}

void s600_can_poll(void)
{
	u8 index;

	s600_can_transmit();

	while (byte_queue_read(&s600_can_rx_queue, &index, 1) > 0) {
		s600_can_process_packet(s600_can_rx_packets + index);
		byte_queue_free(&s600_can_rx_allocator, index);
	}
}

bool s600_can_send_packet(u8 dest, u8 src, u8 type, const void *data, u16 length)
{
	s600_can_packet_t *packet;
	u8 index;

	index = byte_queue_alloc(&s600_can_tx_allocator);
	if (index == 0xFF) {
		return false;
	}

	packet = s600_can_tx_packets + index;

	packet->offset = 0;
	packet->type = type;
	packet->dest = dest;
	packet->src = src;
	packet->length = length;
	memcpy(packet->data, data, length);

	byte_queue_write_byte(&s600_can_tx_queue, index);
	s600_can_transmit();

	return true;
}

void s600_can_send_packet_block(u8 dest, u8 src, u8 type, const void *data, u16 length)
{
	while (!s600_can_send_packet(dest, src, type, data, length)) {
		s600_can_transmit();
	}
}

bool s600_can_send_command(u8 addr, const void *command, u16 length)
{
	return s600_can_send_packet(addr, CONFIG_CAN_ID, 1, command, length);
}

bool s600_can_send_response(u8 addr, const void *response, u16 length)
{
	return s600_can_send_packet(addr, CONFIG_CAN_ID, 2, response, length);
}

bool s600_can_send_event(u8 addr, const void *event, u16 length)
{
	return s600_can_send_packet(addr, CONFIG_CAN_ID, 3, event, length);
}

// ================================================================================

void CAN0_RX1_IRQHandler(void)
{
	s600_can_rx_packet(CAN_FIFO1);
	CAN_RFIFO1(CAN0) |= CAN_RFIFO1_RFD1;
}