#include "common.h"
#include "config.h"
#include "fpga.h"
#include "esplink.h"
#include "xmalloc.h"

#include <driver/gpio.h>
#include <driver/spi_common.h>
#include <driver/spi_master.h>

#define PIN_FPGA_INT	 9
#define PIN_FPGA_CS	10
#define PIN_FPGA_IO0	11
#define PIN_FPGA_CLK	12
#define PIN_FPGA_IO1	13

#define FPGA_SPI_HOST	FSPI	/* SPI2 */

#define FPGA_PRIORITY	10
#define FPGA_SVC_STACK	4096

#define RTC_TIMESYNC_PERIOD (511*configTICK_RATE_HZ)

static spi_bus_config_t spi_bus_config = {
    .data0_io_num    = PIN_FPGA_IO0,
    .data1_io_num    = PIN_FPGA_IO1,
    .sclk_io_num     = PIN_FPGA_CLK,
    .data2_io_num    = -1,
    .data3_io_num    = -1,
    .data4_io_num    = -1,
    .data5_io_num    = -1,
    .data6_io_num    = -1,
    .data7_io_num    = -1,
    .max_transfer_sz = 4096,
    .flags           = SPICOMMON_BUSFLAG_MASTER | SPICOMMON_BUSFLAG_DUAL
};

#define FPGA_IOV_MAX	4

static void ARDUINO_ISR_ATTR spi_callback(spi_transaction_t *);

static const spi_device_interface_config_t spi_device_interface_config = {
    .command_bits     =  8,
    .address_bits     = 32,
    .dummy_bits       =  0,
    .mode             =  0,
    .cs_ena_pretrans  =  0,
    .cs_ena_posttrans =  0,
    .clock_speed_hz   = SPI_MASTER_FREQ_40M,
    .spics_io_num     = PIN_FPGA_CS,
    .flags            = SPI_DEVICE_HALFDUPLEX,
    .queue_size       = FPGA_IOV_MAX,
    .post_cb          = spi_callback
};

static spi_device_handle_t spi_handle;
static TaskHandle_t fpga_task;
static TimerHandle_t fpga_timesync_timer;
static SemaphoreHandle_t spi_mutex;
static EventGroupHandle_t spi_done_evgroup;
static volatile bool spi_abort_all;
static struct esplink_head head;

#define NOTIFY_INDEX	0
#define NOTIFY_FPGA	(1 << 0)
#define NOTIFY_ENABLE	(1 << 1)
#define NOTIFY_DISABLE	(1 << 2)
#define NOTIFY_TIMESYNC	(1 << 3)
#if 0
#define NOTIFY_SPI	(1 << 3)
#define NOTIFY_RINGBUF	(1 << 4)
#endif

static uint32_t notify_poll_for(uint32_t flags)
{
    return ulTaskNotifyValueClearIndexed(NULL, NOTIFY_INDEX, flags);
}

/* This supports multiple flags set */
static uint32_t notify_wait_for(uint32_t flags)
{
    uint32_t notify_value;

    /* Already received? Might already have been waited for... */
    notify_value = notify_poll_for(flags);
    while (!(notify_value & flags)) {
	xTaskNotifyWaitIndexed(NOTIFY_INDEX, 0, flags,
			       &notify_value, portMAX_DELAY);
    }

    return notify_value;
}

static void ARDUINO_ISR_ATTR fpga_notify_from_isr(uint32_t flags)
{
    BaseType_t wakeup = pdFALSE;

    if (xTaskNotifyIndexedFromISR(fpga_task, NOTIFY_INDEX, flags, eSetBits,
				  &wakeup) != pdFAIL)
	portYIELD_FROM_ISR(wakeup);
}

static void fpga_notify_from_task(uint32_t flags)
{
    xTaskNotifyIndexed(fpga_task, NOTIFY_INDEX, flags, eSetBits);
}

static void ARDUINO_ISR_ATTR fpga_interrupt(void)
{
    fpga_notify_from_isr(NOTIFY_FPGA);
}

void fpga_timesync_trigger(void)
{
    fpga_notify_from_task(NOTIFY_TIMESYNC);
}

static void ARDUINO_ISR_ATTR spi_callback(spi_transaction_t *t)
{
    size_t flags = (size_t)t->user;

    if (!flags)
	return;

    BaseType_t wakeup = pdFALSE;
    if (xEventGroupSetBitsFromISR(spi_done_evgroup, (size_t)t->user,
				  &wakeup) != pdFAIL)
	portYIELD_FROM_ISR(wakeup);
}

static void fpga_service_task(void *);
static EventGroupHandle_t fpga_service_evgroup;

void fpga_service_enable(bool on)
{
    uint32_t flag = on ? NOTIFY_ENABLE : NOTIFY_DISABLE;
    fpga_notify_from_task(flag);
    xEventGroupWaitBits(fpga_service_evgroup, flag, 0, pdTRUE, portMAX_DELAY);
}

esp_err_t fpga_service_init(void)
{
    esp_err_t err;

    pinMode(PIN_FPGA_INT, INPUT);

    setenv_bool("status.max80.fpga", false);

    fpga_service_evgroup = null_check(xEventGroupCreate());
    spi_mutex = null_check(xSemaphoreCreateRecursiveMutex());
    spi_done_evgroup = null_check(xEventGroupCreate());

    /* The ordering here attempts to avoid race conditions... */
    if (xTaskCreate(fpga_service_task, "fpga_svc", FPGA_SVC_STACK, NULL,
		    FPGA_PRIORITY, &fpga_task) != pdPASS)
	return ESP_FAIL;

    fpga_timesync_timer =
      null_check(xTimerCreate("rtc_sync", RTC_TIMESYNC_PERIOD, pdTRUE, NULL,
			      (TimerCallbackFunction_t)fpga_timesync_trigger));

    esplink_init();

    xEventGroupSetBits(fpga_service_evgroup, NOTIFY_DISABLE);
    return ESP_OK;
}

static bool fpga_link_enable(void)
{
    esp_err_t err;

    if (spi_handle)
	return true;		/* Already started */

    xEventGroupClearBits(fpga_service_evgroup, NOTIFY_DISABLE);

    err = spi_bus_initialize(FPGA_SPI_HOST, &spi_bus_config, SPI_DMA_CH_AUTO);
    if (err)
	goto init_fail;

    err = spi_bus_add_device(FPGA_SPI_HOST, &spi_device_interface_config,
			     &spi_handle);
    if (err)
	goto free_bus_fail;

    /* Only device on this bus, so acquire it permanently */
    err = spi_device_acquire_bus(spi_handle, portMAX_DELAY);
    if (err)
	goto release_bus_fail;

    xEventGroupClearBits(spi_done_evgroup, EVENT_ALL_BITS);

    pinMode(PIN_FPGA_INT, INPUT);
    attachInterrupt(PIN_FPGA_INT, fpga_interrupt, FALLING);

    xEventGroupSetBits(fpga_service_evgroup, NOTIFY_ENABLE);
    xSemaphoreGiveRecursive(spi_mutex);
    fpga_notify_from_task(NOTIFY_FPGA); /* In case FPGA_INT was already low */
    goto done;

release_bus_fail:
    spi_bus_remove_device(spi_handle);
    spi_handle = NULL;

free_bus_fail:
    spi_bus_free(FPGA_SPI_HOST);

init_fail:
    xEventGroupSetBits(fpga_service_evgroup, NOTIFY_DISABLE);

done:
    return !err;
}

static void fpga_link_disable(void)
{
    if (!spi_handle)
	return;			/* Already stopped */

    xEventGroupClearBits(fpga_service_evgroup, NOTIFY_ENABLE);

    xSemaphoreTakeRecursive(spi_mutex, portMAX_DELAY);

    detachInterrupt(PIN_FPGA_INT);

    spi_device_release_bus(spi_handle);
    spi_bus_remove_device(spi_handle);
    spi_bus_free(FPGA_SPI_HOST);
    spi_handle = NULL;

    xEventGroupSetBits(fpga_service_evgroup, NOTIFY_DISABLE);
}

static void fpga_poll_set_time(void);

static bool fpga_online(void)
{
    fpga_io_read(FPGA_CMD_ACK(EL_UIRQ_READY), ESPLINK_HDR_ADDR,
		 &head, sizeof head);

    if (head.magic != ESPLINK_HEAD_MAGIC || head.hlen <= 8) {
	printf("[FPGA] Bad header received, magic = 0x%08x len = %u\n",
	       head.magic, head.hlen);
	return false;
    }

    if (unlikely(head.hlen < sizeof head)) {
	/* Clear any fields not provided */
	memset((char *)&head + head.hlen, 0, sizeof head - head.hlen);
    }

    printf("[FPGA] Ready, board = %u.%u fixes %02x fpga %u\n",
	   head.board.major, head.board.minor,
	   head.board.fixes, head.board.fpga);

    printf("[FPGA] online, signature \"%.*s\"\n",
	   (int)(sizeof head.signature - 1), head.signature);
    esplink_start(&head);

    setenv_bool("status.max80.fpga", true);
    xSemaphoreGiveRecursive(spi_mutex);

    xTimerStart(fpga_timesync_timer, portMAX_DELAY);
    fpga_poll_set_time();

    return true;
}

static void fpga_offline(void)
{
    memset(&head, 0, sizeof head);
    xSemaphoreTakeRecursive(spi_mutex, portMAX_DELAY);
    xTimerStop(fpga_timesync_timer, portMAX_DELAY);
    setenv_bool("status.max80.fpga", false);
    esplink_start(NULL);	/* Stop esplink */
}

esp_err_t fpga_iov(const struct fpga_iov *iov, size_t niov)
{
    spi_transaction_ext_t trans[FPGA_IOV_MAX];
    size_t ntrans = 0;

    if (niov > FPGA_IOV_MAX)
	return ESP_ERR_INVALID_ARG;

    for (size_t i = 0; i < niov; i++) {
	const struct fpga_iov *iv = &iov[i];

	if (!iv->len && !(iv->cmd & FPGA_CMD_NULL))
	    continue;

	spi_transaction_ext_t *t = &trans[ntrans];
	memset(t, 0, sizeof *t);

	t->base.flags =
	    SPI_TRANS_MODE_DIO |
	    SPI_TRANS_VARIABLE_DUMMY |
	    SPI_TRANS_MULTILINE_CMD |
	    SPI_TRANS_MULTILINE_ADDR;

	t->base.cmd           = iv->cmd;
	t->base.addr          = iv->iaddr;
	if (iv->cmd & FPGA_CMD_RD) {
	    t->base.rxlength  = iv->len << 3;
	    t->base.rx_buffer = iv->rdata;
	    /* Emulate partial word read by adding dummy bits for offset */
	    t->dummy_bits     = 16 + ((iv->iaddr & 3) << 2);
	    if (iv->cmd & FPGA_CMD_STATUS) {
		/*
		 * Include the status "dummy" bits
		 * THIS REQUIRES THE REMOTE ADDRESS TO BE 32-BIT ALIGNED
		 */
		t->base.rxlength += 32;
		t->dummy_bits    -= 16;
	    }
	} else {
	    t->base.length    = iv->len << 3;
	    t->base.tx_buffer = iv->wdata;
	}
	ntrans++;
    }

    if (!ntrans)
	return ESP_OK;

    esp_err_t err = ESP_OK;
    xSemaphoreTakeRecursive(spi_mutex, portMAX_DELAY);
    if (!spi_handle) {
	err = ESP_FAIL;
	goto fail;
    }

    xEventGroupClearBits(spi_done_evgroup, EVENT_ALL_BITS);

    size_t tbit = 1;
    for (size_t i = 0; i < ntrans; i++) {
	spi_transaction_ext_t *t = &trans[i];
	t->base.user = (void *)tbit;
	err = spi_device_queue_trans(spi_handle, &t->base, portMAX_DELAY);
	if (err) {
	    ntrans = i;
	    break;
	}
	tbit <<= 1;
    }

    if (likely(ntrans)) {
	xEventGroupWaitBits(spi_done_evgroup, tbit-1, pdTRUE, pdTRUE,
			    portMAX_DELAY);
	while (ntrans--) {
	    /* This is insanely stupid to have to do when not needed */
	    spi_transaction_t *tp;
	    spi_device_get_trans_result(spi_handle, &tp, 0);
	}
    }

fail:
    xSemaphoreGiveRecursive(spi_mutex);
    return err;
}

esp_err_t fpga_io_write(unsigned int cmd, const volatile void *addr,
			const void *data, size_t len)
{
    struct fpga_iov iov;

    iov.cmd   = cmd | ~FPGA_CMD_RD;
    iov.addr  = addr;
    iov.wdata = data;
    iov.len   = len;

    return fpga_iov(&iov, 1);
}

esp_err_t fpga_io_read(unsigned int cmd, const volatile void *addr,
		       void *data, size_t len)
{
    struct fpga_iov iov;

    iov.cmd   = cmd | FPGA_CMD_RD;
    iov.addr  = addr;
    iov.rdata = data;
    iov.len   = len;

    return fpga_iov(&iov, 1);
}

/*
 * This should be executed after getting an EL_UIRQ_TIME notification;
 * do this in polling mode for best latency.
 */
static void fpga_get_time(void)
{
    esp_err_t err;
    struct tm tm;
    struct timeval tv;
    struct tsbuf {
	/* These two words are the status word normally considered "dummy" */
	uint16_t status;
	uint16_t tick;

	struct esplink_timesync_buf get;
    } tsbuf;

    if (!head.tsync) {
	fpga_io_status(FPGA_CMD_ACK(EL_UIRQ_TIME));
	return;
    }

    spi_transaction_ext_t trans;
    memset(&trans, 0, sizeof trans);

    trans.base.flags =
	SPI_TRANS_MODE_DIO |
	SPI_TRANS_VARIABLE_DUMMY |
	SPI_TRANS_MULTILINE_CMD |
	SPI_TRANS_MULTILINE_ADDR;
    trans.base.rxlength = sizeof tsbuf << 3;
    trans.base.rx_buffer = &tsbuf;

    trans.base.addr = (size_t)&head.tsync->get;
    trans.base.cmd  = FPGA_CMD_RD | FPGA_CMD_ACK(EL_UIRQ_TIME);

    xSemaphoreTakeRecursive(spi_mutex, portMAX_DELAY);
    err = spi_device_polling_transmit(spi_handle, &trans.base);
    xSemaphoreGiveRecursive(spi_mutex);
    if (err)
	return;

    if (time_net_sync_status)
	return;		 /* Ignore time from RTC if SNTP active now */

    tm.tm_sec   = tsbuf.get.tm.sec2 << 1;
    tm.tm_min   = tsbuf.get.tm.min;
    tm.tm_hour  = tsbuf.get.tm.hour;
    tm.tm_mday  = tsbuf.get.tm.mday;
    tm.tm_mon   = tsbuf.get.tm.mon - 1;
    tm.tm_year  = tsbuf.get.tm.year + 80;
    tm.tm_isdst = -1;		/* Unknown */

    /* The third term handles wraparounds due to delay in transit */
    tv.tv_sec = mktime(&tm) + (tsbuf.tick >> 15) +
	((tsbuf.get.tick >= tsbuf.tick) << 1);
    tv.tv_usec = (((uint32_t)tsbuf.tick << 17) * UINT64_C(1000000)) >> 32;

    settimeofday(&tv, NULL);

    print_time("[FPGA] Time set from RTC: ", &tv);
}

static void fpga_poll_set_time(void)
{
    if (!head.tsync)
	return;

    if (!time_net_sync_status) {
	/* Poll for current time; will call fpga_get_time() later */
	fpga_io_status(FPGA_CMD_IRQ(EL_DIRQ_TIME));
	return;
    }

    /* Otherwise transmit time to set the RTC */
    esp_err_t err;
    struct timeval tv;
    struct esplink_timesync_buf tset;

    spi_transaction_t trans;
    memset(&trans, 0, sizeof trans);

    tset.update = 1;

    xSemaphoreTakeRecursive(spi_mutex, portMAX_DELAY);

    gettimeofday(&tv, NULL);
    const struct tm *tm = localtime(&tv.tv_sec);

    tset.tick = ((tv.tv_usec * ((1ULL << (32+15))/1000000+1)) >> 32)
      + ((tv.tv_sec & 1) << 15);
    tset.tm.sec2 = tm->tm_sec >> 1;
    tset.tm.min  = tm->tm_min;
    tset.tm.hour = tm->tm_hour;
    tset.tm.mday = tm->tm_mday;
    tset.tm.mon  = tm->tm_mon + 1;
    tset.tm.year = tm->tm_year - 80;

    trans.flags =
	SPI_TRANS_MODE_DIO |
	SPI_TRANS_MULTILINE_CMD |
	SPI_TRANS_MULTILINE_ADDR;
    trans.length = sizeof tset << 3;
    trans.tx_buffer = &tset;

    trans.addr = (size_t)&head.tsync->set;
    trans.cmd  = FPGA_CMD_WR | FPGA_CMD_IRQ(EL_DIRQ_TIME) |
	FPGA_CMD_ACK(EL_UIRQ_TIME);

    err = spi_device_polling_transmit(spi_handle, &trans);
    xSemaphoreGiveRecursive(spi_mutex);
    if (err)
	return;

    print_time("[FPGA] RTC update: ", &tv);
}

/*
 * Get status in polling mode (small transaction, < 256 CPU cycles).
 * cmd typically would be IRQ/ACK bits.
 */
uint32_t fpga_io_status(unsigned int cmd)
{
    spi_transaction_t trans;
    memset(&trans, 0, sizeof trans);

    trans.flags =
	SPI_TRANS_MODE_DIO |
	SPI_TRANS_MULTILINE_CMD |
	SPI_TRANS_MULTILINE_ADDR |
	SPI_TRANS_USE_RXDATA;

    trans.cmd = cmd | FPGA_CMD_RD;
    trans.addr = 0;
    trans.rxlength = 32;

    esp_err_t err = ESP_OK;
    xSemaphoreTakeRecursive(spi_mutex, portMAX_DELAY);
    err = spi_device_polling_transmit(spi_handle, &trans);
    xSemaphoreGiveRecursive(spi_mutex);

    return err ? 0 : *(const uint32_t *)trans.rx_data;
}

static int fpga_read_func(token_t token, void *buf, size_t len)
{
    const void **pp = token;
    const char *p = *pp;
    esp_err_t err;

    err = fpga_io_read(0, p, buf, len);
    if (err)
	return -1;

    p += len;
    *pp = p;
    return len;
}

static void fpga_ota_update(void)
{
    struct esplink_ota ota;

    fpga_io_read(0, head.ota, &ota, sizeof ota);
    if (!ota.data)
	return;

    esp_update(fpga_read_func, &ota.data, ota.len);
    fpga_io_status(FPGA_CMD_ACK(EL_UIRQ_OTA)|FPGA_CMD_IRQ(EL_DIRQ_DONE));
    reboot_delayed();
}

static void fpga_service_task(void *dummy)
{
    (void)dummy;
    uint32_t status;
    bool fpga_initialized = false;
    enum fpga_state {
	FPGA_DISABLED,		/* FPGA services disabled */
	FPGA_OFFLINE,		/* FPGA services enabled, waiting for FPGA */
	FPGA_ONLINE		/* FPGA services active */
    } fpga_state = FPGA_DISABLED;

    fputs("[FPGA] Starting FPGA services task\n", stdout);

    while (1) {
	uint32_t notifiers, status;

	switch (fpga_state) {
	case FPGA_DISABLED:
	    notifiers = notify_wait_for(NOTIFY_ENABLE);

	    if ((notifiers & NOTIFY_ENABLE) && fpga_link_enable()) {
		fputs("[FPGA] Enabling FPGA services\n", stdout);
		fpga_state = FPGA_OFFLINE;
	    }
	    break;

	case FPGA_OFFLINE:
	  status = fpga_io_status(FPGA_CMD_ACK(EL_UIRQ_READY)|
				  FPGA_CMD_IRQ(EL_DIRQ_HELLO));
	  printf("[FPGA] FPGA status flags = 0x%08x\n", status);

	  if (!digitalRead(PIN_FPGA_INT)) {
	      for (unsigned int i = 1; i < 8; i++) {
		  if (status & (0x100 << i))
		      status = fpga_io_status(FPGA_CMD_ACK(i));
	      }
	      if ((status & 0x301) == 0x300) {
		  if (fpga_online()) {
		      fpga_state = FPGA_ONLINE;
		      break;
		  }
	      }
	  }
	  notifiers = notify_wait_for(NOTIFY_FPGA|NOTIFY_DISABLE);
	  break;

	case FPGA_ONLINE:
	    notifiers = notify_wait_for(NOTIFY_FPGA|NOTIFY_DISABLE|
					NOTIFY_TIMESYNC);

	    if (notifiers & NOTIFY_DISABLE) {
		fpga_offline();
		break;
	    }

	    while (!digitalRead(PIN_FPGA_INT)) {
		status = fpga_io_status(0);

		if ((status & 0x301) != 0x100) {
		    fpga_offline();
		    printf("[FPGA] FPGA offline, status = 0x%08x\n", status);
		    fpga_state = FPGA_OFFLINE;
		    notifiers = 0;
		    break;
		}

		if (status & (0x100 << EL_UIRQ_TIME))
		    fpga_get_time();

		if (status & (0x100 << EL_UIRQ_RINGBUF))
		    esplink_poll();

		if (status & (0x100 << EL_UIRQ_OTA))
		    fpga_ota_update();

		for (unsigned int i = 5; i < 8; i++) {
		    if (status & (0x100 << i)) {
			printf("[FPGA] Invalid interrupt %u received\n", i);
			status = fpga_io_status(FPGA_CMD_ACK(i));
		    }
		}
	    }

	    if (notifiers & NOTIFY_TIMESYNC)
		fpga_poll_set_time();

	    break;
	}

	if (notifiers & NOTIFY_DISABLE) {
	    fputs("[FPGA] Disabling FPGA services\n", stdout);
	    fpga_link_disable();
	    fpga_state = FPGA_DISABLED;
	}
    }
}