#include <MKL46Z4.h>

#include <stdint.h>
#include <stdarg.h>
#include <stdio.h>
#include <string.h>

#include "serial_dma.h"

/* Defines */
#define SDMA_NUM  2
#define SDMA_LEN  1024

enum {
	SDMA_REQ_U0RX = 2,
	SDMA_REQ_U0TX = 3,
	SDMA_REQ_U1RX = 4,
	SDMA_REQ_U1TX = 5,
	SDMA_REQ_U2RX = 6,
	SDMA_REQ_U2TX = 7,
	SDMA_REQ_PTA  = 49,
	SDMA_REQ_PTC  = 51,
	SDMA_REQ_PTD  = 52,
	SDMA_REQ_ON0  = 60,
	SDMA_REQ_ON1  = 61,
	SDMA_REQ_ON2  = 62,
	SDMA_REQ_ON3  = 63,
};

/* Port structure */
struct sdma_t {
	/* DMA channel */
	struct {
		uint32_t sar; // offset 0x00, Source Address Register
		uint32_t dar; // offset 0x04, Destination Address Register
		uint32_t dsr; // offset 0x08, DMA Status Register / Byte Count Register
		uint32_t dcr; // offset 0x0C, DMA Control Register
	} *dma_xmt, *dma_rcv;

	/* DMA mux */
	struct {
		uint8_t  cfg; // offset 0x00, Channel Configuration register
	} *mux_xmt, *mux_rcv;

	/* Pin names */
	struct {
		uint32_t pcr; // offset 0x00, Pin Control register
	} *pin_xmt, *pin_rcv;

	/* UART */
	struct {
		uint8_t  bdh;    // offset 0x00, Baud Rate Register High
		uint8_t  bdl;    // offset 0x01, Baud Rate Register Low
		uint8_t  c1;     // offset 0x02, Control Register 1
		uint8_t  c2;     // offset 0x03, Control Register 2
		uint8_t  s1;     // offset 0x04, Status Register 1
		uint8_t  s2;     // offset 0x05, Status Register 2
		uint8_t  c3;     // offset 0x06, Control Register 3
		uint8_t  d;      // offset 0x07, Data Register
	} *uart;

	/* Data buffering */
	int      index;
	int      length[SDMA_NUM];
	uint8_t  queue[SDMA_NUM][SDMA_LEN];

	/* Timestamping */
	uint32_t time_xmt[2];
	uint32_t time_rcv[2];

	/* Error logging */
	int      stuck;
	int      full;
};

/* DMA Request Sources */
static int sdma_req_rx[] = {
	[SDMA_UART0] SDMA_REQ_U0RX,
	[SDMA_UART1] SDMA_REQ_U1RX,
	[SDMA_UART2] SDMA_REQ_U2RX,
};
static int sdma_req_tx[] = {
	[SDMA_UART0] SDMA_REQ_U0TX,
	[SDMA_UART1] SDMA_REQ_U1TX,
	[SDMA_UART2] SDMA_REQ_U2TX,
};
static int sdma_uart[] = {
	[SDMA_UART0] UART0_BASE,
	[SDMA_UART1] UART1_BASE,
	[SDMA_UART2] UART2_BASE,
};

/* Port data */
static sdma_t sdma_ports[SDMA_NUM_UART];

/* DMA Functions */
sdma_t *sdma_open(sdma_uart_t uart, sdma_dma_t tx_chan, sdma_dma_t rx_chan)
{
	int rxreq = sdma_req_rx[uart]; (void)rxreq;
	int txreq = sdma_req_tx[uart]; (void)txreq;

	// Setup port
	sdma_t *port = &sdma_ports[uart];

	port->uart      = (void*)sdma_uart[uart];
	port->dma_xmt   = (void*)&DMA0->DMA[tx_chan];
	port->dma_rcv   = (void*)&DMA0->DMA[rx_chan];
	port->mux_xmt   = (void*)&DMAMUX0->CHCFG[tx_chan];
	port->mux_rcv   = (void*)&DMAMUX0->CHCFG[rx_chan];

	// Enable DMA Cock
	SIM->SCGC5     |= SIM_SCGC5_PORTA_MASK;
	SIM->SCGC5     |= SIM_SCGC5_PORTC_MASK;
	SIM->SCGC5     |= SIM_SCGC5_PORTD_MASK;
	SIM->SCGC6     |= SIM_SCGC6_DMAMUX_MASK;
	SIM->SCGC7     |= SIM_SCGC7_DMA_MASK;

	// Reset channel
	port->dma_xmt->dsr  = DMA_DSR_BCR_DONE_MASK;

	// Configure DMA transfer
	port->dma_xmt->dar  = (uint32_t)&port->uart->d;
	port->dma_xmt->dcr  = DMA_DCR_CS_MASK
		            | DMA_DCR_SINC_MASK
		            | DMA_DCR_SSIZE(1)
		            | DMA_DCR_DSIZE(1)
		            | DMA_DCR_D_REQ_MASK;

	// Configure DMA Mux
	port->mux_xmt->cfg  = DMAMUX_CHCFG_SOURCE(txreq)
	                    | DMAMUX_CHCFG_ENBL_MASK;

	// Configure UART for DMA Channel 0
	switch (uart) {
		case SDMA_UART0:
			UART0->C5 |= UART0_C5_TDMAE_MASK;
			break;

		case SDMA_UART1:
			UART1->C2 = UART_C2_TIE_MASK
			          | UART_C2_TE_MASK
			          | UART_C2_RE_MASK;
			UART1->C4 = UART_C4_TDMAS_MASK;
			break;

		case SDMA_UART2:
			UART2->C2 = UART_C2_TIE_MASK
			          | UART_C2_TE_MASK
			          | UART_C2_RE_MASK;
			UART2->C4 = UART_C4_TDMAS_MASK;
			break;
	}
	return port;
}

/* Map port to pins - for now, just save for timing */
void sdma_pinmap(sdma_t *port, PinName tx, PinName rx)
{
	port->pin_xmt = (void*)(PORTA_BASE+tx);
	port->pin_rcv = (void*)(PORTA_BASE+rx);
}

/* Write binary data out the DMA output queue */
void sdma_write(sdma_t *port, void *data, int len)
{
	if (port->length[port->index] + len > SDMA_LEN) {
		port->full++;
	} else {
		int   pos = port->length[port->index];
		void *dst = &port->queue[port->index][pos];
		memcpy(dst, data, len);
		port->length[port->index] += len;
	}
}

/* Read binary data from the channel */
void sdma_read(sdma_t *port, void *data, int len)
{
	for (int i = 0; i < len; i++) {
		// wait for byte
		while (!(port->uart->s1 & UART_S1_RDRF_MASK))
			if (port->uart->s1 & UART_S1_OR_MASK)
				port->uart->s1 |= UART_S1_OR_MASK;

		// read the byte
		((uint8_t*)data)[i] = port->uart->d;
	}
}

/* Trigger DMA transmit of the current output queue
 * and swap buffers so we can write into unused space */
void sdma_flush(sdma_t *port, uint64_t *time)
{
	if (port->length[port->index] == 0)
		return;

	// Wait for transmit complete
	while (port->dma_xmt->dsr & DMA_DSR_BCR_BCR_MASK)
		port->stuck++;

	// Reset channel
	port->dma_xmt->dsr  = DMA_DSR_BCR_DONE_MASK;

	// Set source address and length
	port->dma_xmt->sar  = (uint32_t)&port->queue[port->index];
	port->dma_xmt->dsr  = DMA_DSR_BCR_BCR(port->length[port->index]);

	// Enable DMA transmit
	port->dma_xmt->dcr |= DMA_DCR_ERQ_MASK;

	// Swap buffers
	port->length[port->index] = 0;
	port->index = (port->index + 1) % SDMA_NUM;
}

/* Wait for DMA receive complete */
void sdma_wait(sdma_t *port, uint64_t *time)
{
	int req = (void*)port->pin_rcv > (void*)PORTD ? SDMA_REQ_PTD :
	          (void*)port->pin_rcv > (void*)PORTC ? SDMA_REQ_PTC :
	          (void*)port->pin_rcv > (void*)PORTA ? SDMA_REQ_PTA : 0;

	// Reset channel
	port->dma_rcv->dsr = DMA_DSR_BCR_DONE_MASK;

	// Configure channel
	port->dma_rcv->dcr  = DMA_DCR_SINC_MASK
		            | DMA_DCR_DINC_MASK
		            | DMA_DCR_SSIZE(0)
		            | DMA_DCR_DSIZE(0)
		            | DMA_DCR_D_REQ_MASK;

	// Setup muxing
	port->mux_rcv->cfg  = DMAMUX_CHCFG_SOURCE(req)
	                    | DMAMUX_CHCFG_ENBL_MASK;

	// Set address and size
	port->dma_rcv->sar  = (uint32_t)&PIT->LTMR64H;
	port->dma_rcv->dar  = (uint32_t)&port->time_rcv;
	port->dma_rcv->dsr  = DMA_DSR_BCR_BCR(sizeof(uint64_t));

	// Enable DMA transmit
	port->dma_rcv->dcr |= DMA_DCR_ERQ_MASK;

	// set pin to generate DMA req
	port->pin_rcv->pcr  = PORT_PCR_ISF_MASK
	                    | PORT_PCR_IRQC(1)
	                    | PORT_PCR_MUX(3)
	                    | PORT_PCR_PE_MASK;

	// Wait for transmit complete
	while ((port->dma_rcv->dsr & DMA_DSR_BCR_BCR_MASK))
		port->stuck++;

	// Save recv time
	*time = ((uint64_t)~port->time_rcv[0]) << 32
	      | ((uint64_t)~port->time_rcv[1]) << 0;

	// pcr:00030302 dsr:41000008
	printf(" - pcr:%08lx dsr:%08lx time:%08lx:%08lx",
		port->pin_rcv->pcr, port->dma_rcv->dsr,
		(uint32_t)(*time >> 32), (uint32_t)*time);
}

/* Write ASCII data to the output queue */
void sdma_vprintf(sdma_t *port, const char *fmt, va_list ap)
{
	int   pos = port->length[port->index];
	void *dst = &port->queue[port->index][pos];
	port->length[port->index] +=
		vsnprintf((char*)dst, SDMA_LEN-pos, fmt, ap);
}

void sdma_printf(sdma_t *port, const char *fmt, ...)
{
	va_list ap;
	va_start(ap, fmt);
	sdma_vprintf(port, fmt, ap);
	va_end(ap);
}