/* 
 * GigaSpaceWire switch driver for MC-30SF6EM-6U evaluation kit
 * 
 * Copyright (c) 2019 Elvees (support@elvees.com)
 * Author: Dmitry Evtushenko
 *
 */

#include "test.h"
#include "gspwsw.h"
#include "func.h"

#include <multicore.h>
#include <asm/mipsregs.h>



// cp0 registers access macros:

// read C0 coprocessor register.
#define mips_read_c0_register(reg)                               \
    ({                                                           \
        unsigned int __value;                                             \
        asm volatile("mfc0 %0, $%1" : "=r"(__value) : "K"(reg)); \
        __value;                                                 \
    })

#define mips_read_c0_reg(reg)                                   \
    ({                                                          \
        unsigned int __value;                                            \
        asm volatile("mfc0 %0, %1" : "=r"(__value) : "r"(reg)); \
        __value;                                                \
    })

// asm volatile("mfc0 %0, %1" : "=r"(__value) : "r"(reg));


/*
// read C0 coprocessor register with select
#define mips_read_c0_select(reg, sel)                                          \
    ({                                                                         \
        int __value;                                                           \
        asm volatile("mfc0 %0, $%1, %2" : "=r"(__value) : "K"(reg), "K"(sel)); \
        __value;                                                               \
    })*/

// write C0 coprocessor register.
#define mips_write_c0_register( reg, val) \
    do \
		{ \
			asm volatile("mtc0 %z0, $%1\n nop\n nop\n nop" : : "r"((unsigned int)(val)), "K"(reg)); \
    } while (0)

/*
// write C0 coprocessor register with select
#define mips_write_c0_select(reg, sel, value)                                                                   \
    do                                                                                                          \
    {                                                                                                           \
        asm volatile("mtc0 %z0, $%1, %2\n nop\n nop\n nop" : : "r"((unsigned int)(value)), "K"(reg), "K"(sel)); \
    } while (0)
*/


#define  READ_CP0( reg) \
	if( addr==reg) \
		return mips_read_c0_register(reg);

#define  WRITE_CP0( reg, val) \
	if( addr==reg) \
		mips_write_c0_register( reg, val);

u32 read_cp0_reg( u32 addr)
{
	READ_CP0( 0)
	READ_CP0( 1)
	READ_CP0( 2)
	READ_CP0( 3)
	READ_CP0( 4)
	READ_CP0( 5)
	READ_CP0( 6)
	READ_CP0( 7)
	READ_CP0( 8)
	READ_CP0( 9)
	READ_CP0( 10)
	READ_CP0( 11)
	READ_CP0( 12)
	READ_CP0( 13)
	READ_CP0( 14)
	READ_CP0( 15)
	READ_CP0( 16)
	READ_CP0( 17)
	return 0;
}

void write_cp0_reg( u32 addr, u32 val)
{
	WRITE_CP0( 0, val)
	WRITE_CP0( 1, val)
	WRITE_CP0( 2, val)
	WRITE_CP0( 3, val)
	WRITE_CP0( 4, val)
	WRITE_CP0( 5, val)
	WRITE_CP0( 6, val)
	WRITE_CP0( 7, val)
	WRITE_CP0( 8, val)
	WRITE_CP0( 9, val)
	WRITE_CP0( 10, val)
	WRITE_CP0( 11, val)
	WRITE_CP0( 12, val)
	WRITE_CP0( 13, val)
	WRITE_CP0( 14, val)
	WRITE_CP0( 15, val)
	WRITE_CP0( 16, val)
	WRITE_CP0( 17, val)
}



#ifdef USE_DMA_TEST

//////////////////// dma_test_t: ////////////////////

void dma_test_init( dma_test_t *pt)
{
	memset( pt, 0, sizeof(*pt));

	// set defaults:
	// needs > 16 buffers on tx:
	pt->qnt_tx_desc = 20;
	pt->qnt_tx_data = 20;
	pt->qnt_rx_desc = 20;
	pt->qnt_rx_data = 20;

	pt->sz_rx_data_buf = 0x10000; //16384;
	pt->sz_tx_data_buf = 0x10000; //16384;
}

void dma_test_free( gspw_cfg_ctrl_t *pcfg)
{
	gspw_cfg_ctrl_free( pcfg);

	pcfg->test.is_created_chains = 0;
}

int dma_test_create( gspw_cfg_ctrl_t *pcfg)
{
	dma_test_t *pt = &pcfg->test;
	int rc;

	dma_test_free( pcfg);

	if( (rc = dma_chain_init( &pcfg->dch_rx_desc, pt->qnt_rx_desc, sizeof(dma_desc_fi_t),
											 sizeof(gspw_desc_t), &dma_fifo_create_desc, NULL)) )
	{
		return rc;
	}
	if( (rc = dma_chain_init( &pcfg->dch_rx_data, pt->qnt_rx_data, sizeof(dma_data_fi_t),
											 pt->sz_rx_data_buf, &dma_fifo_create_data, &dma_fifo_delete_data)) )
	{
		return rc;
	}
	
	if( (rc = dma_chain_init( &pcfg->dch_tx_desc, pt->qnt_tx_desc, sizeof(dma_desc_fi_t),
											 sizeof(gspw_desc_t), &dma_fifo_create_desc, NULL)) )
	{
		return rc;
	}
	if( (rc = dma_chain_init( &pcfg->dch_tx_data, pt->qnt_tx_data, sizeof(dma_data_fi_t),
											 pt->sz_tx_data_buf, &dma_fifo_create_data, &dma_fifo_delete_data)) )
	{
		return rc;
	}

	pt->is_created_chains = 1;

	return 0;
}

void dma_test_reset( dma_test_t *pt)
{
	pt->pdes_rx_last = NULL;
	pt->pdat_rx_last = NULL;
	pt->pdes_tx_last = NULL;
	pt->pdat_tx_last = NULL;

	pt->cnt_rx_desc = 0;
	pt->cnt_rx_data = 0;
	pt->cnt_tx_desc = 0;
	pt->cnt_tx_data = 0;

	pt->tm_beg = pt->tm_end = 0LL;
	pt->cnt_send_data = pt->cnt_rec_data = 0LL;

	pt->is_ena_print_ih = 0;

	pt->cnt_more1_rx_desc = pt->cnt_more1_rx_data =
		pt->cnt_more1_tx_desc = pt->cnt_more1_tx_data = 0;
}

void gspw_dma_internal_test_start( gspw_cfg_ctrl_t *pcfg)
{
	dma_test_t *pt = &pcfg->test;
	int i;

	if( pt->is_work)
	{
		pr_info( "Could not start internal test, it is running still\n");
		return;
	}

	if( !pt->is_created_chains)
	{
		dma_test_free( pcfg);

		dma_test_create( pcfg);
	}

	dma_test_reset( pt);

	{
		dma_desc_fi_t *pd, *pd_beg;

		pr_info("RX_DESC:\n");
		pd_beg = (dma_desc_fi_t *)pcfg->dch_rx_desc.ff.p_begin;
		for( i=0,pd=pd_beg; (pd); pd=pd->p_next)
		{
			set_dflt_dma_desc( pd, i++);
			if( pd->p_next == pd_beg)
				break;
		}

		pr_info("TX_DESC:\n");
		pd_beg = (dma_desc_fi_t *)pcfg->dch_tx_desc.ff.p_begin;
		for( i=0,pd=pd_beg; (pd); pd=pd->p_next)
		{
			set_dflt_dma_desc( pd, i++);

			pd->desc.size = pcfg->dch_tx_data.sz_data_buf;
			pd->desc.type_ep = EOP;
			pd->desc.unused = 0;
			pd->desc.unused1 = 0;
			pd->desc.port_mask = TX_PACK_PORT_MSK_SWIC0;
			pd->desc.valid = 1;

			if( pd->p_next == pd_beg)
				break;
		}
	}

	{
		dma_data_fi_t *pd, *pd_beg;

		pr_info("RX_DATA:\n");
		pd_beg = (dma_data_fi_t *)pcfg->dch_rx_data.ff.p_begin;
		for( i=0,pd=pd_beg; (pd); pd=pd->p_next)
		{
			set_dflt_dma_data( pd, pcfg->dch_rx_data.sz_data_buf, i++);
			//pd->reg_init.csr |= (SWIC_DMA_CSR_WN_MSK << SWIC_DMA_CSR_WN_OFS);
			if( pd->p_next == pd_beg)
				break;
		}

		pr_info("TX_DATA:\n");
		pd_beg = (dma_data_fi_t *)pcfg->dch_tx_data.ff.p_begin;
		for( i=0,pd=pd_beg; (pd); pd=pd->p_next)
		{
			set_dflt_dma_data( pd, pcfg->dch_tx_data.sz_data_buf, i++);
			//pd->reg_init.csr |= (SWIC_DMA_CSR_WN_MSK << SWIC_DMA_CSR_WN_OFS);
			if( pd->p_next == pd_beg)
				break;
		}
	}

	pt->tm_beg = get_time_of_day();

	MC_CP_GSPW_RX_DES = mips_virt_to_phys((unsigned )&((dma_desc_fi_t *)pcfg->dch_rx_desc.ff.p_begin)->reg_init) | 1;
	MC_CP_GSPW_RX_DAT = mips_virt_to_phys((unsigned )&((dma_data_fi_t *)pcfg->dch_rx_data.ff.p_begin)->reg_init) | 1;
	MC_CP_GSPW_TX_DES = mips_virt_to_phys((unsigned )&((dma_desc_fi_t *)pcfg->dch_tx_desc.ff.p_begin)->reg_init) | 1;
	MC_CP_GSPW_TX_DAT = mips_virt_to_phys((unsigned )&((dma_data_fi_t *)pcfg->dch_tx_data.ff.p_begin)->reg_init) | 1;

	print_dma_all_regs();
	mdelay(10);

	print_dma_all_regs();
	mdelay(10);

	print_dma_all_regs();
	//mdelay(10);

	pcfg->test.is_work = 1;

	pr_info( "gspw_dma_internal_test_start: END\n");
}

void gspw_dma_internal_test_stop( gspw_cfg_ctrl_t *pcfg)
{
	dma_test_t *pt = &pcfg->test;
	dma_desc_fi_t  *pdes, *pdes_beg;
	dma_data_fi_t  *pdat, *pdat_beg;
	u32  ir, ir_prev;
	int i, cnt_equal = 0;
	const int MAX_EQUAL = 10;
	u64 tm_wait_beg, tm_wait_end;
	int rc = 0;

	if( !pcfg->test.is_work)
	{
		pr_info( "Internal test did not run\n");
		return;
	}
	
	pr_info( "gspw_dma_internal_test_stop:\n");
		
	// останавливаем RX:
	//MC_RUN_GSPW_TX_DES &= ~SWIC_DMA_CSR_RUN;
	//MC_RUN_GSPW_TX_DAT &= ~SWIC_DMA_CSR_RUN;

	pcfg->test.is_ena_print_ih = 1;

	// через ригистр CP убираем ссылки на следующие элементы самоинициализации:

	pdes_beg = (dma_desc_fi_t *)pcfg->dch_tx_desc.ff.p_begin;
	for( pdes=pdes_beg; (pdes); pdes=pdes->p_next)
	{
		pdes->reg_init.csr &= ~SWIC_DMA_CSR_CHEN;

		if( pdes->p_next == pdes_beg)
			break;
	}

	/*pdat_beg = (dma_data_fi_t *)pcfg->dch_tx_data.ff.p_begin;
	for( pdat=pdat_beg; (pdat); pdat=pdat->p_next)
	{
		pdat->reg_init.csr &= ~SWIC_DMA_CSR_CHEN;

		if( pdat->p_next == pdat_beg)
			break;
	}*/

	//pcfg->test.is_ena_print_ih = 1;

	// запускаем  RX:
	//MC_RUN_GSPW_TX_DES |= SWIC_DMA_CSR_RUN;
	//MC_RUN_GSPW_TX_DAT |= SWIC_DMA_CSR_RUN;

	
	pr_info( "gspw_dma_internal_test_stop: fin waiting\n");

	//pr_info("TX_DATA: ir=%Xh run=%Xh cp=%Xh\n", MC_IR_GSPW_TX_DAT, MC_RUN_GSPW_TX_DAT, MC_CP_GSPW_TX_DAT);

	// ждем завершения прихода последних RX данных:
	//pr_info("TX_DATA: get_time_of_day()\n");
	tm_wait_beg = get_time_of_day();
	//pr_info("TX_DATA: tm_wait_beg=%llu\n", tm_wait_beg);
	ir_prev = MC_IR_GSPW_RX_DAT;
	pr_info( "gspw_dma_internal_test_stop: ir_pre=%Xh\n", ir_prev);
	for( i=0; (i < 100); ++i)
	{
		//mdelay(10);

		ir = MC_IR_GSPW_RX_DAT;
		pr_info("RX_DATA: ir=%Xh run=%Xh cp=%Xh\n", ir, MC_RUN_GSPW_RX_DAT, MC_CP_GSPW_RX_DAT);
		if( ir == ir_prev)
		{
			if( cnt_equal==0)
				pt->tm_end = get_time_of_day();
			++cnt_equal;
		}
		else
		{
			cnt_equal = 0;
			ir_prev = ir;
		}

		if( cnt_equal >= MAX_EQUAL)
		{
			rc = 1;
			break;
		}
	}

	pcfg->test.is_ena_print_ih = 0;

	pr_info( "gspw_dma_internal_test_stop: end for() rc=%u\n", rc);

	if(rc==0)
	{
		print_dma_all_regs();
		mdelay(10);

		print_dma_all_regs();
		mdelay(10);

		print_dma_all_regs();
		mdelay(10);
	}

	tm_wait_end = get_time_of_day();
	pr_info( "internal_test_stop: waiting_time=%llu\n", tm_wait_end - tm_wait_beg);

	// печатаем финальные результаты:
	pr_info( "Internal test is finished: %s\n", rc ? "normal":"bad");
	gspw_dma_internal_test_print( &pcfg->test);
/*
	// возвращаем значения по умолчанию для всех цепочек DMA:
	dma_rx_desc_chain_set_dflt( &pcfg->dch_rx_desc);
	dma_rx_data_chain_set_dflt( &pcfg->dch_rx_data);
	dma_tx_desc_chain_set_dflt( &pcfg->dch_tx_desc);
	dma_tx_data_chain_set_dflt( &pcfg->dch_tx_data);
*/
	// конец работы:
	pcfg->test.is_work = 0;
}

void gspw_dma_internal_test_print( dma_test_t *pt)
{
	u64  tm = pt->tm_end - pt->tm_beg;
	u64  speed = (tm ? div64_u64( (pt->cnt_rec_data * 8000000), tm) : 0);
	u32  tm_s, tm_ms;
	//u64 speed = tm * 53434534534534LL;
	//u64 speed = div64_u64( tm, 53434534534534LL);

	tm_s = (u32 )div_u64_rem( tm, 1000000, &tm_ms);

	//pr_info( "tm=%llu  end=%llu beg=%llu\n", tm, pt->tm_end, pt->tm_beg);

	pr_info("Internal test state:\n");
	pr_info(" RX desc/data counters = %u / %u\n", pt->cnt_rx_desc, pt->cnt_rx_data);
	pr_info(" TX desc/data counters = %u / %u\n", pt->cnt_tx_desc, pt->cnt_tx_data);
	pr_info(" sent data counter = %llu B\n", pt->cnt_send_data);
	pr_info(" rec data counter  = %llu B\n", pt->cnt_rec_data);
	pr_info(" test time         = %u s %u mks\n", tm_s, tm_ms);
	pr_info(" RX speed          = %llu bps\n", speed);
	speed = (tm ? div64_u64( (pt->cnt_send_data * 8000000), tm) : 0);
	pr_info(" TX speed          = %llu bps\n", speed);

	pr_info(" Cnt more1 RX des/dat = %u/%u\n", pt->cnt_more1_rx_desc, pt->cnt_more1_rx_data);
	pr_info(" Cnt more1 TX des/dat = %u/%u\n", pt->cnt_more1_tx_desc, pt->cnt_more1_tx_data);
}

irqreturn_t dma_test_registrate_rx_desc( gspw_cfg_ctrl_t *pcfg)
{
	dma_test_t *pt = &pcfg->test;
	u32  ir = MC_IR_GSPW_RX_DES;
	dma_chain_t  *pch = &pcfg->dch_rx_desc;
	dma_desc_fi_t  *pd = pt->pdes_rx_last;
	int cnt_pack = 0;

	pd = pd ? pd->p_next : (dma_desc_fi_t *)pcfg->dch_rx_desc.ff.p_begin;
	for( ; (1); pd=pd->p_next)
	{
		if( !pd->desc.valid)
			break;
	
		if( (ir >= pd->reg_init.ir) && (ir < pd->reg_init.ir + pch->sz_data_buf) )
			break;
		else
		{
			++pcfg->test.cnt_rx_desc;
			pcfg->test.cnt_rec_data += pd->desc.size;
			pt->pdes_rx_last = pd;
			if( ++cnt_pack > 1)
				++pt->cnt_more1_rx_desc;
		}
	}

	if( pcfg->test.is_ena_print_ih)
		print_dma_rx_desc_regs( "ih");

	MC_CSR_GSPW_RX_DES;

	return IRQ_HANDLED;
}

irqreturn_t dma_test_registrate_rx_data( gspw_cfg_ctrl_t *pcfg)
{
	dma_test_t *pt = &pcfg->test;
	u32  ir = MC_IR_GSPW_RX_DAT;
	dma_chain_t  *pch = &pcfg->dch_rx_data;
	dma_data_fi_t  *pd = pt->pdat_rx_last;
	int cnt_pack = 0;

	pd = pd ? pd->p_next : (dma_data_fi_t *)pcfg->dch_rx_data.ff.p_begin;
	for( ; (1); pd=pd->p_next)
	{
		if( (ir >= pd->reg_init.ir) && (ir < pd->reg_init.ir + pch->sz_data_buf) )
			break;
		else
		{
			++pcfg->test.cnt_rx_data;
			pt->pdat_rx_last = pd;
			if( ++cnt_pack > 1)
				++pt->cnt_more1_rx_data;
		}
	}

	if( pcfg->test.is_ena_print_ih)
		print_dma_rx_data_regs( "ih");

	MC_CSR_GSPW_RX_DAT;

	return IRQ_HANDLED;
}

irqreturn_t dma_test_registrate_tx_desc( gspw_cfg_ctrl_t *pcfg)
{
	dma_test_t *pt = &pcfg->test;
	u32  ir = MC_IR_GSPW_TX_DES;
	dma_chain_t  *pch = &pcfg->dch_tx_desc;
	dma_desc_fi_t  *pd = pt->pdes_tx_last;
	int cnt_pack = 0;

	pd = pd ? pd->p_next : (dma_desc_fi_t *)pcfg->dch_tx_desc.ff.p_begin;
	for( ; (1); pd=pd->p_next)
	{
		if( !pd->desc.valid)
			break;
	
		if( (ir >= pd->reg_init.ir) && (ir < pd->reg_init.ir + pch->sz_data_buf) )
			break;
		else
		{
			++pcfg->test.cnt_tx_desc;
			pt->pdes_tx_last = pd;
			if( ++cnt_pack > 1)
				++pt->cnt_more1_tx_desc;
		}
	}

	if( pcfg->test.is_ena_print_ih)
		print_dma_tx_desc_regs( "ih");

	MC_CSR_GSPW_TX_DES;

	return IRQ_HANDLED;
}

irqreturn_t dma_test_registrate_tx_data( gspw_cfg_ctrl_t *pcfg)
{
	dma_test_t *pt = &pcfg->test;
	u32  ir = MC_IR_GSPW_TX_DAT;
	dma_chain_t  *pch = &pcfg->dch_tx_data;
	dma_data_fi_t  *pd = pt->pdat_tx_last;
	int cnt_pack = 0;

	pd = pd ? pd->p_next : (dma_data_fi_t *)pcfg->dch_tx_data.ff.p_begin;
	for( ; (1); pd=pd->p_next)
	{
		if( (ir >= pd->reg_init.ir) && (ir < pd->reg_init.ir + pch->sz_data_buf) )
			break;
		else
		{
			++pcfg->test.cnt_tx_data;
			pcfg->test.cnt_send_data += pch->sz_data_buf;
			pt->pdat_tx_last = pd;
			if( ++cnt_pack > 1)
				++pt->cnt_more1_tx_data;
		}
	}

	if( pcfg->test.is_ena_print_ih)
		print_dma_tx_data_regs( "ih");
	
	MC_CSR_GSPW_TX_DAT;

	return IRQ_HANDLED;
}

#endif //USE_DMA_TEST