/**
 *  GraphTFT plugin for the Video Disk Recorder 
 * 
 *  memcpy.c - A plugin for the Video Disk Recorder
 *
 *  (c) 2004 Lars Tegeler, Sascha Volkenandt  
 *
 * This code is distributed under the terms and conditions of the
 * GNU GENERAL PUBLIC LICENSE. See the file COPYING for details.
 *
 * $Id: memcpy.c,v 1.3 2007/12/03 19:58:20 root Exp $
 *
 **/

//The most part of this Code is from the (great and my favorit) analogtv plugin:

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "mmx.h"
#include "memcpy.h"
#include "common.h"

#ifdef ARCH_X86

/* for small memory blocks (<256 bytes) this version is faster */

#define small_memcpy(to,from,n)                 \
   {                                            \
      register unsigned long int dummy;         \
      __asm__ __volatile__(                     \
         "rep; movsb"                           \
         :"=&D"(to), "=&S"(from), "=&c"(dummy)  \
         :"0" (to), "1" (from),"2" (n)          \
         : "memory");                           \
   }

/* linux kernel __memcpy (from: /include/asm/string.h) */

static inline void * __memcpy(void * to, const void * from, size_t n)
{
   int d0, d1, d2;

   if( n < 4 ) 
   {
      small_memcpy(to,from,n);
   }
   else
      __asm__ __volatile__(
         "rep ; movsl\n\t"
         "testb $2,%b4\n\t"
         "je 1f\n\t"
         "movsw\n"
         "1:\ttestb $1,%b4\n\t"
         "je 2f\n\t"
         "movsb\n"
         "2:"
         : "=&c" (d0), "=&D" (d1), "=&S" (d2)
         :"0" (n/4), "q" (n),"1" ((long) to),"2" ((long) from)
         : "memory");
   
   return to;
}

#define SSE_MMREG_SIZE 16
#define MMX_MMREG_SIZE 8

#define MMX1_MIN_LEN 0x800  /* 2K blocks */
#define MIN_LEN 0x40  /* 64-byte blocks */

/* SSE note: i tried to move 128 bytes a time instead of 64 but it
didn't make any measureable difference. i'm using 64 for the sake of
simplicity. [MF] */

static void * sse_memcpy(void * to, const void * from, size_t len)
{
   void *retval;
   size_t i;
   retval = to;
    
   /* PREFETCH has effect even for MOVSB instruction ;) */
   __asm__ __volatile__ (
      "   prefetchnta (%0)\n"
      "   prefetchnta 64(%0)\n"
      "   prefetchnta 128(%0)\n"
      "   prefetchnta 192(%0)\n"
      "   prefetchnta 256(%0)\n"
      : : "r" (from) );
  
   if (len >= MIN_LEN)
   {
      register unsigned long int delta;

      /* Align destinition to MMREG_SIZE -boundary */
      delta = ((unsigned long int)to)&(SSE_MMREG_SIZE-1);

      if(delta)
      {
         delta=SSE_MMREG_SIZE-delta;
         len -= delta;
         small_memcpy(to, from, delta);
      }

      i = len >> 6; /* len/64 */
      len&=63;

      if (((unsigned long)from) & 15)
         /* if SRC is misaligned */

         for(; i>0; i--)
         {
            __asm__ __volatile__ (
               "prefetchnta 320(%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" (from), "r" (to) : "memory");
            ((const unsigned char *)from)+=64;
            ((unsigned char *)to)+=64;
         }
      else 
         /*
           Only if SRC is aligned on 16-byte boundary.
           It allows to use movaps instead of movups, which required data
           to be aligned or a general-protection exception (#GP) is generated.
         */
         for (; i>0; i--)
         {
            __asm__ __volatile__ (
               "prefetchnta 320(%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" (from), "r" (to) : "memory");
            ((const unsigned char *)from)+=64;
            ((unsigned char *)to)+=64;
         }
      /* since movntq is weakly-ordered, a "sfence"
       * is needed to become ordered again. */
      __asm__ __volatile__ ("sfence":::"memory");
      /* enables to use FPU */
      __asm__ __volatile__ ("emms":::"memory");
   }

   /*
    *   Now do the tail of the block
    */

   if (len) 
      __memcpy(to, from, len);

   return retval;
}

static void * mmx_memcpy(void * to, const void * from, size_t len)
{
   void *retval;
   size_t i;
   retval = to;

   if (len >= MMX1_MIN_LEN)
   {
      register unsigned long int delta;
      /* Align destinition to MMREG_SIZE -boundary */
      delta = ((unsigned long int)to)&(MMX_MMREG_SIZE-1);
      if(delta)
      {
         delta=MMX_MMREG_SIZE-delta;
         len -= delta;
         small_memcpy(to, from, delta);
      }
      i = len >> 6; /* len/64 */
      len&=63;
      for(; i>0; i--)
      {
         __asm__ __volatile__ (
            "movq (%0), %%mm0\n"
            "movq 8(%0), %%mm1\n"
            "movq 16(%0), %%mm2\n"
            "movq 24(%0), %%mm3\n"
            "movq 32(%0), %%mm4\n"
            "movq 40(%0), %%mm5\n"
            "movq 48(%0), %%mm6\n"
            "movq 56(%0), %%mm7\n"
            "movq %%mm0, (%1)\n"
            "movq %%mm1, 8(%1)\n"
            "movq %%mm2, 16(%1)\n"
            "movq %%mm3, 24(%1)\n"
            "movq %%mm4, 32(%1)\n"
            "movq %%mm5, 40(%1)\n"
            "movq %%mm6, 48(%1)\n"
            "movq %%mm7, 56(%1)\n"
            :: "r" (from), "r" (to) : "memory");
         ((const unsigned char *)from)+=64;
         ((unsigned char *)to)+=64;
      }
      __asm__ __volatile__ ("emms":::"memory");
   }
   /*
    *   Now do the tail of the block
    */
   if(len) __memcpy(to, from, len);
   return retval;
}

void * mmx2_memcpy(void * to, const void * from, size_t len)
{
   void *retval;
   size_t i;
   retval = to;

   /* PREFETCH has effect even for MOVSB instruction ;) */

   __asm__ __volatile__ (
      "   prefetchnta (%0)\n"
      "   prefetchnta 64(%0)\n"
      "   prefetchnta 128(%0)\n"
      "   prefetchnta 192(%0)\n"
      "   prefetchnta 256(%0)\n"
      : : "r" (from) );

   if(len >= MIN_LEN)
   {
      register unsigned long int delta;

      /* Align destinition to MMREG_SIZE -boundary */
      delta = ((unsigned long int)to)&(MMX_MMREG_SIZE-1);

      if(delta)
      {
         delta=MMX_MMREG_SIZE-delta;
         len -= delta;
         small_memcpy(to, from, delta);
      }

      i = len >> 6; /* len/64 */
      len&=63;

      for(; i>0; i--)
      {
         __asm__ __volatile__ (
            "prefetchnta 320(%0)\n"
            "movq (%0), %%mm0\n"
            "movq 8(%0), %%mm1\n"
            "movq 16(%0), %%mm2\n"
            "movq 24(%0), %%mm3\n"
            "movq 32(%0), %%mm4\n"
            "movq 40(%0), %%mm5\n"
            "movq 48(%0), %%mm6\n"
            "movq 56(%0), %%mm7\n"
            "movntq %%mm0, (%1)\n"
            "movntq %%mm1, 8(%1)\n"
            "movntq %%mm2, 16(%1)\n"
            "movntq %%mm3, 24(%1)\n"
            "movntq %%mm4, 32(%1)\n"
            "movntq %%mm5, 40(%1)\n"
            "movntq %%mm6, 48(%1)\n"
            "movntq %%mm7, 56(%1)\n"
            :: "r" (from), "r" (to) : "memory");
         ((const unsigned char *)from)+=64;
         ((unsigned char *)to)+=64;
      }
      /* since movntq is weakly-ordered, a "sfence"
       * is needed to become ordered again. */
      __asm__ __volatile__ ("sfence":::"memory");
      __asm__ __volatile__ ("emms":::"memory");
   }
   /*
    *   Now do the tail of the block
    */
   if(len) __memcpy(to, from, len);
   return retval;
}

static void *linux_kernel_memcpy(void *to, const void *from, size_t len) 
{
  return __memcpy(to,from,len);
}

#endif /* ARCH_X86 */

static struct
{
      int tag;
      char *name;
      void *(* function)(void *to, const void *from, size_t len);
      unsigned long long time;
      uint32_t cpu_require;
} 
   memcpy_methods[] = 
   {
      { MEMCPY_GLIBC,  "glibc memcpy()", memcpy, 0, 0 },
#ifdef ARCH_X86
      { MEMCPY_KERNEL, "linux kernel memcpy()", linux_kernel_memcpy, 0, 0 },
      { MEMCPY_MMX,    "MMX optimized memcpy()", mmx_memcpy, 0, ACCEL_X86_MMX },
      { MEMCPY_MMXEXT, "MMXEXT optimized memcpy()", mmx2_memcpy, 0, ACCEL_X86_MMXEXT },
# ifndef __FreeBSD__
      { MEMCPY_SSE,    "SSE optimized memcpy()", sse_memcpy, 0, ACCEL_X86_MMXEXT|ACCEL_X86_SSE },
# endif
#endif /* ARCH_X86 */
      { 0, NULL, NULL, 0, 0 }
   };

#ifdef ARCH_X86
static unsigned long long int rdtsc()
{
   unsigned long long int x;
   __asm__ volatile (".byte 0x0f, 0x31" : "=A" (x));     
   return x;
}
#else
static unsigned long long int rdtsc()
{
  /* FIXME: implement an equivalent for using optimized memcpy on other
            architectures */
  return 0;
}
#endif

//-------------------------------------------------------------------------
#define BUFSIZE 1024*1024

int probe_fast_memcpy(int method, bool force)
{
   unsigned long long t;
   void *buf1, *buf2;
   int i, j, best = -1;
   static int config_flags = -1;

   config_flags = detect_accel();

   if( !force && (config_flags & memcpy_methods[method].cpu_require) != memcpy_methods[method].cpu_require )
   {
      method = 0;
      tell(1, "Using %s isn't possible on this cpu\n", memcpy_methods[method - 1].name);
   }

   if (method)
      tell(1, "Using %s\n", memcpy_methods[method - 1].name);
   
   switch (method)
    {
      case MEMCPY_GLIBC:
      fast_memcpy = memcpy;
      return(method);

      case MEMCPY_KERNEL:
      fast_memcpy = linux_kernel_memcpy;
      return(method);

      case MEMCPY_MMX:
      fast_memcpy = mmx_memcpy;
      return(method);

      case MEMCPY_MMXEXT:
      fast_memcpy = mmx2_memcpy;
      return(method);

      case MEMCPY_SSE:
      fast_memcpy = sse_memcpy;
      return(method);

      case MEMCPY_PROBE:
      default:
      fast_memcpy = memcpy;
      break;
    }
  
   // prepare the buffer for testing

   if ((buf1 = malloc(BUFSIZE)) == NULL)
      return MEMCPY_GLIBC;

   
   if ((buf2 = malloc(BUFSIZE)) == NULL) 
   {
      free(buf1);
      return MEMCPY_GLIBC;
   }

   tell(1, "Benchmarking memcpy() methods (smaller is better):\n");

   /* make sure buffers are present on physical memory */

   memcpy(buf1,buf2,BUFSIZE);

   for(i=0; memcpy_methods[i].name; i++)
   {
      if ((config_flags & memcpy_methods[i].cpu_require) != 
          memcpy_methods[i].cpu_require)
      continue;
    
      t = rdtsc();

      for(j=0;j<50;j++)
      {
         memcpy_methods[i].function(buf2,buf1,BUFSIZE);
         memcpy_methods[i].function(buf1,buf2,BUFSIZE);
      }     

      t = rdtsc() - t;
      memcpy_methods[i].time = t;
    
      tell(1, "%-25s : %12lld\n",memcpy_methods[i].name, t);
    
      if (best == -1 || t < memcpy_methods[best].time)
         best = i;
   }
  
   tell(1, "using %s\n", memcpy_methods[best].name);
   fast_memcpy = memcpy_methods[best].function;
  
   free(buf1);
   free(buf2);
   
   return best+1;
}
