/*
    Copyright (C) 2009-2012, The AROS Development Team. All rights reserved.

    Desc: Copy memory.
*/

#define DEBUG 0
#include <aros/debug.h>

#include <aros/libcall.h>
#include <proto/exec.h>

#define DEBUG_XMM 0
#if DEBUG_XMM
static UQUAD copycount = 0;
#define PRECOPY                     \
    UQUAD srcreg[8];                \
    UQUAD *psrc = (UQUAD *)src;     \
    for (int i = 0; i < 8; i++)     \
    {                               \
        __asm__ __volatile__ ("movq (%0), %%rax\n movq %%rax, (%1)" : : "r"(psrc), "r"(srcreg + i) : "rax");    \
        psrc++;                     \
    }

#define POSTCOPY                    \
    copycount++;                    \
    UQUAD dstreg[8];                \
    UQUAD *pdst = (UQUAD *)dst;     \
    for (int i = 0; i < 8; i++)     \
    {                               \
        __asm__ __volatile__ ("movq (%0), %%rax\n movq %%rax, (%1)" : : "r"(pdst), "r"(dstreg + i) : "rax");    \
        pdst++;                     \
    }                               \
    for (int i = 0; i < 8; i++)     \
    {                               \
        if (srcreg[i] != dstreg[i]) \
            bug("diff reg-reg [%d] (%ld) %p vs %p, %lx vs %lx\n",               \
                i, copycount, src + i * 8, dst + i * 8, srcreg[i], dstreg[i]);  \
    }
#endif

/* See rom/exec/copymem.c for documentation */

#define SSE_REG_SIZE 16
#define SSE_REG_MASK 0xF

#define MEMFENCE __asm__ __volatile__ ("sfence":::"memory")
#define MMENABLE __asm__ __volatile__ ("emms":::"memory")

#define __byte_memcpy(src,dst,size)                             \
{                                                               \
    __asm__ __volatile__(                                       \
        "    rep; movsb"                                        \
        : "=&D" (dst), "=&S" (src), "=&c" (dummy)               \
        : "0" (dst), "1" (src), "2" (size)                      \
        : "memory");                                            \
}

#define __long_memcpy(src,dst,size)                             \
{                                                               \
    __asm__ __volatile__(                                       \
        "    rep; movsl" "\n\t"                                 \
        "    testb $2,%b6" "\n\t"                               \
        "    je 1f" "\n\t"                                      \
        "    movsw" "\n"                                        \
        "1:  testb $1,%b6" "\n\t"                               \
        "    je 2f" "\n\t"                                      \
        "    movsb" "\n"                                        \
        "2:"                                                    \
        : "=&D" (dst), "=&S" (src), "=&c" (dummy)               \
        : "0" (dst), "1" (src), "2" (size >> 2), "q" (size)     \
        : "memory");                                            \
}

static __inline__ void __small_memcpy(const void * src, void * dst, ULONG size)
{
    register unsigned long int dummy;
    if( size < 4 ) {
D(bug("[Exec] __byte_memcpy(%p, %p, %ld)\n", src, dst, size));

        __byte_memcpy(src, dst, size);
    }
    else
    {
D(bug("[Exec] __long_memcpy(%p, %p, %ld)\n", src, dst, size));

        __long_memcpy(src, dst, size);
    }
}


AROS_LH3I(void, CopyMem,
    AROS_LHA(CONST_APTR, source, A0),
    AROS_LHA(APTR, dest, A1),
    AROS_LHA(IPTR, size, D0),
    struct ExecBase *, SysBase, 104, Exec)
{
    AROS_LIBFUNC_INIT

    if (!size) return;

    ULONG lcnt = (size >> 6);                   /* size / 64       */

    const void *src = source;
    void *dst = dest;

D(bug("[Exec] CopyMem(%p, %p, %ld)\n", src, dst, size));

    __asm__ __volatile__ (
                "    prefetchnta (%0)\n"
                "    prefetchnta 32(%0)\n"
                "    prefetchnta 64(%0)\n"
                "    prefetchnta 96(%0)\n"
                "    prefetchnta 128(%0)\n"
                "    prefetchnta 160(%0)\n"
                "    prefetchnta 192(%0)\n"
                "    prefetchnta 256(%0)\n"
                "    prefetchnta 288(%0)\n"
                :
                : "r" (src) );

    if ((lcnt > 0) && (size >= (SSE_REG_SIZE * 4)))
    {
D(bug("[Exec] CopyMem: Using SSE Copy.\n"));
        ULONG alignsize = ((SSE_REG_SIZE - ((IPTR)src & SSE_REG_MASK)));

        if ((((IPTR)src & SSE_REG_MASK) != 0) && (((IPTR)(dst + alignsize) & SSE_REG_MASK) == 0))
        {
D(bug("[Exec] CopyMem: Aligning src to %d byte boundary (%d bytes) .. \n", SSE_REG_SIZE, alignsize));

            __small_memcpy(src, dst, alignsize);

            size -= alignsize;
            lcnt = (size >> 6);                 /* size / 64       */
            src += alignsize;
            dst += alignsize;
        }
        if (lcnt > 0) {
            if ((((IPTR)src & SSE_REG_MASK) == 0) && (((IPTR)dst & SSE_REG_MASK) == 0))
            {
                /*
                    # SRC and DST aligned on 16-byte boundary.
                    We can use movaps instead of movups since we meet
                    the alignment constraints (a general-protection fault
                    would be triggered otherwise)
                */
                size -= (lcnt << 6);
                for( ; lcnt > 0; lcnt--)
                {
D(bug("[Exec] CopyMem: SSE Aligned-Copy %p to %p.\n", src, dst));

#if DEBUG_XMM
PRECOPY
#endif
                    __asm__ __volatile__ (
                        "    prefetchnta 320(%0)\n"
                        "    prefetchnta 352(%0)\n"
                        "    movaps (%0), %%xmm0\n"
                        "    movaps 16(%0), %%xmm1\n"
                        "    movaps 32(%0), %%xmm2\n"
                        "    movaps 48(%0), %%xmm3\n"
                        "    movntps %%xmm0, (%1)\n"
                        "    movntps %%xmm1, 16(%1)\n"
                        "    movntps %%xmm2, 32(%1)\n"
                        "    movntps %%xmm3, 48(%1)\n"
                        :
                        : "r" (src), "r" (dst)
                        : "memory");

#if DEBUG_XMM
POSTCOPY
#endif

                    src += (SSE_REG_SIZE * 4);
                    dst += (SSE_REG_SIZE * 4);
                }
            }
            else if (((IPTR)dst & SSE_REG_MASK) == 0)
            {
                /*
                    # SRC is unaligned and DST aligned on 16-byte boundary.
                */
                size -= (lcnt << 6);
                for( ; lcnt > 0; lcnt--)
                {
D(bug("[Exec] CopyMem: SSE Unaligned-Copy %p to %p.\n", src, dst));

                    __asm__ __volatile__ (
                        "    prefetchnta 320(%0)\n"
                        "    prefetchnta 352(%0)\n"
                        "    movups (%0), %%xmm0\n"
                        "    movups 16(%0), %%xmm1\n"
                        "    movups 32(%0), %%xmm2\n"
                        "    movups 48(%0), %%xmm3\n"
                        "    movntps %%xmm0, (%1)\n"
                        "    movntps %%xmm1, 16(%1)\n"
                        "    movntps %%xmm2, 32(%1)\n"
                        "    movntps %%xmm3, 48(%1)\n"
                        :
                        : "r" (src), "r" (dst)
                        : "memory");

                    src += (SSE_REG_SIZE * 4);
                    dst += (SSE_REG_SIZE * 4);
                }
            }
        }
    }

    if (size > 0)
    {
D(bug("[Exec] CopyMem: Copy remaining %ld bytes.\n", size));
        __small_memcpy(src, dst, size);
    }

    /*
        FENCE Memory to re-order again since movntq is weakly-ordered ?
    */
    MEMFENCE;
    /*
        enable FPU use ?
    */
    MMENABLE;

    D(bug("[Exec] CopyMem: Finished.\n"));

    AROS_LIBFUNC_EXIT
} /* CopyMem */

