iwmmxt.c

这个是LINUX下的GDB调度工具的源码

{
  int wRn;

  if ((read_cp15_reg (15, 0, 1) & 3) != 3)
    return ARMul_CANT;

#ifdef DEBUG
  fprintf (stderr, "wacc\n");
#endif  

  wRn = BITS (16, 19);

  switch (BITS (22, 23))
    {
    case Bqual:
      wR [BITS (12, 15)] =
	  wRBITS (wRn, 56, 63) + wRBITS (wRn, 48, 55)
	+ wRBITS (wRn, 40, 47) + wRBITS (wRn, 32, 39)
	+ wRBITS (wRn, 24, 31) + wRBITS (wRn, 16, 23)
	+ wRBITS (wRn,  8, 15) + wRBITS (wRn,  0,  7);
      break;

    case Hqual:
      wR [BITS (12, 15)] =
	  wRBITS (wRn, 48, 63) + wRBITS (wRn, 32, 47)
	+ wRBITS (wRn, 16, 31) + wRBITS (wRn,  0, 15);
      break;

    case Wqual:
      wR [BITS (12, 15)] = wRBITS (wRn, 32, 63) + wRBITS (wRn, 0, 31);
      break;

    default:
      ARMul_UndefInstr (state, instr);
      break;
    }

  wC [wCon] |= WCON_MUP;
  return ARMul_DONE;
}

static int
WADD (ARMul_State * state, ARMword instr)
{
  ARMdword r = 0;
  ARMdword x;
  ARMdword s;
  ARMword  psr = 0;
  int      i;
  int      carry;
  int      overflow;
  int      satrv[8];

  if ((read_cp15_reg (15, 0, 1) & 3) != 3)
    return ARMul_CANT;

#ifdef DEBUG
  fprintf (stderr, "wadd\n");
#endif  

  /* Add two numbers using the specified function,
     leaving setting the carry bit as required.  */
#define ADDx(x, y, m, f) \
   (*f) (wRBITS (BITS (16, 19), (x), (y)) & (m), \
         wRBITS (BITS ( 0,  3), (x), (y)) & (m), \
        & carry, & overflow)

  switch (BITS (22, 23))
    {
    case Bqual:
      for (i = 0; i < 8; i++)
        {
	  switch (BITS (20, 21))
	    {
	    case NoSaturation:
	      s = ADDx ((i * 8), (i * 8) + 7, 0xff, AddS8);
	      satrv [BITIDX8 (i)] = 0;
	      r |= (s & 0xff) << (i * 8);
	      SIMD8_SET (psr, NBIT8 (s), SIMD_NBIT, i);
	      SIMD8_SET (psr, ZBIT8 (s), SIMD_ZBIT, i);
	      SIMD8_SET (psr, carry,     SIMD_CBIT, i);
	      SIMD8_SET (psr, overflow,  SIMD_VBIT, i);
	      break;

	    case UnsignedSaturation:
	      s = ADDx ((i * 8), (i * 8) + 7, 0xff, AddU8);
	      x = IwmmxtSaturateU8 (s, satrv + BITIDX8 (i));
	      r |= (x & 0xff) << (i * 8);
	      SIMD8_SET (psr, NBIT8 (x), SIMD_NBIT, i);
	      SIMD8_SET (psr, ZBIT8 (x), SIMD_ZBIT, i);
	      if (! satrv [BITIDX8 (i)])
		{
		  SIMD8_SET (psr, carry,    SIMD_CBIT, i);
		  SIMD8_SET (psr, overflow, SIMD_VBIT, i);
		}
	      break;

	    case SignedSaturation:
	      s = ADDx ((i * 8), (i * 8) + 7, 0xff, AddS8);
	      x = IwmmxtSaturateS8 (s, satrv + BITIDX8 (i));
	      r |= (x & 0xff) << (i * 8);
	      SIMD8_SET (psr, NBIT8 (x), SIMD_NBIT, i);
	      SIMD8_SET (psr, ZBIT8 (x), SIMD_ZBIT, i);
	      if (! satrv [BITIDX8 (i)])
		{
		  SIMD8_SET (psr, carry,    SIMD_CBIT, i);
		  SIMD8_SET (psr, overflow, SIMD_VBIT, i);
		}
	      break;

	    default:
	      ARMul_UndefInstr (state, instr);
	      return ARMul_DONE;
	    }
	}
      break;

    case Hqual:
      satrv[0] = satrv[2] = satrv[4] = satrv[6] = 0;

      for (i = 0; i < 4; i++)
	{
	  switch (BITS (20, 21))
	    {
	    case NoSaturation:
	      s = ADDx ((i * 16), (i * 16) + 15, 0xffff, AddS16);
	      satrv [BITIDX16 (i)] = 0;
	      r |= (s & 0xffff) << (i * 16);
	      SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
	      SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
	      SIMD16_SET (psr, carry,      SIMD_CBIT, i);
	      SIMD16_SET (psr, overflow,   SIMD_VBIT, i);
	      break;

	    case UnsignedSaturation:
	      s = ADDx ((i * 16), (i * 16) + 15, 0xffff, AddU16);
	      x = IwmmxtSaturateU16 (s, satrv + BITIDX16 (i));
	      r |= (x & 0xffff) << (i * 16);
	      SIMD16_SET (psr, NBIT16 (x), SIMD_NBIT, i);
	      SIMD16_SET (psr, ZBIT16 (x), SIMD_ZBIT, i);
	      if (! satrv [BITIDX16 (i)])
		{
		  SIMD16_SET (psr, carry,    SIMD_CBIT, i);
		  SIMD16_SET (psr, overflow, SIMD_VBIT, i);
		}
	      break;

	    case SignedSaturation:
	      s = ADDx ((i * 16), (i * 16) + 15, 0xffff, AddS16);
	      x = IwmmxtSaturateS16 (s, satrv + BITIDX16 (i));
	      r |= (x & 0xffff) << (i * 16);
	      SIMD16_SET (psr, NBIT16 (x), SIMD_NBIT, i);
	      SIMD16_SET (psr, ZBIT16 (x), SIMD_ZBIT, i);
	      if (! satrv [BITIDX16 (i)])
		{
		  SIMD16_SET (psr, carry,    SIMD_CBIT, i);
		  SIMD16_SET (psr, overflow, SIMD_VBIT, i);
		}
	      break;

	    default:
	      ARMul_UndefInstr (state, instr);
	      return ARMul_DONE;
	    }
	}
      break;

    case Wqual:
      satrv[0] = satrv[1] = satrv[2] = satrv[4] = satrv[5] = satrv[6] = 0;

      for (i = 0; i < 2; i++)
	{
	  switch (BITS (20, 21))
	    {
	    case NoSaturation:
	      s = ADDx ((i * 32), (i * 32) + 31, 0xffffffff, AddS32);
	      satrv [BITIDX32 (i)] = 0;
	      r |= (s & 0xffffffff) << (i * 32);
	      SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
	      SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
	      SIMD32_SET (psr, carry,      SIMD_CBIT, i);
	      SIMD32_SET (psr, overflow,   SIMD_VBIT, i);
	      break;

	    case UnsignedSaturation:
	      s = ADDx ((i * 32), (i * 32) + 31, 0xffffffff, AddU32);
	      x = IwmmxtSaturateU32 (s, satrv + BITIDX32 (i));
	      r |= (x & 0xffffffff) << (i * 32);
	      SIMD32_SET (psr, NBIT32 (x), SIMD_NBIT, i);
	      SIMD32_SET (psr, ZBIT32 (x), SIMD_ZBIT, i);
	      if (! satrv [BITIDX32 (i)])
		{
		  SIMD32_SET (psr, carry,    SIMD_CBIT, i);
		  SIMD32_SET (psr, overflow, SIMD_VBIT, i);
		}
	      break;

	    case SignedSaturation:
	      s = ADDx ((i * 32), (i * 32) + 31, 0xffffffff, AddS32);
	      x = IwmmxtSaturateS32 (s, satrv + BITIDX32 (i));
	      r |= (x & 0xffffffff) << (i * 32);
	      SIMD32_SET (psr, NBIT32 (x), SIMD_NBIT, i);
	      SIMD32_SET (psr, ZBIT32 (x), SIMD_ZBIT, i);
	      if (! satrv [BITIDX32 (i)])
		{
		  SIMD32_SET (psr, carry,    SIMD_CBIT, i);
		  SIMD32_SET (psr, overflow, SIMD_VBIT, i);
		}
	      break;

	    default:
	      ARMul_UndefInstr (state, instr);
	      return ARMul_DONE;
	    }
	}
      break;

    default:
      ARMul_UndefInstr (state, instr);
      return ARMul_DONE;
    }

  wC [wCASF] = psr;
  wR [BITS (12, 15)] = r;
  wC [wCon] |= (WCON_MUP | WCON_CUP);

  SET_wCSSFvec (satrv);
  
#undef ADDx

  return ARMul_DONE;
}

static int
WALIGNI (ARMword instr)
{
  int shift = BITS (20, 22) * 8;

  if ((read_cp15_reg (15, 0, 1) & 3) != 3)
    return ARMul_CANT;

#ifdef DEBUG
  fprintf (stderr, "waligni\n");
#endif  

  if (shift)
    wR [BITS (12, 15)] =
      wRBITS (BITS (16, 19), shift, 63)
      | (wRBITS (BITS (0, 3), 0, shift) << ((64 - shift)));
  else
    wR [BITS (12, 15)] = wR [BITS (16, 19)];
	   
  wC [wCon] |= WCON_MUP;
  return ARMul_DONE;
}

static int
WALIGNR (ARMul_State * state, ARMword instr)
{
  int shift = (wC [BITS (20, 21) + 8] & 0x7) * 8;

  if ((read_cp15_reg (15, 0, 1) & 3) != 3)
    return ARMul_CANT;

#ifdef DEBUG
  fprintf (stderr, "walignr\n");
#endif  

  if (shift)
    wR [BITS (12, 15)] =
      wRBITS (BITS (16, 19), shift, 63)
      | (wRBITS (BITS (0, 3), 0, shift) << ((64 - shift)));
  else
    wR [BITS (12, 15)] = wR [BITS (16, 19)];

  wC [wCon] |= WCON_MUP;
  return ARMul_DONE;
}

static int
WAND (ARMword instr)
{
  ARMdword result;
  ARMword  psr = 0;

  if ((read_cp15_reg (15, 0, 1) & 3) != 3)
    return ARMul_CANT;

#ifdef DEBUG
  fprintf (stderr, "wand\n");
#endif  

  result = wR [BITS (16, 19)] & wR [BITS (0, 3)];
  wR [BITS (12, 15)] = result;

  SIMD64_SET (psr, (result == 0), SIMD_ZBIT);
  SIMD64_SET (psr, (result & (1ULL << 63)), SIMD_NBIT);
  
  wC [wCASF] = psr;
  wC [wCon] |= (WCON_CUP | WCON_MUP);

  return ARMul_DONE;
}

static int
WANDN (ARMword instr)
{
  ARMdword result;
  ARMword  psr = 0;

  if ((read_cp15_reg (15, 0, 1) & 3) != 3)
    return ARMul_CANT;

#ifdef DEBUG
  fprintf (stderr, "wandn\n");
#endif  

  result = wR [BITS (16, 19)] & ~ wR [BITS (0, 3)];
  wR [BITS (12, 15)] = result;

  SIMD64_SET (psr, (result == 0), SIMD_ZBIT);
  SIMD64_SET (psr, (result & (1ULL << 63)), SIMD_NBIT);
  
  wC [wCASF] = psr;
  wC [wCon] |= (WCON_CUP | WCON_MUP);

  return ARMul_DONE;
}

static int
WAVG2 (ARMword instr)
{
  ARMdword r = 0;
  ARMword  psr = 0;
  ARMdword s;
  int      i;
  int      round = BIT (20) ? 1 : 0;

  if ((read_cp15_reg (15, 0, 1) & 3) != 3)
    return ARMul_CANT;

#ifdef DEBUG
  fprintf (stderr, "wavg2\n");
#endif  

#define AVG2x(x, y, m) (((wRBITS (BITS (16, 19), (x), (y)) & (m)) \
		       + (wRBITS (BITS ( 0,  3), (x), (y)) & (m)) \
		       + round) / 2)

  if (BIT (22))
    {
      for (i = 0; i < 4; i++)
	{
	  s = AVG2x ((i * 16), (i * 16) + 15, 0xffff) & 0xffff;
	  SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
	  r |= s << (i * 16);
	}
    }
  else
    {
      for (i = 0; i < 8; i++)
	{
	  s = AVG2x ((i * 8), (i * 8) + 7, 0xff) & 0xff;
	  SIMD8_SET (psr, ZBIT8 (s), SIMD_ZBIT, i);
	  r |= s << (i * 8);
	}
    }

  wR [BITS (12, 15)] = r;
  wC [wCASF] = psr;
  wC [wCon] |= (WCON_CUP | WCON_MUP);

  return ARMul_DONE;
}

static int
WCMPEQ (ARMul_State * state, ARMword instr)
{
  ARMdword r = 0;
  ARMword  psr = 0;
  ARMdword s;
  int      i;

  if ((read_cp15_reg (15, 0, 1) & 3) != 3)
    return ARMul_CANT;

#ifdef DEBUG
  fprintf (stderr, "wcmpeq\n");
#endif  

  switch (BITS (22, 23))
    {
    case Bqual:
      for (i = 0; i < 8; i++)
	{
	  s = wRBYTE (BITS (16, 19), i) == wRBYTE (BITS (0, 3), i) ? 0xff : 0;
	  r |= s << (i * 8);
	  SIMD8_SET (psr, NBIT8 (s), SIMD_NBIT, i);
	  SIMD8_SET (psr, ZBIT8 (s), SIMD_ZBIT, i);
	}
      break;

    case Hqual:
      for (i = 0; i < 4; i++)
	{
	  s = wRHALF (BITS (16, 19), i) == wRHALF (BITS (0, 3), i) ? 0xffff : 0;
	  r |= s << (i * 16);
	  SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
	  SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
	}
      break;

    case Wqual:
      for (i = 0; i < 2; i++)
	{
	  s = wRWORD (BITS (16, 19), i) == wRWORD (BITS (0, 3), i) ? 0xffffffff : 0;
	  r |= s << (i * 32);
	  SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
	  SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
	}
      break;

    default:
      ARMul_UndefInstr (state, instr);
      return ARMul_DONE;
    }

  wC [wCASF] = psr;
  wR [BITS (12, 15)] = r;
  wC [wCon] |= (WCON_CUP | WCON_MUP);

  return ARMul_DONE;
}

static int
WCMPGT (ARMul_State * state, ARMword instr)
{
  ARMdword r = 0;
  ARMword  psr = 0;
  ARMdword s;
  int      i;

  if ((read_cp15_reg (15, 0, 1) & 3) != 3)
    return ARMul_CANT;

#ifdef DEBUG
  fprintf (stderr, "wcmpgt\n");
#endif  

  switch (BITS (22, 23))
    {
    case Bqual:
      if (BIT (21))
	{
	  /* Use a signed comparison.  */
	  for (i = 0; i < 8; i++)
	    {
	      signed char a, b;
	      
	      a = wRBYTE (BITS (16, 19), i);
	      b = wRBYTE (BITS (0, 3), i);

	      s = (a > b) ? 0xff : 0;
	      r |= s << (i * 8);
	      SIMD8_SET (psr, NBIT8 (s), SIMD_NBIT, i);
	      SIMD8_SET (psr, ZBIT8 (s), SIMD_ZBIT, i);
	    }
	}
      else
	{
	  for (i = 0; i < 8; i++)
	    {
	      s = (wRBYTE (BITS (16, 19), i) > wRBYTE (BITS (0, 3), i))
		? 0xff : 0;
	      r |= s << (i * 8);
	      SIMD8_SET (psr, NBIT8 (s), SIMD_NBIT, i);
	      SIMD8_SET (psr, ZBIT8 (s), SIMD_ZBIT, i);
	    }
	}
      break;

    case Hqual:
      if (BIT (21))
	{
	  for (i = 0; i < 4; i++)
	    {
	      signed int a, b;

	      a = wRHALF (BITS (16, 19), i);
	      a = EXTEND16 (a);

	      b = wRHALF (BITS (0, 3), i);
	      b = EXTEND16 (b);

	      s = (a > b) ? 0xffff : 0;		
	      r |= s << (i * 16);
	      SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
	      SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
	    }
	}
      else
	{
	  for (i = 0; i < 4; i++)
	    {
	      s = (wRHALF (BITS (16, 19), i) > wRHALF (BITS (0, 3), i))
		? 0xffff : 0;
	      r |= s << (i * 16);
	      SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
	      SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
	    }
	}
      break;

    case Wqual:
      if (BIT (21))
	{
	  for (i = 0; i < 2; i++)
	    {
	      signed long a, b;

	      a = wRWORD (BITS (16, 19), i);
	      b = wRWORD (BITS (0, 3), i);

	      s = (a > b) ? 0xffffffff : 0;
	      r |= s << (i * 32);
	      SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
	      SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
	    }
	}
      else
	{
	  for (i = 0; i < 2; i++)
	    {
	      s = (wRWORD (BITS (16, 19), i) > wRWORD (BITS (0, 3), i))
		? 0xffffffff : 0;
	      r |= s << (i * 32);
	      SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
	      SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
	    }
	}
      break;

    default:
      ARMul_UndefInstr (state, instr);
      return ARMul_DONE;
    }

  wC [wCASF] = psr;
  wR [BITS (12, 15)] = r;
  wC [wCon] |= (WCON_CUP | WCON_MUP);

  return ARMul_DONE;
}

static ARMword
Compute_Iwmmxt_Address (ARMul_State * state, ARMword instr, int * pFailed)
{
  ARMword  Rn;
  ARMword  addr;
  ARMword  offset;
  ARMword  multiplier;

  * pFailed  = 0;
  Rn         = BITS (16, 19);
  addr       = state->Reg [Rn];
  offset     = BITS (0, 7);
  multiplier = BIT (8) ? 4 : 1;

  if (BIT (24)) /* P */
    {
      /* Pre Indexed Addressing.  */
      if (BIT (23))
	addr += offset * multiplier;
      else
	addr -= offset * multiplier;

      /* Immediate Pre-Indexed.  */
      if (BIT (21)) /* W */
	{
	  if (Rn == 15)
	    {
	      /* Writeback into R15 is UNPREDICTABLE.  */
#ifdef DEBUG
	      fprintf (stderr, "iWMMXt: writeback into r15\n");
#endif
	      * pFailed = 1;
	    }
	  else
	    state->Reg [Rn] = addr;
	}
    }
  else
    {
      /* Post Indexed Addressing.  */
      if (BIT (21)) /* W */
	{
	  /* Handle the write back of the final address.  */
	  if (Rn == 15)
	    {
	      /* Writeback into R15 is UNPREDICTABLE.  */
#ifdef DEBUG
	      fprintf (stderr, "iWMMXt: writeback into r15\n");
#endif  
	      * pFailed = 1;
	    }
	  else
	    {
	      ARMword  increment;

	      if (BIT (23))
		increment = offset * multiplier;
	      else