/* Emulate power6 mf[tf]gpr and fri[zpmn] instructions.
   Copyright (C) 2006 Red Hat, Inc.
   Contributed by Jakub Jelinek <jakub@redhat.com>, 2006.

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.

   It is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.

   You should have received a copy of the GNU Lesser General Public
   License along with the GNU C Library; if not, write to the Free
   Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
   02111-1307 USA.  */

#include <signal.h>
#include <stdio.h>

extern double frip (double), friz (double), frin (double), frim (double);
asm (".globl frip, friz, frin, frim\n.hidden frip, friz, frin, frim\n\t"
#ifdef __powerpc64__
	".section \".toc\",\"aw\"\n"
"8:"	".tc FD_43300000_0[TC],0x4330000000000000\n"
"9:"	".tc FD_3fe00000_0[TC],0x3fe0000000000000\n\t"
	".previous\n\t"
#else
	".rodata\n\t"
	".align 2\n"
"8:"	".long 0x59800000\n"
"9:"	".long 0x3f000000\n\t"
	".previous\n\t"
#endif
	"# frip == ceil\n"
"frip:"	"mffs    11\n\t"
#ifdef __powerpc64__
	"lfd     13,8b@toc(2)\n\t"
#else
	"mflr    11\n\t"
	"bcl     20,31,1f\n"
"1:"	"mflr    9\n\t"
	"addis   9,9,8b-1b@ha\n\t"
	"lfs     13,8b-1b@l(9)\n\t"
	"mtlr    11\n\t"
#endif
	"fabs    0,1\n\t"
	"fsub    12,13,13\n\t"
	"fcmpu   7,0,13\n\t"
	"fcmpu   6,1,12\n\t"
	"bnllr-  7\n\t"
	"mtfsfi  7,2\n\t"
	"ble-    6,2f\n\t"
	"fadd    1,1,13\n\t"
	"fsub    1,1,13\n\t"
	"fabs    1,1\n\t"
	"mtfsf   0x01,11\n\t"
	"blr\n"
"2:"	"bge-    6,3f\n\t"
	"fsub    1,1,13\n\t"
	"fadd    1,1,13\n\t"
	"fnabs   1,1\n"
"3:"	"mtfsf   0x01,11\n\t"
	"blr\n\t"
	"# friz == trunc\n"
"friz:"	"mffs    11\n\t"
#ifdef __powerpc64__
	"lfd     13,8b@toc(2)\n\t"
#else
	"mflr    11\n\t"
	"bcl     20,31,1f\n"
"1:"	"mflr    9\n\t"
	"addis   9,9,8b-1b@ha\n\t"
	"lfs     13,8b-1b@l(9)\n\t"
	"mtlr    11\n\t"
#endif
	"fabs    0,1\n\t"
	"fsub    12,13,13\n\t"
	"fcmpu   7,0,13\n\t"
	"fcmpu   6,1,12\n\t"
	"bnllr-  7\n\t"
	"mtfsfi  7,1\n\t"
	"ble-    6,2f\n\t"
	"fadd    1,1,13\n\t"
	"fsub    1,1,13\n\t"
	"fabs    1,1\n\t"
	"mtfsf   0x01,11\n\t"
	"blr\n"
"2:"	"bge-    6,3f\n\t"
	"fsub    1,1,13\n\t"
	"fadd    1,1,13\n\t"
	"fnabs   1,1\n"
"3:"	"mtfsf   0x01,11\n\t"
	"blr\n\t"
	"# frin == round\n"
"frin:"	"mffs    11\n\t"
#ifdef __powerpc64__
	"lfd     13,8b@toc(2)\n\t"
#else
	"mflr    11\n\t"
	"bcl     20,31,1f\n"
"1:"	"mflr    9\n\t"
	"addis   9,9,8b-1b@ha\n\t"
	"addi    9,9,8b-1b@l\n\t"
	"mtlr    11\n\t"
	"lfs     13,0(9)\n\t"
#endif
	"fabs    0,1\n\t"
	"fsub    12,13,13\n\t"
	"fcmpu   7,0,13\n\t"
	"fcmpu   6,1,12\n\t"
	"bnllr-  7\n\t"
	"mtfsfi  7,1\n\t"
#ifdef __powerpc64__
	"lfd     10,9b@toc(2)\n\t"
#else
	"lfs     10,9b-8b(9)\n\t"
#endif
	"ble-    6,2f\n\t"
	"fadd    1,1,10\n\t"
	"fadd    1,1,13\n\t"
	"fsub    1,1,13\n\t"
	"fabs    1,1\n\t"
	"mtfsf   0x01,11\n\t"
	"blr\n"
"2:"	"fsub    9,1,10\n\t"
	"bge-    6,3f\n\t"
	"fsub    1,9,13\n\t"
	"fadd    1,1,13\n\t"
	"fnabs   1,1\n"
"3:"	"mtfsf   0x01,11\n\t"
	"blr\n\t"
	"# frim == floor\n"
"frim:"	"mffs    11\n\t"
#ifdef __powerpc64__
	"lfd     13,8b@toc(2)\n\t"
#else
	"mflr    11\n\t"
	"bcl     20,31,1f\n"
"1:"	"mflr    9\n\t"
	"addis   9,9,8b-1b@ha\n\t"
	"lfs     13,8b-1b@l(9)\n\t"
	"mtlr    11\n\t"
#endif
	"fabs    0,1\n\t"
	"fsub    12,13,13\n\t"
	"fcmpu   7,0,13\n\t"
	"fcmpu   6,1,12\n\t"
	"bnllr-  7\n\t"
	"mtfsfi  7,3\n\t"
	"ble-    6,2f\n\t"
	"fadd    1,1,13\n\t"
	"fsub    1,1,13\n\t"
	"fabs    1,1\n\t"
	"mtfsf   0x01,11\n\t"
	"blr\n"
"2:"	"bge-    6,3f\n\t"
	"fsub    1,1,13\n\t"
	"fadd    1,1,13\n\t"
	"fnabs   1,1\n"
"3:"	"mtfsf   0x01,11\n\t"
	"blr\n");

#ifdef __powerpc64__
#define m1 0x5555555555555555L
#define m2 0x3333333333333333L
#define m3 0x0f0f0f0f0f0f0f0fL
#else
#define m1 0x55555555
#define m2 0x33333333
#define m3 0x0f0f0f0f
#endif

static inline unsigned long
popcntb (unsigned long n)
{
  n -= (n >> 1) & m1;
  n = (n & m2) + ((n >> 2) & m2);
  n = (n + (n >> 4)) & m3;
  return n;
}

static void
catch_sigill (int signal, struct sigcontext *ctx)
{
  unsigned int insn = *(unsigned int *) (ctx->regs->nip);
#ifdef __powerpc64__
  if ((insn & 0xfc1f07ff) == 0x7c0005be) /* mftgpr */
    {
      unsigned long *regs = (unsigned long *) ctx->regs;
      unsigned fpr = (insn >> 11) & 0x1f;
      unsigned gpr = (insn >> 21) & 0x1f;
      regs[gpr] = regs[fpr + 0x30];
      ctx->regs->nip += 4;
      return;
    }
  if ((insn & 0xfc1f07ff) == 0x7c0004be) /*mffgpr */
    {
      unsigned long *regs = (unsigned long *) ctx->regs;
      unsigned fpr = (insn >> 21) & 0x1f;
      unsigned gpr = (insn >> 11) & 0x1f;
      regs[fpr + 0x30] = regs[gpr];
      ctx->regs->nip += 4;
      return;
    }
#endif
  if ((insn & 0xfc1f073f) == 0xfc000310) /* fri[pznm] */
    {
#ifdef __powerpc64__
      double *regs = (double *) (((char *) ctx->regs) + 0x30 * 8);
      unsigned int *fpscr = (unsigned int *) (((char *) ctx->regs) + 0x50 * 8 + 4);
#else
      double *regs = (double *) (((char *) ctx->regs) + 0x30 * 4);
      unsigned int *fpscr = (unsigned int *) (((char *) ctx->regs) + 0x30 * 4 + 0x20 * 8 + 4);
#endif
      unsigned dest = (insn >> 21) & 0x1f;
      unsigned src = (insn >> 11) & 0x1f;
      switch (insn & 0xc0)
	{
	case 0:
	  regs[dest] = frin (regs[src]);
	  break;
	case 0x40:
	  regs[dest] = friz (regs[src]);
	  break;
	case 0x80:
	  regs[dest] = frip (regs[src]);
	  break;
	case 0xc0:
	  regs[dest] = frim (regs[src]);
	  break;
	}
      /* Update raised exceptions.  */
      union { unsigned int i[2]; double d; } u;
      asm volatile ("mffs %0" : "=f" (u.d));
      u.i[1] &= 0xfffe0000; /* Is this correct?  */
      *fpscr |= u.i[1];
      ctx->regs->nip += 4;
      return;
    }
  if ((insn & 0xfc00ffff) == 0x7c0000f4) /* popcntb */
    {
      unsigned long *regs = (unsigned long *) ctx->regs;
      unsigned dest = (insn >> 16) & 0x1f;
      unsigned src = (insn >> 21) & 0x1f;
      unsigned long res = 0;
      int i;

      regs[dest] = popcntb (regs[src]);
      ctx->regs->nip += 4;
      return;
    }

  struct sigaction sa;
  sa.sa_handler = SIG_DFL;
  sigemptyset (&sa.sa_mask);
  sa.sa_flags = 0;
  sigaction (signal, &sa, NULL);
  raise (signal);
}

static void
__attribute__ ((constructor))
install_handler (void)
{
  struct sigaction sa;
  sa.sa_handler = (void *) catch_sigill;
  sigemptyset (&sa.sa_mask);
  sa.sa_flags = SA_RESTART;
  sigaction (SIGILL, &sa, NULL);
}
