#include "common.h"
#include "config.h"
#include "fpga.h"
#include "esplink.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

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

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       =  4	/* Maybe? */
};

static spi_device_handle_t spi_handle;
static TaskHandle_t fpga_task;
static SemaphoreHandle_t spi_mutex;

#define NOTIFY_INDEX 0

static void ARDUINO_ISR_ATTR fpga_interrupt(void)
{
    BaseType_t do_wakeup = pdFALSE;

    if (!fpga_task)
	return;

    xTaskNotifyIndexedFromISR(fpga_task, NOTIFY_INDEX, 1, eIncrement,
			      &do_wakeup);
    if (do_wakeup)
	portYIELD_FROM_ISR(do_wakeup);
}

static void fpga_service_task(void *);

int fpga_service_start(void)
{
    esp_err_t err;

    pinMode(PIN_FPGA_INT, INPUT_PULLUP);

    if (!spi_mutex) {
	spi_mutex = xSemaphoreCreateMutex();
	if (!spi_mutex)
	    goto failed;
    }

    xSemaphoreTake(spi_mutex, portMAX_DELAY);

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

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

    /* Only device on this bus, so acquire it permanently */
    spi_device_acquire_bus(spi_handle, portMAX_DELAY);

    xSemaphoreGive(spi_mutex);

    if (!fpga_task) {
	/* The ordering here attempts to avoid race conditions... */
	if (xTaskCreate(fpga_service_task, "fpga_svc", 4096, NULL,
			FPGA_PRIORITY, &fpga_task) != pdPASS)
	    goto failed;
	attachInterrupt(PIN_FPGA_INT, fpga_interrupt, FALLING);
	xTaskNotifyIndexed(fpga_task, NOTIFY_INDEX, 1, eIncrement);
	esp_register_shutdown_handler(fpga_service_stop);
    }

    /* All good (hopefully?) */
    return 0;

failed:
    printf("[FPGA] Failed to initialize FPGA SPI bus\n");
    if (spi_mutex)
	xSemaphoreGive(spi_mutex);

    fpga_service_stop();
    return -1;
}

void fpga_service_stop(void)
{
    if (spi_mutex) {
	xSemaphoreTake(spi_mutex, portMAX_DELAY);

	if (fpga_task) {
	    esp_unregister_shutdown_handler(fpga_service_stop);
	    detachInterrupt(PIN_FPGA_INT);
	    vTaskDelete(fpga_task);
	    fpga_task = NULL;
	}

	if (spi_handle) {
	    spi_device_release_bus(spi_handle);
	    spi_bus_remove_device(spi_handle);
	    spi_bus_free(FPGA_SPI_HOST);
	    spi_handle = NULL;
	}

	xSemaphoreGive(spi_mutex);
    }
    printf("[FPGA] FPGA services stopped\n");
}

#define FPGA_CMD_IRQ(x)	((x) << 0)
#define FPGA_CMD_ACK(x)	((x) << 2)
#define FPGA_CMD_ACK2	(2 << 2)
#define FPGA_CMD_ACK3	(3 << 2)
#define FPGA_CMD_WR	(0 << 4)
#define FPGA_CMD_RD	(1 << 4)
#define FPGA_CMD_CONTROL_MASK 0x0f

#define FPGA_HDR_ADDR	0x40000000

static esp_err_t fpga_io_write(unsigned int cmd, uint32_t addr,
			       const void *data, size_t len)
{
    spi_transaction_ext_t trans;
    esp_err_t err;

    if (!len && !(cmd & ~FPGA_CMD_RD))
	return ESP_OK;

    memset(&trans, 0, sizeof trans);
    trans.base.flags   =
	SPI_TRANS_MODE_DIO |
	SPI_TRANS_MULTILINE_CMD |
	SPI_TRANS_MULTILINE_ADDR;

    trans.base.cmd       = cmd | ~FPGA_CMD_RD;
    trans.base.addr      = addr;
    trans.base.tx_buffer = data;
    trans.base.length    = len << 3;

    xSemaphoreTake(spi_mutex, portMAX_DELAY);
    err = spi_device_transmit(spi_handle, (spi_transaction_t *)&trans);
    xSemaphoreGive(spi_mutex);

    return err;
}

static esp_err_t fpga_io_read(unsigned int cmd, uint32_t addr,
			      void *data, size_t len)
{
    spi_transaction_ext_t trans;
    esp_err_t err;

    if (!len && !(cmd & ~FPGA_CMD_RD))
	return ESP_OK;

    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.cmd       = cmd  | FPGA_CMD_RD;
    trans.base.addr      = addr;
    trans.base.rx_buffer = data;
    /* Emulate partial word read by adding dummy bits for offset */
    trans.dummy_bits     = 16 + ((addr & 3) << 2);
    trans.base.rxlength  = len << 3;

    xSemaphoreTake(spi_mutex, portMAX_DELAY);
    err = spi_device_transmit(spi_handle, (spi_transaction_t *)&trans);
    xSemaphoreGive(spi_mutex);

    return err;
}

/* CMD here can be interrupt flags, for example */
static uint32_t fpga_io_status(unsigned int cmd)
{
    spi_transaction_ext_t trans;
    esp_err_t err;

    memset(&trans, 0, sizeof trans);
    trans.base.flags   =
	SPI_TRANS_MODE_DIO |
	SPI_TRANS_MULTILINE_CMD |
	SPI_TRANS_MULTILINE_ADDR |
	SPI_TRANS_USE_RXDATA;

    trans.base.cmd       = cmd | FPGA_CMD_RD;
    trans.base.rxlength  = 32;

    xSemaphoreTake(spi_mutex, portMAX_DELAY);
    err = spi_device_transmit(spi_handle, (spi_transaction_t *)&trans);
    xSemaphoreGive(spi_mutex);

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

static void fpga_service_task(void *dummy)
{
    (void)dummy;
    struct dram_io_head head;
    uint32_t status;

    printf("[FPGA] Starting FPGA services\n");

    /* If the FPGA is already up, need to issue our own active handshake */
    status = fpga_io_status(FPGA_CMD_IRQ(RV_IRQ_HELLO));
    printf("[FPGA] Link status bits = 0x%08x, int = %u\n",
	   status, digitalRead(PIN_FPGA_INT));

    while (1) {
	/* Wait until an interrupt is received */
	xTaskNotifyWaitIndexed(NOTIFY_INDEX, 0, -1U, NULL, portMAX_DELAY);

	while (!digitalRead(PIN_FPGA_INT)) {
	    bool ok = false;
	    uint32_t status = fpga_io_status(0);
	    printf("[FPGA] Link status bits = 0x%08x\n", status);

	    if ((status & 0x000fc010) == 0x00008000) {
		fpga_io_read(FPGA_CMD_ACK(ESP_IRQ_READY), FPGA_HDR_ADDR,
			     &head, sizeof head);

		if (head.magic == DRAM_IO_MAGIC && head.hlen >= sizeof head) {
		    printf("[FPGA] Ready, board = %u.%u fixes %02x fpga %u\n",
			   head.board.major, head.board.minor,
			   head.board.fixes, head.board.fpga);

		    char signature_string[head.signature_len+1];
		    fpga_io_read(0, (size_t)head.signature,
				 signature_string, head.signature_len);
		    signature_string[head.signature_len] = '\0';

		    fpga_io_write(0, (size_t)head.signature + 9, "GUBBAR", 6);

		    printf("[FPGA] \"%s\"\n", signature_string);
		    ok = true;
		}
	    } else {
		printf("[FPGA] None or invalid FPGA response, offline\n");
	    }
	}
    }
}