#include "sdma.h"

#ifdef __DMA_TIME_COUNTING__
        #include "ptimers.h"
#endif
#if __CACHE_ENABLED__
#include "platform.h"
#endif

#define	DMA_PACK_LNG	0x40000


const unsigned int minTransferSizeInBytes = 64;
const unsigned int maxLoopIterations = 256;
const unsigned int maxHeight = 256;

float _dma_working_time_counter = 0;
float _dma_start_time = 0;


PSDMA_CB	sdmaPtr	=	((PSDMA_CB) SDMA_BASE);


unsigned run_channel(unsigned int channel, unsigned int inst0, unsigned int pc)
{
    int state = 0;

    if (sdmaPtr->dbgstatus != 0)
    {
        return 3;
    }

#ifdef __DMA_TIME_COUNTING__
    _dma_start_time = risc_ms_get();
#endif

#if __CACHE_ENABLED__
    flush_and_invalidate_cache();
#endif

    sdmaPtr->dbginst0 = inst0;
    sdmaPtr->dbginst1 = pc;
    sdmaPtr->dbgcmd   = 0x0;

    do
	{
        state = sdmaPtr->chCtrl[channel].csr & 0xf;
        if(state == 0)
		{
            break;
		}
        else
		{
            if (state == 0xf || state == 0xe)
			{
                return state;
                break;
            }
        }
    }
	while(1);

#ifdef __DMA_TIME_COUNTING__
    float ms = risc_ms_get() - _dma_start_time;
    _dma_working_time_counter += ms;
#endif

    return sdmaPtr->dbgstatus;
}


enum ERL_ERROR sdma_copy(int dma_id, unsigned int *dst, unsigned int *src, int size)
{
    // Simple programm - transfer in one loop
    if ((dma_id < 0) || (dma_id > 7))
	{
		return ERL_PROGRAM_ERROR;      // TODO: enum with error types
	}
    if (size <= 0)
	{
		return ERL_PROGRAM_ERROR;		// TODO: enum with error types
	}

    int en64 = 0;
    if (ISALIGNED64(src) && ISALIGNED64(dst)) en64 = 1;

	// setup codes of sdma-microprogram
    unsigned int prog[] = {0x42F501BC, 0x00BC00BD, 0x00000000, 0x02BC1818, 0x00000000, 0x08040020, 0x18000238};

    int maxBlockSize = 0;
    int transfersNumber = 0;
    int reminder = 0;
    maxBlockSize = (minTransferSizeInBytes * maxLoopIterations) << en64;
    transfersNumber = size/maxBlockSize;

    int i = 0;
    unsigned int dmago0 = 0xa0;
    dmago0  = (dma_id << 24) | (dmago0 << 16) | (dma_id << 8) | 0x0;        //(0x00a00000);

    unsigned int* src1 = src;
    unsigned int* dst1 = dst;
    unsigned int dif = (maxBlockSize >> 2);

    // main loop
    if (en64) prog[0] = 0xC2F701BC;
    prog[5] |= (unsigned int)((maxLoopIterations-1) << 8);
    for(i = 0; i < transfersNumber; i++)
    {
        prog[2] = (unsigned int) (src1);
        prog[4] = (unsigned int) (dst1);
        if(run_channel(dma_id, dmago0, (unsigned int) prog) == 3) return ERL_SYSTEM_ERROR;

        src1 += dif;
        dst1 += dif;
    }

    reminder = size%maxBlockSize;
    if(reminder != 0)
    {
        int loop_iter = reminder/(minTransferSizeInBytes<<en64);
        int rem = reminder%(minTransferSizeInBytes<<en64);
        prog[5] = 0x08040020;
        if (loop_iter != 0)
        {
                prog[2] = (unsigned int) (src1);
                prog[4] = (unsigned int) (dst1);
                prog[5] |= (unsigned int)((loop_iter - 1) << 8);
                if (run_channel(dma_id, dmago0, (unsigned int) prog) == 3) return ERL_SYSTEM_ERROR;
        }

        if(rem != 0)
        {
            rem = rem >> 2;
            rem--;
            prog[0] = 0x420501BC;
            prog[1] = 0x00BC0081;
            prog[5] = 0x08040020;
            prog[2] = (unsigned int) (src1+loop_iter*(16<<en64));
            prog[4] = (unsigned int) (dst1+loop_iter*(16<<en64));
            prog[5] |= (unsigned int)(rem << 8);
            if(run_channel(dma_id, dmago0, (unsigned int) prog) == 3) return ERL_SYSTEM_ERROR;
        }
    }
    return ERL_NO_ERROR;
}

enum ERL_ERROR sdma_copy2d(int dma_id, unsigned int *dst, unsigned int stride_dst, unsigned int *src, unsigned int stride_src, int width, int height)
{
    if ((dma_id < 0) || (dma_id > 7))
    {
		return ERL_PROGRAM_ERROR;      // TODO: enum with error types
    }
    if (width <= 0)
    {
	return ERL_PROGRAM_ERROR;
    }
    if (height <= 0)
    {
        return ERL_PROGRAM_ERROR;
    }
    if ((stride_dst < width) || (stride_src < width))
    {
        return ERL_PROGRAM_ERROR;
    }
    /*if (stride_dst == 0) stride_dst = width;
    if (stride_src == 0) stride_src = width;
    */

    int transfersNumber = 0;
    transfersNumber = height/maxHeight;

	// setup codes of sdma-microprogram
    unsigned int prog_slow[] = {0x420501BC, 0x00BC0081, 0x00000000, 0x02BC1818, 0x00000000, 0x00220020, 0x023C0804, 0x56000054, 0x0C380000, 0x18181800};
    unsigned int prog_fast[] = {0x42F501BC, 0x00BC00BD, 0x00000000, 0x02BC1818, 0x00000000, 0x00220020, 0x023C0804, 0x56000054, 0x0C380000, 0x18181800};

    unsigned int dmago0 = 0xa0;
    dmago0  = (dma_id << 24) | (dmago0 << 16) | (dma_id << 8) | 0x0;        //(0x00a00000);
    int loop_iter = 0;
    int reminder = 0;

    int i = 0;
    for(i = 0; i < transfersNumber; i++)
    {
      loop_iter = width/minTransferSizeInBytes;
      reminder = width%minTransferSizeInBytes;
      if (loop_iter != 0)
      {
        prog_fast[2] = (unsigned int) (src);
        prog_fast[4] = (unsigned int) (dst);
        prog_fast[5] |= (unsigned int)((loop_iter - 1) <<24);
        prog_fast[5] |= (unsigned int)((maxHeight - 1) << 8);
        prog_fast[7] |= (unsigned int)((stride_src - width + reminder) << 8);
        prog_fast[8] |= (unsigned int)(stride_dst - width + reminder);
        if (run_channel(dma_id, dmago0, (unsigned int) prog_fast) == 3) return ERL_SYSTEM_ERROR;
      }
      if (reminder != 0)
      {
        prog_slow[2] = (unsigned int) (src+loop_iter*16);
        prog_slow[4] = (unsigned int) (dst+loop_iter*16);
        prog_slow[5] |= (unsigned int)(((reminder>>2) - 1) <<24);
        prog_slow[5] |= (unsigned int)((maxHeight - 1) << 8);
        prog_slow[7] |= (unsigned int)((stride_src - reminder) << 8);
        prog_slow[8] |= (unsigned int)(stride_dst - reminder);
        if (run_channel(dma_id, dmago0, (unsigned int) prog_slow) == 3) return ERL_SYSTEM_ERROR;
      }
      src += (stride_src>>2)*maxHeight;
      dst += (stride_dst>>2)*maxHeight;
    }
    int heightRem = height%maxHeight;
    if (heightRem != 0)
    {
      loop_iter = width/minTransferSizeInBytes;
      reminder = width%minTransferSizeInBytes;
      if (loop_iter != 0)
      {
        prog_fast[2] = (unsigned int) (src);
        prog_fast[4] = (unsigned int) (dst);
        prog_fast[5] = 0x00220020;
        prog_fast[7] = 0x56000054;
        prog_fast[8] = 0x0C380000;
        prog_fast[5] |= (unsigned int)((loop_iter - 1) <<24);
        prog_fast[5] |= (unsigned int)((heightRem - 1) << 8);
        prog_fast[7] |= (unsigned int)((stride_src - width + reminder) << 8);
        prog_fast[8] |= (unsigned int)(stride_dst - width + reminder);
        if (run_channel(dma_id, dmago0, (unsigned int) prog_fast) == 3) return ERL_SYSTEM_ERROR;
      }
      if (reminder != 0)
      {
        prog_slow[2] = (unsigned int) (src+loop_iter*16);
        prog_slow[4] = (unsigned int) (dst+loop_iter*16);
        prog_slow[5] = 0x00220020;
        prog_slow[7] = 0x56000054;
        prog_slow[8] = 0x0C380000;
        prog_slow[5] |= (unsigned int)(((reminder>>2) - 1) <<24);
        prog_slow[5] |= (unsigned int)((heightRem - 1) << 8);
        prog_slow[7] |= (unsigned int)((stride_src - reminder) << 8);
        prog_slow[8] |= (unsigned int)(stride_dst - reminder);
        if (run_channel(dma_id, dmago0, (unsigned int) prog_slow) == 3) return ERL_SYSTEM_ERROR;
      }
    }
    return ERL_NO_ERROR;
}

enum ERL_ERROR sdma_copy_sampling(unsigned int dma_id,unsigned int* dst, int dst_or,unsigned int* src, int src_or,unsigned int size, int en64)
{
    // Simple programm - transfer in one loop
    if ((dma_id < 0) || (dma_id > 7))
	{
		return ERL_PROGRAM_ERROR;      // TODO: enum with error types
	}
    if (size <= 0)
	{
		return ERL_PROGRAM_ERROR;		// TODO: enum with error types
	}
    if (en64!=0 && en64!=1) return ERL_PROGRAM_ERROR;

	// setup codes of sdma-microprogram
    unsigned int prog_inc_inc[] = {0x020401BC, 0x00BC0081, 0x00000000, 0x02BC1818, 0x00000000, 0x08040020, 0x56000054, 0x08380000, 0x18181800};
    unsigned int prog_dec_dec[] = {0x020401BC, 0x00BC0081, 0x00000000, 0x02BC1818, 0x00000000, 0x08040020, 0x5E00005C, 0x08380000, 0x18181800};
    unsigned int prog_inc_dec[] = {0x020401BC, 0x00BC0081, 0x00000000, 0x02BC1818, 0x00000000, 0x08040020, 0x5E000054, 0x08380000, 0x18181800};
    unsigned int prog_dec_inc[] = {0x020401BC, 0x00BC0081, 0x00000000, 0x02BC1818, 0x00000000, 0x08040020, 0x5600005C, 0x08380000, 0x18181800};
    unsigned int *prog;

    if (src_or >= 0)
    {
        if (dst_or >= 0) prog = prog_inc_inc;
        else
        {
            prog = prog_inc_dec;
        }
    }
    else
    {
        if (dst_or >= 0) prog = prog_dec_inc;
        else
        {
            prog = prog_dec_dec;
        }
    }

    if (en64 == 1)
    {
        prog[0] = 0x820601BC;
        if (!ISALIGNED64(dst) || !ISALIGNED64(src)) return ERL_PROGRAM_ERROR;
    }

    int s_or = src_or * (4<<en64);
    int d_or = dst_or * (4<<en64);
    int s_or_masked = s_or&0xFFFF;
    int d_or_masked = d_or&0xFFFF;

    int maxBlockSize = 0;
    int transfersNumber = 0;
    int reminder = 0;
    maxBlockSize = (4<<en64) * maxLoopIterations;				// 256 words
    transfersNumber = size/maxBlockSize;

    int i = 0;
    unsigned int dmago0 = 0xa0;
    dmago0  = (dma_id << 24) | (dmago0 << 16) | (dma_id << 8) | 0x0;        //(0x00a00000);

    unsigned int* src1 = src;
    unsigned int* dst1 = dst;
    unsigned int dif = (maxBlockSize >> (2+en64));

    // main loop
    for(i = 0; i < transfersNumber; i++)
    {
        prog[2] = (unsigned int) (src1);
        prog[4] = (unsigned int) (dst1);
        prog[5] |= (unsigned int)((maxLoopIterations-1) << 8);
        prog[6] |= (unsigned int)(s_or_masked << 8);
        prog[7] |= (unsigned int)(d_or_masked);
        if(run_channel(dma_id, dmago0, (unsigned int) prog) == 3) return ERL_SYSTEM_ERROR;

        src1 += dif*s_or>>2;
        dst1 += dif*d_or>>2;
    }

    reminder = size%maxBlockSize;
    if(reminder != 0)
    {
        int loop_iter = reminder>>(2+en64);
        prog[2] = (unsigned int) (src1);
        prog[4] = (unsigned int) (dst1);
        prog[5] |= (unsigned int)((loop_iter - 1) << 8);
        prog[6] |= (unsigned int)(s_or_masked << 8);
        prog[7] |= (unsigned int)(d_or_masked);
        if (run_channel(dma_id, dmago0, (unsigned int) prog) == 3) return ERL_SYSTEM_ERROR;
    }
    return ERL_NO_ERROR;
}