#include "mmb0.h"
#include "ads1271.h"
#include <stdlib.h>

#define DMACH_MCBSP 0
#define DMACH_COPY 1

static volatile struct {
	unsigned int iface_mode:3,
		osc_source:2,
		conv_mode:2,
		acquiring:1, // 1 = first stage FIFO running
		acquirereq:1, // 1 = enable first stage FIFO
		packing:1, // 1 = second stage FIFO writing
		packreq:1, // 1 = enable second stage FIFO
		// which buffer the ADS DMA is writing to
		// 0 = adsbuf1, 1 = adsbuf2
		adsdest:1;
} flags;

static u32 adsbuf1, adsbuf2;
static bfifo_t *adsfifo;
static u16 adsblocksize, adschannels; // blocksize is in frames
static volatile u32 adsacquired; // no. of frames acquired; could (in theory) wrap
static u32 adsoverrun;

void delay(u32 c)
{
	volatile u32 i=c;
	
	while (i--);
}

static void mcbsp_start(void)
{
	SPCR2(1)|=SPCR2_GRST;
	delay(200);
	SPCR1(1)|=SPCR1_RRST;
	SPCR2(1)|=SPCR2_XRST;
	delay(100);
}

#if 0
static void mcbsp_stop(void)
{
	SPCR2(1)&=~SPCR2_XRST;
	SPCR1(1)&=~SPCR1_RRST;
	SPCR2(1)&=~SPCR2_GRST;	
	delay(200);
}
#endif

void ads_set_conv_mode(int mode)
{
	if (mode==flags.conv_mode) return;
	switch(mode) {
		case ADS_CONV_MODE_HS:
			flags.conv_mode=ADS_CONV_MODE_HS;
			pin_set_GPIO1(0);
			pin_set_GPIO0(0);
			break;
		case ADS_CONV_MODE_HR:
			flags.conv_mode=ADS_CONV_MODE_HR;
			pin_set_GPIO1(1);
			break;
		default:
			flags.conv_mode=ADS_CONV_MODE_LP;
			pin_set_GPIO1(0);
			pin_set_GPIO0(1);
			break;
	}
}

int ads_conv_mode(void)
{
	return flags.conv_mode;
}

#if 0
static void ads_set_iface_spi0(void)
{
	pin_set_ctl(PIN_FSXON|PIN_CLKXON|PIN_CLKRON|PIN_CLKBON);
	pin_set_GPIO3(0);
	pin_set_GPIO4(1);
	pin_set_GPIO5(0); // obclksel
	mmb0_activate_card();

	// mcbsp config for spi0, first channel only
	SPCR2(1)&=~SPCR2_XRST;
	SPCR1(1)&=~SPCR1_RRST;
	SPCR2(1)&=~SPCR2_GRST;
	SPCR1(1)|=0x4000;
	PCR(1)=0x187;
	RCR2(1)=0;
	XCR1(1)=0;
	XCR2(1)=0;
	SRGR1(1)=0x100;
	SRGR2(1)=0x3000;
}
#endif

static void ads_set_iface_spi1(void)
{
	pin_set_ctl(PIN_FSXON|PIN_CLKRON|PIN_CLKBON);
	pin_set_GPIO3(1);
	pin_set_GPIO4(0);
	pin_set_GPIO5(0); // obclksel
	mmb0_activate_card();

	// mcbsp config for spi0, first channel only
	//SPCR2(1)&=~SPCR2_XRST;
	SPCR1(1)&=~SPCR1_RRST;
	SPCR2(1)&=~SPCR2_GRST;
	SPCR1(1)|=0x4000;
	PCR(1)=0x086;
	RCR2(1)=0;
	XCR1(1)=0;
	XCR2(1)=0;
	SRGR1(1)=0x100;
	SRGR2(1)=0x9000;
}

static void adsDMA(u32 adr)
{
	DMACDSAU(DMACH_MCBSP)=(adr>>16)&0xffff;
	DMACDSAL(DMACH_MCBSP)=(adr)&0xfffe;
}

static void copyDMA(u32 source, u32 dest)
{
	DMACDSAU(DMACH_COPY)=(dest>>16)&0xffff;
	DMACDSAL(DMACH_COPY)=(dest)&0xffff;
	DMACSSAU(DMACH_COPY)=(source>>16)&0xffff;
	DMACSSAL(DMACH_COPY)=(source)&0xffff;
}

void isr_dma0(unsigned long arg)
{
	u16 stat=DMACSR(DMACH_MCBSP);
	u32 wpos;

	if (stat&DMACSR_BLOCK) {
		if (!flags.acquirereq) {
			flags.acquiring=0;
			flags.packreq=0;
			flags.packing=0;
			DMACCR(DMACH_MCBSP)&=~DMACCR_EN;
			return;
		}
		flags.acquiring=1;
		flags.adsdest=!flags.adsdest;
		if (DMACCR(DMACH_COPY)&DMACCR_EN) {
			++adsoverrun;
			flags.packing=0;
			flags.packreq=0;
			DMACCR(DMACH_COPY)&=~DMACCR_EN;
			bfifo_wend(adsfifo);
		} else if (bfifo_iswriting(adsfifo)) {
			bfifo_wend(adsfifo);
			//++adsacquired;
			//if (adsblocks&&(adsacquired>=adsblocks))
			//	flags.packreq=0;
			if (!flags.packreq)
				flags.packing=0;
		}
		if (flags.packreq)
			flags.packing=1;
		if (flags.packing&&(!adsoverrun)) {
			wpos=bfifo_wstart(adsfifo);
			if (wpos) {
				copyDMA(flags.adsdest?adsbuf1:adsbuf2,wpos);
				DMACCR(DMACH_COPY)|=DMACCR_EN;
			} else {
				++adsoverrun;
				flags.packing=0;
			}
		}
		while (DMACCR(DMACH_MCBSP)&DMACCR_ENDPROG);
		adsDMA(flags.adsdest?adsbuf1:adsbuf2);
		//DMACCR(DMACH_MCBSP)|=DMACCR_EN|DMACCR_AUTOINIT;
		DMACCR(DMACH_MCBSP)|=DMACCR_ENDPROG;
	}
}

void ads_acquire_start(u16 blocksize, u16 channels, u32 buf1, u32 buf2)
{
	if (flags.acquiring) return;
	flags.adsdest=0;
	adsblocksize=blocksize;
	adschannels=channels;
	adsbuf1=buf1;
	adsbuf2=buf2;
	flags.acquirereq=1;
	adsoverrun=0;

	ads_sync();

	// set up MCBSP channels
	RCR1(1)=0x80|((channels-1)<<8);

	// ADS DMA channel
	DMACICR(DMACH_MCBSP)=DMACICR_BLOCKIE;
	DMACCR(DMACH_MCBSP)=DMACCR_DSTAMODE_AUTOMATIC|
		DMACCR_SRCAMODE_DOUBLE|
		DMACCR_PRIO|
		DMACCR_FS|
		DMACCR_REVT1;
	DMACSDP(DMACH_MCBSP)=DMACSDP_DSTBEN_DISABLE|
		DMACSDP_DST_EMIF|
		DMACSDP_SRC_IO|
		DMACSDP_DATATYPE_16;
	DMACSEI(DMACH_MCBSP)=1;
	DMACSFI(DMACH_MCBSP)=-3;
	DMACDEI(DMACH_MCBSP)=2;
	DMACDFI(DMACH_MCBSP)=2;
	DMACFN(DMACH_MCBSP)=adsblocksize*adschannels;
	DMACEN(DMACH_MCBSP)=2;
	// MCBSP1 DRR is at 0x2C00:0x2C01
	DMACSSAU(DMACH_MCBSP)=0;
	DMACSSAL(DMACH_MCBSP)=0x2C00<<1;
	adsDMA(adsbuf1);
	DMACCR(DMACH_MCBSP)|=DMACCR_ENDPROG;

	// copy DMA config
	DMACICR(DMACH_COPY)=0;
	DMACCR(DMACH_COPY)=DMACCR_DSTAMODE_AUTOMATIC|
		DMACCR_SRCAMODE_DOUBLE;
	DMACSDP(DMACH_COPY)=DMACSDP_DSTPACK|
		DMACSDP_SRCPACK|
		DMACSDP_DST_EMIF|
		DMACSDP_SRC_EMIF|
		DMACSDP_DATATYPE_8;
	DMACSEI(DMACH_COPY)=1;
	DMACSFI(DMACH_COPY)=2;
	DMACEN(DMACH_COPY)=3;
	DMACFN(DMACH_COPY)=adsblocksize*adschannels;

	// start ch 0 DMA
	DMACCR(DMACH_MCBSP)|=DMACCR_EN;

	// start ADC
	mcbsp_start();

	while (DMACCR(DMACH_MCBSP)&DMACCR_ENDPROG);

	// we're acquiring, set up next transfer
	DMACCR(DMACH_MCBSP)|=DMACCR_AUTOINIT;
	adsDMA(adsbuf2);
	DMACCR(DMACH_MCBSP)|=DMACCR_ENDPROG;

	// done, we're running
}

void ads_acquire_stop(void)
{
	flags.acquirereq=0;
}

int ads_acquiring(void)
{
	if (!(DMACCR(DMACH_MCBSP)&DMACCR_EN)) {
		flags.acquiring=0;
		if (DMACSR(DMACH_MCBSP)&DMACSR_TIMEOUT)
			adsoverrun++;
	}
	return flags.acquiring;
}

void ads_pack_start(bfifo_t *f, u32 blocks)
{
	if (flags.packing) return;
	if (!f) return;
	adsfifo=f;
	//adsblocks=blocks;
	adsoverrun=0;
	//adsacquired=0;
	flags.packreq=1;
}

void ads_pack_stop(void)
{
	flags.packreq=0;
}

int ads_packing(void)
{
	return flags.packing;
}

void ads_sync(void)
{
	pin_set_CNTL(0);
	delay(100);
	pin_set_CNTL(1);
}

void ads_init(void)
{
	pin_set_GPIO0_dir(1);
	pin_set_GPIO0(0);
	pin_set_GPIO1_dir(1);
	pin_set_GPIO1(0);
	pin_set_GPIO2_dir(0);
	pin_set_GPIO3_dir(1);
	pin_set_GPIO4_dir(1);
	pin_set_GPIO5_dir(1);
	pin_set_CNTL_dir(1);
	pin_set_CNTL(0);
	flags.conv_mode=ADS_CONV_MODE_HR;
	flags.osc_source=3;
	flags.iface_mode=7;
	flags.acquiring=0;
	flags.acquirereq=0;
	flags.packing=0;
	flags.packreq=0;

	// set up global dma stuff
	DMAGCR=0;
	DMAGSCR=1;
	DMAGTCR=3;

	// config mcbsp
	SPCR1(1)=0;
	SPCR2(1)=0;
	ads_set_iface_spi1();
}