#include <MKL46Z4.h>

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

#include "timer_dma.h"
#include "serial_irq.h"

/* Defines */
enum {
	TDMA_REQ_PTA  = 49,
	TDMA_REQ_PTC  = 51,
	TDMA_REQ_PTD  = 52,
};

/* Port structure */
struct tdma_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;

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

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

	/* Time stamping */
	uint32_t time[2];

	/* Save state */
	int      req;
	int      irqc;
};

/* Port data */
static tdma_t tdma_ports[TDMA_NUM_CHAN];

/* Global timer initialization */
void tdma_init(void)
{
	static int tdma_init_done = 0;
	if (tdma_init_done)
		return;

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

	// Enable timer Clock
	SIM->SCGC6 |= SIM_SCGC6_PIT_MASK;

	// Enable PIT
	PIT->MCR               = 0;

	// Channel 0
	PIT->CHANNEL[0].LDVAL  = 0xFFFFFFFF;
	PIT->CHANNEL[0].TCTRL  = 0;

	// Channel 1
	PIT->CHANNEL[1].LDVAL  = 0xFFFFFFFF;
	PIT->CHANNEL[1].TCTRL  = PIT_TCTRL_CHN_MASK;

	// Start timers
	PIT->CHANNEL[0].TCTRL |= PIT_TCTRL_TEN_MASK;
	PIT->CHANNEL[1].TCTRL |= PIT_TCTRL_TEN_MASK;

	// Done
	tdma_init_done = 1;
}

/* DMA Functions */
tdma_t *tdma_open(tdma_chan_t chan, int alt, PinName pin, PinMode mode)
{
	int req  = pin >= PTD0 ? TDMA_REQ_PTD :
	           pin >= PTC0 ? TDMA_REQ_PTC :
	           pin >= PTA0 ? TDMA_REQ_PTA : 0;

	int irqc = mode == PullDown ? 1 : 2;

	// Initialize global registers
	tdma_init();

	// Allocate port
	tdma_t *port    = &tdma_ports[chan];

	// Setup port pointers
	port->dma       = (void*)&DMA0->DMA[chan];
	port->mux       = (void*)&DMAMUX0->CHCFG[chan];
	port->pin       = (void*)(PORTA_BASE + pin);

	// Reset DMA channel
	port->dma->dsr  = DMA_DSR_BCR_DONE_MASK;

	// Configure DMA channel
	port->dma->dcr  = DMA_DCR_SINC_MASK         // Source increment
	                | DMA_DCR_DINC_MASK         // Dest increment
	                | DMA_DCR_SSIZE(0)          // 32-bit access
	                | DMA_DCR_DSIZE(0)          // 32-bit access
	                | DMA_DCR_D_REQ_MASK;       // Only run once

	// Setup and enable DMA MUX
	port->mux->cfg  = DMAMUX_CHCFG_SOURCE(req)  // Request source
	                | DMAMUX_CHCFG_ENBL_MASK;   // Enable DMA mux

	// Set pin to generate DMA req
	port->pin->pcr  = PORT_PCR_ISF_MASK         // Clear ISR flag
	                | PORT_PCR_MUX(alt)         // Pin mapping
	                | PORT_PCR_IRQC(irqc)       // DMA on falling edge
	                | mode;                     // Pin pull up/down

	// Save IRC for later
	port->req       = req;
	port->irqc      = irqc;

	return port;
}

void tdma_start(tdma_t *port)
{
	if (!port)
		return;

	//sirq_printf("isfr2: %08x\r\n", PORTD->ISFR);

	// Clear previous time
	port->time[0] = 0;
	port->time[1] = 0;

	// Reset DMA Mux
	port->mux->cfg &= DMAMUX_CHCFG_ENBL_MASK;

	// Freeze DMA channel
	port->dma->dcr &= ~DMA_DCR_ERQ_MASK;

	// Reset DMA channel
	port->dma->dsr  =  DMA_DSR_BCR_DONE_MASK;

	// Set addresses and size
	port->dma->sar  =  (uint32_t)&PIT->LTMR64H;  // Global timer
	port->dma->dar  =  (uint32_t)&port->time;    // Temp timer buffer
	port->dma->dsr  =  DMA_DSR_BCR_BCR(8);       // 64-bit timer

	// Set pin to generate DMA req
	port->pin->pcr |=  PORT_PCR_ISF_MASK;
	port->pin->pcr |=  PORT_PCR_IRQC(port->irqc);

	// Enable port request
	port->dma->dcr |=  DMA_DCR_ERQ_MASK;

	// Enable DMA Mux
	port->mux->cfg  = DMAMUX_CHCFG_SOURCE(port->req)
	                | DMAMUX_CHCFG_ENBL_MASK;

	//sirq_printf("isfr3: %08x\r\n", PORTD->ISFR);
}

void tdma_stop(tdma_t *port, int wait)
{
	if (!port)
		return;

	//sirq_printf("isfr0: %08x\r\n", PORTD->ISFR);

	for (int i = 0; port->dma->dsr & DMA_DSR_BCR_BCR_MASK; i++)
		if (i > wait)
			return;

	// Disable DMA Mux
	port->mux->cfg &= DMAMUX_CHCFG_ENBL_MASK;

	// Freeze DMA channel
	port->dma->dcr &= ~DMA_DCR_ERQ_MASK;

	// Reset DMA channel
	port->dma->dsr  =  DMA_DSR_BCR_DONE_MASK;

	// Disable pin DMA request
	port->pin->pcr &= ~PORT_PCR_IRQC_MASK;
	port->pin->pcr |=  PORT_PCR_ISF_MASK;

	//sirq_printf("isfr1: %08x\r\n", PORTD->ISFR);
}

int tdma_stamp(tdma_t *port, uint64_t *time)
{
	uint64_t clocks;

	if (!port)
		return 0;

	if (port->dma->dsr & DMA_DSR_BCR_BCR_MASK)
		return 0;

	// Read the timestamp
	clocks = ((uint64_t)~port->time[0]) << 32
	       | ((uint64_t)~port->time[1]) << 0;

	// Convert to nanoseconds
	*time  = clocks * 1000 / 24;

	return 1;
}

uint64_t tdma_time(void)
{
	uint32_t tmh = PIT->LTMR64H;
	uint32_t tml = PIT->LTMR64L;

	// Read the timestamp
	uint64_t clocks = ((uint64_t)~tmh) << 32
	                | ((uint64_t)~tml) << 0;

	// Convert to nanoseconds
	return clocks * 125 / 3;
}

void tdma_debug(tdma_t *port)
{
	int dsr = port->dma->dsr;
	int dcr = port->dma->dcr;

	printf("dsr: %s %s %s %s %s %s %d\r\n",
			 dsr & DMA_DSR_BCR_CE_MASK   ? "CE"      : "ce",
			 dsr & DMA_DSR_BCR_BES_MASK  ? "BSE"     : "bse",
			 dsr & DMA_DSR_BCR_BED_MASK  ? "BED"     : "bed",
			 dsr & DMA_DSR_BCR_REQ_MASK  ? "REQ"     : "req",
			 dsr & DMA_DSR_BCR_BSY_MASK  ? "BSY"     : "bsy",
			 dsr & DMA_DSR_BCR_DONE_MASK ? "DONE"    : "done",
			 dsr & DMA_DSR_BCR_BCR_MASK);

	printf("dcr: %s %s %s %s %s %s %s %s ssize=%d:%d mod=%d:%d link=%d:%d:%d\r\n",
			 dcr & DMA_DCR_EINT_MASK     ? "EINT"    : "eint",
			 dcr & DMA_DCR_ERQ_MASK      ? "ERQ"     : "erq",
			 dcr & DMA_DCR_CS_MASK       ? "CS"      : "cs",
			 dcr & DMA_DCR_AA_MASK       ? "AA"      : "aa",
			 dcr & DMA_DCR_EADREQ_MASK   ? "EADRREQ" : "eadrreq",
			 dcr & DMA_DCR_SINC_MASK     ? "SINC"    : "sinc",
			 dcr & DMA_DCR_DINC_MASK     ? "DINC"    : "dinc",
			 dcr & DMA_DCR_D_REQ_MASK    ? "DREQ"    : "dreq",
			(dcr & DMA_DCR_SSIZE_MASK ) >> DMA_DCR_SSIZE_SHIFT,
			(dcr & DMA_DCR_DSIZE_MASK ) >> DMA_DCR_DSIZE_SHIFT,
			(dcr & DMA_DCR_SMOD_MASK  ) >> DMA_DCR_SMOD_SHIFT,
			(dcr & DMA_DCR_DMOD_MASK  ) >> DMA_DCR_DMOD_SHIFT,
			(dcr & DMA_DCR_LINKCC_MASK) >> DMA_DCR_LINKCC_SHIFT,
			(dcr & DMA_DCR_LCH1_MASK  ) >> DMA_DCR_LCH1_SHIFT,
			(dcr & DMA_DCR_LCH2_MASK  ) >> DMA_DCR_LCH2_SHIFT);
}