
/*******************************************************************************
*
* File sse_fcts.c
*
* Copyright (C) 2005 Martin Luescher
*
* This software is distributed under the terms of the GNU General Public
* License (GPL)
*
* Programs for transcendental functions using SSE instructions
*
* The externally accessible functions are 
*
*   void sse_ln1px(sse_float *x,sse_float *lx)
*     Computes lx=ln(1+x) to a relative accuracy better than 2.1e-7,
*     assuming -1.0+2^(-24)<=x<=2^126
*
*   void sse_sincos(sse_float *x,sse_float *sx,sse_float *cx)
*     Computes sx=sin(pi*x) and cx=cos(pi*x), assuming -1.0<=x<=1.0.
*     For |x|<=0.5 the relative accuracy of sx is better than 2.0e-7.
*     Otherwise the formula sx=sign(x)*sin(pi*(1.0-|x|)) is used, and
*     the cosine is obtained through cx=sin(pi*(0.5-|x|)) at all x
*
* Notes:
*
* The functions operate on the four components of the sse_float *x
* simultaneously. There is no argument check, i.e. it is assumed that
* the components of *x are within the specified bounds
*
* The parameters must point to memory locations that are aligned to a
* 16 Byte boundary. Otherwise the processor will raise an exception and
* the program terminates abnormally
*
* These programs can only be used when one of the SSE compiler flags
* is switched on. The ISO C code does not use these functions (the
* math library functions are used in this case)
*
*******************************************************************************/

#define SSE_FCTS_C

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "su3.h"

#if (defined SSE)

#include "sse.h"

static int init=0;
static sse_int mask,p,not_sgn;
static sse_float cln[9],csn[5];
static sse_float xfh,xf1,xf2,xf4,xf127;
static sse_float xfa,xfb,ln2,pf;


static void sse_init(void)
{
   int i;

   cln[0].c1= 0.961203933f;
   cln[1].c1=-0.167358673f;
   cln[2].c1= 0.019275252f;
   cln[3].c1=-0.002490170f;
   cln[4].c1= 0.000342671f;
   cln[5].c1=-0.000049081f;
   cln[6].c1= 0.000007227f;
   cln[7].c1=-0.000001086f;
   cln[8].c1= 0.000000166f;
   
   for (i=0;i<9;i++)
   {
      cln[i].c2=cln[i].c1;
      cln[i].c3=cln[i].c1;
      cln[i].c4=cln[i].c1;      
   }
   
   csn[0].c1= 1.276278962f;
   csn[1].c1=-0.285261569f;
   csn[2].c1= 0.009118016f;
   csn[3].c1=-0.000136588f;
   csn[4].c1= 0.000001185f;

   for (i=0;i<5;i++)
   {
      csn[i].c2=csn[i].c1;
      csn[i].c3=csn[i].c1;
      csn[i].c4=csn[i].c1;      
   }
   
   mask.c1=0x7f800000;
   mask.c2=0x7f800000;
   mask.c3=0x7f800000;
   mask.c4=0x7f800000;

   not_sgn.c1=0x7fffffff;
   not_sgn.c2=0x7fffffff;
   not_sgn.c3=0x7fffffff;
   not_sgn.c4=0x7fffffff;   
   
   ln2.c1=(float)(log(2.0));
   ln2.c2=ln2.c1;
   ln2.c3=ln2.c1;
   ln2.c4=ln2.c1;   

   xfh.c1=0.5f;
   xfh.c2=0.5f;
   xfh.c3=0.5f;
   xfh.c4=0.5f;

   xf1.c1=1.0f;
   xf1.c2=1.0f;
   xf1.c3=1.0f;
   xf1.c4=1.0f;

   xf2.c1=2.0f;
   xf2.c2=2.0f;
   xf2.c3=2.0f;
   xf2.c4=2.0f;   

   xf4.c1=4.0f;
   xf4.c2=4.0f;
   xf4.c3=4.0f;
   xf4.c4=4.0f;    

   xf127.c1=127.0f;
   xf127.c2=127.0f;
   xf127.c3=127.0f;
   xf127.c4=127.0f;

   xfa.c1=(float)(4.0/0.75);
   xfa.c2=xfa.c1;
   xfa.c3=xfa.c2;
   xfa.c4=xfa.c3; 

   xfb.c1=(float)(-0.5/0.75);
   xfb.c2=xfb.c1;
   xfb.c3=xfb.c1;
   xfb.c4=xfb.c1;

   init=1;
}


void sse_ln1px(sse_float *x,sse_float *lx)
{
   if (init==0)
      sse_init();

   __asm__ __volatile__ ("movaps %1, %%xmm0 \n\t"
                         "movaps %2, %%xmm1 \n\t"
                         "movaps %3, %%xmm2 \n\t"
                         "addps %%xmm0, %%xmm1 \n\t"
                         "andps %%xmm1, %%xmm2 \n\t"
                         "mulps %4, %%xmm2 \n\t"
                         "addps %%xmm2, %%xmm1 \n\t"
                         "andps %3, %%xmm1 \n\t"
                         "movaps %%xmm1, %0 \n\t"
                         "xorps %3, %%xmm1 \n\t"
                         "mulps %4, %%xmm1 \n\t"
                         "movaps %2, %%xmm3 \n\t"
                         "subps %%xmm1, %%xmm3 \n\t"
                         "mulps %%xmm0, %%xmm1 \n\t"
                         "subps %%xmm3, %%xmm1 \n\t"
                         "movaps %5, %%xmm2 \n\t"
                         "mulps %%xmm1, %%xmm2 \n\t"
                         "addps %6, %%xmm2"
                         :
                         "=m" (p)
                         :
                         "m" (*x),
                         "m" (xf1),
                         "m" (mask),
                         "m" (xfh),
                         "m" (xfa),
                         "m" (xfb)
                         :
                         "xmm0", "xmm1", "xmm2", "xmm3");

   __asm__ __volatile__ ("movaps %0, %%xmm3 \n\t"
                         "movaps %0, %%xmm4 \n\t"
                         "mulps %%xmm2, %%xmm3 \n\t"
                         "subps %2, %%xmm4 \n\t"
                         "addps %1, %%xmm3 \n\t"
                         "movaps %%xmm3, %%xmm5 \n\t"
                         "mulps %%xmm2, %%xmm3 \n\t"
                         "subps %3, %%xmm5 \n\t"
                         "subps %%xmm4, %%xmm3 \n\t"
                         "movaps %%xmm3, %%xmm6 \n\t"
                         "mulps %%xmm2, %%xmm3 \n\t"
                         "subps %4, %%xmm6 \n\t"
                         "subps %%xmm5, %%xmm3 \n\t"
                         :
                         :
                         "m" (cln[8]),
                         "m" (cln[7]),
                         "m" (cln[6]),
                         "m" (cln[5]),
                         "m" (cln[4])
                         :
                         "xmm3", "xmm4", "xmm5", "xmm6");
                         
   __asm__ __volatile__ ("movaps %%xmm3, %%xmm7 \n\t"
                         "mulps %%xmm2, %%xmm3 \n\t"
                         "subps %0, %%xmm7 \n\t"
                         "subps %%xmm6, %%xmm3 \n\t"
                         "movaps %%xmm3, %%xmm5 \n\t"
                         "mulps %%xmm2, %%xmm3 \n\t"
                         "subps %1, %%xmm5 \n\t"
                         "subps %%xmm7, %%xmm3 \n\t"
                         "movaps %%xmm3, %%xmm6 \n\t"
                         "mulps %%xmm2, %%xmm3 \n\t"
                         "subps %2, %%xmm6 \n\t"
                         "subps %%xmm5, %%xmm3 \n\t"
                         "movaps %%xmm3, %%xmm7 \n\t"
                         "mulps %%xmm2, %%xmm3 \n\t"
                         "subps %3, %%xmm7 \n\t"
                         "subps %%xmm6, %%xmm3"
                         :
                         :
                         "m" (cln[3]),
                         "m" (cln[2]),
                         "m" (cln[1]),
                         "m" (cln[0])
                         :
                         "xmm3", "xmm5", "xmm6", "xmm7");

   p.c1=p.c1>>23;
   p.c2=p.c2>>23;
   p.c3=p.c3>>23;
   p.c4=p.c4>>23;
   pf.c1=(float)(p.c1);
   pf.c2=(float)(p.c2);
   pf.c3=(float)(p.c3);
   pf.c4=(float)(p.c4);

   __asm__ __volatile__ ("mulps %1, %%xmm3 \n\t"
                         "mulps %%xmm2, %%xmm3 \n\t"
                         "subps %%xmm7, %%xmm3 \n\t"
                         "mulps %%xmm3, %%xmm1 \n\t"
                         "movaps %2, %%xmm4 \n\t"
                         "subps %3, %%xmm4 \n\t"
                         "mulps %4, %%xmm4 \n\t"
                         "addps %%xmm4, %%xmm1\n\t"
                         "movaps %%xmm1, %0"
                         :
                         "=m" (*lx)
                         :
                         "m" (xfh),
                         "m" (pf),
                         "m" (xf127),
                         "m" (ln2)
                         :
                         "xmm1", "xmm3", "xmm4");
}


void sse_sincos(sse_float *x,sse_float *sx,sse_float *cx)
{
   if (init==0)
      sse_init();

   __asm__ __volatile__ ("movaps %0, %%xmm0 \n\t"
                         "movaps %1, %%xmm1 \n\t"
                         "movaps %2, %%xmm2 \n\t"
                         "movaps %2, %%xmm3 \n\t"
                         "movaps %3, %%xmm4 \n\t"
                         "andps %%xmm0, %%xmm1 \n\t"
                         "movaps %%xmm1, %%xmm5 \n\t"
                         "mulps %%xmm4, %%xmm1 \n\t"                         
                         "subps %%xmm1, %%xmm2 \n\t"
                         "mulps %%xmm4, %%xmm2 \n\t"
                         "cmpps $0x1, %%xmm1, %%xmm3 \n\t"
                         "andps %%xmm3, %%xmm2 \n\t"
                         "addps %%xmm2, %%xmm1 \n\t"
                         "xorps %%xmm0, %%xmm5 \n\t"
                         "xorps %%xmm5, %%xmm1 \n\t"
                         "movaps %4, %%xmm3 \n\t"
                         "mulps %%xmm1, %%xmm3 \n\t"
                         "mulps %%xmm1, %%xmm3 \n\t"
                         "subps %%xmm4, %%xmm3"
                         :
                         :
                         "m" (*x),
                         "m" (not_sgn),
                         "m" (xf1),
                         "m" (xf2),
                         "m" (xf4)
                         :
                         "xmm0", "xmm1", "xmm2", "xmm3", "xmm4", "xmm5");

   __asm__ __volatile__ ("movaps %0, %%xmm4 \n\t"
                         "movaps %0, %%xmm5 \n\t"
                         "mulps %%xmm3, %%xmm4 \n\t"
                         "subps %2, %%xmm5 \n\t"
                         "addps %1, %%xmm4 \n\t"
                         "movaps %%xmm4, %%xmm6 \n\t"
                         "mulps %%xmm3, %%xmm4 \n\t"
                         "subps %3, %%xmm6 \n\t"
                         "subps %%xmm5, %%xmm4 \n\t"
                         "movaps %%xmm4, %%xmm7 \n\t"
                         "mulps %%xmm3, %%xmm4 \n\t"
                         "subps %4, %%xmm7 \n\t"
                         "subps %%xmm6, %%xmm4"
                         :
                         :
                         "m" (csn[4]),
                         "m" (csn[3]),
                         "m" (csn[2]),
                         "m" (csn[1]),
                         "m" (csn[0])
                         :
                         "xmm4", "xmm5", "xmm6", "xmm7");

   __asm__ __volatile__ ("mulps %0, %%xmm4 \n\t"
                         "mulps %%xmm3, %%xmm4 \n\t"
                         "subps %%xmm7, %%xmm4 \n\t"
                         "mulps %%xmm4, %%xmm1"
                         :
                         :
                         "m" (xfh)
                         :
                         "xmm1", "xmm4");

   __asm__ __volatile__ ("movaps %0, %%xmm4 \n\t"
                         "movaps %1, %%xmm2 \n\t"
                         "movaps %2, %%xmm5 \n\t"
                         "andps %%xmm0, %%xmm4 \n\t"
                         "mulps %%xmm5, %%xmm4 \n\t"                         
                         "subps %%xmm4, %%xmm2 \n\t"
                         "movaps %3, %%xmm3 \n\t"                         
                         "mulps %%xmm2, %%xmm3 \n\t"
                         "mulps %%xmm2, %%xmm3 \n\t"
                         "subps %%xmm5, %%xmm3"
                         :
                         :
                         "m" (not_sgn),
                         "m" (xf1),
                         "m" (xf2),
                         "m" (xf4)
                         :
                         "xmm2", "xmm3", "xmm4", "xmm5");

   __asm__ __volatile__ ("movaps %0, %%xmm4 \n\t"
                         "movaps %0, %%xmm5 \n\t"
                         "mulps %%xmm3, %%xmm4 \n\t"
                         "subps %2, %%xmm5 \n\t"
                         "addps %1, %%xmm4 \n\t"
                         "movaps %%xmm4, %%xmm6 \n\t"
                         "mulps %%xmm3, %%xmm4 \n\t"
                         "subps %3, %%xmm6 \n\t"
                         "subps %%xmm5, %%xmm4 \n\t"
                         "movaps %%xmm4, %%xmm7 \n\t"
                         "mulps %%xmm3, %%xmm4 \n\t"
                         "subps %4, %%xmm7 \n\t"
                         "subps %%xmm6, %%xmm4"
                         :
                         :
                         "m" (csn[4]),
                         "m" (csn[3]),
                         "m" (csn[2]),
                         "m" (csn[1]),
                         "m" (csn[0])
                         :
                         "xmm4", "xmm5", "xmm6", "xmm7");
   
   __asm__ __volatile__ ("movaps %%xmm1, %0\n\t"
                         "mulps %2, %%xmm4 \n\t"
                         "mulps %%xmm3, %%xmm4 \n\t"
                         "subps %%xmm7, %%xmm4 \n\t"
                         "mulps %%xmm4, %%xmm2\n\t"
                         "movaps %%xmm2, %1"
                         :
                         "=m" (*sx),
                         "=m" (*cx)
                         :
                         "m" (xfh)
                         :
                         "xmm2", "xmm4");
}

#endif
