[RV64_DYNAREC] Added more opcodes (#719)

* Added 0F C2 CMPPS opcode

* Added 66 0F F6 PSADBW opcode

* Added 66 0F F5 PMADDWD opcode

* Added 66 0F 12 MOVLPD opcode

* Added 66 0F 51 SQRTPD opcode

* Added 66 0F D3 PSRLQ opcode

* Added some fastnan handling to make tests happy

3 files changed, 155 insertions, 2 deletions

diff --git a/src/dynarec/rv64/dynarec_rv64_0f.c b/src/dynarec/rv64/dynarec_rv64_0f.c
index 47624789..d60f7be0 100644
--- a/src/dynarec/rv64/dynarec_rv64_0f.c
+++ b/src/dynarec/rv64/dynarec_rv64_0f.c
@@ -993,6 +993,57 @@ uintptr_t dynarec64_0F(dynarec_rv64_t* dyn, uintptr_t addr, uintptr_t ip, int ni
             if(!rex.w)
                 ZEROUP(gd);
             break;
+        case 0xC2:
+            INST_NAME("CMPPS Gx, Ex, Ib");
+            nextop = F8;
+            GETGX(x1);
+            GETEX(x2, 1);
+            u8 = F8;
+            d0 = fpu_get_scratch(dyn);
+            d1 = fpu_get_scratch(dyn);
+            for(int i=0; i<4; ++i) {
+                FLW(d0, gback, i*4);
+                FLW(d1, wback, fixedaddress+i*4);
+                if ((u8&7) == 0) {                                      // Equal
+                    FEQS(x3, d0, d1);
+                } else if ((u8&7) == 4) {                               // Not Equal or unordered
+                    FEQS(x3, d0, d1);
+                    XORI(x3, x3, 1);
+                } else {
+                    // x4 = !(isnan(d0) || isnan(d1))
+                    FEQS(x4, d0, d0);
+                    FEQS(x3, d1, d1);
+                    AND(x3, x3, x4);
+
+                    switch(u8&7) {
+                    case 1: BEQ_MARK(x3, xZR); FLTS(x3, d0, d1); break; // Less than
+                    case 2: BEQ_MARK(x3, xZR); FLES(x3, d0, d1); break; // Less or equal
+                    case 3: XORI(x3, x3, 1); break;                     // NaN
+                    case 5: {                                           // Greater or equal or unordered
+                        BEQ(x3, xZR, 12); // MARK2
+                        FLES(x3, d1, d0);
+                        J(8); // MARK;
+                        break;
+                    }
+                    case 6: {                                           // Greater or unordered, test inverted, N!=V so unordered or less than (inverted)
+                        BEQ(x3, xZR, 12); // MARK2
+                        FLTS(x3, d1, d0);
+                        J(8); // MARK;
+                        break;
+                    }
+                    case 7: break;                                      // Not NaN
+                    }
+                    
+                    // MARK2;
+                    if ((u8&7) == 5 || (u8&7) == 6) {
+                        MOV32w(x3, 1);
+                    }
+                    // MARK;
+                }
+                NEG(x3, x3);
+                SW(x3, gback, i*4);
+            }
+            break;
         case 0xC6: // TODO: Optimize this!
             INST_NAME("SHUFPS Gx, Ex, Ib");
             nextop = F8;
diff --git a/src/dynarec/rv64/dynarec_rv64_660f.c b/src/dynarec/rv64/dynarec_rv64_660f.c
index 1f2babbc..79c5712d 100644
--- a/src/dynarec/rv64/dynarec_rv64_660f.c
+++ b/src/dynarec/rv64/dynarec_rv64_660f.c
@@ -66,6 +66,20 @@ uintptr_t dynarec64_660F(dynarec_rv64_t* dyn, uintptr_t addr, uintptr_t ip, int
             SSE_LOOP_MV_Q2(x3);
             if(!MODREG) SMWRITE2();
             break;
+        case 0x12:
+            INST_NAME("MOVLPD Gx, Eq");
+            nextop = F8;
+            GETGX(x1);
+            if(MODREG) {
+                // access register instead of memory is bad opcode!
+                DEFAULT;
+                return addr;
+            }
+            SMREAD();
+            addr = geted(dyn, addr, ninst, nextop, &wback, x2, x3, &fixedaddress, rex, NULL, 1, 0);
+            LD(x3, wback, fixedaddress);
+            SD(x3, gback, 0);
+            break;
         case 0x14:
             INST_NAME("UNPCKLPD Gx, Ex");
             nextop = F8;
@@ -342,7 +356,29 @@ uintptr_t dynarec64_660F(dynarec_rv64_t* dyn, uintptr_t addr, uintptr_t ip, int
                     DEFAULT;
             }
             break;
-
+        case 0x51:
+            INST_NAME("SQRTPD Gx, Ex");
+            nextop = F8;
+            GETGX(x1);
+            GETEX(x2, 0);
+            d0 = fpu_get_scratch(dyn);
+            if(!box64_dynarec_fastnan) {
+                d1 = fpu_get_scratch(dyn);
+                FMVDX(d1, xZR);
+            }
+            for (int i=0; i<2; ++i) {
+                FLD(d0, wback, fixedaddress+i*8);
+                if(!box64_dynarec_fastnan) {
+                    FLTD(x3, d0, d1);
+                }
+                FSQRTD(d0, d0);
+                if(!box64_dynarec_fastnan) {
+                    BEQ(x3, xZR, 8);
+                    FNEGD(d0, d0);
+                }
+                FSD(d0, gback, i*8);
+            }
+            break;
         case 0x54:
             INST_NAME("ANDPD Gx, Ex");
             nextop = F8;
@@ -1042,6 +1078,25 @@ uintptr_t dynarec64_660F(dynarec_rv64_t* dyn, uintptr_t addr, uintptr_t ip, int
             u8 = (F8)&7;
             LHU(gd, wback, fixedaddress+u8*2);
             break;
+        case 0xD3:
+            INST_NAME("PSRLQ Gx,Ex");
+            nextop = F8;
+            GETGX(x1);
+            GETEX(x2, 0);
+            LD(x3, wback, fixedaddress);
+            ADDI(x4, xZR, 64);
+            BGE_MARK(x3, x4);
+            ANDI(x3, x3, 0xff);
+            for (int i=0; i<2; ++i) {
+                LD(x5, gback, 8*i);
+                SRL(x5, x5, x3);
+                SD(x5, gback, 8*i);
+            }
+            B_NEXT_nocond;
+            MARK;
+            SD(xZR, gback, 0);
+            SD(xZR, gback, 8);
+            break;
         case 0xD4:
             INST_NAME("PADDQ Gx,Ex");
             nextop = F8;
@@ -1200,6 +1255,45 @@ uintptr_t dynarec64_660F(dynarec_rv64_t* dyn, uintptr_t addr, uintptr_t ip, int
             MUL(x3, x3, x4);
             SD(x3, gback, 0);
             break;
+        case 0xF5:
+            INST_NAME("PMADDWD Gx, Ex");
+            nextop = F8;
+            GETGX(x1);
+            GETEX(x2, 0);
+            for (int i=0; i<4; ++i) {
+                // GX->sd[i] = (int32_t)(GX->sw[i*2+0])*EX->sw[i*2+0] + 
+                //             (int32_t)(GX->sw[i*2+1])*EX->sw[i*2+1];
+                LH(x3, gback, 2*(i*2+0));
+                LH(x4, wback, fixedaddress+2*(i*2+0));
+                MULW(x5, x3, x4);
+                LH(x3, gback, 2*(i*2+1));
+                LH(x4, wback, fixedaddress+2*(i*2+1));
+                MULW(x6, x3, x4);
+                ADDW(x5, x5, x6);
+                SW(x5, gback, 4*i);
+            }
+            break;
+        case 0xF6:
+            INST_NAME("PSADBW Gx, Ex");
+            nextop = F8;
+            GETGX(x1);
+            GETEX(x2, 0);
+            MV(x6, xZR);
+            for (int i=0; i<16; ++i) {
+                LBU(x3, gback, i);
+                LBU(x4, wback, fixedaddress+i);
+                SUBW(x3, x3, x4);
+                SRAIW(x5, x3, 31);
+                XOR(x3, x5, x3);
+                SUBW(x3, x3, x5);
+                ANDI(x3, x3, 0xff);
+                ADDW(x6, x6, x3);
+                if (i==7 || i == 15) {
+                    SD(x6, gback, i+1-8);
+                    if (i==7) MV(x6, xZR);
+                }
+            }
+            break;
         case 0xF8:
             INST_NAME("PSUBB Gx,Ex");
             nextop = F8;
diff --git a/src/dynarec/rv64/dynarec_rv64_f20f.c b/src/dynarec/rv64/dynarec_rv64_f20f.c
index 11b4f506..13e89083 100644
--- a/src/dynarec/rv64/dynarec_rv64_f20f.c
+++ b/src/dynarec/rv64/dynarec_rv64_f20f.c
@@ -178,12 +178,20 @@ uintptr_t dynarec64_F20F(dynarec_rv64_t* dyn, uintptr_t addr, uintptr_t ip, int
                     DEFAULT;
             }
             break;
-        case 0x51: // TODO: fastnan handling
+        case 0x51:
             INST_NAME("SQRTSD Gx, Ex");
             nextop = F8;
             GETEXSD(d0, 0);
             GETGXSD_empty(d1);
+            if(!box64_dynarec_fastnan) {
+                FMVDX(d1, xZR);
+                FLTD(x3, d0, d1);
+            }
             FSQRTD(d1, d0);
+            if(!box64_dynarec_fastnan) {
+                BEQ(x3, xZR, 8);
+                FNEGD(d1, d1);
+            }
             break;
         case 0x58:
             INST_NAME("ADDSD Gx, Ex");