#include <MKL46Z4.h>

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

#include "serial_dma.h"

/* Defines */
#define UART sdma_uart
#define DMA  (&(DMA0->DMA[sdma_chan]))
#define MUX  ((dma_mux_t*)&(DMAMUX0->CHCFG[sdma_chan]))

#define NUM  2
#define LEN  1024

/* Types */
typedef struct {uint8_t CHCFG;} dma_mux_t;

/* Setup data */
static UART0_Type *sdma_uart;
static int         sdma_chan;

/* Error logging */
static int         sdma_stuck;
static int         sdma_full;

/* Data buffering */
static int         sdma_index;
static int         sdma_length[NUM];
static uint8_t     sdma_queue[NUM][LEN];

/* DMA Functions */
void sdma_setup(UART0_Type *uart, int channel)
{
	// Save configuration data
	sdma_uart = uart;
	sdma_chan = channel;

	// Enable DMA Cock
	SIM->SCGC6   |= SIM_SCGC6_DMAMUX_MASK;
	SIM->SCGC7   |= SIM_SCGC7_DMA_MASK;

	// Reset channel
	DMA->DSR_BCR  = DMA_DSR_BCR_DONE_MASK;

	// Configure DMA transfer
	DMA->DAR      = (uint32_t)&UART0->D;
	DMA->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
	MUX->CHCFG    = DMAMUX_CHCFG_SOURCE(3) |
	                DMAMUX_CHCFG_ENBL_MASK;

	// Configure UART for DMA Channel 0
	UART->C5     |= UART0_C5_TDMAE_MASK;
}

/* Write binary data out the DMA output queue */
void sdma_write(void *data, int len)
{
	if (sdma_length[sdma_index] + len > LEN) {
		sdma_full++;
	} else {
		int   pos = sdma_length[sdma_index];
		void *dst = &sdma_queue[sdma_index][pos];
		memcpy(dst, data, len);
		sdma_length[sdma_index] += len;
	}
}

/* Write ASCII data to the output queue */
void sdma_vprintf(const char *fmt, va_list ap)
{
	int   pos = sdma_length[sdma_index];
	void *dst = &sdma_queue[sdma_index][pos];
	sdma_length[sdma_index] +=
		vsnprintf((char*)dst, LEN-pos, fmt, ap);
}

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

/* Trigger DMA transmit of the current output queue
 * and swap buffers so we can write into unused space */
void sdma_flush(void)
{
	if (sdma_length[sdma_index] == 0)
		return;

	// Wait for transmit complete
	while (DMA->DSR_BCR & DMA_DSR_BCR_BCR_MASK)
		sdma_stuck++;

	// Reset channel
	DMA->DSR_BCR  = DMA_DSR_BCR_DONE_MASK;

	// Set source address and length
	DMA->SAR      = (uint32_t)&sdma_queue[sdma_index];
	DMA->DSR_BCR  = DMA_DSR_BCR_BCR(sdma_length[sdma_index]);

	// Enable DMA transmit
	DMA->DCR     |= DMA_DCR_ERQ_MASK;

	// Swap buffers
	sdma_length[sdma_index] = 0;
	sdma_index = (sdma_index + 1) % NUM;
}