| 1 | /* |
| 2 | * z64 |
| 3 | * |
| 4 | * Copyright (C) 2007 ziggy |
| 5 | * |
| 6 | * This program is free software; you can redistribute it and/or modify |
| 7 | * it under the terms of the GNU General Public License as published by |
| 8 | * the Free Software Foundation; either version 2 of the License, or |
| 9 | * (at your option) any later version. |
| 10 | * |
| 11 | * This program is distributed in the hope that it will be useful, |
| 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 14 | * GNU General Public License for more details. |
| 15 | * |
| 16 | * You should have received a copy of the GNU General Public License along |
| 17 | * with this program; if not, write to the Free Software Foundation, Inc., |
| 18 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. |
| 19 | * |
| 20 | **/ |
| 21 | |
| 22 | #include "rsp.h" |
| 23 | |
| 24 | inline UINT32 get_cop0_reg(RSP_REGS & rsp, int reg) |
| 25 | { |
| 26 | if (reg >= 0 && reg < 8) |
| 27 | { |
| 28 | return sp_read_reg(rsp ,reg); |
| 29 | } |
| 30 | else if (reg >= 8 && reg < 16) |
| 31 | { |
| 32 | return n64_dp_reg_r(rsp, reg - 8, 0x00000000); |
| 33 | } |
| 34 | else |
| 35 | { |
| 36 | log(M64MSG_ERROR, "RSP: get_cop0_reg: %d", reg); |
| 37 | } |
| 38 | } |
| 39 | |
| 40 | inline void set_cop0_reg(RSP_REGS & rsp, int reg, UINT32 data) |
| 41 | { |
| 42 | if (reg >= 0 && reg < 8) |
| 43 | { |
| 44 | sp_write_reg(rsp, reg, data); |
| 45 | } |
| 46 | else if (reg >= 8 && reg < 16) |
| 47 | { |
| 48 | n64_dp_reg_w(rsp, reg - 8, data, 0x00000000); |
| 49 | } |
| 50 | else |
| 51 | { |
| 52 | log(M64MSG_ERROR, "RSP: set_cop0_reg: %d, %08X\n", reg, data); |
| 53 | } |
| 54 | } |
| 55 | |
| 56 | static const int vector_elements_2[16][8] = |
| 57 | { |
| 58 | { 0, 1, 2, 3, 4, 5, 6, 7 }, // none |
| 59 | { 0, 1, 2, 3, 4, 5, 6, 7 }, // ??? |
| 60 | { 0, 0, 2, 2, 4, 4, 6, 6 }, // 0q |
| 61 | { 1, 1, 3, 3, 5, 5, 7, 7 }, // 1q |
| 62 | { 0, 0, 0, 0, 4, 4, 4, 4 }, // 0h |
| 63 | { 1, 1, 1, 1, 5, 5, 5, 5 }, // 1h |
| 64 | { 2, 2, 2, 2, 6, 6, 6, 6 }, // 2h |
| 65 | { 3, 3, 3, 3, 7, 7, 7, 7 }, // 3h |
| 66 | { 0, 0, 0, 0, 0, 0, 0, 0 }, // 0 |
| 67 | { 1, 1, 1, 1, 1, 1, 1, 1 }, // 1 |
| 68 | { 2, 2, 2, 2, 2, 2, 2, 2 }, // 2 |
| 69 | { 3, 3, 3, 3, 3, 3, 3, 3 }, // 3 |
| 70 | { 4, 4, 4, 4, 4, 4, 4, 4 }, // 4 |
| 71 | { 5, 5, 5, 5, 5, 5, 5, 5 }, // 5 |
| 72 | { 6, 6, 6, 6, 6, 6, 6, 6 }, // 6 |
| 73 | { 7, 7, 7, 7, 7, 7, 7, 7 }, // 7 |
| 74 | }; |
| 75 | INLINE UINT16 SATURATE_ACCUM(int accum, int slice, UINT16 negative, UINT16 positive) |
| 76 | { |
| 77 | if ((INT16)ACCUM_H(accum) < 0) |
| 78 | { |
| 79 | if ((UINT16)(ACCUM_H(accum)) != 0xffff) |
| 80 | { |
| 81 | return negative; |
| 82 | } |
| 83 | else |
| 84 | { |
| 85 | if ((INT16)ACCUM_M(accum) >= 0) |
| 86 | { |
| 87 | return negative; |
| 88 | } |
| 89 | else |
| 90 | { |
| 91 | if (slice == 0) |
| 92 | { |
| 93 | return ACCUM_L(accum); |
| 94 | } |
| 95 | else if (slice == 1) |
| 96 | { |
| 97 | return ACCUM_M(accum); |
| 98 | } |
| 99 | } |
| 100 | } |
| 101 | } |
| 102 | else |
| 103 | { |
| 104 | if ((UINT16)(ACCUM_H(accum)) != 0) |
| 105 | { |
| 106 | return positive; |
| 107 | } |
| 108 | else |
| 109 | { |
| 110 | if ((INT16)ACCUM_M(accum) < 0) |
| 111 | { |
| 112 | return positive; |
| 113 | } |
| 114 | else |
| 115 | { |
| 116 | if (slice == 0) |
| 117 | { |
| 118 | return ACCUM_L(accum); |
| 119 | } |
| 120 | else |
| 121 | { |
| 122 | return ACCUM_M(accum); |
| 123 | } |
| 124 | } |
| 125 | } |
| 126 | } |
| 127 | |
| 128 | return 0; |
| 129 | } |
| 130 | |
| 131 | INLINE UINT16 SATURATE_ACCUM1(int accum, UINT16 negative, UINT16 positive) |
| 132 | { |
| 133 | if ((INT16)ACCUM_H(accum) < 0) |
| 134 | { |
| 135 | if ((UINT16)(ACCUM_H(accum)) != 0xffff) |
| 136 | return negative; |
| 137 | else |
| 138 | { |
| 139 | if ((INT16)ACCUM_M(accum) >= 0) |
| 140 | return negative; |
| 141 | else |
| 142 | return ACCUM_M(accum); |
| 143 | } |
| 144 | } |
| 145 | else |
| 146 | { |
| 147 | if ((UINT16)(ACCUM_H(accum)) != 0) |
| 148 | return positive; |
| 149 | else |
| 150 | { |
| 151 | if ((INT16)ACCUM_M(accum) < 0) |
| 152 | return positive; |
| 153 | else |
| 154 | return ACCUM_M(accum); |
| 155 | } |
| 156 | } |
| 157 | |
| 158 | return 0; |
| 159 | } |
| 160 | |
| 161 | #define WRITEBACK_RESULT() \ |
| 162 | do { \ |
| 163 | VREG_S(VDREG, 0) = vres[0]; \ |
| 164 | VREG_S(VDREG, 1) = vres[1]; \ |
| 165 | VREG_S(VDREG, 2) = vres[2]; \ |
| 166 | VREG_S(VDREG, 3) = vres[3]; \ |
| 167 | VREG_S(VDREG, 4) = vres[4]; \ |
| 168 | VREG_S(VDREG, 5) = vres[5]; \ |
| 169 | VREG_S(VDREG, 6) = vres[6]; \ |
| 170 | VREG_S(VDREG, 7) = vres[7]; \ |
| 171 | } while(0) |
| 172 | |
| 173 | #if 0 |
| 174 | inline void rsp_execute_one(UINT32 op) |
| 175 | { |
| 176 | switch (op >> 26) |
| 177 | { |
| 178 | case 0x00: /* SPECIAL */ |
| 179 | { |
| 180 | switch (op & 0x3f) |
| 181 | { |
| 182 | case 0x00: /* SLL */ if (RDREG) RDVAL = (UINT32)RTVAL << SHIFT; break; |
| 183 | case 0x02: /* SRL */ if (RDREG) RDVAL = (UINT32)RTVAL >> SHIFT; break; |
| 184 | case 0x03: /* SRA */ if (RDREG) RDVAL = (INT32)RTVAL >> SHIFT; break; |
| 185 | case 0x04: /* SLLV */ if (RDREG) RDVAL = (UINT32)RTVAL << (RSVAL & 0x1f); break; |
| 186 | case 0x06: /* SRLV */ if (RDREG) RDVAL = (UINT32)RTVAL >> (RSVAL & 0x1f); break; |
| 187 | case 0x07: /* SRAV */ if (RDREG) RDVAL = (INT32)RTVAL >> (RSVAL & 0x1f); break; |
| 188 | case 0x08: /* JR */ JUMP_PC(RSVAL); break; |
| 189 | case 0x09: /* JALR */ JUMP_PC_L(RSVAL, RDREG); break; |
| 190 | case 0x0d: /* BREAK */ |
| 191 | { |
| 192 | *z64_rspinfo.SP_STATUS_REG |= (SP_STATUS_HALT | SP_STATUS_BROKE ); |
| 193 | if ((*z64_rspinfo.SP_STATUS_REG & SP_STATUS_INTR_BREAK) != 0 ) { |
| 194 | *z64_rspinfo.MI_INTR_REG |= 1; |
| 195 | z64_rspinfo.CheckInterrupts(); |
| 196 | } |
| 197 | //sp_set_status(0x3); |
| 198 | |
| 199 | #if LOG_INSTRUCTION_EXECUTION |
| 200 | fprintf(exec_output, "\n---------- break ----------\n\n"); |
| 201 | #endif |
| 202 | break; |
| 203 | } |
| 204 | case 0x20: /* ADD */ if (RDREG) RDVAL = (INT32)(RSVAL + RTVAL); break; |
| 205 | case 0x21: /* ADDU */ if (RDREG) RDVAL = (INT32)(RSVAL + RTVAL); break; |
| 206 | case 0x22: /* SUB */ if (RDREG) RDVAL = (INT32)(RSVAL - RTVAL); break; |
| 207 | case 0x23: /* SUBU */ if (RDREG) RDVAL = (INT32)(RSVAL - RTVAL); break; |
| 208 | case 0x24: /* AND */ if (RDREG) RDVAL = RSVAL & RTVAL; break; |
| 209 | case 0x25: /* OR */ if (RDREG) RDVAL = RSVAL | RTVAL; break; |
| 210 | case 0x26: /* XOR */ if (RDREG) RDVAL = RSVAL ^ RTVAL; break; |
| 211 | case 0x27: /* NOR */ if (RDREG) RDVAL = ~(RSVAL | RTVAL); break; |
| 212 | case 0x2a: /* SLT */ if (RDREG) RDVAL = (INT32)RSVAL < (INT32)RTVAL; break; |
| 213 | case 0x2b: /* SLTU */ if (RDREG) RDVAL = (UINT32)RSVAL < (UINT32)RTVAL; break; |
| 214 | default: unimplemented_opcode(op); break; |
| 215 | } |
| 216 | break; |
| 217 | } |
| 218 | |
| 219 | case 0x01: /* REGIMM */ |
| 220 | { |
| 221 | switch (RTREG) |
| 222 | { |
| 223 | case 0x00: /* BLTZ */ if ((INT32)(RSVAL) < 0) JUMP_REL(SIMM16); break; |
| 224 | case 0x01: /* BGEZ */ if ((INT32)(RSVAL) >= 0) JUMP_REL(SIMM16); break; |
| 225 | // VP according to the doc, link is performed even when condition fails, |
| 226 | // this sound pretty stupid but let's try it that way |
| 227 | case 0x11: /* BGEZAL */ LINK(31); if ((INT32)(RSVAL) >= 0) JUMP_REL(SIMM16); break; |
| 228 | //case 0x11: /* BGEZAL */ if ((INT32)(RSVAL) >= 0) JUMP_REL_L(SIMM16, 31); break; |
| 229 | default: unimplemented_opcode(op); break; |
| 230 | } |
| 231 | break; |
| 232 | } |
| 233 | |
| 234 | case 0x02: /* J */ JUMP_ABS(UIMM26); break; |
| 235 | case 0x03: /* JAL */ JUMP_ABS_L(UIMM26, 31); break; |
| 236 | case 0x04: /* BEQ */ if (RSVAL == RTVAL) JUMP_REL(SIMM16); break; |
| 237 | case 0x05: /* BNE */ if (RSVAL != RTVAL) JUMP_REL(SIMM16); break; |
| 238 | case 0x06: /* BLEZ */ if ((INT32)RSVAL <= 0) JUMP_REL(SIMM16); break; |
| 239 | case 0x07: /* BGTZ */ if ((INT32)RSVAL > 0) JUMP_REL(SIMM16); break; |
| 240 | case 0x08: /* ADDI */ if (RTREG) RTVAL = (INT32)(RSVAL + SIMM16); break; |
| 241 | case 0x09: /* ADDIU */ if (RTREG) RTVAL = (INT32)(RSVAL + SIMM16); break; |
| 242 | case 0x0a: /* SLTI */ if (RTREG) RTVAL = (INT32)(RSVAL) < ((INT32)SIMM16); break; |
| 243 | case 0x0b: /* SLTIU */ if (RTREG) RTVAL = (UINT32)(RSVAL) < (UINT32)((INT32)SIMM16); break; |
| 244 | case 0x0c: /* ANDI */ if (RTREG) RTVAL = RSVAL & UIMM16; break; |
| 245 | case 0x0d: /* ORI */ if (RTREG) RTVAL = RSVAL | UIMM16; break; |
| 246 | case 0x0e: /* XORI */ if (RTREG) RTVAL = RSVAL ^ UIMM16; break; |
| 247 | case 0x0f: /* LUI */ if (RTREG) RTVAL = UIMM16 << 16; break; |
| 248 | |
| 249 | case 0x10: /* COP0 */ |
| 250 | { |
| 251 | switch ((op >> 21) & 0x1f) |
| 252 | { |
| 253 | case 0x00: /* MFC0 */ if (RTREG) RTVAL = get_cop0_reg(RDREG); break; |
| 254 | case 0x04: /* MTC0 */ set_cop0_reg(RDREG, RTVAL); break; |
| 255 | default: |
| 256 | printf("unimplemented cop0 %x (%x)\n", (op >> 21) & 0x1f, op); |
| 257 | break; |
| 258 | } |
| 259 | break; |
| 260 | } |
| 261 | |
| 262 | case 0x12: /* COP2 */ |
| 263 | { |
| 264 | switch ((op >> 21) & 0x1f) |
| 265 | { |
| 266 | case 0x00: /* MFC2 */ |
| 267 | { |
| 268 | // 31 25 20 15 10 6 0 |
| 269 | // --------------------------------------------------- |
| 270 | // | 010010 | 00000 | TTTTT | DDDDD | IIII | 0000000 | |
| 271 | // --------------------------------------------------- |
| 272 | // |
| 273 | |
| 274 | int el = (op >> 7) & 0xf; |
| 275 | UINT16 b1 = VREG_B(VS1REG, (el+0) & 0xf); |
| 276 | UINT16 b2 = VREG_B(VS1REG, (el+1) & 0xf); |
| 277 | if (RTREG) RTVAL = (INT32)(INT16)((b1 << 8) | (b2)); |
| 278 | break; |
| 279 | } |
| 280 | case 0x02: /* CFC2 */ |
| 281 | { |
| 282 | // 31 25 20 15 10 0 |
| 283 | // ------------------------------------------------ |
| 284 | // | 010010 | 00010 | TTTTT | DDDDD | 00000000000 | |
| 285 | // ------------------------------------------------ |
| 286 | // |
| 287 | |
| 288 | // VP to sign extend or to not sign extend ? |
| 289 | //if (RTREG) RTVAL = (INT16)rsp.flag[RDREG]; |
| 290 | if (RTREG) RTVAL = rsp.flag[RDREG]; |
| 291 | break; |
| 292 | } |
| 293 | case 0x04: /* MTC2 */ |
| 294 | { |
| 295 | // 31 25 20 15 10 6 0 |
| 296 | // --------------------------------------------------- |
| 297 | // | 010010 | 00100 | TTTTT | DDDDD | IIII | 0000000 | |
| 298 | // --------------------------------------------------- |
| 299 | // |
| 300 | |
| 301 | int el = (op >> 7) & 0xf; |
| 302 | VREG_B(VS1REG, (el+0) & 0xf) = (RTVAL >> 8) & 0xff; |
| 303 | VREG_B(VS1REG, (el+1) & 0xf) = (RTVAL >> 0) & 0xff; |
| 304 | break; |
| 305 | } |
| 306 | case 0x06: /* CTC2 */ |
| 307 | { |
| 308 | // 31 25 20 15 10 0 |
| 309 | // ------------------------------------------------ |
| 310 | // | 010010 | 00110 | TTTTT | DDDDD | 00000000000 | |
| 311 | // ------------------------------------------------ |
| 312 | // |
| 313 | |
| 314 | rsp.flag[RDREG] = RTVAL & 0xffff; |
| 315 | break; |
| 316 | } |
| 317 | |
| 318 | case 0x10: case 0x11: case 0x12: case 0x13: case 0x14: case 0x15: case 0x16: case 0x17: |
| 319 | case 0x18: case 0x19: case 0x1a: case 0x1b: case 0x1c: case 0x1d: case 0x1e: case 0x1f: |
| 320 | { |
| 321 | handle_vector_ops(op); |
| 322 | break; |
| 323 | } |
| 324 | |
| 325 | default: unimplemented_opcode(op); break; |
| 326 | } |
| 327 | break; |
| 328 | } |
| 329 | |
| 330 | case 0x20: /* LB */ if (RTREG) RTVAL = (INT32)(INT8)READ8(RSVAL + SIMM16); break; |
| 331 | case 0x21: /* LH */ if (RTREG) RTVAL = (INT32)(INT16)READ16(RSVAL + SIMM16); break; |
| 332 | case 0x23: /* LW */ if (RTREG) RTVAL = READ32(RSVAL + SIMM16); break; |
| 333 | case 0x24: /* LBU */ if (RTREG) RTVAL = (UINT8)READ8(RSVAL + SIMM16); break; |
| 334 | case 0x25: /* LHU */ if (RTREG) RTVAL = (UINT16)READ16(RSVAL + SIMM16); break; |
| 335 | case 0x28: /* SB */ WRITE8(RSVAL + SIMM16, RTVAL); break; |
| 336 | case 0x29: /* SH */ WRITE16(RSVAL + SIMM16, RTVAL); break; |
| 337 | case 0x2b: /* SW */ WRITE32(RSVAL + SIMM16, RTVAL); break; |
| 338 | case 0x32: /* LWC2 */ handle_lwc2(op); break; |
| 339 | case 0x3a: /* SWC2 */ handle_swc2(op); break; |
| 340 | |
| 341 | default: |
| 342 | { |
| 343 | unimplemented_opcode(op); |
| 344 | break; |
| 345 | } |
| 346 | } |
| 347 | } |
| 348 | |
| 349 | #endif |