#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;
}