Add basic implementations of ROUNDxx, BLENDPS/BLENDPD, and PBLENDW

Author: fuel-pcbox

blink/cvt.c

@@ -36,6 +36,38 @@
 #define kOpCvt0f5b  16
 #define kOpCvt0fE6  20
 
+static float SseRoundSingle(struct Machine *m, float x) {
+  switch ((m->mxcsr & kMxcsrRc) >> 13) {
+    case 0:
+      return rint(x);
+    case 1:
+      return floor(x);
+    case 2:
+      return ceil(x);
+    case 3:
+      return trunc(x);
+    default:
+      __builtin_unreachable();
+  }
+}
+
+static float SseRoundSingleImm(struct Machine *m, float x, int imm) {
+  int roundmode = imm & 3;
+  if((imm >> 2) & 1) roundmode = (m->mxcsr & kMxcsrRc) >> 13;
+  switch (roundmode) {
+    case 0:
+      return rint(x);
+    case 1:
+      return floor(x);
+    case 2:
+      return ceil(x);
+    case 3:
+      return trunc(x);
+    default:
+      __builtin_unreachable();
+  }
+}
+
 static double SseRoundDouble(struct Machine *m, double x) {
   switch ((m->mxcsr & kMxcsrRc) >> 13) {
     case 0:
@@ -51,6 +83,73 @@ static double SseRoundDouble(struct Machine *m, double x) {
   }
 }
 
+static double SseRoundDoubleImm(struct Machine *m, double x, int imm) {
+  int roundmode = imm & 3;
+  if((imm >> 2) & 1) roundmode = (m->mxcsr & kMxcsrRc) >> 13;
+  switch (roundmode) {
+    case 0:
+      return rint(x);
+    case 1:
+      return floor(x);
+    case 2:
+      return ceil(x);
+    case 3:
+      return trunc(x);
+    default:
+      __builtin_unreachable();
+  }
+}
+
+static void OpPpiWpdRoundps(P) {
+  u8 *p;
+  unsigned i;
+  i32 n[4];
+  union FloatPun f[2];
+  p = GetModrmRegisterXmmPointerRead16(A);
+  f[0].i = Read32(p + 0);
+  f[1].i = Read32(p + 4);
+  f[2].i = Read32(p + 8);
+  f[3].i = Read32(p + 12);
+  for (i = 0; i < 4; ++i) n[i] = SseRoundSingleImm(m, f[i].f, uimm0);
+  Put32(XmmReg(m, rde) + 0, n[0]);
+  Put32(XmmReg(m, rde) + 4, n[1]);
+  Put32(XmmReg(m, rde) + 8, n[2]);
+  Put32(XmmReg(m, rde) + 12, n[3]);
+}
+
+static void OpPpiWpdRoundpd(P) {
+  u8 *p;
+  unsigned i;
+  i64 n[2];
+  union DoublePun d[2];
+  p = GetModrmRegisterXmmPointerRead16(A);
+  d[0].i = Read64(p + 0);
+  d[1].i = Read64(p + 8);
+  for (i = 0; i < 2; ++i) n[i] = SseRoundDoubleImm(m, d[i].f, uimm0);
+  Put64(XmmReg(m, rde) + 0, n[0]);
+  Put64(XmmReg(m, rde) + 8, n[1]);
+}
+
+static void OpPpiWpdRoundss(P) {
+  u8 *p;
+  i32 n;
+  union FloatPun f;
+  p = GetModrmRegisterXmmPointerRead16(A);
+  f.i = Read32(p);
+  n = SseRoundSingleImm(m, f.f, uimm0);
+  Put32(XmmReg(m, rde), n);
+}
+
+static void OpPpiWpdRoundsd(P) {
+  u8 *p;
+  i32 n;
+  union DoublePun d;
+  p = GetModrmRegisterXmmPointerRead16(A);
+  d.i = Read64(p);
+  n = SseRoundDoubleImm(m, d.f, uimm0);
+  Put64(XmmReg(m, rde), n);
+}
+
 static void OpGdqpWssCvttss2si(P) {
   i64 n;
   union FloatPun f;
@@ -73,7 +172,7 @@ static void OpGdqpWssCvtss2si(P) {
   i64 n;
   union FloatPun f;
   f.i = Read32(GetModrmRegisterXmmPointerRead4(A));
-  n = rintf(f.f);
+  n = SseRoundSingle(f.f);
   if (!Rexw(rde)) n &= 0xffffffff;
   Put64(RegRexrReg(m, rde), n);
 }

blink/ssefloat.c

@@ -228,6 +228,54 @@ void OpShufpsd(P) {
   }
 }
 
+static void OpBlendps(P) {
+  u8 *p;
+  union FloatPun x[4], y[4], z[4];
+  p = GetModrmRegisterXmmPointerRead16(A);
+  IGNORE_RACES_START();
+  y[0].i = Read32(p + 0 * 4);
+  y[1].i = Read32(p + 1 * 4);
+  y[2].i = Read32(p + 2 * 4);
+  y[3].i = Read32(p + 3 * 4);
+  p = XmmRexrReg(m, rde);
+  x[0].i = Read32(p + 0 * 4);
+  x[1].i = Read32(p + 1 * 4);
+  x[2].i = Read32(p + 2 * 4);
+  x[3].i = Read32(p + 3 * 4);
+  if(uimm0 & 1) z[0].f = y[0].f;
+  else z[0].f = x[0].f;
+  if(uimm0 & 2) z[1].f = y[1].f;
+  else z[1].f = x[1].f;
+  if(uimm0 & 4) z[2].f = y[2].f;
+  else z[2].f = x[2].f;
+  if(uimm0 & 8) z[3].f = y[3].f;
+  else z[3].f = x[3].f;
+  Write32(p + 0 * 4, z[0].i);
+  Write32(p + 1 * 4, z[1].i);
+  Write32(p + 2 * 4, z[2].i);
+  Write32(p + 3 * 4, z[3].i);
+  IGNORE_RACES_END();
+}
+
+static void OpBlendpd(P) {
+  u8 *p;
+  union DoublePun x[2], y[2], z[2];
+  p = GetModrmRegisterXmmPointerRead16(A);
+  IGNORE_RACES_START();
+  y[0].i = Read64(p + 0 * 8);
+  y[1].i = Read64(p + 1 * 8);
+  p = XmmRexrReg(m, rde);
+  x[0].i = Read64(p + 0 * 8);
+  x[1].i = Read64(p + 1 * 8);
+  if(uimm0 & 1) z[0].f = y[0].f;
+  else z[0].f = x[0].f;
+  if(uimm0 & 2) z[1].f = y[1].f;
+  else z[1].f = x[1].f;
+  Write64(p + 0 * 8, z[0].i);
+  Write64(p + 1 * 8, z[1].i);
+  IGNORE_RACES_END();
+}
+
 static void Movmskps(P) {
   u8 *p = GetModrmRegisterXmmPointerRead16(A);
   IGNORE_RACES_START();

blink/ssemov.c

@@ -89,6 +89,25 @@ void OpMovntiMdqpGdqp(P) {
   IGNORE_RACES_END();
 }
 
+void OpPBlendW(P) {
+  u8* p;
+  unsigned i;
+  u16 x[8], y[8], z[8];
+  
+  IGNORE_RACES_START();
+  for(i = 0; i < 8; i++)
+  {
+    p = GetModrmRegisterXmmPointerRead16(A);
+    y[i] = Read16(p + i * 2);
+    p = XmmRexrReg(m, rde);
+    x[i] = Read16(p + i * 2);
+    if(uimm0 & (1 << i)) z[i] = y[i];
+    else z[i] = x[i];
+    Write16(p + i * 2, z[i]);
+  }
+  IGNORE_RACES_END();
+}
+
 static void MovdqaVdqWdq(P) {
   IGNORE_RACES_START();
   memcpy(XmmRexrReg(m, rde), GetXmmAddress(A), 16);