#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	3
#define FPGA_SVC_STACK	4096

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 SemaphoreHandle_t spi_mutex;
static EventGroupHandle_t spi_done_evgroup;
static volatile bool spi_abort_all;

#define NOTIFY_INDEX	0
#define NOTIFY_FPGA	(1 << 0)
#define NOTIFY_ENABLE	(1 << 1)
#define NOTIFY_DISABLE	(1 << 2)
#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);
}

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_PULLUP);

    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;

    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, -1);
    
    pinMode(PIN_FPGA_INT, INPUT_PULLUP);
    attachInterrupt(PIN_FPGA_INT, fpga_interrupt, FALLING);

    xEventGroupSetBits(fpga_service_evgroup, NOTIFY_ENABLE);
    xSemaphoreGiveRecursive(spi_mutex);
    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 bool fpga_online(void)
{
    struct esplink_head head;

    fpga_io_read(FPGA_CMD_ACK(EL_UIRQ_READY), ESPLINK_HDR_ADDR,
		 &head, sizeof head);

    if (head.magic != ESPLINK_HEAD_MAGIC || head.hlen <= 8)
	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);

    if (((size_t)head.signature_len - 1) >= 127)
	return false;

    char signature_string[head.signature_len+1];
    fpga_io_read(0, head.signature,
		 signature_string, head.signature_len);
    signature_string[head.signature_len] = '\0';
    fpga_io_write(0, (char *)head.signature + 9, "GUBBAR", 6);

    printf("[FPGA] online, signature \"%s\"\n", signature_string);
    esplink_start(&head);

    xSemaphoreGiveRecursive(spi_mutex);
    return true;
}

static void fpga_offline(void)
{
    xSemaphoreTakeRecursive(spi_mutex, portMAX_DELAY);
    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     = (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, ~(EventBits_t)0);

    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 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 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);
}

/*
 * 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 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;

    printf("[FPGA] Starting FPGA services task\n");
    
    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] FPGA services enabled\n", stdout);
		fpga_state = FPGA_OFFLINE;
	    }
	    break;

	case FPGA_OFFLINE:
	    fpga_io_status(FPGA_CMD_IRQ(EL_DIRQ_HELLO));

	    notifiers = notify_wait_for(NOTIFY_FPGA|NOTIFY_DISABLE);
	    if (notifiers & NOTIFY_DISABLE)
		break;

	    status = fpga_io_status(FPGA_CMD_ACK(EL_UIRQ_READY));

	    if ((status & ~0xfce) == 0x9030 && fpga_online()) {
		fpga_state = FPGA_ONLINE;
	    }

	    break;

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

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

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

		if ((status & ~0xfce) != 0x9010) {
		    fpga_offline();
		    fputs("[FPGA] FPGA offline\n", stdout);
		    fpga_state = FPGA_OFFLINE;
		    break;
		}

		if (status & 0x40)
		    esplink_poll();

		if (status & 0x80) {
		    fputs("[FPGA] invalid upstream interrupt 3\n", stdout);
		    fpga_io_status(FPGA_CMD_ACK(3));
		}	    
	    }
	    break;
	}

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