//#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;
}
}
}
