private void DoDataProcessing(uint shifterOperand)
{
var rn = (curInstruction >> 16) & 0xF;
var rd = (curInstruction >> 12) & 0xF;
uint alu;
var registerShift = (curInstruction & (1 << 4)) == (1 << 4);
if (rn == 15 && ((curInstruction >> 25) & 0x7) == 0 && registerShift)
{
rn = registers[rn] + 4;
}
else
{
rn = registers[rn];
}
var opcode = (curInstruction >> 21) & 0xF;
if (((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;
OverflowCarryAdd(rn, shifterOperand, registers[rd]);
break;
case OP_ADD:
registers[rd] = rn + shifterOperand;
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
OverflowCarryAdd(rn, shifterOperand, registers[rd]);
break;
case OP_AND:
registers[rd] = rn & shifterOperand;
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
carry = shifterCarry;
break;
case OP_BIC:
registers[rd] = rn & ~shifterOperand;
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
carry = shifterCarry;
break;
case OP_CMN:
alu = rn + shifterOperand;
negative = alu >> 31;
zero = alu == 0 ? 1U : 0U;
OverflowCarryAdd(rn, shifterOperand, alu);
break;
case OP_CMP:
alu = rn - shifterOperand;
negative = alu >> 31;
zero = alu == 0 ? 1U : 0U;
OverflowCarrySub(rn, shifterOperand, alu);
break;
case OP_EOR:
registers[rd] = rn ^ shifterOperand;
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
carry = shifterCarry;
break;
case OP_MOV:
registers[rd] = shifterOperand;
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
carry = shifterCarry;
break;
case OP_MVN:
registers[rd] = ~shifterOperand;
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
carry = shifterCarry;
break;
case OP_ORR:
registers[rd] = rn | shifterOperand;
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
carry = shifterCarry;
break;
case OP_RSB:
registers[rd] = shifterOperand - rn;
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
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;
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;
OverflowCarrySub(rn, shifterOperand, registers[rd]);
break;
case OP_SUB:
registers[rd] = rn - shifterOperand;
negative = registers[rd] >> 31;
zero = registers[rd] == 0 ? 1U : 0U;
OverflowCarrySub(rn, shifterOperand, registers[rd]);
break;
case OP_TEQ:
alu = rn ^ shifterOperand;
negative = alu >> 31;
zero = alu == 0 ? 1U : 0U;
carry = shifterCarry;
break;
case OP_TST:
alu = rn & shifterOperand;
negative = alu >> 31;
zero = alu == 0 ? 1U : 0U;
carry = shifterCarry;
break;
}
if (rd == 15)
{
// Prevent writing if no SPSR exists (this will be true for USER or SYSTEM mode)
if (parent.SPSRExists)
{
parent.WriteCpsr(parent.SPSR);
}
UnpackFlags();
// Check for branch back to Thumb Mode
if ((parent.CPSR & Arm7Processor.T_MASK) == Arm7Processor.T_MASK)
{
thumbMode = true;
return;
}
// Otherwise, flush the instruction queue
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 ((curInstruction & (1 << 16)) == 1 << 16 && parent.SPSRExists)
{
parent.SPSR &= 0xFFFFFF00;
parent.SPSR |= shifterOperand & 0x000000FF;
}
if ((curInstruction & (1 << 17)) == 1 << 17 && parent.SPSRExists)
{
parent.SPSR &= 0xFFFF00FF;
parent.SPSR |= shifterOperand & 0x0000FF00;
}
if ((curInstruction & (1 << 18)) == 1 << 18 && parent.SPSRExists)
{
parent.SPSR &= 0xFF00FFFF;
parent.SPSR |= shifterOperand & 0x00FF0000;
}
if ((curInstruction & (1 << 19)) == 1 << 19 && parent.SPSRExists)
{
parent.SPSR &= 0x00FFFFFF;
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 (parent.SPSRExists)
{
registers[rd] = parent.SPSR;
}
break;
case OP_TEQ:
if (((curInstruction >> 4) & 0xf) == 1)
{
// BX
var rm = curInstruction & 0xf;
PackFlags();
parent.CPSR &= ~Arm7Processor.T_MASK;
parent.CPSR |= (registers[rm] & 1) << Arm7Processor.T_BIT;
registers[15] = registers[rm] & (~1U);
UnpackFlags();
// Check for branch back to Thumb Mode
if ((parent.CPSR & Arm7Processor.T_MASK) == Arm7Processor.T_MASK)
{
thumbMode = true;
return;
}
// Queue will be flushed later because rd == 15
}
else if (((curInstruction >> 4) & 0xf) == 0)
{
// MSR CPSR, shifterOperand
var userMode = (parent.CPSR & 0x1F) == Arm7Processor.USR;
PackFlags();
var tmpCPSR = parent.CPSR;
if ((curInstruction & (1 << 16)) == 1 << 16 && !userMode)
{
tmpCPSR &= 0xFFFFFF00;
tmpCPSR |= shifterOperand & 0x000000FF;
}
if ((curInstruction & (1 << 17)) == 1 << 17 && !userMode)
{
tmpCPSR &= 0xFFFF00FF;
tmpCPSR |= shifterOperand & 0x0000FF00;
}
if ((curInstruction & (1 << 18)) == 1 << 18 && !userMode)
{
tmpCPSR &= 0xFF00FFFF;
tmpCPSR |= shifterOperand & 0x00FF0000;
}
if ((curInstruction & (1 << 19)) == 1 << 19)
{
tmpCPSR &= 0x00FFFFFF;
tmpCPSR |= shifterOperand & 0xFF000000;
}
parent.WriteCpsr(tmpCPSR);
UnpackFlags();
// Check for branch back to Thumb Mode
if ((parent.CPSR & Arm7Processor.T_MASK) == Arm7Processor.T_MASK)
{
thumbMode = true;
return;
}
// Queue will be flushed since rd == 15, so adjust the PC
registers[15] -= 4;
}
break;
case OP_TST:
// MRS rd, CPSR
PackFlags();
registers[rd] = parent.CPSR;
break;
}
if (rd == 15)
{
// Flush the queue
FlushQueue();
}
}
}
