1273 lines
48 KiB
C#
1273 lines
48 KiB
C#
//#define ARM_DEBUG
|
|
|
|
namespace GarboDev
|
|
{
|
|
using System;
|
|
|
|
public class ArmCore
|
|
{
|
|
private const uint COND_EQ = 0; // Z set
|
|
private const uint COND_NE = 1; // Z clear
|
|
private const uint COND_CS = 2; // C set
|
|
private const uint COND_CC = 3; // C clear
|
|
private const uint COND_MI = 4; // N set
|
|
private const uint COND_PL = 5; // N clear
|
|
private const uint COND_VS = 6; // V set
|
|
private const uint COND_VC = 7; // V clear
|
|
private const uint COND_HI = 8; // C set and Z clear
|
|
private const uint COND_LS = 9; // C clear or Z set
|
|
private const uint COND_GE = 10; // N equals V
|
|
private const uint COND_LT = 11; // N not equal to V
|
|
private const uint COND_GT = 12; // Z clear AND (N equals V)
|
|
private const uint COND_LE = 13; // Z set OR (N not equal to V)
|
|
private const uint COND_AL = 14; // Always
|
|
private const uint COND_NV = 15; // Never execute
|
|
|
|
private const uint OP_AND = 0x0;
|
|
private const uint OP_EOR = 0x1;
|
|
private const uint OP_SUB = 0x2;
|
|
private const uint OP_RSB = 0x3;
|
|
private const uint OP_ADD = 0x4;
|
|
private const uint OP_ADC = 0x5;
|
|
private const uint OP_SBC = 0x6;
|
|
private const uint OP_RSC = 0x7;
|
|
private const uint OP_TST = 0x8;
|
|
private const uint OP_TEQ = 0x9;
|
|
private const uint OP_CMP = 0xA;
|
|
private const uint OP_CMN = 0xB;
|
|
private const uint OP_ORR = 0xC;
|
|
private const uint OP_MOV = 0xD;
|
|
private const uint OP_BIC = 0xE;
|
|
private const uint OP_MVN = 0xF;
|
|
|
|
private delegate void ExecuteInstruction();
|
|
private ExecuteInstruction[] NormalOps = null;
|
|
|
|
private Arm7Processor parent;
|
|
private Memory memory;
|
|
private uint[] registers;
|
|
|
|
private uint instructionQueue;
|
|
private uint curInstruction;
|
|
|
|
// CPU flags
|
|
private uint zero, carry, negative, overflow;
|
|
private uint shifterCarry;
|
|
|
|
private bool thumbMode;
|
|
|
|
public ArmCore(Arm7Processor parent, Memory memory)
|
|
{
|
|
this.parent = parent;
|
|
this.memory = memory;
|
|
this.registers = this.parent.Registers;
|
|
|
|
this.NormalOps = new ExecuteInstruction[8]
|
|
{
|
|
this.DataProcessing,
|
|
this.DataProcessingImmed,
|
|
this.LoadStoreImmediate,
|
|
this.LoadStoreRegister,
|
|
this.LoadStoreMultiple,
|
|
this.Branch,
|
|
this.CoprocessorLoadStore,
|
|
this.SoftwareInterrupt
|
|
};
|
|
}
|
|
|
|
public void BeginExecution()
|
|
{
|
|
this.FlushQueue();
|
|
}
|
|
|
|
public void Step()
|
|
{
|
|
this.UnpackFlags();
|
|
|
|
this.curInstruction = this.instructionQueue;
|
|
this.instructionQueue = this.memory.ReadU32(registers[15]);
|
|
registers[15] += 4;
|
|
|
|
uint cond = 0;
|
|
switch (this.curInstruction >> 28)
|
|
{
|
|
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)
|
|
{
|
|
// Execute the instruction
|
|
this.NormalOps[(curInstruction >> 25) & 0x7]();
|
|
}
|
|
|
|
this.parent.Cycles -= this.memory.WaitCycles;
|
|
|
|
if ((this.parent.CPSR & Arm7Processor.T_MASK) == Arm7Processor.T_MASK)
|
|
{
|
|
this.parent.ReloadQueue();
|
|
}
|
|
|
|
this.PackFlags();
|
|
}
|
|
|
|
public void Execute()
|
|
{
|
|
this.UnpackFlags();
|
|
this.thumbMode = false;
|
|
|
|
while (this.parent.Cycles > 0)
|
|
{
|
|
this.curInstruction = this.instructionQueue;
|
|
this.instructionQueue = this.memory.ReadU32Aligned(registers[15]);
|
|
registers[15] += 4;
|
|
|
|
if ((this.curInstruction >> 28) == COND_AL)
|
|
{
|
|
this.NormalOps[(curInstruction >> 25) & 0x7]();
|
|
}
|
|
else
|
|
{
|
|
uint cond = 0;
|
|
switch (this.curInstruction >> 28)
|
|
{
|
|
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)
|
|
{
|
|
// Execute the instruction
|
|
this.NormalOps[(curInstruction >> 25) & 0x7]();
|
|
}
|
|
}
|
|
|
|
this.parent.Cycles -= this.memory.WaitCycles;
|
|
|
|
if (this.thumbMode)
|
|
{
|
|
this.parent.ReloadQueue();
|
|
break;
|
|
}
|
|
|
|
#if ARM_DEBUG
|
|
// Check the current PC
|
|
if (this.parent.Breakpoints.ContainsKey(registers[15] - 4U))
|
|
{
|
|
this.parent.BreakpointHit = true;
|
|
break;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
this.PackFlags();
|
|
}
|
|
|
|
#region Barrel Shifter
|
|
private const uint SHIFT_LSL = 0;
|
|
private const uint SHIFT_LSR = 1;
|
|
private const uint SHIFT_ASR = 2;
|
|
private const uint SHIFT_ROR = 3;
|
|
|
|
private uint BarrelShifter(uint shifterOperand)
|
|
{
|
|
uint type = (shifterOperand >> 5) & 0x3;
|
|
|
|
bool registerShift = (shifterOperand & (1 << 4)) == (1 << 4);
|
|
|
|
uint rm = registers[shifterOperand & 0xF];
|
|
|
|
int amount;
|
|
if (registerShift)
|
|
{
|
|
uint rs = (shifterOperand >> 8) & 0xF;
|
|
if (rs == 15)
|
|
{
|
|
amount = (int)((registers[rs] + 0x4) & 0xFF);
|
|
}
|
|
else
|
|
{
|
|
amount = (int)(registers[rs] & 0xFF);
|
|
}
|
|
|
|
if ((shifterOperand & 0xF) == 15)
|
|
{
|
|
rm += 4;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
amount = (int)((shifterOperand >> 7) & 0x1F);
|
|
}
|
|
|
|
if (registerShift)
|
|
{
|
|
if (amount == 0)
|
|
{
|
|
this.shifterCarry = this.carry;
|
|
return rm;
|
|
}
|
|
|
|
switch (type)
|
|
{
|
|
case SHIFT_LSL:
|
|
if (amount < 32)
|
|
{
|
|
this.shifterCarry = (rm >> (32 - amount)) & 1;
|
|
return rm << amount;
|
|
}
|
|
else if (amount == 32)
|
|
{
|
|
this.shifterCarry = rm & 1;
|
|
return 0;
|
|
}
|
|
else
|
|
{
|
|
this.shifterCarry = 0;
|
|
return 0;
|
|
}
|
|
|
|
case SHIFT_LSR:
|
|
if (amount < 32)
|
|
{
|
|
this.shifterCarry = (rm >> (amount - 1)) & 1;
|
|
return rm >> amount;
|
|
}
|
|
else if (amount == 32)
|
|
{
|
|
this.shifterCarry = (rm >> 31) & 1;
|
|
return 0;
|
|
}
|
|
else
|
|
{
|
|
this.shifterCarry = 0;
|
|
return 0;
|
|
}
|
|
|
|
case SHIFT_ASR:
|
|
if (amount >= 32)
|
|
{
|
|
if ((rm & (1 << 31)) == 0)
|
|
{
|
|
this.shifterCarry = 0;
|
|
return 0;
|
|
}
|
|
else
|
|
{
|
|
this.shifterCarry = 1;
|
|
return 0xFFFFFFFF;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
this.shifterCarry = (rm >> (amount - 1)) & 1;
|
|
return (uint)(((int)rm) >> amount);
|
|
}
|
|
|
|
case SHIFT_ROR:
|
|
if ((amount & 0x1F) == 0)
|
|
{
|
|
this.shifterCarry = (rm >> 31) & 1;
|
|
return rm;
|
|
}
|
|
else
|
|
{
|
|
amount &= 0x1F;
|
|
this.shifterCarry = (rm >> amount) & 1;
|
|
return (rm >> amount) | (rm << (32 - amount));
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
switch (type)
|
|
{
|
|
case SHIFT_LSL:
|
|
if (amount == 0)
|
|
{
|
|
this.shifterCarry = this.carry;
|
|
return rm;
|
|
}
|
|
else
|
|
{
|
|
this.shifterCarry = (rm >> (32 - amount)) & 1;
|
|
return rm << amount;
|
|
}
|
|
|
|
case SHIFT_LSR:
|
|
if (amount == 0)
|
|
{
|
|
this.shifterCarry = (rm >> 31) & 1;
|
|
return 0;
|
|
}
|
|
else
|
|
{
|
|
this.shifterCarry = (rm >> (amount - 1)) & 1;
|
|
return rm >> amount;
|
|
}
|
|
|
|
case SHIFT_ASR:
|
|
if (amount == 0)
|
|
{
|
|
if ((rm & (1 << 31)) == 0)
|
|
{
|
|
this.shifterCarry = 0;
|
|
return 0;
|
|
}
|
|
else
|
|
{
|
|
this.shifterCarry = 1;
|
|
return 0xFFFFFFFF;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
this.shifterCarry = (rm >> (amount - 1)) & 1;
|
|
return (uint)(((int)rm) >> amount);
|
|
}
|
|
|
|
case SHIFT_ROR:
|
|
if (amount == 0)
|
|
{
|
|
// Actually an RRX
|
|
this.shifterCarry = rm & 1;
|
|
return (this.carry << 31) | (rm >> 1);
|
|
}
|
|
else
|
|
{
|
|
this.shifterCarry = (rm >> (amount - 1)) & 1;
|
|
return (rm >> amount) | (rm << (32 - amount));
|
|
}
|
|
}
|
|
}
|
|
|
|
// Should never happen...
|
|
throw new Exception("Barrel Shifter has messed up.");
|
|
}
|
|
#endregion
|
|
|
|
#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 DoDataProcessing(uint shifterOperand)
|
|
{
|
|
uint rn = (this.curInstruction >> 16) & 0xF;
|
|
uint rd = (this.curInstruction >> 12) & 0xF;
|
|
uint alu;
|
|
|
|
bool registerShift = (this.curInstruction & (1 << 4)) == (1 << 4);
|
|
if (rn == 15 && ((this.curInstruction >> 25) & 0x7) == 0 && registerShift)
|
|
{
|
|
rn = registers[rn] + 4;
|
|
}
|
|
else
|
|
{
|
|
rn = registers[rn];
|
|
}
|
|
|
|
uint opcode = (this.curInstruction >> 21) & 0xF;
|
|
|
|
if (((this.curInstruction >> 20) & 1) == 1)
|
|
{
|
|
// Set flag bit set
|
|
switch (opcode)
|
|
{
|
|
case OP_ADC:
|
|
registers[rd] = rn + shifterOperand + carry;
|
|
|
|
negative = registers[rd] >> 31;
|
|
zero = registers[rd] == 0 ? 1U : 0U;
|
|
this.OverflowCarryAdd(rn, shifterOperand, registers[rd]);
|
|
break;
|
|
|
|
case OP_ADD:
|
|
registers[rd] = rn + shifterOperand;
|
|
|
|
negative = registers[rd] >> 31;
|
|
zero = registers[rd] == 0 ? 1U : 0U;
|
|
this.OverflowCarryAdd(rn, shifterOperand, registers[rd]);
|
|
break;
|
|
|
|
case OP_AND:
|
|
registers[rd] = rn & shifterOperand;
|
|
|
|
negative = registers[rd] >> 31;
|
|
zero = registers[rd] == 0 ? 1U : 0U;
|
|
carry = this.shifterCarry;
|
|
break;
|
|
|
|
case OP_BIC:
|
|
registers[rd] = rn & ~shifterOperand;
|
|
|
|
negative = registers[rd] >> 31;
|
|
zero = registers[rd] == 0 ? 1U : 0U;
|
|
carry = this.shifterCarry;
|
|
break;
|
|
|
|
case OP_CMN:
|
|
alu = rn + shifterOperand;
|
|
|
|
negative = alu >> 31;
|
|
zero = alu == 0 ? 1U : 0U;
|
|
this.OverflowCarryAdd(rn, shifterOperand, alu);
|
|
break;
|
|
|
|
case OP_CMP:
|
|
alu = rn - shifterOperand;
|
|
|
|
negative = alu >> 31;
|
|
zero = alu == 0 ? 1U : 0U;
|
|
this.OverflowCarrySub(rn, shifterOperand, alu);
|
|
break;
|
|
|
|
case OP_EOR:
|
|
registers[rd] = rn ^ shifterOperand;
|
|
|
|
negative = registers[rd] >> 31;
|
|
zero = registers[rd] == 0 ? 1U : 0U;
|
|
carry = this.shifterCarry;
|
|
break;
|
|
|
|
case OP_MOV:
|
|
registers[rd] = shifterOperand;
|
|
|
|
negative = registers[rd] >> 31;
|
|
zero = registers[rd] == 0 ? 1U : 0U;
|
|
carry = this.shifterCarry;
|
|
break;
|
|
|
|
case OP_MVN:
|
|
registers[rd] = ~shifterOperand;
|
|
|
|
negative = registers[rd] >> 31;
|
|
zero = registers[rd] == 0 ? 1U : 0U;
|
|
carry = this.shifterCarry;
|
|
break;
|
|
|
|
case OP_ORR:
|
|
registers[rd] = rn | shifterOperand;
|
|
|
|
negative = registers[rd] >> 31;
|
|
zero = registers[rd] == 0 ? 1U : 0U;
|
|
carry = this.shifterCarry;
|
|
break;
|
|
|
|
case OP_RSB:
|
|
registers[rd] = shifterOperand - rn;
|
|
|
|
negative = registers[rd] >> 31;
|
|
zero = registers[rd] == 0 ? 1U : 0U;
|
|
this.OverflowCarrySub(shifterOperand, rn, registers[rd]);
|
|
break;
|
|
|
|
case OP_RSC:
|
|
registers[rd] = shifterOperand - rn - (1U - carry);
|
|
|
|
negative = registers[rd] >> 31;
|
|
zero = registers[rd] == 0 ? 1U : 0U;
|
|
this.OverflowCarrySub(shifterOperand, rn, registers[rd]);
|
|
break;
|
|
|
|
case OP_SBC:
|
|
registers[rd] = rn - shifterOperand - (1U - carry);
|
|
|
|
negative = registers[rd] >> 31;
|
|
zero = registers[rd] == 0 ? 1U : 0U;
|
|
this.OverflowCarrySub(rn, shifterOperand, registers[rd]);
|
|
break;
|
|
|
|
case OP_SUB:
|
|
registers[rd] = rn - shifterOperand;
|
|
|
|
negative = registers[rd] >> 31;
|
|
zero = registers[rd] == 0 ? 1U : 0U;
|
|
this.OverflowCarrySub(rn, shifterOperand, registers[rd]);
|
|
break;
|
|
|
|
case OP_TEQ:
|
|
alu = rn ^ shifterOperand;
|
|
|
|
negative = alu >> 31;
|
|
zero = alu == 0 ? 1U : 0U;
|
|
carry = this.shifterCarry;
|
|
break;
|
|
|
|
case OP_TST:
|
|
alu = rn & shifterOperand;
|
|
|
|
negative = alu >> 31;
|
|
zero = alu == 0 ? 1U : 0U;
|
|
carry = this.shifterCarry;
|
|
break;
|
|
}
|
|
|
|
if (rd == 15)
|
|
{
|
|
// Prevent writing if no SPSR exists (this will be true for USER or SYSTEM mode)
|
|
if (this.parent.SPSRExists) this.parent.WriteCpsr(this.parent.SPSR);
|
|
this.UnpackFlags();
|
|
|
|
// Check for branch back to Thumb Mode
|
|
if ((this.parent.CPSR & Arm7Processor.T_MASK) == Arm7Processor.T_MASK)
|
|
{
|
|
this.thumbMode = true;
|
|
return;
|
|
}
|
|
|
|
// Otherwise, flush the instruction queue
|
|
this.FlushQueue();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Set flag bit not set
|
|
switch (opcode)
|
|
{
|
|
case OP_ADC: registers[rd] = rn + shifterOperand + carry; break;
|
|
case OP_ADD: registers[rd] = rn + shifterOperand; break;
|
|
case OP_AND: registers[rd] = rn & shifterOperand; break;
|
|
case OP_BIC: registers[rd] = rn & ~shifterOperand; break;
|
|
case OP_EOR: registers[rd] = rn ^ shifterOperand; break;
|
|
case OP_MOV: registers[rd] = shifterOperand; break;
|
|
case OP_MVN: registers[rd] = ~shifterOperand; break;
|
|
case OP_ORR: registers[rd] = rn | shifterOperand; break;
|
|
case OP_RSB: registers[rd] = shifterOperand - rn; break;
|
|
case OP_RSC: registers[rd] = shifterOperand - rn - (1U - carry); break;
|
|
case OP_SBC: registers[rd] = rn - shifterOperand - (1U - carry); break;
|
|
case OP_SUB: registers[rd] = rn - shifterOperand; break;
|
|
|
|
case OP_CMN:
|
|
// MSR SPSR, shifterOperand
|
|
if ((this.curInstruction & (1 << 16)) == 1 << 16 && this.parent.SPSRExists)
|
|
{
|
|
this.parent.SPSR &= 0xFFFFFF00;
|
|
this.parent.SPSR |= shifterOperand & 0x000000FF;
|
|
}
|
|
if ((this.curInstruction & (1 << 17)) == 1 << 17 && this.parent.SPSRExists)
|
|
{
|
|
this.parent.SPSR &= 0xFFFF00FF;
|
|
this.parent.SPSR |= shifterOperand & 0x0000FF00;
|
|
}
|
|
if ((this.curInstruction & (1 << 18)) == 1 << 18 && this.parent.SPSRExists)
|
|
{
|
|
this.parent.SPSR &= 0xFF00FFFF;
|
|
this.parent.SPSR |= shifterOperand & 0x00FF0000;
|
|
}
|
|
if ((this.curInstruction & (1 << 19)) == 1 << 19 && this.parent.SPSRExists)
|
|
{
|
|
this.parent.SPSR &= 0x00FFFFFF;
|
|
this.parent.SPSR |= shifterOperand & 0xFF000000;
|
|
}
|
|
|
|
// Queue will be flushed since rd == 15, so adjust the PC
|
|
registers[15] -= 4;
|
|
break;
|
|
|
|
case OP_CMP:
|
|
// MRS rd, SPSR
|
|
if (this.parent.SPSRExists) registers[rd] = this.parent.SPSR;
|
|
break;
|
|
|
|
case OP_TEQ:
|
|
if (((this.curInstruction >> 4) & 0xf) == 1)
|
|
{
|
|
// BX
|
|
uint rm = this.curInstruction & 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 Thumb Mode
|
|
if ((this.parent.CPSR & Arm7Processor.T_MASK) == Arm7Processor.T_MASK)
|
|
{
|
|
this.thumbMode = true;
|
|
return;
|
|
}
|
|
|
|
// Queue will be flushed later because rd == 15
|
|
}
|
|
else if (((this.curInstruction >> 4) & 0xf) == 0)
|
|
{
|
|
// MSR CPSR, shifterOperand
|
|
bool userMode = (this.parent.CPSR & 0x1F) == Arm7Processor.USR;
|
|
|
|
this.PackFlags();
|
|
|
|
uint tmpCPSR = this.parent.CPSR;
|
|
|
|
if ((this.curInstruction & (1 << 16)) == 1 << 16 && !userMode)
|
|
{
|
|
tmpCPSR &= 0xFFFFFF00;
|
|
tmpCPSR |= shifterOperand & 0x000000FF;
|
|
}
|
|
if ((this.curInstruction & (1 << 17)) == 1 << 17 && !userMode)
|
|
{
|
|
tmpCPSR &= 0xFFFF00FF;
|
|
tmpCPSR |= shifterOperand & 0x0000FF00;
|
|
}
|
|
if ((this.curInstruction & (1 << 18)) == 1 << 18 && !userMode)
|
|
{
|
|
tmpCPSR &= 0xFF00FFFF;
|
|
tmpCPSR |= shifterOperand & 0x00FF0000;
|
|
}
|
|
if ((this.curInstruction & (1 << 19)) == 1 << 19)
|
|
{
|
|
tmpCPSR &= 0x00FFFFFF;
|
|
tmpCPSR |= shifterOperand & 0xFF000000;
|
|
}
|
|
|
|
this.parent.WriteCpsr(tmpCPSR);
|
|
|
|
this.UnpackFlags();
|
|
|
|
// Check for branch back to Thumb Mode
|
|
if ((this.parent.CPSR & Arm7Processor.T_MASK) == Arm7Processor.T_MASK)
|
|
{
|
|
this.thumbMode = true;
|
|
return;
|
|
}
|
|
|
|
// Queue will be flushed since rd == 15, so adjust the PC
|
|
registers[15] -= 4;
|
|
}
|
|
break;
|
|
|
|
case OP_TST:
|
|
// MRS rd, CPSR
|
|
this.PackFlags();
|
|
registers[rd] = this.parent.CPSR;
|
|
break;
|
|
}
|
|
|
|
if (rd == 15)
|
|
{
|
|
// Flush the queue
|
|
this.FlushQueue();
|
|
}
|
|
}
|
|
}
|
|
|
|
private void DataProcessing()
|
|
{
|
|
// Special instruction
|
|
switch ((this.curInstruction >> 4) & 0xF)
|
|
{
|
|
case 0x9:
|
|
// Multiply or swap instructions
|
|
this.MultiplyOrSwap();
|
|
return;
|
|
case 0xB:
|
|
// Load/Store Unsigned halfword
|
|
this.LoadStoreHalfword();
|
|
return;
|
|
case 0xD:
|
|
// Load/Store Signed byte
|
|
this.LoadStoreHalfword();
|
|
return;
|
|
case 0xF:
|
|
// Load/Store Signed halfword
|
|
this.LoadStoreHalfword();
|
|
return;
|
|
}
|
|
|
|
this.DoDataProcessing(this.BarrelShifter(this.curInstruction));
|
|
}
|
|
|
|
private void DataProcessingImmed()
|
|
{
|
|
uint immed = this.curInstruction & 0xFF;
|
|
int rotateAmount = (int)(((this.curInstruction >> 8) & 0xF) * 2);
|
|
|
|
immed = (immed >> rotateAmount) | (immed << (32 - rotateAmount));
|
|
|
|
if (rotateAmount == 0)
|
|
{
|
|
this.shifterCarry = this.carry;
|
|
}
|
|
else
|
|
{
|
|
this.shifterCarry = (immed >> 31) & 1;
|
|
}
|
|
|
|
this.DoDataProcessing(immed);
|
|
}
|
|
|
|
private void LoadStore(uint offset)
|
|
{
|
|
uint rn = (this.curInstruction >> 16) & 0xF;
|
|
uint rd = (this.curInstruction >> 12) & 0xF;
|
|
|
|
uint address = registers[rn];
|
|
|
|
bool preIndexed = (this.curInstruction & (1 << 24)) == 1 << 24;
|
|
bool byteTransfer = (this.curInstruction & (1 << 22)) == 1 << 22;
|
|
bool writeback = (this.curInstruction & (1 << 21)) == 1 << 21;
|
|
|
|
// Add or subtract offset
|
|
if ((this.curInstruction & (1 << 23)) != 1 << 23) offset = (uint)-offset;
|
|
|
|
if (preIndexed)
|
|
{
|
|
address += offset;
|
|
|
|
if (writeback)
|
|
{
|
|
registers[rn] = address;
|
|
}
|
|
}
|
|
|
|
if ((this.curInstruction & (1 << 20)) == 1 << 20)
|
|
{
|
|
// Load
|
|
if (byteTransfer)
|
|
{
|
|
registers[rd] = this.memory.ReadU8(address);
|
|
}
|
|
else
|
|
{
|
|
registers[rd] = this.memory.ReadU32(address);
|
|
}
|
|
|
|
// ARM9 fix here
|
|
|
|
if (rd == 15)
|
|
{
|
|
registers[rd] &= ~3U;
|
|
this.FlushQueue();
|
|
}
|
|
|
|
if (!preIndexed)
|
|
{
|
|
if (rn != rd)
|
|
registers[rn] = address + offset;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Store
|
|
uint amount = registers[rd];
|
|
if (rd == 15) amount += 4;
|
|
|
|
if (byteTransfer)
|
|
{
|
|
this.memory.WriteU8(address, (byte)(amount & 0xFF));
|
|
}
|
|
else
|
|
{
|
|
this.memory.WriteU32(address, amount);
|
|
}
|
|
|
|
if (!preIndexed)
|
|
{
|
|
registers[rn] = address + offset;
|
|
}
|
|
}
|
|
}
|
|
|
|
private void LoadStoreImmediate()
|
|
{
|
|
this.LoadStore(this.curInstruction & 0xFFF);
|
|
}
|
|
|
|
private void LoadStoreRegister()
|
|
{
|
|
// The barrel shifter expects a 0 in bit 4 for immediate shifts, this is implicit in
|
|
// the meaning of the instruction, so it is fine
|
|
this.LoadStore(this.BarrelShifter(this.curInstruction));
|
|
}
|
|
|
|
private void LoadStoreMultiple()
|
|
{
|
|
uint rn = (this.curInstruction >> 16) & 0xF;
|
|
|
|
this.PackFlags();
|
|
uint curCpsr = this.parent.CPSR;
|
|
|
|
bool preIncrement = (this.curInstruction & (1 << 24)) != 0;
|
|
bool up = (this.curInstruction & (1 << 23)) != 0;
|
|
bool writeback = (this.curInstruction & (1 << 21)) != 0;
|
|
|
|
uint address;
|
|
uint bitsSet = 0;
|
|
for (int i = 0; i < 16; i++) if (((this.curInstruction >> i) & 1) != 0) bitsSet++;
|
|
|
|
if (preIncrement)
|
|
{
|
|
if (up)
|
|
{
|
|
// Increment before
|
|
address = this.registers[rn] + 4;
|
|
if (writeback) this.registers[rn] += bitsSet * 4;
|
|
}
|
|
else
|
|
{
|
|
// Decrement before
|
|
address = this.registers[rn] - (bitsSet * 4);
|
|
if (writeback) this.registers[rn] -= bitsSet * 4;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (up)
|
|
{
|
|
// Increment after
|
|
address = this.registers[rn];
|
|
if (writeback) this.registers[rn] += bitsSet * 4;
|
|
}
|
|
else
|
|
{
|
|
// Decrement after
|
|
address = this.registers[rn] - (bitsSet * 4) + 4;
|
|
if (writeback) this.registers[rn] -= bitsSet * 4;
|
|
}
|
|
}
|
|
|
|
if ((this.curInstruction & (1 << 20)) != 0)
|
|
{
|
|
if ((this.curInstruction & (1 << 22)) != 0 && ((this.curInstruction >> 15) & 1) == 0)
|
|
{
|
|
// Switch to user mode temporarily
|
|
this.parent.WriteCpsr((curCpsr & ~0x1FU) | Arm7Processor.USR);
|
|
}
|
|
|
|
// Load multiple
|
|
for (int i = 0; i < 15; i++)
|
|
{
|
|
if (((this.curInstruction >> i) & 1) != 1) continue;
|
|
this.registers[i] = this.memory.ReadU32Aligned(address & (~0x3U));
|
|
address += 4;
|
|
}
|
|
|
|
if (((this.curInstruction >> 15) & 1) == 1)
|
|
{
|
|
// Arm9 fix here
|
|
|
|
this.registers[15] = this.memory.ReadU32Aligned(address & (~0x3U));
|
|
|
|
if ((this.curInstruction & (1 << 22)) != 0)
|
|
{
|
|
// Load the CPSR from the SPSR
|
|
if (this.parent.SPSRExists)
|
|
{
|
|
this.parent.WriteCpsr(this.parent.SPSR);
|
|
this.UnpackFlags();
|
|
|
|
// Check for branch back to Thumb Mode
|
|
if ((this.parent.CPSR & Arm7Processor.T_MASK) == Arm7Processor.T_MASK)
|
|
{
|
|
this.thumbMode = true;
|
|
this.registers[15] &= ~0x1U;
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
this.registers[15] &= ~0x3U;
|
|
this.FlushQueue();
|
|
}
|
|
else
|
|
{
|
|
if ((this.curInstruction & (1 << 22)) != 0)
|
|
{
|
|
// Switch back to the correct mode
|
|
this.parent.WriteCpsr(curCpsr);
|
|
this.UnpackFlags();
|
|
|
|
if ((this.parent.CPSR & Arm7Processor.T_MASK) == Arm7Processor.T_MASK)
|
|
{
|
|
this.thumbMode = true;
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if ((this.curInstruction & (1 << 22)) != 0)
|
|
{
|
|
// Switch to user mode temporarily
|
|
this.parent.WriteCpsr((curCpsr & ~0x1FU) | Arm7Processor.USR);
|
|
}
|
|
|
|
if (((this.curInstruction >> (int)rn) & 1) != 0 && writeback &&
|
|
(this.curInstruction & ~(0xFFFFFFFF << (int)rn)) == 0)
|
|
{
|
|
// If the lowest register is also the writeback, we use the original value
|
|
// Does anybody do this????
|
|
throw new Exception("Unhandled STM state");
|
|
}
|
|
else
|
|
{
|
|
// Store multiple
|
|
for (int i = 0; i < 15; i++)
|
|
{
|
|
if (((this.curInstruction >> i) & 1) == 0) continue;
|
|
this.memory.WriteU32(address, this.registers[i]);
|
|
address += 4;
|
|
}
|
|
|
|
if (((this.curInstruction >> 15) & 1) != 0)
|
|
{
|
|
this.memory.WriteU32(address, this.registers[15] + 4U);
|
|
}
|
|
}
|
|
|
|
if ((this.curInstruction & (1 << 22)) != 0)
|
|
{
|
|
// Switch back to the correct mode
|
|
this.parent.WriteCpsr(curCpsr);
|
|
this.UnpackFlags();
|
|
}
|
|
}
|
|
}
|
|
|
|
private void Branch()
|
|
{
|
|
if ((this.curInstruction & (1 << 24)) != 0)
|
|
{
|
|
this.registers[14] = (this.registers[15] - 4U) & ~3U;
|
|
}
|
|
|
|
uint branchOffset = this.curInstruction & 0x00FFFFFF;
|
|
if (branchOffset >> 23 == 1) branchOffset |= 0xFF000000;
|
|
|
|
this.registers[15] += branchOffset << 2;
|
|
|
|
this.FlushQueue();
|
|
}
|
|
|
|
private void CoprocessorLoadStore()
|
|
{
|
|
throw new Exception("Unhandled opcode - coproc load/store");
|
|
}
|
|
|
|
private void SoftwareInterrupt()
|
|
{
|
|
// Adjust PC for prefetch
|
|
this.registers[15] -= 4U;
|
|
this.parent.EnterException(Arm7Processor.SVC, 0x8, false, false);
|
|
}
|
|
|
|
private void MultiplyOrSwap()
|
|
{
|
|
if ((this.curInstruction & (1 << 24)) == 1 << 24)
|
|
{
|
|
// Swap instruction
|
|
uint rn = (this.curInstruction >> 16) & 0xF;
|
|
uint rd = (this.curInstruction >> 12) & 0xF;
|
|
uint rm = this.curInstruction & 0xF;
|
|
|
|
if ((this.curInstruction & (1 << 22)) != 0)
|
|
{
|
|
// SWPB
|
|
byte tmp = this.memory.ReadU8(registers[rn]);
|
|
this.memory.WriteU8(registers[rn], (byte)(registers[rm] & 0xFF));
|
|
registers[rd] = tmp;
|
|
}
|
|
else
|
|
{
|
|
// SWP
|
|
uint tmp = this.memory.ReadU32(registers[rn]);
|
|
this.memory.WriteU32(registers[rn], registers[rm]);
|
|
registers[rd] = tmp;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Multiply instruction
|
|
switch ((this.curInstruction >> 21) & 0x7)
|
|
{
|
|
case 0:
|
|
case 1:
|
|
{
|
|
// Multiply/Multiply + Accumulate
|
|
uint rd = (this.curInstruction >> 16) & 0xF;
|
|
uint rn = registers[(this.curInstruction >> 12) & 0xF];
|
|
uint rs = (this.curInstruction >> 8) & 0xF;
|
|
uint rm = this.curInstruction & 0xF;
|
|
|
|
int cycles = 4;
|
|
// Multiply cycle calculations
|
|
if ((registers[rs] & 0xFFFFFF00) == 0 || (registers[rs] & 0xFFFFFF00) == 0xFFFFFF00)
|
|
{
|
|
cycles = 1;
|
|
}
|
|
else if ((registers[rs] & 0xFFFF0000) == 0 || (registers[rs] & 0xFFFF0000) == 0xFFFF0000)
|
|
{
|
|
cycles = 2;
|
|
}
|
|
else if ((registers[rs] & 0xFF000000) == 0 || (registers[rs] & 0xFF000000) == 0xFF000000)
|
|
{
|
|
cycles = 3;
|
|
}
|
|
|
|
registers[rd] = registers[rs] * registers[rm];
|
|
this.parent.Cycles -= cycles;
|
|
|
|
if ((this.curInstruction & (1 << 21)) == 1 << 21)
|
|
{
|
|
registers[rd] += rn;
|
|
this.parent.Cycles -= 1;
|
|
}
|
|
|
|
if ((this.curInstruction & (1 << 20)) == 1 << 20)
|
|
{
|
|
negative = registers[rd] >> 31;
|
|
zero = registers[rd] == 0 ? 1U : 0U;
|
|
}
|
|
break;
|
|
}
|
|
|
|
case 2:
|
|
case 3:
|
|
throw new Exception("Invalid multiply");
|
|
|
|
case 4:
|
|
case 5:
|
|
case 6:
|
|
case 7:
|
|
{
|
|
// Multiply/Signed Multiply Long
|
|
uint rdhi = (this.curInstruction >> 16) & 0xF;
|
|
uint rdlo = (this.curInstruction >> 12) & 0xF;
|
|
uint rs = (this.curInstruction >> 8) & 0xF;
|
|
uint rm = this.curInstruction & 0xF;
|
|
|
|
int cycles = 5;
|
|
// Multiply cycle calculations
|
|
if ((registers[rs] & 0xFFFFFF00) == 0 || (registers[rs] & 0xFFFFFF00) == 0xFFFFFF00)
|
|
{
|
|
cycles = 2;
|
|
}
|
|
else if ((registers[rs] & 0xFFFF0000) == 0 || (registers[rs] & 0xFFFF0000) == 0xFFFF0000)
|
|
{
|
|
cycles = 3;
|
|
}
|
|
else if ((registers[rs] & 0xFF000000) == 0 || (registers[rs] & 0xFF000000) == 0xFF000000)
|
|
{
|
|
cycles = 4;
|
|
}
|
|
|
|
this.parent.Cycles -= cycles;
|
|
|
|
switch ((this.curInstruction >> 21) & 0x3)
|
|
{
|
|
case 0:
|
|
{
|
|
// UMULL
|
|
ulong result = ((ulong)registers[rm]) * registers[rs];
|
|
registers[rdhi] = (uint)(result >> 32);
|
|
registers[rdlo] = (uint)(result & 0xFFFFFFFF);
|
|
break;
|
|
}
|
|
case 1:
|
|
{
|
|
// UMLAL
|
|
ulong accum = (((ulong)registers[rdhi]) << 32) | registers[rdlo];
|
|
ulong result = ((ulong)registers[rm]) * registers[rs];
|
|
result += accum;
|
|
registers[rdhi] = (uint)(result >> 32);
|
|
registers[rdlo] = (uint)(result & 0xFFFFFFFF);
|
|
break;
|
|
}
|
|
case 2:
|
|
{
|
|
// SMULL
|
|
long result = ((long)((int)registers[rm])) * ((long)((int)registers[rs]));
|
|
registers[rdhi] = (uint)(result >> 32);
|
|
registers[rdlo] = (uint)(result & 0xFFFFFFFF);
|
|
break;
|
|
}
|
|
case 3:
|
|
{
|
|
// SMLAL
|
|
long accum = (((long)((int)registers[rdhi])) << 32) | registers[rdlo];
|
|
long result = ((long)((int)registers[rm])) * ((long)((int)registers[rs]));
|
|
result += accum;
|
|
registers[rdhi] = (uint)(result >> 32);
|
|
registers[rdlo] = (uint)(result & 0xFFFFFFFF);
|
|
break;
|
|
}
|
|
}
|
|
|
|
if ((this.curInstruction & (1 << 20)) == 1 << 20)
|
|
{
|
|
negative = registers[rdhi] >> 31;
|
|
zero = (registers[rdhi] == 0 && registers[rdlo] == 0) ? 1U : 0U;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
private void LoadStoreHalfword()
|
|
{
|
|
uint rn = (this.curInstruction >> 16) & 0xF;
|
|
uint rd = (this.curInstruction >> 12) & 0xF;
|
|
|
|
uint address = registers[rn];
|
|
|
|
bool preIndexed = (this.curInstruction & (1 << 24)) != 0;
|
|
bool byteTransfer = (this.curInstruction & (1 << 5)) == 0;
|
|
bool signedTransfer = (this.curInstruction & (1 << 6)) != 0;
|
|
bool writeback = (this.curInstruction & (1 << 21)) != 0;
|
|
|
|
uint offset;
|
|
if ((this.curInstruction & (1 << 22)) != 0)
|
|
{
|
|
// Immediate offset
|
|
offset = ((this.curInstruction & 0xF00) >> 4) | (this.curInstruction & 0xF);
|
|
}
|
|
else
|
|
{
|
|
// Register offset
|
|
offset = this.registers[this.curInstruction & 0xF];
|
|
}
|
|
|
|
// Add or subtract offset
|
|
if ((this.curInstruction & (1 << 23)) == 0) offset = (uint)-offset;
|
|
|
|
if (preIndexed)
|
|
{
|
|
address += offset;
|
|
|
|
if (writeback)
|
|
{
|
|
registers[rn] = address;
|
|
}
|
|
}
|
|
|
|
if ((this.curInstruction & (1 << 20)) != 0)
|
|
{
|
|
// Load
|
|
if (byteTransfer)
|
|
{
|
|
if (signedTransfer)
|
|
{
|
|
registers[rd] = this.memory.ReadU8(address);
|
|
if ((registers[rd] & 0x80) != 0)
|
|
{
|
|
registers[rd] |= 0xFFFFFF00;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
registers[rd] = this.memory.ReadU8(address);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (signedTransfer)
|
|
{
|
|
registers[rd] = this.memory.ReadU16(address);
|
|
if ((registers[rd] & 0x8000) != 0)
|
|
{
|
|
registers[rd] |= 0xFFFF0000;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
registers[rd] = this.memory.ReadU16(address);
|
|
}
|
|
}
|
|
|
|
if (rd == 15)
|
|
{
|
|
registers[rd] &= ~3U;
|
|
this.FlushQueue();
|
|
}
|
|
|
|
if (!preIndexed)
|
|
{
|
|
if (rn != rd)
|
|
registers[rn] = address + offset;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Store
|
|
if (byteTransfer)
|
|
{
|
|
this.memory.WriteU8(address, (byte)(registers[rd] & 0xFF));
|
|
}
|
|
else
|
|
{
|
|
this.memory.WriteU16(address, (ushort)(registers[rd] & 0xFFFF));
|
|
}
|
|
|
|
if (!preIndexed)
|
|
{
|
|
registers[rn] = address + offset;
|
|
}
|
|
}
|
|
}
|
|
#endregion Opcodes
|
|
|
|
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.ReadU32(registers[15]);
|
|
registers[15] += 4;
|
|
}
|
|
}
|
|
} |