/* TODO: -verify DSP adds, subtracts, multiplies -MCR/MRC to R15 is special - handle it TOTOv6: -Endianness support (incl for pagetable lookups) */ #include "CPU.h" #include "math64.h" #define ARM_MODE_2_REG 0x0F #define ARM_MODE_2_WORD 0x10 #define ARM_MODE_2_LOAD 0x20 #define ARM_MODE_2_T 0x40 #define ARM_MODE_2_INV 0x80 #define ARM_MODE_3_REG 0x0F //flag for actual reg number used #define ARM_MODE_3_TYPE 0x30 //flag for the below 4 types #define ARM_MODE_3_H 0x00 #define ARM_MODE_3_SH 0x10 #define ARM_MODE_3_SB 0x20 #define ARM_MODE_3_D 0x30 #define ARM_MODE_3_LOAD 0x40 #define ARM_MODE_3_INV 0x80 #define ARM_MODE_4_REG 0x0F #define ARM_MODE_4_INC 0x10 //incr or decr #define ARM_MODE_4_BFR 0x20 //before or after #define ARM_MODE_4_WBK 0x40 //writeback? #define ARM_MODE_4_S 0x80 //S bit set? #define ARM_MODE_5_REG 0x0F #define ARM_MODE_5_ADD_BEFORE 0x10 //add value before? #define ARM_MODE_5_ADD_AFTER 0x20 //add value after? #define ARM_MODE_5_IS_OPTION 0x40 //is value option (as opposed to offset) #define ARM_MODE_5_RR 0x80 //MCRR or MRCC instrs #ifdef ARM_V6 #define ARM_CPSR_UND_AND (~(ARM_SR_M | ARM_SR_E)) #define ARM_CPSR_UND_ORR (ARM_SR_I | ARM_SR_MODE_UND | (cpu->EEE ? ARM_SR_E : 0)) #define ARM_CPSR_SWI_AND (~(ARM_SR_M | ARM_SR_E)) #define ARM_CPSR_SWI_ORR (ARM_SR_I | ARM_SR_MODE_SVC | (cpu->EEE ? ARM_SR_E : 0)) #define ARM_CPSR_PAB_AND (~(ARM_SR_M | ARM_SR_E)) #define ARM_CPSR_PAB_ORR (ARM_SR_I | ARM_SR_A | ARM_SR_MODE_ABT | (cpu->EEE ? ARM_SR_E : 0)) #define ARM_CPSR_DAB_AND (~(ARM_SR_M | ARM_SR_E)) #define ARM_CPSR_DAB_ORR (ARM_SR_I | ARM_SR_A | ARM_SR_MODE_ABT | (cpu->EEE ? ARM_SR_E : 0)) #define ARM_CPSR_IRQ_AND (~(ARM_SR_M | ARM_SR_E)) #define ARM_CPSR_IRQ_ORR (ARM_SR_I | ARM_SR_A | ARM_SR_MODE_IRQ | (cpu->EEE ? ARM_SR_E : 0)) #define ARM_CPSR_FIQ_AND (~(ARM_SR_M | ARM_SR_E)) #define ARM_CPSR_FIQ_ORR (ARM_SR_I | ARM_SR_A | ARM_SR_F | ARM_SR_MODE_IRQ | (cpu->EEE ? ARM_SR_E : 0)) #else #define ARM_CPSR_UND_AND (~ARM_SR_M) #define ARM_CPSR_UND_ORR (ARM_SR_I | ARM_SR_MODE_UND) #define ARM_CPSR_SWI_AND (~ARM_SR_M) #define ARM_CPSR_SWI_ORR (ARM_SR_I | ARM_SR_MODE_SVC) #define ARM_CPSR_PAB_AND (~ARM_SR_M) #define ARM_CPSR_PAB_ORR (ARM_SR_I | ARM_SR_MODE_ABT) #define ARM_CPSR_DAB_AND (~ARM_SR_M) #define ARM_CPSR_DAB_ORR (ARM_SR_I | ARM_SR_MODE_ABT) #define ARM_CPSR_IRQ_AND (~ARM_SR_M) #define ARM_CPSR_IRQ_ORR (ARM_SR_I | ARM_SR_MODE_IRQ) #define ARM_CPSR_FIQ_AND (~ARM_SR_M) #define ARM_CPSR_FIQ_ORR (ARM_SR_I | ARM_SR_F | ARM_SR_MODE_FIQ) #endif static _INLINE_ UInt32 cpuPrvROR(UInt32 val, UInt8 ror){ if(ror) val = (val >> (UInt32)ror) | (val << (UInt32)(32 - ror)); return val; } static _INLINE_ void cpuPrvSetPC(ArmCpu* cpu, UInt32 pc){ cpu->regs[15] = pc &~ 1UL; cpu->CPSR &=~ ARM_SR_T; if(pc & 1) cpu->CPSR |= ARM_SR_T; else if(pc & 2) cpu->emulErrF(cpu, "Attempt to branch to non-word-aligned ARM address"); } static _INLINE_ UInt32 cpuPrvGetReg(ArmCpu* cpu, UInt8 reg, Boolean wasT, Boolean specialPC){ UInt32 ret; ret = cpu->regs[reg]; if(reg == 15) ret += (wasT) ? 2 : 4; if(wasT && specialPC) ret &=~ 3UL; return ret; } static _INLINE_ void cpuPrvSetReg(ArmCpu* cpu, UInt8 reg, UInt32 val){ if(reg == 15){ cpuPrvSetPC(cpu, val); } else cpu->regs[reg] = val; } UInt32 cpuGetRegExternal(ArmCpu* cpu, UInt8 reg){ if(reg < 16){ // real reg return (reg == 15) ? (cpu->regs[15] + ((cpu->CPSR & ARM_SR_T) ? 1 : 0)) : cpu->regs[reg]; } else if(reg == ARM_REG_NUM_CPSR){ return cpu->CPSR; } else if(reg == ARM_REG_NUM_SPSR){ return cpu->SPSR; } return 0; } void cpuSetReg(ArmCpu* cpu, UInt8 reg, UInt32 val){ cpuPrvSetReg(cpu, reg, val); } #define cpuSetReg _DO_NOT_USE_cpuSetReg_IN_CPU_C_ static ArmBankedRegs* cpuPrvModeToBankedRegsPtr(ArmCpu* cpu, UInt8 mode){ switch(mode){ case ARM_SR_MODE_USR: case ARM_SR_MODE_SYS: return &cpu->bank_usr; case ARM_SR_MODE_FIQ: return &cpu->bank_fiq; case ARM_SR_MODE_IRQ: return &cpu->bank_irq; case ARM_SR_MODE_SVC: return &cpu->bank_svc; case ARM_SR_MODE_ABT: return &cpu->bank_abt; case ARM_SR_MODE_UND: return &cpu->bank_und; default: cpu->emulErrF(cpu, "Invalid mode passed to cpuPrvModeToBankedRegsPtr()"); return NULL; } } static void cpuPrvSwitchToMode(ArmCpu* cpu, UInt8 newMode){ ArmBankedRegs *saveTo, *getFrom; UInt32 tmp; UInt8 i, curMode; curMode = cpu->CPSR & ARM_SR_M; if(curMode == newMode) return; if(curMode == ARM_SR_MODE_FIQ || newMode == ARM_SR_MODE_FIQ){ //bank/unbank the fiq regs for(i = 0; i < 5; i++){ tmp = cpu->extra_regs[i]; cpu->extra_regs[i] = cpu->regs[i + 8]; cpu->regs[i + 8] = tmp; } } saveTo = cpuPrvModeToBankedRegsPtr(cpu, curMode); getFrom = cpuPrvModeToBankedRegsPtr(cpu, newMode); if(saveTo == getFrom) return; //we're done if no regs to switch [this happens if we switch user<->system] saveTo->R13 = cpu->regs[13]; saveTo->R14 = cpu->regs[14]; saveTo->SPSR = cpu->SPSR; cpu->regs[13] = getFrom->R13; cpu->regs[14] = getFrom->R14; cpu->SPSR = getFrom->SPSR; cpu->CPSR = (cpu->CPSR &~ ARM_SR_M) | newMode; } static void cpuPrvException(ArmCpu* cpu, UInt32 vector_pc, UInt32 lr, UInt32 newCPSR){ UInt32 cpsr = cpu->CPSR; cpuPrvSwitchToMode(cpu, newCPSR & ARM_SR_M); cpu->CPSR = newCPSR; cpu->SPSR = cpsr; cpu->regs[14] = lr; cpu->regs[15] = vector_pc; } static void cpuPrvHandleMemErr(ArmCpu* cpu, UInt32 addr, _UNUSED_ UInt8 sz, _UNUSED_ Boolean write, Boolean instrFetch, UInt8 fsr){ if(cpu->setFaultAdrF) cpu->setFaultAdrF(cpu, addr, fsr); if(instrFetch){ //handle prefetch abort cpuPrvException(cpu, cpu->vectorBase + ARM_VECTOR_OFFT_P_ABT, cpu->regs[15] + 4, ARM_CPSR_PAB_ORR | (cpu->CPSR & ARM_CPSR_PAB_AND)); } else{ //handle data abort cpuPrvException(cpu, cpu->vectorBase + ARM_VECTOR_OFFT_D_ABT, cpu->regs[15] + ((cpu->CPSR & ARM_SR_T) ? 6 : 4) /* instr + 8*/, ARM_CPSR_DAB_ORR | (cpu->CPSR & ARM_CPSR_DAB_AND)); } } static _INLINE_ UInt32 cpuPrvArmAdrMode_1(ArmCpu* cpu, UInt32 instr, Boolean* carryOutP, Boolean wasT, Boolean specialPC){ UInt32 ret; UInt8 v, a; Boolean co = ((cpu->CPSR & ARM_SR_C) != 0); //be default carry out = C flag if(instr & 0x02000000UL){ //immed v = (instr >> 7) & 0x1E; ret = cpuPrvROR(instr & 0xFF, v); if (v) co = ((ret & 0x80000000UL) != 0); } else{ v = (instr >> 5) & 3; //get shift type ret = cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC); //get Rm if(instr & 0x00000010UL){ //reg with reg shift a = cpuPrvGetReg(cpu, (instr >> 8) & 0x0F, wasT, specialPC); //get the relevant part of Rs, we only care for lower 8 bits (note we use uint8 for this) if(a != 0){ //else all is already good switch(v){ //perform shifts case 0: //LSL if(a < 32){ co = (ret >> (32 - a)) & 1; ret = ret << a; } else if(a == 32){ co = ret & 1; ret = 0; } else{ // >32 co = 0; ret = 0; } break; case 1: //LSR if(a < 32){ co = (ret >> (a - 1)) & 1; ret = ret >> a; } else if(a == 32){ co = ret >> 31; ret = 0; } else{ // >32 co = 0; ret = 0; } break; case 2: //ASR if(a < 32){ co = (ret >> (a - 1)) & 1; ret = ((Int32)ret >> a); } else{ // >=32 if(ret & 0x80000000UL){ co = 1; ret = 0xFFFFFFFFUL; } else{ co = 0; ret = 0; } } break; case 3: //ROR if(a == 0){ //nothing... } else{ a &= 0x1F; if(a == 0){ co = ret >> 31; } else{ co = (ret >> (a - 1)) & 1; ret = cpuPrvROR(ret, a); } } break; } } } else{ //reg with immed shift a = (instr >> 7) & 0x1F; //get imm switch(v){ case 0: //LSL if(a == 0){ //nothing } else{ co = (ret >> (32 - a)) & 1; ret = ret << a; } break; case 1: //LSR if(a == 0){ co = ret >> 31; ret = 0; } else{ co = (ret >> (a - 1)) & 1; ret = ret >> a; } break; case 2: //ASR if(a == 0){ if(ret & 0x80000000UL){ co = 1; ret = 0xFFFFFFFFUL; } else{ co = 0; ret = 0; } } else{ co = (ret >> (a - 1)) & 1; if(ret & 0x80000000UL){ ret = (ret >> a) | (0xFFFFFFFFUL << (32 - a)); } else{ ret = ret >> a; } } break; case 3: //ROR or RRX if(a == 0){ //RRX a = co; co = ret & 1; ret = ret >> 1; if(a) ret |= 0x80000000UL; } else{ co = (ret >> (a - 1)) & 1; ret = cpuPrvROR(ret, a); } break; } } } *carryOutP = co; return ret; } /* idea: addbefore is what to add to add to base reg before addressing, addafter is what to add after. we ALWAYS do writeback, but if not requested by instr, it will be zero for [Rx, 5] baseReg = x addbefore = 5 addafter = -5 for [Rx, 5]! baseReg = x addBefore = 0 addafter = 0 for [Rx], 5 baseReg = x addBefore = 0 addAfter = 5 t = T bit (LDR vs LDRT) baseReg is returned in return val along with flags: ARM_MODE_2_REG is mask for reg ARM_MODE_2_WORD is flag for word access ARM_MODE_2_LOAD is flag for load ARM_MODE_2_INV is flag for invalid instructions ARM_MODE_2_T is flag for T */ static _INLINE_ UInt8 cpuPrvArmAdrMode_2(ArmCpu* cpu, UInt32 instr, UInt32* addBeforeP, UInt32* addWritebackP, Boolean wasT, Boolean specialPC){ UInt32 val; UInt8 reg, shift; reg = (instr >> 16) & 0x0F; if(!(instr & 0x02000000UL)){ //immediate val = instr & 0xFFFUL; } else{ //[scaled] register if(instr & 0x00000010UL) reg |= ARM_MODE_2_INV; //invalid instrucitons need to be reported val = cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC); shift = (instr >> 7) & 0x1F; switch((instr >> 5) & 3){ case 0: //LSL val <<= shift; break; case 1: //LSR val = shift ? (val >> shift) : 0; break; case 2: //ASR val = shift ? (UInt32)((((Int32)val) >> shift)) : ((val & 0x80000000UL) ? 0xFFFFFFFFUL : 0x00000000UL); break; case 3: //ROR/RRX if(shift){ val = cpuPrvROR(val, shift); } else{ //RRX val = val >> 1; if(cpu->CPSR & ARM_SR_C) val |= 0x80000000UL; } } } if(!(instr & 0x00400000UL)) reg |= ARM_MODE_2_WORD; if(instr & 0x00100000UL) reg |= ARM_MODE_2_LOAD; if(!(instr & 0x00800000UL)) val = -val; if(!(instr & 0x01000000UL)){ *addBeforeP = 0; *addWritebackP = val; if(instr & 0x00200000UL) reg |= ARM_MODE_2_T; } else if(instr & 0x00200000UL){ *addBeforeP = val; *addWritebackP = val; } else{ *addBeforeP = val; *addWritebackP = 0; } return reg; } /* same comments as for addr mode 2 apply #define ARM_MODE_3_REG 0x0F //flag for actual reg number used #define ARM_MODE_3_TYPE 0x30 //flag for the below 4 types #define ARM_MODE_3_H 0x00 #define ARM_MODE_3_SH 0x10 #define ARM_MODE_3_SB 0x20 #define ARM_MODE_3_D 0x30 #define ARM_MODE_3_LOAD 0x40 #define ARM_MODE_3_INV 0x80 */ static _INLINE_ UInt8 cpuPrvArmAdrMode_3(ArmCpu* cpu, UInt32 instr, UInt32* addBeforeP, UInt32* addWritebackP, Boolean wasT, Boolean specialPC){ UInt32 val; UInt8 reg; Boolean S, H, L; reg = (instr >> 16) & 0x0F; if(instr & 0x00400000UL){ //immediate val = ((instr >> 4) & 0xF0) | (instr & 0x0F); } else{ if(instr & 0x00000F00UL) reg |= ARM_MODE_3_INV; //bits 8-11 must be 1 always val = cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC); } L = (instr & 0x00100000UL) != 0; H = (instr & 0x00000020UL) != 0; S = (instr & 0x00000040UL) != 0; if(S && H){ reg |= ARM_MODE_3_SH; } else if(S){ reg |= ARM_MODE_3_SB; } else if(H){ reg |= ARM_MODE_3_H; } else{ reg |= ARM_MODE_3_INV; //S == 0 && H == 0 is invalid mode 3 operation } if((instr & 0x00000090UL) != 0x00000090UL) reg |= ARM_MODE_3_INV; //bits 4 and 7 must be 1 always if(S && !L){ //LDRD/STRD is encoded thusly reg = (reg &~ ARM_MODE_3_TYPE) | ARM_MODE_3_D; L = !H; } if(L) reg |= ARM_MODE_3_LOAD; if(!(instr & 0x00800000UL)) val = -val; if(!(instr & 0x01000000UL)){ *addBeforeP = 0; *addWritebackP = val; if(instr & 0x00200000UL) reg |= ARM_MODE_3_INV; //W must be 0 in this case, else unpredictable (in this case - invalid instr) } else if(instr & 0x00200000UL){ *addBeforeP = val; *addWritebackP = val; } else{ *addBeforeP = val; *addWritebackP = 0; } return reg; } /* #define ARM_MODE_4_REG 0x0F #define ARM_MODE_4_INC 0x10 //incr or decr #define ARM_MODE_4_BFR 0x20 //after or before #define ARM_MODE_4_WBK 0x40 //writeback? #define ARM_MODE_4_S 0x80 //S bit set? */ static UInt8 cpuPrvArmAdrMode_4(_UNUSED_ ArmCpu* cpu, UInt32 instr, UInt16* regs){ UInt8 reg; *regs = instr; reg = (instr >> 16) & 0x0F; if(instr & 0x00400000UL) reg |= ARM_MODE_4_S; if(instr & 0x00200000UL) reg |= ARM_MODE_4_WBK; if(instr & 0x00800000UL) reg |= ARM_MODE_4_INC; if(instr & 0x01000000UL) reg |= ARM_MODE_4_BFR; return reg; } /* #define ARM_MODE_5_REG 0x0F #define ARM_MODE_5_ADD_BEFORE 0x10 //add value before? #define ARM_MODE_5_ADD_AFTER 0x20 //add value after? #define ARM_MODE_5_IS_OPTION 0x40 //is value option (as opposed to offset) #define ARM_MODE_5_RR 0x80 */ static _INLINE_ UInt8 cpuPrvArmAdrMode_5(_UNUSED_ ArmCpu* cpu, UInt32 instr, UInt32* valP){ UInt32 val; UInt8 reg; val = instr & 0xFF; reg = (instr >> 16) & 0x0F; if(!(instr & 0x01000000UL)){ //unindexed or postindexed if(instr & 0x00200000UL){ //postindexed reg |= ARM_MODE_5_ADD_AFTER; } else{ //unindexed if(!(instr & 0x00800000UL)) reg |= ARM_MODE_5_RR; //U must be 1 for unindexed, else it is MCRR/MRCC reg |= ARM_MODE_5_IS_OPTION; } } else{ //offset or preindexed reg |= ARM_MODE_5_ADD_BEFORE; if(instr & 0x00200000UL){ //preindexed reg |= ARM_MODE_5_ADD_AFTER; } } if(!(reg & ARM_MODE_5_IS_OPTION)){ val = val << 2; if(!(instr & 0x00800000UL)) val = -val; } *valP = val; return reg; } static _INLINE_ void cpuPrvSetPSR(ArmCpu* cpu, UInt8 mask, Boolean privileged, Boolean R, UInt32 val){ if(R){ //setting SPSR in sys or usr mode is no harm since they arent used, so just do it without any checks cpu->SPSR = val; } else{ UInt32 newCPSR = cpu->CPSR; if(privileged){ if(mask & 1){ cpuPrvSwitchToMode(cpu, val & ARM_SR_M); newCPSR = (newCPSR & 0xFFFFFF00UL) | (val & 0x000000FFUL); } if(mask & 2) newCPSR = (newCPSR & 0xFFFF00FFUL) | (val & 0x0000FF00UL); if(mask & 4) newCPSR = (newCPSR & 0xFF00FFFFUL) | (val & 0x00FF0000UL); } if(mask & 8) newCPSR = (newCPSR & 0x00FFFFFFUL) | (val & 0xFF000000UL); cpu->CPSR = newCPSR; } } static _INLINE_ Boolean cpuPrvSignedAdditionOverflows(UInt32 a, UInt32 b, UInt32 sum){ return ((a ^ b ^ 0x80000000UL) & (a ^ sum)) >> 31; } static _INLINE_ Boolean cpuPrvSignedSubtractionOverflows(UInt32 a, UInt32 b, UInt32 diff){ //diff = a - b return ((a ^ b) & (a ^ diff)) >> 31; } static _INLINE_ UInt32 cpuPrvMedia_signedSaturate32(UInt32 sign){ return (sign & 0x80000000UL) ? 0xFFFFFFFFUL : 0; } #ifdef ARM_V6 static _INLINE_ UInt32 cpuPrvMedia_sxtb(UInt8 v){ UInt32 r = v; if(v & 0x80) r |= 0xFFFFFF00UL; return r; } static _INLINE_ UInt32 cpuPrvMedia_uxtb(UInt8 v){ UInt32 r = v; return r; } static _INLINE_ UInt32 cpuPrvMedia_sxth(UInt16 v){ UInt32 r = v; if(v & 0x8000UL) r |= 0xFFFF0000UL; return r; } static _INLINE_ UInt32 cpuPrvMedia_uxth(UInt16 v){ UInt32 r = v; return r; } static _INLINE_ UInt32 cpuPrvMedia_sxt16(UInt32 v){ if(v & 0x00800000UL){ v |= 0xFF000000UL; } else{ v &=~ 0xFF000000UL; } if(v & 0x00000080UL){ v |= 0x0000FF00UL; } else{ v &=~ 0x0000FF00UL; } return v; } static _INLINE_ UInt32 cpuPrvMedia_uxt16(UInt32 v){ v &=~ 0xFF00FF00UL; return v; } static _INLINE_ UInt32 cpuPrvMedia_dualAdd(UInt32 a, UInt32 b){ //3 ands, 2 ads, 1 sub UInt32 sum = a + b; sum -= (((a & 0xFFFFUL) + (b & 0xFFFFUL)) & 0xFFFF0000UL); //think about it...it should work... return sum; } static _INLINE_ UInt32 cpuPrvMedia_SignedSat(UInt32 val, UInt8 from_bits, UInt8 to_bits){ //returns value. [orred with 0x80000000UL to indicate sat was done] UInt32 check_mask, top_in; if(from_bits <= to_bits) return val; //no saturation needed top_in = (1UL << (from_bits - 1)); check_mask = (1UL << to_bits) - (1UL << from_bits); if(val & top_in){ //input was negative if((val & check_mask) != check_mask){ //saturate val = top_in | 0x80000000UL; } } else{ if(val & check_mask){ //saturate val = (top_in - 1) | 0x80000000UL; } } return val; } static _INLINE_ Boolean cpuPrvMediaInstrs(ArmCpu* cpu, UInt32 instr, Boolean wasT, Boolean specialPC){ UInt64 v64; UInt32 v32a, v32b, v32c, v32d; Boolean vB = false; UInt8 v8a, v8b; UInt16 v16; switch((instr >> 20) & 0x1F){ case 0: //parrallel addition/subtraction signed case 1: case 2: case 3: //TODO break; case 4: //parrallel addition/subtraction unsigned case 5: case 6: case 7: //TODO break; case 8: //packing/unpacking/saturation/reversal case 9: case 10: case 11: case 12: case 13: case 14: case 15: v8a = (instr >> 16) & 0x0F; v8b = (instr >> 5) & 7; switch((instr >> 20) & 7){ case 0: switch(v8b){ case 0: //PKH case 2: case 4: case 6: v32a = cpuPrvGetReg(cpu, (instr >> 16) & 0x0F, wasT, specialPC); //Rn v32b = cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC); //Rm v8a = (instr >> 7) & 0x1F; if(instr & 0x00000040UL){ //ASR if(!v8a) v8a = 32; v32b = (((Int32)v32b) >> v8a); v32c = (v32b & 0xFFFFUL) | (v32a & 0xFFFF0000UL); } else{ //LSL v32b <<= v8a; v32c = (v32a & 0xFFFFUL) | (v32b & 0xFFFF0000UL); } cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, v32c); break; case 3: //SXTAB16 SXTB16 vB = true; //sign extend goto xt16; case 5: //SEL v32a = cpuPrvGetReg(cpu, (instr >> 16) & 0x0F, wasT, specialPC); //Rn v32b = cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC); //Rm v32c = 0; if(cpu->CPSR & ARM_SR_GE_0) v32c |= 0x000000FFUL; if(cpu->CPSR & ARM_SR_GE_1) v32c |= 0x0000FF00UL; if(cpu->CPSR & ARM_SR_GE_2) v32c |= 0x00FF0000UL; if(cpu->CPSR & ARM_SR_GE_2) v32c |= 0xFF000000UL; v32c = (v32a & v32c) | (v32b & ~v32c); cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, v32c); break; default: return false; } break; case 2: switch(v8b){ case 1: //SSAT16 //TODO; break; case 3: //SXTAB SXTB vB = true; //sign extend goto xtb; case 0: case 2: case 4: case 6: goto ssat; break; default: return false; } break; case 3: switch(v8b){ case 1: //REV //TODO; break; case 3: //SXTAH SXTH vB = true; //sign extend goto xth; case 5: //REV16 //TOOD; break; case 0: //SSAT case 2: case 4: case 6: ssat: //TODO break; default: return false; } break; case 4: //UXTAB16 UXTB16 if(v8b != 3) return false; vB = false; //not sign extend xt16: v32a = (v8a == 15) ? 0 : cpuPrvGetReg(cpu, v8a, wasT, specialPC); //Rn v32b = cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC); //Rm v32b = cpuPrvROR(v32b, (instr >> 7) & 0x18); v32b = vB ? cpuPrvMedia_sxt16(v32b) : cpuPrvMedia_uxt16(v32b); v32c = cpuPrvMedia_dualAdd(v32a, v32b); cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, v32a); break; case 6: switch(v8b){ case 1: //USAT16 //TODO; break; case 3: //UXTAB UXTB vB = false; //not sign extend xtb: v32a = (v8a == 15) ? 0 : cpuPrvGetReg(cpu, v8a, wasT, specialPC); //Rn v32b = cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC) //Rm v32b = cpuPrvROR(v32b, (instr >> 7) & 0x18); v32b = vB ? cpuPrvMedia_sxtb(v32b) : cpuPrvMedia_uxtb(v32b); v32a += v32b; cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, v32a); break; case 0: case 2: case 4: case 6: goto usat; default: return false; } break; case 7: switch(v8b){ case 1: //RBIT v32a = cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC); v32b = 0; v32c = 0x80000000UL; v32d = 0x00000001UL; //faster ways exist, but this is smaller code for(v8a = 0; v8a < 32; v8a++, v32c >>= 1, v32d <<= 1) if(v32a & v32c) v32b |= v32d; cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, v32b); break; case 3: //UXTAH UXTH vB = false; //not sign extend xth: v32a = (v8a == 15) ? 0 : cpuPrvGetReg(cpu, v8a, wasT, specialPC); //Rn v32b = cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC) //Rm v32b = cpuPrvROR(v32b, (instr >> 7) & 0x18); v32b = vB ? cpuPrvMedia_sxth(v32b) : cpuPrvMedia_uxth(v32b); v32a += v32b; cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, v32a); break; case 5: //REVSH //TODO; break; case 0: //USAT case 2: case 4: case 6: usat: //TODO; break; default: return false; } break; default: return false; } break; case 16: //signed multiplies case 17: case 18: case 19: case 20: case 21: case 22: case 23: v8a = (instr >> 12) & 0x0F; if((instr & 0x00700000UL) == 0x00000000){ //SMLAD, SMUAD, SMLSD, SMUSD v32a = cpuPrvGetReg(cpu, (instr >> 8) & 0x0F, wasT, specialPC); //Rm v32b = cpuPrvGetReg(cpu, (instr >> 0) & 0x0F, wasT, specialPC); //Rn if(instr & 0x00000020UL) v32a = cpuPrvROR(v32a, 16); v32c = (Int32)((Int16)(v32a & 0xFFFFUL)) * (Int32)((Int16)(v32b & 0xFFFFUL)); v32a = (Int32)((Int16)(v32a >> 16)) * (Int32)((Int16)(v32b >> 16)); v32b = (v8a == 15) ? 0 : cpuPrvGetReg(cpu, v8a, wasT, specialPC); if((instr & 0x000000C0UL) == 0x00000000){ //SMLAD, SMUAD //now add them and mark Q flag is we overflow v32d = v32a + v32c; vB = cpuPrvSignedAdditionOverflows(v32a, v32c, v32d); } else if((instr & 0x000000C0UL) == 0x00000040UL){ //SMLSD, SMUSD v32d = v32a - v32c; vB = cpuPrvSignedSubtractionOverflows(v32a, v32c, v32d); } else return false; v32a = v32d + v32b; vB = vB || cpuPrvSignedAdditionOverflows(v32d, v32b, v32a); cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, v32a); } else if((instr & 0x00700000UL) == 0x00400000UL){ //SMLALD, SMLSLD v64 = u64_from_halves(cpuPrvGetReg(cpu, (instr >> 16) & 0x0F, wasT, specialPC), cpuPrvGetReg(cpu, (instr >> 12) & 0x0F, wasT, specialPC)); v32a = cpuPrvGetReg(cpu, (instr >> 8) & 0x0F, wasT, specialPC); //Rm v32b = cpuPrvGetReg(cpu, (instr >> 0) & 0x0F, wasT, specialPC); //Rn if(instr & 0x00000020UL) v32a = cpuPrvROR(v32a, 16); v32c = (Int32)((Int16)(v32a & 0xFFFFUL)) * (Int32)((Int16)(v32b & 0xFFFFUL)); v32a = (Int32)((Int16)(v32a >> 16)) * (Int32)((Int16)(v32b >> 16)); if((instr & 0x000000C0UL) == 0x00000000){ //SMLALD v64 = u64_add(v64, u64_add(u64_xtnd32(u64_32_to_64(v32a)), u64_xtnd32(u64_32_to_64(v32c)))); } else if((instr & 0x000000C0UL) == 0x00000040){ //SMLSLD v64 = u64_add(v64, u64_sub(u64_xtnd32(u64_32_to_64(v32a)), u64_xtnd32(u64_32_to_64(v32c)))); } else return false; cpuPrvSetReg(cpu, (instr >> 16) & 0x0F, u64_get_hi(v64)); cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, u64_64_to_32(v64)); } else if((instr & 0x00700000UL) == 0x00500000UL){ //SMMLA, SMMUL, SMMLS v32a = cpuPrvGetReg(cpu, (instr >> 8) & 0x0F, wasT, specialPC); //Rm v32b = cpuPrvGetReg(cpu, (instr >> 0) & 0x0F, wasT, specialPC); //Rn v32c = cpuPrvGetReg(cpu, (instr >> 12) & 0x0F, wasT, specialPC); //Ra v64 = u64_smul3232(v32a, v32b); if((instr & 0x000000C0UL) == 0x00000000){ //SMMLA, SMMUL if(v8a == 15){ //SMMUL //nothing to do here } else{ //SMMLA v64 = u64_add(v64, u64_shl(u64_32_to_64(v32c), 32)); } } else if((instr & 0x000000C0UL) == 0x000000C0){ //SMMLS v64 = u64_sub(v64, u64_shl(u64_32_to_64(v32c), 32)); } else return false; if(instr & 0x00000010UL) v64 = u64_add(v64, u64_32_to_64(0x80000000UL)); //round if requested cpuPrvSetReg(cpu, (instr >> 16) & 0x0F, u64_get_hi(v64)); } else return false; if(vB){ //overflow cpu->CPSR |= ARM_SR_Q; } break; case 24: //USAD8 USADA8 if(instr & 0x000000E0UL) return false; v32a = ((instr & 0x0000F000UL) == 0x0000F000UL) ? 0 : cpuPrvGetReg(cpu, (instr >> 12) & 0x0F, wasT, specialPC); //Ra v32b = cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC); //Rn v32c = cpuPrvGetReg(cpu, (instr >> 8) & 0x0F, wasT, specialPC); //Rm for(v8a = 0; v8a < 4; v8a++){ v16 = (v32b & 0xFF) - (v32c & 0xFF); if(v16 & 0x8000U) v16 = -v16; v32a += v16; v32b >>= 8; v32c >>= 8; } cpuPrvSetReg(cpu, (instr >> 16) & 0x0F, v32a); break; case 26: case 27: vB = true; //sign extend goto bitfield_extract; case 30: case 31: vB = false; //do not sign extend bitfield_extract: if((instr & 0x00000060UL) == 0x00000040){ //unsigned bitfield extract v32a = cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC); v8a = ((instr >> 16) & 0x1F) + 1; v8b = (instr >> 7) & 0x1F; v32a >>= v8b; v32c = 0xFFFFFFFFUL << v8a; v32a &=~ v32c; if(vB){ //sign extend if(v32a & (1UL << (v8a - 1))) v32a |= v32c; } cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, v32a); } else return false; break; case 28: case 29: if(instr & 0x00000060UL){ return false; } else{ //BFC BFI v8a = 31 - ((instr >> 16) & 0x1F); v8b = (instr >> 7) & 0x1F; v32a = 0xFFFFFFFFUL; v32a >>= v8b; v32a <<= v8b + v8a; v32a >>= v8a; v32b = cpuPrvGetReg(cpu, (instr >> 12) & 0x0F, wasT, specialPC); v32b &= ~v32a; if((instr & 0x0000000FUL) == 0x0000000F){ //BFC //bits already clear } else{ //BFI v32c = cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC); v32c >>= v8b; v32c <<= v8b + v8a; v32c >>= v8a; v32b |= v32c; } cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, v32b); } break; default: return false; } return true; } #endif static Err cpuPrvExecInstr(ArmCpu* cpu, UInt32 instr, UInt32 instrPC/* lower bit always clear */, Boolean wasT , Boolean privileged, Boolean specialPC/* for thumb*/){ Boolean specialInstr = false, usesUsrRegs, execute = false, L, ok; UInt8 fsr; usesUsrRegs = ((cpu->CPSR & ARM_SR_M) == ARM_SR_MODE_USR) || ((cpu->CPSR & ARM_SR_M) == ARM_SR_MODE_SYS); //check condition code { switch(instr >> 29UL){ case 0: //EQ / NE execute = (cpu->CPSR & ARM_SR_Z) != 0; break; case 1: //CS / CC execute = (cpu->CPSR & ARM_SR_C) != 0; break; case 2: //MI/PL execute = (cpu->CPSR & ARM_SR_N) != 0; break; case 3: //VS/VC execute = (cpu->CPSR & ARM_SR_V) != 0; break; case 4: //HI/LS execute = (cpu->CPSR & ARM_SR_C) && !(cpu->CPSR & ARM_SR_Z); break; case 5: //GE/LT execute = !(cpu->CPSR & ARM_SR_N) == !(cpu->CPSR & ARM_SR_V); //check for N == V break; case 6: //GT/LE execute = !(cpu->CPSR & ARM_SR_N) == !(cpu->CPSR & ARM_SR_V); //check for N == V execute = execute && !(cpu->CPSR & ARM_SR_Z); //enforce Z==0 break; case 7: specialInstr = (instr & 0x10000000UL) != 0; execute = true; break; } if((instr & 0x10000000UL) && !specialInstr) execute = !execute; //invert for inverted conditions } //execute, if needed if(execute){ UInt64 v64; register UInt32 adr, tmp, v32; UInt32 m32, x32; //non-register 32-bit val UInt16 v16; UInt8 va8, vb8 = 0, vc8; switch((instr >> 24) & 0x0F){ case 0: case 1: //data processing immediate shift, register shift and misc instrs and mults if(specialInstr) goto invalid_instr; if((instr & 0x00000090UL) == 0x00000090){ //multiplies, extra load/stores (table 3.2) if((instr & 0x00000060UL) == 0x00000000){ //swp[b], mult(acc), mult(acc) long if(instr & 0x01000000UL){ //SWB/SWPB switch((instr >> 20) & 0x0F){ case 0: //SWP adr = cpuPrvGetReg(cpu, (instr >> 16) & 0x0F, wasT, specialPC); ok = cpu->memF(cpu, &m32, adr, 4, false, privileged, &fsr); if(!ok){ cpuPrvHandleMemErr(cpu, adr, 4, false, false, fsr); goto instr_done; } tmp = m32; m32 = cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC); ok = cpu->memF(cpu, &m32, adr, 4, true, privileged, &fsr); if(!ok){ cpuPrvHandleMemErr(cpu, adr, 4, true, false, fsr); goto instr_done; } cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, tmp); break; case 4: //SWPB adr = cpuPrvGetReg(cpu, (instr >> 16) & 0x0F, wasT, specialPC); ok = cpu->memF(cpu, &vc8, adr, 1, false, privileged, &fsr); if(!ok){ cpuPrvHandleMemErr(cpu, adr, 1, false, false, fsr); goto instr_done; } va8 = vc8; vc8 = cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC); ok = cpu->memF(cpu, &vc8, adr, 1, true, privileged, &fsr); if(!ok){ cpuPrvHandleMemErr(cpu, adr, 1, true, false, fsr); goto instr_done; } cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, va8); break; #ifdef ARM_V6 /* about exclusives: these are for SMP. we do not have SMP, so for now they always succeed */ case 8: //STREX if((instr & 0x00000F00UL) != 0x00000F00UL) goto invalid_instr; adr = cpuPrvGetReg(cpu, (instr >> 16) & 0x0F, wasT, specialPC); tmp = cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC); ok = cpu->memF(cpu, &tmp, adr, 4, true, privileged, &fsr); if(!ok){ cpuPrvHandleMemErr(cpu, adr, 4, true, false, fsr); goto instr_done; } cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, 0); //0 -> success break; case 9: //LDREX if((instr & 0x00000F0FUL) != 0x00000F0FUL) goto invalid_instr; adr = cpuPrvGetReg(cpu, (instr >> 16) & 0x0F, wasT, specialPC); ok = cpu->memF(cpu, &tmp, adr, 4, false, privileged, &fsr); if(!ok){ cpuPrvHandleMemErr(cpu, adr, 4, false, false, fsr); goto instr_done; } cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, tmp); break; /* these are a v7/v6K thing - we do not bother case 10: //STREXD //TODO case 11: //LDREXD //TODO case 12: //STREXB //TODO case 13: //LDREXB //TODO case 14: //STREXH //TODO case 15: //LDREXH //TODO */ #endif default: goto invalid_instr; } } else switch((instr >> 20) & 0x0F){ //multiplies case 0: //MUL case 1: tmp = 0; if(instr & 0x0000F000UL) goto invalid_instr; goto mul32; case 2: //MLA case 3: tmp = cpuPrvGetReg(cpu, (instr >> 12 ) & 0x0F, wasT, specialPC); mul32: tmp += cpuPrvGetReg(cpu, (instr >> 8) & 0x0F, wasT, specialPC) * cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC); cpuPrvSetReg(cpu, (instr >> 16) & 0x0F, tmp); if(instr & 0x00100000UL){ //S adr = cpu->CPSR &~ (ARM_SR_Z | ARM_SR_N); if(!tmp) adr |= ARM_SR_Z; if(tmp & 0x80000000UL) adr |= ARM_SR_N; cpu->CPSR = adr; } goto instr_done; #ifdef ARM_V6 case 4: //UMAAL v64 = u64_32_to_64((instr >> 12) & 0x0F); v64 = u64_add32(v64, (instr >> 16) & 0x0F); goto mul64; case 6: //MLS tmp = cpuPrvGetReg(cpu, (instr >> 8) & 0x0F, wasT, specialPC); tmp *= cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC); tmp = cpuPrvGetReg(cpu, (instr >> 12) & 0x0F, wasT, specialPC) - tmp; cpuPrvSetReg(cpu, (instr >> 16) & 0x0F, tmp); break; #endif case 8: //UMULL case 9: case 12: //SMULL case 13: v64 = u64_zero(); goto mul64; case 10: //UMLAL case 11: case 14: //SMLAL case 15: v64 = u64_from_halves(cpuPrvGetReg(cpu, (instr >> 16) & 0x0F, wasT, specialPC), cpuPrvGetReg(cpu, (instr >> 12) & 0x0F, wasT, specialPC)); mul64: adr = cpuPrvGetReg(cpu, (instr >> 8) & 0x0F, wasT, specialPC); tmp = cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC); v64 = u64_add(v64, (instr & 0x00400000UL) ? u64_smul3232(adr, tmp) : u64_umul3232(adr, tmp)); v32 = u64_get_hi(v64); cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, u64_64_to_32(v64)); cpuPrvSetReg(cpu, (instr >> 16) & 0x0F, v32); if(instr & 0x00100000UL){ //S adr = cpu->CPSR &~ (ARM_SR_Z | ARM_SR_N); if(u64_isZero(v64)) adr |= ARM_SR_Z; if(v32 & 0x80000000UL) adr |= ARM_SR_N; cpu->CPSR = adr; } break; default: goto invalid_instr; } } else{ //load/store signed/unsigned byte/halfword/two_words UInt32 store[2] = {0,0}; UInt8* store8 = (UInt8*)store; UInt16* store16 = (UInt16*)store; va8 = cpuPrvArmAdrMode_3(cpu, instr, &m32, &x32, wasT, specialPC); tmp = m32; v32 = x32; if(va8 & ARM_MODE_3_INV ) goto invalid_instr; adr = cpuPrvGetReg(cpu, va8 & ARM_MODE_3_REG, wasT, specialPC); switch(va8 & ARM_MODE_3_TYPE){ case ARM_MODE_3_H: case ARM_MODE_3_SH: vb8 = 2; break; case ARM_MODE_3_SB: vb8 = 1; break; case ARM_MODE_3_D: vb8 = 8; break; } if(va8 & ARM_MODE_3_LOAD){ ok = cpu->memF(cpu, store, adr + tmp, vb8, false, privileged, &fsr); if(!ok){ cpuPrvHandleMemErr(cpu, adr + tmp, vb8, false, false, fsr); goto instr_done; } if(vb8 == 1){ tmp = *store8; if(tmp & 0x80) tmp |= 0xFFFFFF00UL; //sign-extend cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, tmp); } else if(vb8 == 2){ tmp = *store16; if(((va8 & ARM_MODE_3_TYPE) == ARM_MODE_3_SH) && (tmp & 0x8000UL)) tmp |= 0xFFFF0000UL; cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, tmp); } else{ cpuPrvSetReg(cpu, ((instr >> 12) & 0x0F) + 0, store[0]); cpuPrvSetReg(cpu, ((instr >> 12) & 0x0F) + 1, store[1]); } if(v32) cpuPrvSetReg(cpu, va8 & ARM_MODE_3_REG, v32 + adr); } else{ if(v32){ v32 += adr; va8 |= ARM_MODE_3_INV; //re-use flag to mean writeback } if(vb8 == 1){ *store8 = cpuPrvGetReg(cpu, (instr >> 12) & 0x0F, wasT, specialPC); } else if(vb8 == 2){ *store16 = cpuPrvGetReg(cpu, (instr >> 12) & 0x0F, wasT, specialPC); } else{ store[0] = cpuPrvGetReg(cpu, ((instr >> 12) & 0x0F) + 0, wasT, specialPC); store[1] = cpuPrvGetReg(cpu, ((instr >> 12) & 0x0F) + 1, wasT, specialPC); } adr += tmp; ok = cpu->memF(cpu, store, adr, vb8, true, privileged, &fsr); if(!ok){ cpuPrvHandleMemErr(cpu, adr, vb8, true, false, fsr); goto instr_done; } if(va8 & ARM_MODE_3_INV) cpuPrvSetReg(cpu, va8 & ARM_MODE_3_REG, v32); } } goto instr_done; } else if((instr & 0x01900000UL) == 0x01000000UL){ //misc instrs (table 3.3) tmp = (instr >> 4) & 0x0F; switch(tmp){ case 0: //move reg to PSR or move PSR to reg if((instr & 0x00BF0FFFUL) == 0x000F0000UL){ //move PSR to reg cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, (instr & 0x00400000UL) ? cpu->SPSR : cpu->CPSR); //access in user and sys mode is undefined. for us that means returning garbage that is currently in "cpu->SPSR" } else if((instr & 0x00B0FFF0UL) == 0x0020F000UL){ //move reg to PSR cpuPrvSetPSR(cpu, (instr >> 16) & 0x0F, privileged, (instr & 0x00400000UL) !=0, cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC)); } else goto invalid_instr; goto instr_done; case 1: //BLX/BX/BXJ or CLZ case 3: if(instr & 0x00400000UL){ //CLZ if((instr & 0x0FFF0FF0UL) != 0x016F0F10UL) goto invalid_instr; tmp = cpuPrvGetReg(cpu, instr &0xF, wasT, specialPC); adr = 0x80000000UL; for(va8 = 0; va8 < 32; va8++, adr >>= 1) if(tmp & adr) break; cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, va8); } else{ //BL / BLX / BXJ if((instr & 0x0FFFFF00UL) != 0x012FFF00UL) goto invalid_instr; if((instr & 0x00000030UL) == 0x00000030UL) cpuPrvSetReg(cpu, 14, instrPC + (wasT ? 3 : 4)); //save return value for BLX cpuPrvSetPC(cpu, cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC)); } goto instr_done; case 5: //enhanced DSP adds/subtracts if(instr & 0x00000F00UL) goto invalid_instr; tmp = cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC); //Rm adr = cpuPrvGetReg(cpu, (instr >> 16) & 0x0F, wasT, specialPC); //Rn vb8 = 0; //used as boolead for if saturation happened switch((instr >> 21) & 3){ //what op? case 0: //QADD v32 = tmp + adr; vb8 = cpuPrvSignedAdditionOverflows(adr, tmp, v32); if(vb8) v32 = cpuPrvMedia_signedSaturate32(adr); break; case 1: //QSUB v32 = tmp - adr; vb8 = cpuPrvSignedAdditionOverflows(tmp, adr, v32); if(vb8) v32 = cpuPrvMedia_signedSaturate32(tmp); break; case 2: //QDADD v32 = adr << 1; vb8 = cpuPrvSignedAdditionOverflows(adr, tmp, v32); if(vb8) v32 = cpuPrvMedia_signedSaturate32(adr); adr = v32; v32 = tmp + adr; if(cpuPrvSignedAdditionOverflows(adr, tmp, v32)){ vb8 = 1; v32 = cpuPrvMedia_signedSaturate32(adr); } break; case 3: //QDSUB v32 = adr << 1; vb8 = cpuPrvSignedAdditionOverflows(adr, tmp, v32); if(vb8) v32 = cpuPrvMedia_signedSaturate32(adr); adr = v32; v32 = tmp + adr; if(cpuPrvSignedAdditionOverflows(adr, tmp, v32)){ vb8 = 1; v32 = cpuPrvMedia_signedSaturate32(adr); } break; default: v32 = 0; //make compiler happy; break; } cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, v32); //save result if(vb8) cpu->CPSR |= ARM_SR_Q; goto instr_done; case 7: //soft breakpoint cpuPrvException(cpu, cpu->vectorBase + ARM_VECTOR_OFFT_P_ABT, instrPC + 4, ARM_CPSR_PAB_ORR | (cpu->CPSR & ARM_CPSR_PAB_ORR)); goto instr_done; case 8: //enhanced DSP multiplies case 9: case 10: case 11: case 12: case 13: case 14: case 15: if((instr & 0x00000090UL) != 0x00000080UL) goto invalid_instr; tmp = cpuPrvGetReg(cpu, instr & 0x0F, wasT, specialPC); //Rm adr = cpuPrvGetReg(cpu, (instr >> 8) & 0x0F, wasT, specialPC); //Rs vb8 = 0; //used as boolead for if saturation happened va8 = 0; //used as temporary boolean flag switch((instr >> 21) & 3){ //what op? case 0: //SMLAxy if(instr & 0x20) tmp >>= 16; else tmp &= 0xFFFFUL; if(tmp & 0x8000UL) tmp |= 0xFFFF0000UL; if(instr & 0x40) adr >>= 16; else adr &= 0xFFFFUL; if(adr & 0x8000UL) adr |= 0xFFFF0000UL; tmp *= adr; adr = cpuPrvGetReg(cpu, (instr >> 12) & 0x0F, wasT, specialPC); //Rn v32 = tmp + adr; vb8 = cpuPrvSignedAdditionOverflows(adr, tmp, v32); break; case 1: //SMLAWy/SMULWy if(instr & 0x40) adr >>= 16; else adr &= 0xFFFFUL; if(adr & 0x8000UL) adr |= 0xFFFF0000UL; adr = u64_64_to_32(u64_shr(u64_smul3232(tmp, adr), 16)); //do the multiplication, WAS: adr = (((UInt64)tmp) * ((UInt64)adr)) >> 16; if(instr & 0x20){ //SMULWy v32 = adr; if(instr & 0x0000F000UL) goto invalid_instr; } else{ //SMLAWy tmp = cpuPrvGetReg(cpu, (instr >> 12) & 0x0F, wasT, specialPC); //Rn v32 = adr + tmp; vb8 = cpuPrvSignedAdditionOverflows(adr, tmp, v32); } break; case 2: //SMLALxy if(instr & 0x20) tmp >>= 16; else tmp &= 0xFFFFUL; if(tmp & 0x8000UL) tmp |= 0xFFFF0000UL; if(instr & 0x40) adr >>= 16; else adr &= 0xFFFFUL; if(adr & 0x8000UL) adr |= 0xFFFF0000UL; adr *= tmp; if(adr & 0x80000000UL) va8 |= 1; //neg tmp = cpuPrvGetReg(cpu, (instr >> 12) & 0x0F, wasT, specialPC); //RdLo if((tmp + adr) < tmp) va8 |= 2; //carry cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, tmp); v32 = cpuPrvGetReg(cpu, (instr >> 16) & 0x0F, wasT, specialPC); //RdHi if(va8 & 2) v32++; if(va8 & 1) v32--; break; case 3: //SMULxy if(instr & 0x0000F000UL) goto invalid_instr; if(instr & 0x20) tmp >>= 16; else tmp &= 0xFFFFUL; if(tmp & 0x8000UL) tmp |= 0xFFFF0000UL; if(instr & 0x40) adr >>= 16; else adr &= 0xFFFFUL; if(adr & 0x8000UL) adr |= 0xFFFF0000UL; v32 = tmp * adr; break; default: //make compiler happy v32 = 0; break; } cpuPrvSetReg(cpu, (instr >> 16) & 0x0F, v32); if(vb8) cpu->CPSR |= ARM_SR_Q; goto instr_done; default: goto invalid_instr; } } goto data_processing; break; case 2: case 3: //data process immediate val, move imm to SR if(specialInstr) goto invalid_instr; if((instr & 0x01900000UL) == 0x01000000UL){ //all NON-data-processing instrs in this space are here if(instr & 0x00200000UL){ //MSR imm and hints if((instr & 0x00400000UL) || (instr & 0x000F0000UL)){ //move imm to PSR cpuPrvSetPSR(cpu, (instr >> 16) & 0x0F, privileged, (instr & 0x00400000UL) != 0, cpuPrvROR(instr & 0xFF, ((instr >> 8) & 0x0F) * 2)); } else{ #ifdef ARM_V6 if((instr & 0x000000F0UL) == 0x000000F0){ //debug hint err_str("DEBUG hint 0x"); err_hex(instr & 0x0F); err_str(" at 0x"); err_hex(instrPC); err_str("\r\n"); } else switch(instr){ case 0: //NOP break; case 1: //YIELD; break; case 2: //WFE break; case 3: //WFI break; case 4: //SEV break; } goto instr_done; //all hints are treated as valid and do nothing... #endif goto invalid_instr; } } else if(instr & 0x00400000UL){ //MOVT (high halfword 16-bit immediate load) adr = cpuPrvGetReg(cpu, (instr >> 12) & 0x0F, wasT, specialPC) & 0xFFFFUL; adr |= ((instr & 0xFFFUL) << 16) | ((instr & 0xF0000UL) << 12); cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, adr); } else{ //MOVW (16-bit immediate load) cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, (instr & 0xFFFUL) | ((instr >> 4) & 0xF000UL)); } goto instr_done; } data_processing: //data processing { Boolean carryOut, carryIn, V, S, store = true; S = (instr & 0x00100000UL) != 0; V = (cpu->CPSR & ARM_SR_V) != 0; carryIn = (cpu->CPSR & ARM_SR_C) != 0; tmp = cpuPrvArmAdrMode_1(cpu, instr, &carryOut, wasT, specialPC); va8 = (instr >> 16) & 0x0F; switch((instr >> 21) & 0x0F){ case 0: //AND tmp = cpuPrvGetReg(cpu, va8, wasT, specialPC) & tmp; break; case 1: //EOR tmp = cpuPrvGetReg(cpu, va8, wasT, specialPC) ^ tmp; break; case 2: //SUB adr = cpuPrvGetReg(cpu, va8, wasT, specialPC); v32 = adr - tmp; if(S) V = cpuPrvSignedSubtractionOverflows(adr, tmp, v32); if(S) carryOut = adr >= tmp; tmp = v32; break; case 3: //RSB adr = cpuPrvGetReg(cpu, va8, wasT, specialPC); v32 = tmp - adr; if(S) V = cpuPrvSignedSubtractionOverflows(tmp, adr, v32); if(S) carryOut = tmp >= adr; tmp = v32; break; case 4: //ADD adr = cpuPrvGetReg(cpu, va8, wasT, specialPC); v32 = adr + tmp; if(S) V = cpuPrvSignedAdditionOverflows(adr, tmp, v32); if(S) carryOut = v32 < adr; tmp = v32; break; case 5: //ADC adr = cpuPrvGetReg(cpu, va8, wasT, specialPC); if(carryIn){ v32 = adr + tmp + 1; if(S) carryOut = v32 <= adr; } else{ v32 = adr + tmp; if(S) carryOut = v32 < adr; } if(S) V = cpuPrvSignedAdditionOverflows(adr, tmp, v32); tmp = v32; break; case 6: //SBC adr = cpuPrvGetReg(cpu, va8, wasT, specialPC); if(carryIn){ v32 = adr - tmp; if(S) carryOut = adr >= tmp; } else{ v32 = adr - tmp - 1; if(S) carryOut = adr > tmp; } if(S) V = cpuPrvSignedSubtractionOverflows(adr, tmp, v32); tmp = v32; break; case 7: //RSC adr = cpuPrvGetReg(cpu, va8, wasT, specialPC); if(carryIn){ v32 = tmp - adr; if(S) carryOut = tmp >= adr; } else{ v32 = tmp - adr - 1; if(S) carryOut = tmp > adr; } if(S) V = cpuPrvSignedSubtractionOverflows(tmp, adr, v32); tmp = v32; break; case 8: //TST if(!S) goto invalid_instr; store = false; tmp = cpuPrvGetReg(cpu, va8, wasT, specialPC) & tmp; break; case 9: //TEQ if(!S) goto invalid_instr; store = false; tmp = cpuPrvGetReg(cpu, va8, wasT, specialPC) ^ tmp; break; case 10: //CMP if(!S) goto invalid_instr; store = false; adr = cpuPrvGetReg(cpu, va8, wasT, specialPC); V = cpuPrvSignedSubtractionOverflows(adr, tmp, adr - tmp); //((adr ^ tmp) & (adr ^ (adr - tmp))) >> 31; carryOut = adr >= tmp; tmp = adr - tmp; break; case 11: //CMN if(!S) goto invalid_instr; store = false; adr = cpuPrvGetReg(cpu, va8, wasT, specialPC); v32 = adr + tmp; V = cpuPrvSignedAdditionOverflows(adr, tmp, v32); carryOut = v32 < adr; tmp = v32; break; case 12: //ORR tmp = cpuPrvGetReg(cpu, va8, wasT, specialPC) | tmp; break; case 13: //MOV //tmp already equals tmp break; case 14: //BIC tmp = cpuPrvGetReg(cpu, va8, wasT, specialPC) & ~tmp; break; case 15: //MVN tmp = ~tmp; break; } vb8 = (instr >> 12) & 0x0F; if(S){ //update flags or restore CPSR if(!usesUsrRegs && vb8 == 15 && store){ UInt32 sr; sr = cpu->SPSR; cpuPrvSwitchToMode(cpu, sr & ARM_SR_M); cpu->CPSR = sr; cpu->regs[15] = tmp; //do it right here - if we let it use cpuPrvSetReg, it will check lower bit... store = false; } else{ adr = cpu->CPSR &~ (ARM_SR_Z | ARM_SR_N | ARM_SR_C | ARM_SR_V); if(!tmp) adr |= ARM_SR_Z; if(tmp & 0x80000000UL) adr |= ARM_SR_N; if(carryOut) adr |= ARM_SR_C; if(V) adr |= ARM_SR_V; cpu->CPSR = adr; } } if(store){ if(vb8 == 15){ cpuPrvSetReg(cpu, vb8, tmp &~ 1UL); cpu->CPSR &=~ ARM_SR_T; if(tmp & 1) cpu->CPSR |= ARM_SR_T; } else{ cpu->regs[vb8] = tmp; //not pc - no need for func call cpuPrvSetReg(cpu, vb8, tmp); } } goto instr_done; } break; case 4: case 5: //load/stor imm offset goto load_store_mode_2; break; case 6: case 7: //load/store reg offset if(instr & 0x00000010UL){ //media and undefined instrs #ifdef ARM_V6 if(cpuPrvMediaInstrs(cpu, instr, wasT, specialPC)) goto instr_done; #endif goto invalid_instr; } load_store_mode_2: if(specialInstr){ //handle PLD if((instr & 0x0D70F000UL) == 0x0550F000UL) goto instr_done; //PLD goto invalid_instr; } va8 = cpuPrvArmAdrMode_2(cpu, instr, &m32, &x32, wasT, specialPC); tmp = m32; v32 = x32; if(va8 & ARM_MODE_2_INV) goto invalid_instr; if(va8 & ARM_MODE_2_T) privileged = false; vb8 = (va8 & ARM_MODE_2_WORD) ? 4 : 1; //get operation size adr = cpuPrvGetReg(cpu, va8 & ARM_MODE_2_REG, wasT, specialPC); if(va8 & ARM_MODE_2_LOAD){ ok = cpu->memF(cpu, &m32, adr + tmp, vb8, false, privileged, &fsr); if(!ok){ cpuPrvHandleMemErr(cpu, adr + tmp, vb8, false, false, fsr); goto instr_done; } if(vb8 == 1) m32 = *(UInt8*)&m32; //endian-free way to make it a valid 8-bit value, if need be tmp = m32; cpuPrvSetReg(cpu, (instr >> 12) & 0x0F, tmp); if(v32) cpuPrvSetReg(cpu, va8 & ARM_MODE_2_REG, v32 + adr); } else{ if(v32){ v32 += adr; va8 |= ARM_MODE_2_INV; //re-use flag to mean writeack } adr += tmp; if(vb8 == 1){ m32 = 0; *(UInt8*)&m32 = cpuPrvGetReg(cpu, (instr >> 12) & 0x0F, wasT, specialPC); } else{ m32 = cpuPrvGetReg(cpu, (instr >> 12) & 0x0F, wasT, specialPC); } ok = cpu->memF(cpu, &m32, adr, vb8, true, privileged, &fsr); if(!ok){ cpuPrvHandleMemErr(cpu, adr, vb8, true, false, fsr); goto instr_done; } if(va8 & ARM_MODE_2_INV) cpuPrvSetReg(cpu, va8 & ARM_MODE_2_REG, v32); } goto instr_done; case 8: case 9: //load/store multiple if(specialInstr) goto invalid_instr; va8 = cpuPrvArmAdrMode_4(cpu, instr, &v16); if((va8 & ARM_MODE_4_S) && usesUsrRegs) goto invalid_instr; //no S mode please in modes with no baked regs //or SPSR L = (instr & 0x00100000UL) != 0; tmp = adr = cpuPrvGetReg(cpu, va8 & ARM_MODE_4_REG, wasT, specialPC); specialInstr = L && (va8 & ARM_MODE_4_S) && (v16 & 0x8000UL) && !usesUsrRegs; //specialInstr = "copyCPSR" for(vc8 = 0; vc8 < 16; vc8++){ vb8 = (va8 & ARM_MODE_4_INC) ? vc8 : 15 - vc8; if(v16 & (1UL << vb8)){ UInt32* reg = cpu->regs + vb8; if(L){ if(va8 & ARM_MODE_4_S){ //handle LDM(2) and LDM(3) if(v16 & 0x8000UL){ //handle LDM(3) /* nothing to do here, we did all we need above in line beginning with "specialInstr=" */ } else if(!usesUsrRegs){ //handle LDM(2) if(vb8 >= 8 && vb8 <= 12 && (cpu->CPSR & ARM_SR_M) == ARM_SR_MODE_FIQ){ //handle fiq/usr banked regs reg = cpu->extra_regs + vb8 - 8; } else if(vb8 == 13){ reg = &cpu->bank_usr.R13; } else if(vb8 == 14){ reg = &cpu->bank_usr.R14; } } } else if(vb8 == 15){ //handle LDM(1)'s use of PC /* nothing to do here - all is handled below */ } } else if(va8 & ARM_MODE_4_S){ //handle STM(2)'s access to user regs if(!usesUsrRegs){ if(vb8 >= 8 && vb8 <= 12 && (cpu->CPSR & ARM_SR_M) == ARM_SR_MODE_FIQ){ //handle fiq/usr banked regs reg = cpu->extra_regs + vb8 - 8; } else if(vb8 == 13){ reg = &cpu->bank_usr.R13; } else if(vb8 == 14){ reg = &cpu->bank_usr.R14; } } } if(va8 & ARM_MODE_4_BFR) adr += (va8 & ARM_MODE_4_INC) ? 4L : -4L; ok = cpu->memF(cpu, reg, adr, 4, !L, privileged, &fsr); if(!ok){ cpuPrvHandleMemErr(cpu, adr, 4, !L, false, fsr); if(v16 & (1UL << (va8 & ARM_MODE_4_REG))) cpuPrvSetReg(cpu, va8 & ARM_MODE_4_REG, tmp); goto instr_done; } if(!(va8 & ARM_MODE_4_BFR)) adr += (va8 & ARM_MODE_4_INC) ? 4L : -4L; } } if(va8 & ARM_MODE_4_WBK){ cpuPrvSetReg(cpu, va8 & ARM_MODE_4_REG, adr); } if(specialInstr){ //process LDM(3) SPSR->CPSR copy v32 = cpu->SPSR; cpuPrvSwitchToMode(cpu, v32 & ARM_SR_M); cpu->CPSR = v32; } else if((v16 & 0x8000U) && !(va8 & ARM_MODE_4_S)){ //we just loaded PC if(cpu->regs[15] & 1){ cpu->regs[15] &=~ 1UL; cpu->CPSR |= ARM_SR_T; } else{ cpu->CPSR &=~ ARM_SR_T; } } goto instr_done; case 10: case 11: //B/BL/BLX(if cond=0b1111) tmp = instr & 0x00FFFFFFUL; //get offset if(tmp & 0x00800000UL) tmp |= 0xFF000000UL; //sign extend tmp = tmp << (wasT ? 1 : 2); //shift left 2(ARM) or 1(thumb) tmp += instrPC + (wasT ? 4 : 8); //add where PC would point in an ARM if(specialInstr){ //handle BLX if(instr & 0x01000000UL) tmp += 2; cpu->regs[14] = instrPC + (wasT ? 2 : 4); if(!(cpu->CPSR & ARM_SR_T)) tmp |= 1UL; //set T flag if needed } else{ //not BLX -> differentiate between BL and B if(instr & 0x01000000UL) cpu->regs[14] = instrPC + (wasT ? 2 : 4); if(cpu->CPSR & ARM_SR_T) tmp |= 1UL; //keep T flag as needed } cpuPrvSetPC(cpu, tmp); goto instr_done; case 12: case 13: //coprocessor load/store and double register transfers va8 = cpuPrvArmAdrMode_5(cpu, instr, &m32); v32 = m32; vb8 = (instr >> 8) & 0x0F; if(vb8 >= 14){ //cp14 and cp15 are for priviledged users only if(!privileged) goto invalid_instr; } else if(!(cpu->CPAR & (1UL << vb8))) goto invalid_instr; //others are access-controlled by CPAR if(va8 & ARM_MODE_5_RR){ //handle MCRR, MRCC if(!cpu->coproc[vb8].twoRegF) goto invalid_instr; if(!cpu->coproc[vb8].twoRegF(cpu, cpu->coproc[vb8].userData, (instr & 0x00100000UL) != 0, (instr >> 4) & 0x0F,(instr >> 12) & 0x0F, (instr >> 16) & 0x0F, instr & 0x0F)) goto invalid_instr; } else{ vc8 = v32; tmp = adr = cpuPrvGetReg(cpu, va8 & ARM_MODE_5_REG, wasT, specialPC); tmp += v32; if(!cpu->coproc[vb8].memAccess) goto invalid_instr; if(!cpu->coproc[vb8].memAccess(cpu, cpu->coproc[vb8].userData, specialInstr, (instr & 0x00400000UL) !=0, !(instr & 0x00100000UL), (instr >> 12) & 0x0F, (va8 & ARM_MODE_5_ADD_BEFORE) ? tmp : adr, (va8 & ARM_MODE_5_IS_OPTION) ? &vc8 : NULL)) goto invalid_instr; if(va8 & ARM_MODE_5_ADD_AFTER) cpuPrvSetReg(cpu, va8 & ARM_MODE_5_REG, tmp); } goto instr_done; case 14: //coprocessor data processing and register transfers vb8 = (instr >> 8) & 0x0F; if(vb8 >= 14){ //cp14 and cp15 are for priviledged users only if(!privileged) goto invalid_instr; } else if(!(cpu->CPAR & (1UL << vb8))) goto invalid_instr; //others are access-controlled by CPAR if(instr & 0x00000010UL){ //MCR/MRC if(!cpu->coproc[vb8].regXfer) goto invalid_instr; if(!cpu->coproc[vb8].regXfer(cpu, cpu->coproc[vb8].userData, specialInstr, (instr & 0x00100000UL) != 0, (instr >> 21) & 0x07, (instr >> 12) & 0x0F, (instr >> 16) & 0x0F, instr & 0x0F, (instr >> 5) & 0x07)) goto invalid_instr; } else{ //CDP if(!cpu->coproc[vb8].dataProcessing) goto invalid_instr; if(!cpu->coproc[vb8].dataProcessing(cpu, cpu->coproc[vb8].userData, specialInstr, (instr >> 20) & 0x0F, (instr >> 12) & 0x0F, (instr >> 16) & 0x0F, instr & 0x0F, (instr >> 5) & 0x07)) goto invalid_instr; } goto instr_done; case 15: //SWI if(specialInstr) goto invalid_instr; cpuPrvException(cpu, cpu->vectorBase + ARM_VECTOR_OFFT_SWI, instrPC + (wasT ? 2 : 4), ARM_CPSR_SWI_ORR | (cpu->CPSR & ARM_CPSR_SWI_AND)); goto instr_done; } invalid_instr: if(instr == HYPERCALL_ARM && privileged){ if(cpu->hypercallF && cpu->hypercallF(cpu)) goto instr_done; } err_str("Invalid instr 0x"); err_hex(instr); err_str(" seen at 0x"); err_hex(instrPC); err_str(". CPSR=0x"); err_hex(cpu->CPSR); err_str("\r\n"); cpuPrvException(cpu, cpu->vectorBase + ARM_VECTOR_OFFT_UND, instrPC + (wasT ? 2 : 4), ARM_CPSR_UND_ORR | (cpu->CPSR & ARM_CPSR_UND_AND)); instr_done:; } return errNone; } static Err cpuPrvCycleArm(ArmCpu* cpu){ Boolean privileged, ok; UInt32 instr, pc; UInt8 fsr; privileged = (cpu->CPSR & ARM_SR_M) != ARM_SR_MODE_USR; //fetch instruction { ok = icacheFetch(&cpu->ic, pc = cpu->regs[15], 4, privileged, &fsr, &instr); if(!ok){ cpuPrvHandleMemErr(cpu, cpu->regs[15], 4, false, true, fsr); return errNone; //exit here so that debugger can see us execute first instr of execption handler } cpu->regs[15] += 4; } return cpuPrvExecInstr(cpu, instr, pc, false, privileged, false); } static Err cpuPrvCycleThumb(ArmCpu* cpu){ Boolean privileged, vB, specialPC = false, ok; UInt32 instr = 0xE0000000UL /*most likely thing*/, pc; UInt16 instrT, v16; UInt8 v8, fsr; privileged = (cpu->CPSR & ARM_SR_M) != ARM_SR_MODE_USR; pc = cpu->regs[15]; ok = icacheFetch(&cpu->ic, pc, 2, privileged, &fsr, &instrT); if(!ok){ cpuPrvHandleMemErr(cpu, pc, 2, false, true, fsr); return errNone; //exit here so that debugger can see us execute first instr of execption handler } cpu->regs[15] += 2; switch(instrT >> 12){ case 0: // LSL(1) LSR(1) ASR(1) ADD(1) SUB(1) ADD(3) SUB(3) case 1: if((instrT & 0x1800) != 0x1800){ // LSL(1) LSR(1) ASR(1) instr |= 0x01B00000UL | ((instrT & 0x7) << 12) | ((instrT >> 3) & 7) | ((instrT >> 6) & 0x60) | ((instrT << 1) & 0xF80); } else{ vB = (instrT & 0x0200) != 0; // SUB or ADD ? instr |= ((vB ? 5UL : 9UL) << 20) | (((UInt32)(instrT & 0x38)) << 13) | ((instrT & 0x07) << 12) | ((instrT >> 6) & 0x07); if(instrT & 0x0400){ // ADD(1) SUB(1) instr |= 0x02000000UL; } else{ // ADD(3) SUB(3) // nothing to do here } } break; case 2: // MOV(1) CMP(1) ADD(2) SUB(2) case 3: instr |= instrT & 0x00FF; switch((instrT >> 11) & 3){ case 0: // MOV(1) instr |= 0x03B00000UL | ((instrT & 0x0700) << 4); break; case 1: // CMP(1) instr |= 0x03500000UL | (((UInt32)(instrT & 0x0700)) << 8); break; case 2: // ADD(2) instr |= 0x02900000UL | ((instrT & 0x0700) << 4) | (((UInt32)(instrT & 0x0700)) << 8); break; case 3: // SUB(2) instr |= 0x02500000UL | ((instrT & 0x0700) << 4) | (((UInt32)(instrT & 0x0700)) << 8); break; } break; case 4: // LDR(3) ADD(4) CMP(3) MOV(3) BX MVN CMP(2) CMN TST ADC SBC NEG MUL LSL(2) LSR(2) ASR(2) ROR AND EOR ORR BIC if(instrT & 0x0800){ // LDR(3) instr |= 0x059F0000UL | ((instrT & 0xFF) << 2) | ((instrT & 0x700) << 4); specialPC = true; } else if(instrT & 0x0400){ // ADD(4) CMP(3) MOV(3) BX UInt8 vD; vD = (instrT & 7) | ((instrT >> 4) & 0x08); v8 = (instrT >> 3) & 0xF; switch((instrT >> 8) & 3){ case 0: // ADD(4) instr |= 0x00800000UL | (((UInt32)vD) << 16) | (((UInt32)vD) << 12) | v8; break; case 1: // CMP(3) instr |= 0x01500000UL | (((UInt32)vD) << 16) | v8; break; case 2: // MOV(3) instr |= 0x01A00000UL | (((UInt32)vD) << 12) | v8; break; case 3: // BX if(instrT == 0x4778){ //special handing for thumb's "BX PC" as aparently docs are wrong on it cpuPrvSetPC(cpu, (cpu->regs[15] + 2) &~ 3UL); goto instr_done; } instr |= 0x012FFF10UL | ((instrT >> 3) & 0x0F); break; } } else{ // AND EOR LSL(2) LSR(2) ASR(2) ADC SBC ROR TST NEG CMP(2) CMN ORR MUL BIC MVN (in val_tabl order) static const UInt32 val_tabl[16] = {0x00100000UL, 0x00300000UL, 0x01B00010UL, 0x01B00030UL, 0x01B00050UL, 0x00B00000UL, 0x00D00000UL, 0x01B00070UL, 0x01100000UL, 0x02700000UL, 0x01500000UL, 0x01700000UL, 0x01900000UL, 0x00100090UL, 0x01D00000UL, 0x01F00000UL}; //00 = none //10 = bit0 val //11 = bit3 val //MVN BIC MUL ORR CMN CMP(2) NEG TST ROR SBC ADC ASR(2) LSR(2) LSL(2) EOR AND const UInt32 use16 = 0x2AAE280AUL; //0010 1010 1010 1110 0010 1000 0000 1010 const UInt32 use12 = 0xA208AAAAUL; //1010 0010 0000 1000 1010 1010 1010 1010 const UInt32 use8 = 0x0800C3F0UL; //0000 1000 0000 0000 1100 0011 1111 0000 const UInt32 use0 = 0xFFF3BEAFUL; //1111 1111 1111 0011 1011 1110 1010 1111 UInt8 vals[4] = {0}; vals[2] = (instrT & 7); vals[3] = (instrT >> 3) & 7; v8 = (instrT >> 6) & 15; instr |= val_tabl[v8]; v8 <<= 1; instr |= ((UInt32)(vals[(use16 >> v8) & 3UL])) << 16; instr |= ((UInt32)(vals[(use12 >> v8) & 3UL])) << 12; instr |= ((UInt32)(vals[(use8 >> v8) & 3UL])) << 8; instr |= ((UInt32)(vals[(use0 >> v8) & 3UL])) << 0; } break; case 5: // STR(2) STRH(2) STRB(2) LDRSB LDR(2) LDRH(2) LDRB(2) LDRSH (in val_tbl orver) { static const UInt32 val_tabl[8] = {0x07800000UL, 0x018000B0UL, 0x07C00000UL, 0x019000D0UL, 0x07900000UL, 0x019000B0UL, 0x07D00000UL, 0x019000F0UL}; instr |= ((instrT >> 6) & 7) | ((instrT & 7) << 12) | (((UInt32)(instrT & 0x38)) << 13) | val_tabl[(instrT >> 9) & 7]; } break; case 6: // LDR(1) STR(1) (bit11 set = ldr) instr |= ((instrT & 7) << 12) | (((UInt32)(instrT & 0x38)) << 13) | ((instrT >> 4) & 0x7C) | 0x05800000UL; if(instrT & 0x0800) instr |= 0x00100000UL; break; case 7: // LDRB(1) STRB(1) (bit11 set = ldrb) instr |= ((instrT & 7) << 12) | (((UInt32)(instrT & 0x38)) << 13) | ((instrT >> 6) & 0x1F) | 0x05C00000UL; if(instrT & 0x0800) instr |= 0x00100000UL; break; case 8: // LDRH(1) STRH(1) (bit11 set = ldrh) instr |= ((instrT & 7) << 12) | (((UInt32)(instrT & 0x38)) << 13) | ((instrT >> 5) & 0x0E) | ((instrT >> 1) & 0x300) | 0x01C000B0UL; if(instrT & 0x0800) instr |= 0x00100000UL; break; case 9: // LDR(4) STR(3) (bit11 set = ldr) instr |= ((instrT & 0x700) << 4) | ((instrT & 0xFF) << 2) | 0x058D0000UL; if(instrT & 0x0800) instr |= 0x00100000UL; break; case 10: // ADD(5) ADD(6) (bit11 set = add(6)) instr |= ((instrT & 0x700) << 4) | (instrT &0xFF) | 0x028D0F00UL; //encode add to SP, line below sets the bit needed to reference PC instead when needed) if(!(instrT & 0x0800)) instr |= 0x00020000UL; else specialPC = true; break; case 11: // ADD(7) SUB(4) PUSH POP BKPT if((instrT & 0x0600) == 0x0400){ //PUSH POP instr |= (instrT & 0xFF) | 0x000D0000UL; if(instrT & 0x0800){ //POP if(instrT & 0x0100) instr |= 0x00008000UL; instr |= 0x08B00000UL; } else{ //PUSH if(instrT & 0x0100) instr |= 0x00004000UL; instr |= 0x09200000UL; } } else if(instrT & 0x0100){ goto undefined; } else switch((instrT >> 9) & 7){ case 0: // ADD(7) SUB(4) instr |= 0x020DDF00UL | (instrT & 0x7F) | ((instrT & 0x0080) ? 0x00400000UL : 0x00800000UL); break; #ifdef ARM_V6 case 1: //SXTH SXTB UXTH UXTB instr |= 0x060F0070UL | ((instrT >> 3) & 7) | ((instrT & 7) << 12); switch((instrT >> 6) & 3){ case 0: //SXTH instr |= 0x00B00000UL; break; case 1: //SXTB instr |= 0x00A00000UL; break; case 2: //UXTH instr |= 0x00F00000UL; break; case 3: //UXTB instr |= 0x00E00000UL; break; } break; case 3: //SETEND, CPY if((instrT & 0x00FE) == 0x0050){ //SETEND instr |= 0x01010000UL; if(instrT & 0x0008) instr |= 0x00000200UL; } else if((instrT & 0x00E8) == 0x0060){ //CPS instr |= 0x01080000UL | ((instrT & 7) << 6); if(instrT & 0x0010) instr |= 0x00040000UL; } else goto undefined; break; case 5: //REV REV16, REVSH instr |= 0x060F0F00UL | ((instrT >> 3) & 7) | ((instrT & 7) << 12); switch((instrT >> 6) & 3){ case 0: //REV instr |= 0x00B00030UL; break; case 1: //REV16 instr |= 0x00B000B0UL; break; case 2: // ??? goto undefined; case 3: instr |= 0x00F000B0UL; break; } break; #endif case 7: //BKPT instr |= 0x01200070UL | (instrT & 0x0F) | ((instrT & 0xF0) << 4); break; default: goto undefined; } break; case 12: // LDMIA STMIA (bit11 set = ldmia) instr |= 0x08800000UL | (((UInt32)(instrT & 0x700)) << 8) | (instrT & 0xFF); if(instrT & 0x0800) instr |= 0x00100000UL; if(!((1UL << ((instrT >> 8) & 0x07)) & instrT)) instr |= 0x00200000UL; //set W bit if needed break; case 13: // B(1), SWI, undefined instr space v8 = ((instrT >> 8) & 0x0F); if(v8 == 14){ // undefined instr goto undefined; } else if(v8 == 15){ // SWI instr |= 0x0F000000UL | (instrT & 0xFF); } else{ // B(1) instr = (((UInt32)v8) << 28) | 0x0A000000UL | (instrT & 0xFF); if(instrT & 0x80) instr |= 0x00FFFF00UL; } break; case 14: // B(2) BL BLX(1) undefined instr space case 15: v16 = (instrT & 0x7FF); switch((instrT >> 11) & 3){ case 0: //B(2) instr |= 0x0A000000UL | v16; if(instrT & 0x0400) instr |= 0x00FFF800UL; break; case 1: //BLX(1)_suffix instr = cpu->regs[15]; cpu->regs[15] = (cpu->regs[14] + 2 + (((UInt32)v16) << 1)) &~ 3UL; cpu->regs[14] = instr | 1UL; cpu->CPSR &=~ ARM_SR_T; goto instr_done; case 2: //BLX(1)_prefix BL_prefix instr = v16; if(instrT & 0x0400) instr |= 0x000FF800UL; cpu->regs[14] = cpu->regs[15] + (instr << 12); goto instr_done; case 3: //BL_suffix instr = cpu->regs[15]; cpu->regs[15] = cpu->regs[14] + 2 + (((UInt32)v16) << 1); cpu->regs[14] = instr | 1UL; goto instr_done; } if(instrT & 0x0800) goto undefined; //avoid BLX_suffix and undefined instr space in there instr |= 0x0A000000UL | (instrT & 0x7FF); if(instrT & 0x0400) instr |= 0x00FFF800UL; break; } instr_execute: return cpuPrvExecInstr(cpu, instr, pc, true, privileged, specialPC); instr_done: return errNone; undefined: if(instrT == HYPERCALL_THUMB){ instr = HYPERCALL_ARM; goto instr_execute; } instr = 0xE7F000F0UL | (instrT & 0x0F) | ((instrT & 0xFFF0) << 4); //guranteed undefined instr, inside it we store the original thumb instr :)=-) goto instr_execute; } Err cpuInit(ArmCpu* cpu, UInt32 pc, ArmCpuMemF memF, ArmCpuEmulErr emulErrF, ArmCpuHypercall hypercallF, ArmSetFaultAdrF setFaultAdrF){ if(!TYPE_CHECK){ emulErrF(cpu, "Type size error! CPU init aborted"); return errInternal; } __mem_zero(cpu, sizeof(ArmCpu)); cpu->CPSR = ARM_SR_I | ARM_SR_F | ARM_SR_MODE_SVC; //start w/o interrupts in supervisor mode cpuPrvSetPC(cpu, pc); cpu->memF = memF; cpu->emulErrF = emulErrF; cpu->hypercallF = hypercallF; cpu->setFaultAdrF = setFaultAdrF; icacheInit(&cpu->ic, cpu, memF); return errNone; } Err cpuDeinit(_UNUSED_ ArmCpu* cpu){ return errNone; } void cpuCycle(ArmCpu* cpu){ UInt32 vector, newCPSR; if(cpu->waitingFiqs && !(cpu->CPSR & ARM_SR_F)){ newCPSR = ARM_CPSR_FIQ_ORR | (cpu->CPSR & ARM_CPSR_FIQ_AND); vector = cpu->vectorBase + ARM_VECTOR_OFFT_FIQ; } else if(cpu->waitingIrqs && !(cpu->CPSR & ARM_SR_I)){ newCPSR = ARM_CPSR_IRQ_ORR | (cpu->CPSR & ARM_CPSR_IRQ_AND); vector = cpu->vectorBase + ARM_VECTOR_OFFT_IRQ; } #ifdef ARM_V6 else if(cpu->impreciseAbtWaiting && !(cpu->CPSR & ARM_SR_A)){ newCPSR = ARM_CPSR_DAB_ORR | (cpu->CPSR & ARM_CPSR_DAB_AND); vector = cpu->vectorBase + ARM_VECTOR_OFFT_D_ABT; } #endif else{ goto normal; } cpuPrvException(cpu, vector, cpu->regs[15] + 4, newCPSR); normal: if(cpu->CPSR & ARM_SR_T){ cpuPrvCycleThumb(cpu); } else{ cpuPrvCycleArm(cpu); } } void cpuIrq(ArmCpu* cpu, Boolean fiq, Boolean raise){ //unraise when acknowledged if(fiq){ if(raise){ cpu->waitingFiqs++; } else if(cpu->waitingFiqs){ cpu->waitingFiqs--; } else{ cpu->emulErrF(cpu,"Cannot unraise FIQ when none raised"); } } else{ if(raise){ cpu->waitingIrqs++; } else if(cpu->waitingIrqs){ cpu->waitingIrqs--; } else{ cpu->emulErrF(cpu,"Cannot unraise IRQ when none raised"); } } } void cpuIcacheInval(ArmCpu* cpu){ icacheInval(&cpu->ic); } void cpuIcacheInvalAddr(ArmCpu* cpu, UInt32 addr){ icacheInvalAddr(&cpu->ic, addr); } void cpuCoprocessorRegister(ArmCpu* cpu, UInt8 cpNum, ArmCoprocessor* coproc){ cpu->coproc[cpNum] = *coproc; } void cpuCoprocessorUnregister(ArmCpu* cpu, UInt8 cpNum){ ArmCoprocessor cp; __mem_zero(&cp, sizeof(ArmCoprocessor)); cpu->coproc[cpNum] = cp; } void cpuSetVectorAddr(ArmCpu* cpu, UInt32 adr){ cpu->vectorBase = adr; } UInt16 cpuGetCPAR(ArmCpu* cpu){ return cpu->CPAR; } void cpuSetCPAR(ArmCpu* cpu, UInt16 cpar){ cpu->CPAR = cpar; } #ifdef ARM_V6 void cpuSignalImpreciseAbt(ArmCpu* cpu, Boolean raise){ cpu->impreciseAbtWaiting = raise; } #endif