//#define ARM_DEBUG
namespace GarboDev
{
using System;
using System.Collections.Generic;
using System.Text;
public class ThumbCore
{
private const int COND_EQ = 0; // Z set
private const int COND_NE = 1; // Z clear
private const int COND_CS = 2; // C set
private const int COND_CC = 3; // C clear
private const int COND_MI = 4; // N set
private const int COND_PL = 5; // N clear
private const int COND_VS = 6; // V set
private const int COND_VC = 7; // V clear
private const int COND_HI = 8; // C set and Z clear
private const int COND_LS = 9; // C clear or Z set
private const int COND_GE = 10; // N equals V
private const int COND_LT = 11; // N not equal to V
private const int COND_GT = 12; // Z clear AND (N equals V)
private const int COND_LE = 13; // Z set OR (N not equal to V)
private const int COND_AL = 14; // Always
private const int COND_NV = 15; // Never execute
private const int OP_AND = 0x0;
private const int OP_EOR = 0x1;
private const int OP_LSL = 0x2;
private const int OP_LSR = 0x3;
private const int OP_ASR = 0x4;
private const int OP_ADC = 0x5;
private const int OP_SBC = 0x6;
private const int OP_ROR = 0x7;
private const int OP_TST = 0x8;
private const int OP_NEG = 0x9;
private const int OP_CMP = 0xA;
private const int OP_CMN = 0xB;
private const int OP_ORR = 0xC;
private const int OP_MUL = 0xD;
private const int OP_BIC = 0xE;
private const int OP_MVN = 0xF;
private Arm7Processor parent;
private Memory memory;
private uint[] registers;
// CPU flags
private uint zero, carry, negative, overflow;
private ushort curInstruction, instructionQueue;
private delegate void ExecuteInstruction();
private ExecuteInstruction[] NormalOps = null;
public ThumbCore(Arm7Processor parent, Memory memory)
{
this.parent = parent;
this.memory = memory;
this.registers = this.parent.Registers;
this.NormalOps = new ExecuteInstruction[256]
{
OpLslImm, OpLslImm, OpLslImm, OpLslImm, OpLslImm, OpLslImm, OpLslImm, OpLslImm,
OpLsrImm, OpLsrImm, OpLsrImm, OpLsrImm, OpLsrImm, OpLsrImm, OpLsrImm, OpLsrImm,
OpAsrImm, OpAsrImm, OpAsrImm, OpAsrImm, OpAsrImm, OpAsrImm, OpAsrImm, OpAsrImm,
OpAddRegReg, OpAddRegReg, OpSubRegReg, OpSubRegReg, OpAddRegImm, OpAddRegImm, OpSubRegImm, OpSubRegImm,
OpMovImm, OpMovImm, OpMovImm, OpMovImm, OpMovImm, OpMovImm, OpMovImm, OpMovImm,
OpCmpImm, OpCmpImm, OpCmpImm, OpCmpImm, OpCmpImm, OpCmpImm, OpCmpImm, OpCmpImm,
OpAddImm, OpAddImm, OpAddImm, OpAddImm, OpAddImm, OpAddImm, OpAddImm, OpAddImm,
OpSubImm, OpSubImm, OpSubImm, OpSubImm, OpSubImm, OpSubImm, OpSubImm, OpSubImm,
OpArith, OpArith, OpArith, OpArith, OpAddHi, OpCmpHi, OpMovHi, OpBx,
OpLdrPc, OpLdrPc, OpLdrPc, OpLdrPc, OpLdrPc, OpLdrPc, OpLdrPc, OpLdrPc,
OpStrReg, OpStrReg, OpStrhReg, OpStrhReg, OpStrbReg, OpStrbReg, OpLdrsbReg, OpLdrsbReg,
OpLdrReg, OpLdrReg, OpLdrhReg, OpLdrhReg, OpLdrbReg, OpLdrbReg, OpLdrshReg, OpLdrshReg,
OpStrImm, OpStrImm, OpStrImm, OpStrImm, OpStrImm, OpStrImm, OpStrImm, OpStrImm,
OpLdrImm, OpLdrImm, OpLdrImm, OpLdrImm, OpLdrImm, OpLdrImm, OpLdrImm, OpLdrImm,
OpStrbImm, OpStrbImm, OpStrbImm, OpStrbImm, OpStrbImm, OpStrbImm, OpStrbImm, OpStrbImm,
OpLdrbImm, OpLdrbImm, OpLdrbImm, OpLdrbImm, OpLdrbImm, OpLdrbImm, OpLdrbImm, OpLdrbImm,
OpStrhImm, OpStrhImm, OpStrhImm, OpStrhImm, OpStrhImm, OpStrhImm, OpStrhImm, OpStrhImm,
OpLdrhImm, OpLdrhImm, OpLdrhImm, OpLdrhImm, OpLdrhImm, OpLdrhImm, OpLdrhImm, OpLdrhImm,
OpStrSp, OpStrSp, OpStrSp, OpStrSp, OpStrSp, OpStrSp, OpStrSp, OpStrSp,
OpLdrSp, OpLdrSp, OpLdrSp, OpLdrSp, OpLdrSp, OpLdrSp, OpLdrSp, OpLdrSp,
OpAddPc, OpAddPc, OpAddPc, OpAddPc, OpAddPc, OpAddPc, OpAddPc, OpAddPc,
OpAddSp, OpAddSp, OpAddSp, OpAddSp, OpAddSp, OpAddSp, OpAddSp, OpAddSp,
OpSubSp, OpUnd, OpUnd, OpUnd, OpPush, OpPushLr, OpUnd, OpUnd,
OpUnd, OpUnd, OpUnd, OpUnd, OpPop, OpPopPc, OpUnd, OpUnd,
OpStmia, OpStmia, OpStmia, OpStmia, OpStmia, OpStmia, OpStmia, OpStmia,
OpLdmia, OpLdmia, OpLdmia, OpLdmia, OpLdmia, OpLdmia, OpLdmia, OpLdmia,
OpBCond, OpBCond, OpBCond, OpBCond, OpBCond, OpBCond, OpBCond, OpBCond,
OpBCond, OpBCond, OpBCond, OpBCond, OpBCond, OpBCond, OpUnd, OpSwi,
OpB, OpB, OpB, OpB, OpB, OpB, OpB, OpB,
OpUnd, OpUnd, OpUnd, OpUnd, OpUnd, OpUnd, OpUnd, OpUnd,
OpBl1, OpBl1, OpBl1, OpBl1, OpBl1, OpBl1, OpBl1, OpBl1,
OpBl2, OpBl2, OpBl2, OpBl2, OpBl2, OpBl2, OpBl2, OpBl2
};
}
public void BeginExecution()
{
this.FlushQueue();
}
public void Step()
{
this.UnpackFlags();
this.curInstruction = this.instructionQueue;
this.instructionQueue = this.memory.ReadU16(registers[15]);
registers[15] += 2;
// Execute the instruction
this.NormalOps[this.curInstruction >> 8]();
this.parent.Cycles -= this.memory.WaitCycles;
if ((this.parent.CPSR & Arm7Processor.T_MASK) != Arm7Processor.T_MASK)
{
if ((this.curInstruction >> 8) != 0xDF) this.parent.ReloadQueue();
}
this.PackFlags();
}
public void Execute()
{
this.UnpackFlags();
while (this.parent.Cycles > 0)
{
this.curInstruction = this.instructionQueue;
this.instructionQueue = this.memory.ReadU16(registers[15]);
registers[15] += 2;
// Execute the instruction
this.NormalOps[this.curInstruction >> 8]();
this.parent.Cycles -= this.memory.WaitCycles;
if ((this.parent.CPSR & Arm7Processor.T_MASK) != Arm7Processor.T_MASK)
{
if ((this.curInstruction >> 8) != 0xDF) this.parent.ReloadQueue();
break;
}
// Check the current PC
#if ARM_DEBUG
if (this.parent.Breakpoints.ContainsKey(registers[15] - 2U))
{
this.parent.BreakpointHit = true;
break;
}
#endif
}
this.PackFlags();
}
#region Flag helpers
public void OverflowCarryAdd(uint a, uint b, uint r)
{
overflow = ((a & b & ~r) | (~a & ~b & r)) >> 31;
carry = ((a & b) | (a & ~r) | (b & ~r)) >> 31;
}
public void OverflowCarrySub(uint a, uint b, uint r)
{
overflow = ((a & ~b & ~r) | (~a & b & r)) >> 31;
carry = ((a & ~b) | (a & ~r) | (~b & ~r)) >> 31;
}
#endregion
#region Opcodes
private void OpLslImm()
{
// 0x00 - 0x07
// lsl rd, rm, #immed
int rd = this.curInstruction & 0x7;
int rm = (this.curInstruction >> 3) & 0x7;
int immed = (this.curInstruction >> 6) & 0x1F;
if (immed == 0)
{
registers[rd] = registers[rm];
} else
{
carry = (registers[rm] >> (32 - immed)) & 0x1;
registers[rd] = registers[rm] << immed;
}
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
}
private void OpLsrImm()
{
// 0x08 - 0x0F
// lsr rd, rm, #immed
int rd = this.curInstruction & 0x7;
int rm = (this.curInstruction >> 3) & 0x7;
int immed = (this.curInstruction >> 6) & 0x1F;
if (immed == 0)
{
carry = registers[rm] >> 31;
registers[rd] = 0;
}
else
{
carry = (registers[rm] >> (immed - 1)) & 0x1;
registers[rd] = registers[rm] >> immed;
}
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
}
private void OpAsrImm()
{
// asr rd, rm, #immed
int rd = this.curInstruction & 0x7;
int rm = (this.curInstruction >> 3) & 0x7;
int immed = (this.curInstruction >> 6) & 0x1F;
if (immed == 0)
{
carry = registers[rm] >> 31;
if (carry == 1) registers[rd] = 0xFFFFFFFF;
else registers[rd] = 0;
}
else
{
carry = (registers[rm] >> (immed - 1)) & 0x1;
registers[rd] = (uint)(((int)registers[rm]) >> immed);
}
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
}
private void OpAddRegReg()
{
// add rd, rn, rm
int rd = this.curInstruction & 0x7;
int rn = (this.curInstruction >> 3) & 0x7;
int rm = (this.curInstruction >> 6) & 0x7;
uint orn = registers[rn];
uint orm = registers[rm];
registers[rd] = orn + orm;
this.OverflowCarryAdd(orn, orm, registers[rd]);
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
}
private void OpSubRegReg()
{
// sub rd, rn, rm
int rd = this.curInstruction & 0x7;
int rn = (this.curInstruction >> 3) & 0x7;
int rm = (this.curInstruction >> 6) & 0x7;
uint orn = registers[rn];
uint orm = registers[rm];
registers[rd] = orn - orm;
this.OverflowCarrySub(orn, orm, registers[rd]);
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
}
private void OpAddRegImm()
{
// add rd, rn, #immed
int rd = this.curInstruction & 0x7;
int rn = (this.curInstruction >> 3) & 0x7;
uint immed = (uint)((this.curInstruction >> 6) & 0x7);
uint orn = registers[rn];
registers[rd] = orn + immed;
this.OverflowCarryAdd(orn, immed, registers[rd]);
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
}
private void OpSubRegImm()
{
// sub rd, rn, #immed
int rd = this.curInstruction & 0x7;
int rn = (this.curInstruction >> 3) & 0x7;
uint immed = (uint)((this.curInstruction >> 6) & 0x7);
uint orn = registers[rn];
registers[rd] = orn - immed;
this.OverflowCarrySub(orn, immed, registers[rd]);
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
}
private void OpMovImm()
{
// mov rd, #immed
int rd = (this.curInstruction >> 8) & 0x7;
registers[rd] = (uint)(this.curInstruction & 0xFF);
negative = 0;
zero = registers[rd] == 0 ? 1U : 0U;
}
private void OpCmpImm()
{
// cmp rn, #immed
int rn = (this.curInstruction >> 8) & 0x7;
uint alu = registers[rn] - (uint)(this.curInstruction & 0xFF);
this.OverflowCarrySub(registers[rn], (uint)(this.curInstruction & 0xFF), alu);
negative = alu >> 31;
zero = alu == 0 ? 1U : 0U;
}
private void OpAddImm()
{
// add rd, #immed
int rd = (this.curInstruction >> 8) & 0x7;
uint ord = registers[rd];
registers[rd] += (uint)(this.curInstruction & 0xFF);
this.OverflowCarryAdd(ord, (uint)(this.curInstruction & 0xFF), registers[rd]);
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
}
private void OpSubImm()
{
// sub rd, #immed
int rd = (this.curInstruction >> 8) & 0x7;
uint ord = registers[rd];
registers[rd] -= (uint)(this.curInstruction & 0xFF);
this.OverflowCarrySub(ord, (uint)(this.curInstruction & 0xFF), registers[rd]);
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
}
private void OpArith()
{
int rd = this.curInstruction & 0x7;
uint rn = registers[(this.curInstruction >> 3) & 0x7];
uint orig, alu;
int shiftAmt;
switch ((this.curInstruction >> 6) & 0xF)
{
case OP_ADC:
orig = registers[rd];
registers[rd] += rn + carry;
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
this.OverflowCarryAdd(orig, rn, registers[rd]);
break;
case OP_AND:
registers[rd] &= rn;
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
break;
case OP_ASR:
shiftAmt = (int)(rn & 0xFF);
if (shiftAmt == 0)
{
// Do nothing
}
else if (shiftAmt < 32)
{
carry = (registers[rd] >> (shiftAmt - 1)) & 0x1;
registers[rd] = (uint)(((int)registers[rd]) >> shiftAmt);
}
else
{
carry = (registers[rd] >> 31) & 1;
if (carry == 1) registers[rd] = 0xFFFFFFFF;
else registers[rd] = 0;
}
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
break;
case OP_BIC:
registers[rd] &= ~rn;
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
break;
case OP_CMN:
alu = registers[rd] + rn;
negative = alu >> 31;
zero = alu == 0 ? 1U : 0U;
this.OverflowCarryAdd(registers[rd], rn, alu);
break;
case OP_CMP:
alu = registers[rd] - rn;
negative = alu >> 31;
zero = alu == 0 ? 1U : 0U;
this.OverflowCarrySub(registers[rd], rn, alu);
break;
case OP_EOR:
registers[rd] ^= rn;
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
break;
case OP_LSL:
shiftAmt = (int)(rn & 0xFF);
if (shiftAmt == 0)
{
// Do nothing
}
else if (shiftAmt < 32)
{
carry = (registers[rd] >> (32 - shiftAmt)) & 0x1;
registers[rd] <<= shiftAmt;
}
else if (shiftAmt == 32)
{
carry = registers[rd] & 0x1;
registers[rd] = 0;
}
else
{
carry = 0;
registers[rd] = 0;
}
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
break;
case OP_LSR:
shiftAmt = (int)(rn & 0xFF);
if (shiftAmt == 0)
{
// Do nothing
}
else if (shiftAmt < 32)
{
carry = (registers[rd] >> (shiftAmt - 1)) & 0x1;
registers[rd] >>= shiftAmt;
}
else if (shiftAmt == 32)
{
carry = (registers[rd] >> 31) & 0x1;
registers[rd] = 0;
}
else
{
carry = 0;
registers[rd] = 0;
}
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
break;
case OP_MUL:
int mulCycles = 4;
// Multiply cycle calculations
if ((rn & 0xFFFFFF00) == 0 || (rn & 0xFFFFFF00) == 0xFFFFFF00)
{
mulCycles = 1;
}
else if ((rn & 0xFFFF0000) == 0 || (rn & 0xFFFF0000) == 0xFFFF0000)
{
mulCycles = 2;
}
else if ((rn & 0xFF000000) == 0 || (rn & 0xFF000000) == 0xFF000000)
{
mulCycles = 3;
}
this.parent.Cycles -= mulCycles;
registers[rd] *= rn;
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
break;
case OP_MVN:
registers[rd] = ~rn;
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
break;
case OP_NEG:
registers[rd] = 0 - rn;
this.OverflowCarrySub(0, rn, registers[rd]);
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
break;
case OP_ORR:
registers[rd] |= rn;
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
break;
case OP_ROR:
shiftAmt = (int)(rn & 0xFF);
if (shiftAmt == 0)
{
// Do nothing
}
else if ((shiftAmt & 0x1F) == 0)
{
carry = registers[rd] >> 31;
}
else
{
shiftAmt &= 0x1F;
carry = (registers[rd] >> (shiftAmt - 1)) & 0x1;
registers[rd] = (registers[rd] >> shiftAmt) | (registers[rd] << (32 - shiftAmt));
}
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
break;
case OP_SBC:
orig = registers[rd];
registers[rd] = (registers[rd] - rn) - (1U - carry);
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
this.OverflowCarrySub(orig, rn, registers[rd]);
break;
case OP_TST:
alu = registers[rd] & rn;
negative = alu >> 31;
zero = alu == 0 ? 1U : 0U;
break;
default:
throw new Exception("The coder screwed up on the thumb alu op...");
}
}
private void OpAddHi()
{
int rd = ((this.curInstruction & (1 << 7)) >> 4) | (this.curInstruction & 0x7);
int rm = (this.curInstruction >> 3) & 0xF;
registers[rd] += registers[rm];
if (rd == 15)
{
registers[rd] &= ~1U;
this.FlushQueue();
}
}
private void OpCmpHi()
{
int rd = ((this.curInstruction & (1 << 7)) >> 4) | (this.curInstruction & 0x7);
int rm = (this.curInstruction >> 3) & 0xF;
uint alu = registers[rd] - registers[rm];
negative = alu >> 31;
zero = alu == 0 ? 1U : 0U;
this.OverflowCarrySub(registers[rd], registers[rm], alu);
}
private void OpMovHi()
{
int rd = ((this.curInstruction & (1 << 7)) >> 4) | (this.curInstruction & 0x7);
int rm = (this.curInstruction >> 3) & 0xF;
registers[rd] = registers[rm];
if (rd == 15)
{
registers[rd] &= ~1U;
this.FlushQueue();
}
}
private void OpBx()
{
int rm = (this.curInstruction >> 3) & 0xf;
this.PackFlags();
this.parent.CPSR &= ~Arm7Processor.T_MASK;
this.parent.CPSR |= (registers[rm] & 1) << Arm7Processor.T_BIT;
registers[15] = registers[rm] & (~1U);
this.UnpackFlags();
// Check for branch back to Arm Mode
if ((this.parent.CPSR & Arm7Processor.T_MASK) != Arm7Processor.T_MASK)
{
return;
}
this.FlushQueue();
}
private void OpLdrPc()
{
int rd = (this.curInstruction >> 8) & 0x7;
registers[rd] = this.memory.ReadU32((registers[15] & ~2U) + (uint)((this.curInstruction & 0xFF) * 4));
this.parent.Cycles--;
}
private void OpStrReg()
{
this.memory.WriteU32(registers[(this.curInstruction >> 3) & 0x7] + registers[(this.curInstruction >> 6) & 0x7],
registers[this.curInstruction & 0x7]);
}
private void OpStrhReg()
{
this.memory.WriteU16(registers[(this.curInstruction >> 3) & 0x7] + registers[(this.curInstruction >> 6) & 0x7],
(ushort)(registers[this.curInstruction & 0x7] & 0xFFFF));
}
private void OpStrbReg()
{
this.memory.WriteU8(registers[(this.curInstruction >> 3) & 0x7] + registers[(this.curInstruction >> 6) & 0x7],
(byte)(registers[this.curInstruction & 0x7] & 0xFF));
}
private void OpLdrsbReg()
{
registers[this.curInstruction & 0x7] =
this.memory.ReadU8(registers[(this.curInstruction >> 3) & 0x7] + registers[(this.curInstruction >> 6) & 0x7]);
if ((registers[this.curInstruction & 0x7] & (1 << 7)) != 0)
{
registers[this.curInstruction & 0x7] |= 0xFFFFFF00;
}
this.parent.Cycles--;
}
private void OpLdrReg()
{
registers[this.curInstruction & 0x7] =
this.memory.ReadU32(registers[(this.curInstruction >> 3) & 0x7] + registers[(this.curInstruction >> 6) & 0x7]);
this.parent.Cycles--;
}
private void OpLdrhReg()
{
registers[this.curInstruction & 0x7] =
this.memory.ReadU16(registers[(this.curInstruction >> 3) & 0x7] + registers[(this.curInstruction >> 6) & 0x7]);
this.parent.Cycles--;
}
private void OpLdrbReg()
{
registers[this.curInstruction & 0x7] =
this.memory.ReadU8(registers[(this.curInstruction >> 3) & 0x7] + registers[(this.curInstruction >> 6) & 0x7]);
this.parent.Cycles--;
}
private void OpLdrshReg()
{
registers[this.curInstruction & 0x7] =
this.memory.ReadU16(registers[(this.curInstruction >> 3) & 0x7] + registers[(this.curInstruction >> 6) & 0x7]);
if ((registers[this.curInstruction & 0x7] & (1 << 15)) != 0)
{
registers[this.curInstruction & 0x7] |= 0xFFFF0000;
}
this.parent.Cycles--;
}
private void OpStrImm()
{
this.memory.WriteU32(registers[(this.curInstruction >> 3) & 0x7] + (uint)(((this.curInstruction >> 6) & 0x1F) * 4),
registers[this.curInstruction & 0x7]);
}
private void OpLdrImm()
{
registers[this.curInstruction & 0x7] =
this.memory.ReadU32(registers[(this.curInstruction >> 3) & 0x7] + (uint)(((this.curInstruction >> 6) & 0x1F) * 4));
this.parent.Cycles--;
}
private void OpStrbImm()
{
this.memory.WriteU8(registers[(this.curInstruction >> 3) & 0x7] + (uint)((this.curInstruction >> 6) & 0x1F),
(byte)(registers[this.curInstruction & 0x7] & 0xFF));
}
private void OpLdrbImm()
{
registers[this.curInstruction & 0x7] =
this.memory.ReadU8(registers[(this.curInstruction >> 3) & 0x7] + (uint)((this.curInstruction >> 6) & 0x1F));
this.parent.Cycles--;
}
private void OpStrhImm()
{
this.memory.WriteU16(registers[(this.curInstruction >> 3) & 0x7] + (uint)(((this.curInstruction >> 6) & 0x1F) * 2),
(ushort)(registers[this.curInstruction & 0x7] & 0xFFFF));
}
private void OpLdrhImm()
{
registers[this.curInstruction & 0x7] =
this.memory.ReadU16(registers[(this.curInstruction >> 3) & 0x7] + (uint)(((this.curInstruction >> 6) & 0x1F) * 2));
this.parent.Cycles--;
}
private void OpStrSp()
{
this.memory.WriteU32(registers[13] + (uint)((this.curInstruction & 0xFF) * 4),
registers[(this.curInstruction >> 8) & 0x7]);
}
private void OpLdrSp()
{
registers[(this.curInstruction >> 8) & 0x7] =
this.memory.ReadU32(registers[13] + (uint)((this.curInstruction & 0xFF) * 4));
}
private void OpAddPc()
{
registers[(this.curInstruction >> 8) & 0x7] =
(registers[15] & ~2U) + (uint)((this.curInstruction & 0xFF) * 4);
}
private void OpAddSp()
{
registers[(this.curInstruction >> 8) & 0x7] =
registers[13] + (uint)((this.curInstruction & 0xFF) * 4);
}
private void OpSubSp()
{
if ((this.curInstruction & (1 << 7)) != 0)
registers[13] -= (uint)((this.curInstruction & 0x7F) * 4);
else
registers[13] += (uint)((this.curInstruction & 0x7F) * 4);
}
private void OpPush()
{
for (int i = 7; i >= 0; i--)
{
if (((this.curInstruction >> i) & 1) != 0)
{
registers[13] -= 4;
this.memory.WriteU32(registers[13], registers[i]);
}
}
}
private void OpPushLr()
{
registers[13] -= 4;
this.memory.WriteU32(registers[13], registers[14]);
for (int i = 7; i >= 0; i--)
{
if (((this.curInstruction >> i) & 1) != 0)
{
registers[13] -= 4;
this.memory.WriteU32(registers[13], registers[i]);
}
}
}
private void OpPop()
{
for (int i = 0; i < 8; i++)
{
if (((this.curInstruction >> i) & 1) != 0)
{
registers[i] = this.memory.ReadU32(registers[13]);
registers[13] += 4;
}
}
this.parent.Cycles--;
}
private void OpPopPc()
{
for (int i = 0; i < 8; i++)
{
if (((this.curInstruction >> i) & 1) != 0)
{
registers[i] = this.memory.ReadU32(registers[13]);
registers[13] += 4;
}
}
registers[15] = this.memory.ReadU32(registers[13]) & (~1U);
registers[13] += 4;
// ARM9 check here
this.FlushQueue();
this.parent.Cycles--;
}
private void OpStmia()
{
int rn = (this.curInstruction >> 8) & 0x7;
for (int i = 0; i < 8; i++)
{
if (((this.curInstruction >> i) & 1) != 0)
{
this.memory.WriteU32(registers[rn] & (~3U), registers[i]);
registers[rn] += 4;
}
}
}
private void OpLdmia()
{
int rn = (this.curInstruction >> 8) & 0x7;
uint address = registers[rn];
for (int i = 0; i < 8; i++)
{
if (((this.curInstruction >> i) & 1) != 0)
{
registers[i] = this.memory.ReadU32Aligned(address & (~3U));
address += 4;
}
}
if (((this.curInstruction >> rn) & 1) == 0)
{
registers[rn] = address;
}
}
private void OpBCond()
{
uint cond = 0;
switch ((this.curInstruction >> 8) & 0xF)
{
case COND_AL: cond = 1; break;
case COND_EQ: cond = zero; break;
case COND_NE: cond = 1 - zero; break;
case COND_CS: cond = carry; break;
case COND_CC: cond = 1 - carry; break;
case COND_MI: cond = negative; break;
case COND_PL: cond = 1 - negative; break;
case COND_VS: cond = overflow; break;
case COND_VC: cond = 1 - overflow; break;
case COND_HI: cond = carry & (1 - zero); break;
case COND_LS: cond = (1 - carry) | zero; break;
case COND_GE: cond = (1 - negative) ^ overflow; break;
case COND_LT: cond = negative ^ overflow; break;
case COND_GT: cond = (1 - zero) & (negative ^ (1 - overflow)); break;
case COND_LE: cond = (negative ^ overflow) | zero; break;
}
if (cond == 1)
{
uint offset = (uint)(this.curInstruction & 0xFF);
if ((offset & (1 << 7)) != 0) offset |= 0xFFFFFF00;
registers[15] += offset << 1;
this.FlushQueue();
}
}
private void OpSwi()
{
registers[15] -= 4U;
this.parent.EnterException(Arm7Processor.SVC, 0x8, false, false);
}
private void OpB()
{
uint offset = (uint)(this.curInstruction & 0x7FF);
if ((offset & (1 << 10)) != 0) offset |= 0xFFFFF800;
registers[15] += offset << 1;
this.FlushQueue();
}
private void OpBl1()
{
uint offset = (uint)(this.curInstruction & 0x7FF);
if ((offset & (1 << 10)) != 0) offset |= 0xFFFFF800;
registers[14] = registers[15] + (offset << 12);
}
private void OpBl2()
{
uint tmp = registers[15];
registers[15] = registers[14] + (uint)((this.curInstruction & 0x7FF) << 1);
registers[14] = (tmp - 2U) | 1;
this.FlushQueue();
}
private void OpUnd()
{
throw new Exception("Unknown opcode");
}
#endregion
private void PackFlags()
{
this.parent.CPSR &= 0x0FFFFFFF;
this.parent.CPSR |= this.negative << Arm7Processor.N_BIT;
this.parent.CPSR |= this.zero << Arm7Processor.Z_BIT;
this.parent.CPSR |= this.carry << Arm7Processor.C_BIT;
this.parent.CPSR |= this.overflow << Arm7Processor.V_BIT;
}
private void UnpackFlags()
{
this.negative = (this.parent.CPSR >> Arm7Processor.N_BIT) & 1;
this.zero = (this.parent.CPSR >> Arm7Processor.Z_BIT) & 1;
this.carry = (this.parent.CPSR >> Arm7Processor.C_BIT) & 1;
this.overflow = (this.parent.CPSR >> Arm7Processor.V_BIT) & 1;
}
private void FlushQueue()
{
this.instructionQueue = this.memory.ReadU16(registers[15]);
registers[15] += 2;
}
}
}
