diff --git a/include/cpu/cpu.hh b/include/cpu/cpu.hh index fa120fd..f290fba 100644 --- a/include/cpu/cpu.hh +++ b/include/cpu/cpu.hh @@ -14,14 +14,14 @@ class Cpu { void step(); private: - static constexpr size_t GPR_COUNT = 16; + static constexpr uint8_t GPR_COUNT = 16; - static constexpr size_t GPR_FIQ_FIRST = 8; - static constexpr size_t GPR_SVC_FIRST = 13; - static constexpr size_t GPR_ABT_FIRST = 13; - static constexpr size_t GPR_IRQ_FIRST = 13; - static constexpr size_t GPR_UND_FIRST = 13; - static constexpr size_t GPR_SYS_USR_FIRST = 8; + static constexpr uint8_t GPR_FIQ_FIRST = 8; + static constexpr uint8_t GPR_SVC_FIRST = 13; + static constexpr uint8_t GPR_ABT_FIRST = 13; + static constexpr uint8_t GPR_IRQ_FIRST = 13; + static constexpr uint8_t GPR_UND_FIRST = 13; + static constexpr uint8_t GPR_SYS_USR_FIRST = 8; std::shared_ptr bus; std::array gpr; // general purpose registers @@ -29,7 +29,13 @@ class Cpu { Psr cpsr; // current program status register Psr spsr; // status program status register - uint32_t& pc = gpr[15]; + static constexpr uint8_t PC_INDEX = 15; + uint32_t& pc = gpr[PC_INDEX]; + + bool is_flushed; + + void chg_mode(const Mode to); + void exec_arm(const ArmInstruction instruction); struct { std::array fiq; @@ -49,7 +55,4 @@ class Cpu { Psr irq; Psr und; } spsr_banked; // banked saved program status registers - - void chg_mode(const Mode to); - void exec_arm(const ArmInstruction instruction); }; diff --git a/include/cpu/instruction.hh b/include/cpu/instruction.hh index 48c6838..a0f618b 100644 --- a/include/cpu/instruction.hh +++ b/include/cpu/instruction.hh @@ -79,6 +79,38 @@ class ArmInstruction { bool pre; }; + struct BlockDataTransfer { + uint16_t regs; + uint8_t rn; + bool load; + bool write; + bool s; + bool up; + bool pre; + }; + + struct DataProcessing { + std::variant operand; + uint8_t rd; + uint8_t rn; + bool set; + OpCode opcode; + }; + + struct PsrTransfer { + enum class Type { + Mrs, + Msr, + Msr_flg + }; + + uint32_t operand; + bool spsr; + Type type; + // ignored outside MSR_flg + bool imm; + }; + struct CoprocessorDataTransfer { uint8_t offset; uint8_t cpn; @@ -120,6 +152,9 @@ class ArmInstruction { SingleDataSwap, SingleDataTransfer, HalfwordTransfer, + BlockDataTransfer, + DataProcessing, + PsrTransfer, CoprocessorDataTransfer, CoprocessorDataOperation, CoprocessorRegisterTransfer, diff --git a/include/cpu/psr.hh b/include/cpu/psr.hh index e7caed3..affbe49 100644 --- a/include/cpu/psr.hh +++ b/include/cpu/psr.hh @@ -8,6 +8,9 @@ class Psr { // clear the reserved bits i.e, [8:27] Psr(uint32_t raw); + uint32_t raw() const; + void set_all(uint32_t raw); + // Mode : [4:0] Mode mode() const; void set_mode(Mode mode); @@ -45,8 +48,8 @@ class Psr { bool condition(Condition cond) const; private: - static constexpr uint32_t PSR_CLEAR_RESERVED = 0xf00000ff; - static constexpr uint32_t PSR_CLEAR_MODE = 0x0b00000; + static constexpr uint32_t PSR_CLEAR_RESERVED = 0xF00000FF; + static constexpr uint32_t PSR_CLEAR_MODE = 0xFFFFFFE0; uint32_t psr; }; diff --git a/include/cpu/utility.hh b/include/cpu/utility.hh index 6df4874..918bfb2 100644 --- a/include/cpu/utility.hh +++ b/include/cpu/utility.hh @@ -65,6 +65,12 @@ enum class OpCode { MVN = 0b1111 }; +// https://fmt.dev/dev/api.html#std-ostream-support +std::ostream& +operator<<(std::ostream& os, const OpCode cond); +template<> +struct fmt::formatter : ostream_formatter {}; + enum class ShiftType { LSL = 0b00, LSR = 0b01, diff --git a/src/cpu/cpu.cc b/src/cpu/cpu.cc index a0b05c4..edb5870 100644 --- a/src/cpu/cpu.cc +++ b/src/cpu/cpu.cc @@ -1,4 +1,5 @@ #include "cpu/cpu.hh" +#include "cpu/utility.hh" #include "util/bits.hh" #include "util/log.hh" #include @@ -11,15 +12,17 @@ Cpu::Cpu(Bus& bus) , gpr({ 0 }) , cpsr(0) , spsr(0) + , is_flushed(false) , gpr_banked({ { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, { 0 } }) , spsr_banked({ 0, 0, 0, 0, 0 }) { - cpsr.set_mode(Mode::System); + cpsr.set_mode(Mode::Supervisor); cpsr.set_irq_disabled(true); cpsr.set_fiq_disabled(true); cpsr.set_state(State::Arm); log_info("CPU successfully initialised"); - // PC is always two instructions ahead in the pipeline + // PC always points to two instructions ahead + // PC - 2 is the instruction being executed pc += 2 * ARM_INSTRUCTION_SIZE; } @@ -35,7 +38,7 @@ Cpu::chg_mode(const Mode to) { * concatenate views */ #define STORE_BANKED(mode, MODE) \ std::copy(gpr.begin() + GPR_##MODE##_FIRST, \ - gpr.begin() + GPR_COUNT - 1, \ + gpr.begin() + gpr.size() - 1, \ gpr_banked.mode.begin()) switch (from) { @@ -145,23 +148,25 @@ Cpu::exec_arm(const ArmInstruction instruction) { pc = gpr[data.rn]; // ignore [1:0] bits for arm and 0 bit for thumb - rst_nth_bit(pc, 0); + rst_bit(pc, 0); if (state == State::Arm) - rst_nth_bit(pc, 1); + rst_bit(pc, 1); + + // pc is affected so flush the pipeline + is_flushed = true; }, [this](ArmInstruction::Branch& data) { - auto offset = data.offset; - // lsh 2 and sign extend the 26 bit offset to 32 bits - offset <<= 2; - - if (get_nth_bit(offset, 25)) - offset |= 0xFC000000; - if (data.link) gpr[14] = pc - ARM_INSTRUCTION_SIZE; - pc += offset - ARM_INSTRUCTION_SIZE; + // data.offset accounts for two instructions ahead when + // disassembling, so need to adjust + pc = + static_cast(pc) - 2 * ARM_INSTRUCTION_SIZE + data.offset; + + // pc is affected so flush the pipeline + is_flushed = true; }, [this, pc_error](ArmInstruction::Multiply& data) { if (data.rd == data.rm) @@ -176,8 +181,8 @@ Cpu::exec_arm(const ArmInstruction instruction) { gpr[data.rm] * gpr[data.rs] + (data.acc ? gpr[data.rn] : 0); if (data.set) { - cpsr.set_z(!static_cast(gpr[data.rd])); - cpsr.set_n(get_nth_bit(gpr[data.rd], 31)); + cpsr.set_z(gpr[data.rd] == 0); + cpsr.set_n(get_bit(gpr[data.rd], 31)); cpsr.set_c(0); } }, @@ -199,8 +204,8 @@ Cpu::exec_arm(const ArmInstruction instruction) { static_cast(gpr[data.rdlo]) : 0); - gpr[data.rdlo] = get_bit_range(eval, 0, 31); - gpr[data.rdhi] = get_bit_range(eval, 32, 63); + gpr[data.rdlo] = bit_range(eval, 0, 31); + gpr[data.rdhi] = bit_range(eval, 32, 63); } else { int64_t eval = @@ -210,23 +215,22 @@ Cpu::exec_arm(const ArmInstruction instruction) { static_cast(gpr[data.rdlo]) : 0); - gpr[data.rdlo] = get_bit_range(eval, 0, 31); - gpr[data.rdhi] = get_bit_range(eval, 32, 63); + gpr[data.rdlo] = bit_range(eval, 0, 31); + gpr[data.rdhi] = bit_range(eval, 32, 63); } if (data.set) { - cpsr.set_z(!(static_cast(gpr[data.rdhi]) || - static_cast(gpr[data.rdlo]))); - cpsr.set_n(get_nth_bit(gpr[data.rdhi], 31)); + cpsr.set_z(gpr[data.rdhi] == 0 && gpr[data.rdlo] == 0); + cpsr.set_n(get_bit(gpr[data.rdhi], 31)); cpsr.set_c(0); cpsr.set_v(0); } }, [](ArmInstruction::Undefined) { log_warn("Undefined instruction"); }, - [this, pc_warn](ArmInstruction::SingleDataSwap& data) { - pc_warn(data.rm); - pc_warn(data.rn); - pc_warn(data.rd); + [this, pc_error](ArmInstruction::SingleDataSwap& data) { + pc_error(data.rm); + pc_error(data.rn); + pc_error(data.rd); if (data.byte) { gpr[data.rd] = bus->read_byte(gpr[data.rn]); @@ -244,6 +248,10 @@ Cpu::exec_arm(const ArmInstruction instruction) { log_warn("Write-back enabled with post-indexing in {}", typeid(data).name()); + if (data.rn == PC_INDEX && data.write) + log_warn("Write-back enabled with base register as PC {}", + typeid(data).name()); + if (data.write) pc_warn(data.rn); @@ -262,18 +270,21 @@ Cpu::exec_arm(const ArmInstruction instruction) { pc_error(shift->data.operand); pc_error(shift->rm); - eval_shift(shift->data.type, gpr[shift->rm], amount, carry); + offset = + eval_shift(shift->data.type, gpr[shift->rm], amount, carry); cpsr.set_c(carry); } // PC is always two instructions ahead - if (data.rn == 15) + if (data.rn == PC_INDEX) address -= 2 * ARM_INSTRUCTION_SIZE; if (data.pre) address += (data.up ? offset : -offset); + debug(address); + // load if (data.load) { // byte @@ -285,7 +296,7 @@ Cpu::exec_arm(const ArmInstruction instruction) { // store } else { // take PC into consideration - if (data.rd == 15) + if (data.rd == PC_INDEX) address += ARM_INSTRUCTION_SIZE; // byte @@ -301,6 +312,9 @@ Cpu::exec_arm(const ArmInstruction instruction) { if (!data.pre || data.write) gpr[data.rn] = address; + + if (data.rd == PC_INDEX && data.load) + is_flushed = true; }, [this, pc_warn, pc_error](ArmInstruction::HalfwordTransfer& data) { uint32_t address = gpr[data.rn]; @@ -331,14 +345,14 @@ Cpu::exec_arm(const ArmInstruction instruction) { gpr[data.rd] = bus->read_halfword(address); // sign extend the halfword - if (get_nth_bit(gpr[data.rd], 15)) + if (get_bit(gpr[data.rd], PC_INDEX)) gpr[data.rd] |= 0xFFFF0000; // byte } else { gpr[data.rd] = bus->read_byte(address); // sign extend the byte - if (get_nth_bit(gpr[data.rd], 7)) + if (get_bit(gpr[data.rd], 7)) gpr[data.rd] |= 0xFFFFFF00; } // unsigned halfword @@ -348,7 +362,7 @@ Cpu::exec_arm(const ArmInstruction instruction) { // store } else { // take PC into consideration - if (data.rd == 15) + if (data.rd == PC_INDEX) address += ARM_INSTRUCTION_SIZE; // halfword @@ -361,28 +375,337 @@ Cpu::exec_arm(const ArmInstruction instruction) { if (!data.pre || data.write) gpr[data.rn] = address; + + if (data.rd == PC_INDEX && data.load) + is_flushed = true; + }, + [this, pc_error](ArmInstruction::BlockDataTransfer& data) { + uint32_t address = gpr[data.rn]; + Mode mode = cpsr.mode(); + uint8_t alignment = 4; // word + uint8_t i = 0; + uint8_t n_regs = std::popcount(data.regs); + + pc_error(data.rn); + + if (cpsr.mode() == Mode::User && data.s) { + log_error("Bit S is set outside priviliged modes in {}", + typeid(data).name()); + } + + // we just change modes to load user registers + if ((!get_bit(data.regs, PC_INDEX) && data.s) || + (!data.load && data.s)) { + chg_mode(Mode::User); + + if (data.write) { + log_error("Write-back enable for user bank registers in {}", + typeid(data).name()); + } + } + + // account for decrement + if (!data.up) + address -= (n_regs - 1) * alignment; + + if (data.pre) + address += (data.up ? alignment : -alignment); + + if (data.load) { + if (get_bit(data.regs, PC_INDEX) && data.s && data.load) { + // current mode's spsr is already loaded when it was + // switched + spsr = cpsr; + } + + for (i = 0; i < GPR_COUNT; i++) { + if (get_bit(data.regs, i)) { + gpr[i] = bus->read_word(address); + address += alignment; + } + } + } else { + for (i = 0; i < GPR_COUNT; i++) { + if (get_bit(data.regs, i)) { + bus->write_word(address, gpr[i]); + address += alignment; + } + } + } + + if (!data.pre) + address += (data.up ? alignment : -alignment); + + // reset back to original address + offset if incremented earlier + if (data.up) + address -= n_regs * alignment; + + if (!data.pre || data.write) + gpr[data.rn] = address; + + if (data.load && get_bit(data.regs, PC_INDEX)) + is_flushed = true; + + // load back the original mode registers + chg_mode(mode); + }, + [this, pc_error](ArmInstruction::PsrTransfer& data) { + if (data.spsr && cpsr.mode() == Mode::User) { + log_error("Accessing SPSR in User mode in {}", + typeid(data).name()); + } + + Psr& psr = data.spsr ? spsr : cpsr; + + switch (data.type) { + case ArmInstruction::PsrTransfer::Type::Mrs: + pc_error(data.operand); + gpr[data.operand] = psr.raw(); + break; + case ArmInstruction::PsrTransfer::Type::Msr: + pc_error(data.operand); + + if (cpsr.mode() != Mode::User) { + psr.set_all(gpr[data.operand]); + break; + } + case ArmInstruction::PsrTransfer::Type::Msr_flg: + psr.set_n(get_bit(data.operand, 31)); + psr.set_z(get_bit(data.operand, 30)); + psr.set_c(get_bit(data.operand, 29)); + psr.set_v(get_bit(data.operand, 28)); + break; + } + }, + [this, pc_error](ArmInstruction::DataProcessing& data) { + uint32_t op_1 = gpr[data.rn]; + uint32_t op_2 = 0; + + uint32_t result = 0; + + bool overflow = cpsr.v(); + bool carry = cpsr.c(); + bool negative = cpsr.n(); + bool zero = cpsr.z(); + + if (const uint32_t* immediate = + std::get_if(&data.operand)) { + op_2 = *immediate; + } else if (const Shift* shift = std::get_if(&data.operand)) { + uint8_t amount = + (shift->data.immediate ? shift->data.operand + : gpr[shift->data.operand] & 0xFF); + + bool carry = cpsr.c(); + + if (!shift->data.immediate) + pc_error(shift->data.operand); + pc_error(shift->rm); + + op_2 = + eval_shift(shift->data.type, gpr[shift->rm], amount, carry); + + cpsr.set_c(carry); + + // PC is 12 bytes ahead when shifting + if (data.rn == PC_INDEX) + op_1 += ARM_INSTRUCTION_SIZE; + } + + switch (data.opcode) { + case OpCode::AND: { + result = op_1 & op_2; + + negative = get_bit(result, 31); + } break; + case OpCode::EOR: { + result = op_1 ^ op_2; + + negative = get_bit(result, 31); + } break; + case OpCode::SUB: { + bool s1 = get_bit(op_1, 31); + bool s2 = get_bit(op_2, 31); + result = op_1 - op_2; + negative = get_bit(result, 31); + carry = op_1 < op_2; + overflow = s1 != s2 && s2 == negative; + } break; + case OpCode::RSB: { + bool s1 = get_bit(op_1, 31); + bool s2 = get_bit(op_2, 31); + result = op_2 - op_1; + + negative = get_bit(result, 31); + carry = op_2 < op_1; + overflow = s1 != s2 && s1 == negative; + } break; + case OpCode::ADD: { + bool s1 = get_bit(op_1, 31); + bool s2 = get_bit(op_2, 31); + + // result_ is 33 bits + uint64_t result_ = op_2 + op_1; + result = result_ & 0xFFFFFFFF; + + negative = get_bit(result, 31); + carry = get_bit(result_, 32); + overflow = s1 == s2 && s1 != negative; + } break; + case OpCode::ADC: { + bool s1 = get_bit(op_1, 31); + bool s2 = get_bit(op_2, 31); + + uint64_t result_ = op_2 + op_1 + carry; + result = result_ & 0xFFFFFFFF; + + negative = get_bit(result, 31); + carry = get_bit(result_, 32); + overflow = s1 == s2 && s1 != negative; + } break; + case OpCode::SBC: { + bool s1 = get_bit(op_1, 31); + bool s2 = get_bit(op_2, 31); + + uint64_t result_ = op_1 - op_2 + carry - 1; + result = result_ & 0xFFFFFFFF; + + negative = get_bit(result, 31); + carry = get_bit(result_, 32); + overflow = s1 != s2 && s2 == negative; + } break; + case OpCode::RSC: { + bool s1 = get_bit(op_1, 31); + bool s2 = get_bit(op_2, 31); + + uint64_t result_ = op_1 - op_2 + carry - 1; + result = result_ & 0xFFFFFFFF; + + negative = get_bit(result, 31); + carry = get_bit(result_, 32); + overflow = s1 != s2 && s1 == negative; + } break; + case OpCode::TST: { + result = op_1 & op_2; + + negative = get_bit(result, 31); + } break; + case OpCode::TEQ: { + result = op_1 ^ op_2; + + negative = get_bit(result, 31); + } break; + case OpCode::CMP: { + bool s1 = get_bit(op_1, 31); + bool s2 = get_bit(op_2, 31); + + result = op_1 - op_2; + + negative = get_bit(result, 31); + carry = op_1 < op_2; + overflow = s1 != s2 && s2 == negative; + } break; + case OpCode::CMN: { + bool s1 = get_bit(op_1, 31); + bool s2 = get_bit(op_2, 31); + + uint64_t result_ = op_2 + op_1; + result = result_ & 0xFFFFFFFF; + + negative = get_bit(result, 31); + carry = get_bit(result_, 32); + overflow = s1 == s2 && s1 != negative; + } break; + case OpCode::ORR: { + result = op_1 | op_2; + + negative = get_bit(result, 31); + } break; + case OpCode::MOV: { + result = op_2; + + negative = get_bit(result, 31); + } break; + case OpCode::BIC: { + result = op_1 & ~op_2; + + negative = get_bit(result, 31); + } break; + case OpCode::MVN: { + result = ~op_2; + + negative = get_bit(result, 31); + } break; + } + + zero = result == 0; + + debug(carry); + debug(overflow); + debug(zero); + debug(negative); + + auto set_conditions = [=]() { + cpsr.set_c(carry); + cpsr.set_v(overflow); + cpsr.set_n(negative); + cpsr.set_z(zero); + }; + + if (data.set) { + if (data.rd == 15) { + if (cpsr.mode() == Mode::User) + log_error("Running {} in User mode", + typeid(data).name()); + } else { + set_conditions(); + } + } + + if (data.opcode == OpCode::TST || data.opcode == OpCode::TEQ || + data.opcode == OpCode::CMP || data.opcode == OpCode::CMN) { + set_conditions(); + } else { + gpr[data.rd] = result; + if (data.rd == 15 || data.opcode == OpCode::MVN) + is_flushed = true; + } }, [this](ArmInstruction::SoftwareInterrupt) { chg_mode(Mode::Supervisor); pc = 0x08; spsr = cpsr; }, - [](auto& data) { log_error("{} instruction", typeid(data).name()); } }, + [](auto& data) { + log_error("Unimplemented {} instruction", typeid(data).name()); + } }, data); } void Cpu::step() { + // Current instruction is two instructions behind PC uint32_t cur_pc = pc - 2 * ARM_INSTRUCTION_SIZE; if (cpsr.state() == State::Arm) { - ArmInstruction instruction(bus->read_word(cur_pc)); - log_info("{:#034b}", bus->read_word(cur_pc)); + debug(cur_pc); + uint32_t x = bus->read_word(cur_pc); + ArmInstruction instruction(x); + log_info("{:#034b}", x); exec_arm(instruction); - log_info("{:#010X} : {}", cur_pc, instruction.disassemble()); + log_info("0x{:08X} : {}", cur_pc, instruction.disassemble()); - pc += ARM_INSTRUCTION_SIZE; + if (is_flushed) { + // if flushed, do not increment the PC, instead set it to two + // instructions ahead to account for flushed "fetch" and "decode" + // instructions + pc += 2 * ARM_INSTRUCTION_SIZE; + is_flushed = false; + } else { + // if not flushed continue like normal + pc += ARM_INSTRUCTION_SIZE; + } } } diff --git a/src/cpu/instruction.cc b/src/cpu/instruction.cc index a15e64e..6d73d20 100644 --- a/src/cpu/instruction.cc +++ b/src/cpu/instruction.cc @@ -1,8 +1,10 @@ #include "cpu/instruction.hh" +#include "cpu/utility.hh" #include "util/bits.hh" +#include ArmInstruction::ArmInstruction(uint32_t insn) - : condition(static_cast(get_bit_range(insn, 28, 31))) { + : condition(static_cast(bit_range(insn, 28, 31))) { // Branch and exhcange if ((insn & 0x0FFFFFF0) == 0x012FFF10) { uint8_t rn = insn & 0b1111; @@ -11,19 +13,27 @@ ArmInstruction::ArmInstruction(uint32_t insn) // Branch } else if ((insn & 0x0E000000) == 0x0A000000) { - bool link = get_nth_bit(insn, 24); - uint32_t offset = get_bit_range(insn, 0, 23); + bool link = get_bit(insn, 24); + uint32_t offset = bit_range(insn, 0, 23); + + // lsh 2 and sign extend the 26 bit offset to 32 bits + offset <<= 2; + + if (get_bit(offset, 25)) + offset |= 0xFC000000; + + offset += 2 * ARM_INSTRUCTION_SIZE; data = Branch{ .link = link, .offset = offset }; // Multiply } else if ((insn & 0x0FC000F0) == 0x00000090) { - uint8_t rm = get_bit_range(insn, 0, 3); - uint8_t rs = get_bit_range(insn, 8, 11); - uint8_t rn = get_bit_range(insn, 12, 15); - uint8_t rd = get_bit_range(insn, 16, 19); - bool set = get_nth_bit(insn, 20); - bool acc = get_nth_bit(insn, 21); + uint8_t rm = bit_range(insn, 0, 3); + uint8_t rs = bit_range(insn, 8, 11); + uint8_t rn = bit_range(insn, 12, 15); + uint8_t rd = bit_range(insn, 16, 19); + bool set = get_bit(insn, 20); + bool acc = get_bit(insn, 21); data = Multiply{ .rm = rm, .rs = rs, .rn = rn, .rd = rd, .set = set, .acc = acc @@ -31,13 +41,13 @@ ArmInstruction::ArmInstruction(uint32_t insn) // Multiply long } else if ((insn & 0x0F8000F0) == 0x00800090) { - uint8_t rm = get_bit_range(insn, 0, 3); - uint8_t rs = get_bit_range(insn, 8, 11); - uint8_t rdlo = get_bit_range(insn, 12, 15); - uint8_t rdhi = get_bit_range(insn, 16, 19); - bool set = get_nth_bit(insn, 20); - bool acc = get_nth_bit(insn, 21); - bool uns = get_nth_bit(insn, 22); + uint8_t rm = bit_range(insn, 0, 3); + uint8_t rs = bit_range(insn, 8, 11); + uint8_t rdlo = bit_range(insn, 12, 15); + uint8_t rdhi = bit_range(insn, 16, 19); + bool set = get_bit(insn, 20); + bool acc = get_bit(insn, 21); + bool uns = get_bit(insn, 22); data = MultiplyLong{ .rm = rm, .rs = rs, @@ -53,10 +63,10 @@ ArmInstruction::ArmInstruction(uint32_t insn) // Single data swap } else if ((insn & 0x0FB00FF0) == 0x01000090) { - uint8_t rm = get_bit_range(insn, 0, 3); - uint8_t rd = get_bit_range(insn, 12, 15); - uint8_t rn = get_bit_range(insn, 16, 19); - bool byte = get_nth_bit(insn, 22); + uint8_t rm = bit_range(insn, 0, 3); + uint8_t rd = bit_range(insn, 12, 15); + uint8_t rn = bit_range(insn, 16, 19); + bool byte = get_bit(insn, 22); data = SingleDataSwap{ .rm = rm, .rd = rd, .rn = rn, .byte = byte }; @@ -64,28 +74,28 @@ ArmInstruction::ArmInstruction(uint32_t insn) } else if ((insn & 0x0C000000) == 0x04000000) { std::variant offset; - uint8_t rd = get_bit_range(insn, 12, 15); - uint8_t rn = get_bit_range(insn, 16, 19); - bool load = get_nth_bit(insn, 20); - bool write = get_nth_bit(insn, 21); - bool byte = get_nth_bit(insn, 22); - bool up = get_nth_bit(insn, 23); - bool pre = get_nth_bit(insn, 24); - bool imm = get_nth_bit(insn, 25); + uint8_t rd = bit_range(insn, 12, 15); + uint8_t rn = bit_range(insn, 16, 19); + bool load = get_bit(insn, 20); + bool write = get_bit(insn, 21); + bool byte = get_bit(insn, 22); + bool up = get_bit(insn, 23); + bool pre = get_bit(insn, 24); + bool imm = get_bit(insn, 25); if (imm) { - uint8_t rm = get_bit_range(insn, 0, 3); - bool reg = get_nth_bit(insn, 4); + uint8_t rm = bit_range(insn, 0, 3); + bool reg = get_bit(insn, 4); ShiftType shift_type = - static_cast(get_bit_range(insn, 5, 6)); - uint8_t operand = get_bit_range(insn, (reg ? 8 : 7), 11); + static_cast(bit_range(insn, 5, 6)); + uint8_t operand = bit_range(insn, (reg ? 8 : 7), 11); offset = Shift{ .rm = rm, .data = ShiftData{ .type = shift_type, .immediate = !reg, .operand = operand } }; } else { - offset = static_cast(get_bit_range(insn, 0, 11)); + offset = static_cast(bit_range(insn, 0, 11)); } data = SingleDataTransfer{ .offset = offset, @@ -99,18 +109,18 @@ ArmInstruction::ArmInstruction(uint32_t insn) // Halfword transfer } else if ((insn & 0x0E000090) == 0x00000090) { - uint8_t offset = get_bit_range(insn, 0, 3); - bool half = get_nth_bit(insn, 5); - bool sign = get_nth_bit(insn, 6); - uint8_t rd = get_bit_range(insn, 12, 15); - uint8_t rn = get_bit_range(insn, 16, 19); - bool load = get_nth_bit(insn, 20); - bool write = get_nth_bit(insn, 21); - bool imm = get_nth_bit(insn, 22); - bool up = get_nth_bit(insn, 23); - bool pre = get_nth_bit(insn, 24); + uint8_t offset = bit_range(insn, 0, 3); + bool half = get_bit(insn, 5); + bool sign = get_bit(insn, 6); + uint8_t rd = bit_range(insn, 12, 15); + uint8_t rn = bit_range(insn, 16, 19); + bool load = get_bit(insn, 20); + bool write = get_bit(insn, 21); + bool imm = get_bit(insn, 22); + bool up = get_bit(insn, 23); + bool pre = get_bit(insn, 24); - offset |= (imm ? get_bit_range(insn, 8, 11) << 2 : 0); + offset |= (imm ? bit_range(insn, 8, 11) << 2 : 0); data = HalfwordTransfer{ .offset = offset, .half = half, @@ -125,44 +135,99 @@ ArmInstruction::ArmInstruction(uint32_t insn) // Block data transfer } else if ((insn & 0x0E000000) == 0x08000000) { - /*static constexpr array syn = { "STM", "LDM" }; + uint16_t regs = bit_range(insn, 0, 15); + uint8_t rn = bit_range(insn, 16, 19); + bool load = get_bit(insn, 20); + bool write = get_bit(insn, 21); + bool s = get_bit(insn, 22); + bool up = get_bit(insn, 23); + bool pre = get_bit(insn, 24); - uint16_t regs = get_bit_range(insn, 0, 15); - uint8_t rn = get_bit_range(insn, 16, 19); - bool load = get_nth_bit(insn, 20); - bool write = get_nth_bit(insn, 21); - bool s = get_nth_bit(insn, 22); - bool up = get_nth_bit(insn, 23); - bool pre = get_nth_bit(insn, 24); + data = BlockDataTransfer{ .regs = regs, + .rn = rn, + .load = load, + .write = write, + .s = s, + .up = up, + .pre = pre }; - // disassembly - { - uint8_t lpu = load << 2 | pre << 1 | up; - std::string addr_mode; + // Data Processing + } else if ((insn & 0x0C000000) == 0x00000000) { + uint8_t rd = bit_range(insn, 12, 15); + uint8_t rn = bit_range(insn, 16, 19); + bool set = get_bit(insn, 20); + OpCode opcode = static_cast(bit_range(insn, 21, 24)); + bool imm = get_bit(insn, 25); - switch(lpu) { + if ((opcode == OpCode::TST || opcode == OpCode::CMP) && !set) { + data = PsrTransfer{ .operand = rd, + .spsr = get_bit(insn, 22), + .type = PsrTransfer::Type::Mrs, + .imm = 0 }; + } else if ((opcode == OpCode::TEQ || opcode == OpCode::CMN) && !set) { + bool imm = get_bit(insn, 25); + uint32_t operand = 0; + + if (imm) { + operand = bit_range(insn, 0, 3); + } else { + uint32_t immediate = bit_range(insn, 0, 7); + uint8_t rotate = bit_range(insn, 8, 11); + + operand = std::rotr(immediate, rotate * 2); } - }*/ - data = Undefined{}; + data = PsrTransfer{ .operand = operand, + .spsr = get_bit(insn, 22), + .type = (get_bit(insn, 16) + ? PsrTransfer::Type::Msr + : PsrTransfer::Type::Msr_flg), + .imm = imm }; + } else { + std::variant operand; - // Software Interrupt - // What to do here? + if (imm) { + uint32_t immediate = bit_range(insn, 0, 7); + uint8_t rotate = bit_range(insn, 8, 11); + + operand = std::rotr(immediate, rotate * 2); + + } else { + uint8_t rm = bit_range(insn, 0, 3); + bool reg = get_bit(insn, 4); + ShiftType shift_type = + static_cast(bit_range(insn, 5, 6)); + uint8_t sh_operand = bit_range(insn, (reg ? 8 : 7), 11); + + operand = Shift{ .rm = rm, + .data = ShiftData{ .type = shift_type, + .immediate = !reg, + .operand = sh_operand } }; + } + + data = DataProcessing{ .operand = operand, + .rd = rd, + .rn = rn, + .set = set, + .opcode = opcode }; + } + + // Software interrupt } else if ((insn & 0x0F000000) == 0x0F000000) { data = SoftwareInterrupt{}; // Coprocessor data transfer } else if ((insn & 0x0E000000) == 0x0C000000) { - uint8_t offset = get_bit_range(insn, 0, 7); - uint8_t cpn = get_bit_range(insn, 8, 11); - uint8_t crd = get_bit_range(insn, 12, 15); - uint8_t rn = get_bit_range(insn, 16, 19); - bool load = get_nth_bit(insn, 20); - bool write = get_nth_bit(insn, 21); - bool len = get_nth_bit(insn, 22); - bool up = get_nth_bit(insn, 23); - bool pre = get_nth_bit(insn, 24); + uint8_t offset = bit_range(insn, 0, 7); + uint8_t cpn = bit_range(insn, 8, 11); + uint8_t crd = bit_range(insn, 12, 15); + uint8_t rn = bit_range(insn, 16, 19); + bool load = get_bit(insn, 20); + bool write = get_bit(insn, 21); + bool len = get_bit(insn, 22); + bool up = get_bit(insn, 23); + bool pre = get_bit(insn, 24); data = CoprocessorDataTransfer{ .offset = offset, .cpn = cpn, @@ -176,12 +241,12 @@ ArmInstruction::ArmInstruction(uint32_t insn) // Coprocessor data operation } else if ((insn & 0x0F000010) == 0x0E000000) { - uint8_t crm = get_bit_range(insn, 0, 4); - uint8_t cp = get_bit_range(insn, 5, 7); - uint8_t cpn = get_bit_range(insn, 8, 11); - uint8_t crd = get_bit_range(insn, 12, 15); - uint8_t crn = get_bit_range(insn, 16, 19); - uint8_t cp_opc = get_bit_range(insn, 20, 23); + uint8_t crm = bit_range(insn, 0, 4); + uint8_t cp = bit_range(insn, 5, 7); + uint8_t cpn = bit_range(insn, 8, 11); + uint8_t crd = bit_range(insn, 12, 15); + uint8_t crn = bit_range(insn, 16, 19); + uint8_t cp_opc = bit_range(insn, 20, 23); data = CoprocessorDataOperation{ .crm = crm, .cp = cp, @@ -192,13 +257,13 @@ ArmInstruction::ArmInstruction(uint32_t insn) // Coprocessor register transfer } else if ((insn & 0x0F000010) == 0x0E000010) { - uint8_t crm = get_bit_range(insn, 0, 4); - uint8_t cp = get_bit_range(insn, 5, 7); - uint8_t cpn = get_bit_range(insn, 8, 11); - uint8_t rd = get_bit_range(insn, 12, 15); - uint8_t crn = get_bit_range(insn, 16, 19); - bool load = get_nth_bit(insn, 20); - uint8_t cp_opc = get_bit_range(insn, 21, 23); + uint8_t crm = bit_range(insn, 0, 4); + uint8_t cp = bit_range(insn, 5, 7); + uint8_t cpn = bit_range(insn, 8, 11); + uint8_t rd = bit_range(insn, 12, 15); + uint8_t crn = bit_range(insn, 16, 19); + bool load = get_bit(insn, 20); + uint8_t cp_opc = bit_range(insn, 21, 23); data = CoprocessorRegisterTransfer{ .crm = crm, .cp = cp, @@ -319,6 +384,81 @@ ArmInstruction::disassemble() { (data.pre ? expression : ""), (data.pre ? (data.write ? "!" : "") : expression)); }, + [this](BlockDataTransfer& data) { + std::string regs; + + for (uint8_t i = 0; i < 16; i++) { + if (get_bit(data.regs, i)) + fmt::format_to(std::back_inserter(regs), "R{:d},", i); + }; + + regs.pop_back(); + + return fmt::format("{}{}{}{} R{:d}{},{{{}}}{}", + (data.load ? "LDM" : "STM"), + condition, + (data.up ? 'I' : 'D'), + (data.pre ? 'B' : 'A'), + data.rn, + (data.write ? "!" : ""), + regs, + (data.s ? "^" : "")); + }, + [this](PsrTransfer& data) { + if (data.type == PsrTransfer::Type::Mrs) { + return fmt::format("MRS{} R{:d},{}", + condition, + data.operand, + (data.spsr ? "SPSR_all" : "CPSR_all")); + } else { + return fmt::format( + "MSR{} {}_{},{}{}", + condition, + (data.spsr ? "SPSR" : "CPSR"), + (data.type == PsrTransfer::Type::Msr_flg ? "flg" : "all"), + (data.imm ? '#' : 'R'), + data.operand); + } + }, + [this](DataProcessing& data) { + std::string op_2; + + if (const uint32_t* operand = + std::get_if(&data.operand)) { + op_2 = fmt::format("#{:d}", *operand); + } else if (const Shift* shift = std::get_if(&data.operand)) { + op_2 = fmt::format("R{:d},{} {}{:d}", + shift->rm, + shift->data.type, + (shift->data.immediate ? '#' : 'R'), + shift->data.operand); + } + + switch (data.opcode) { + case OpCode::MOV: + case OpCode::MVN: + return fmt::format("{}{}{} R{:d},{}", + data.opcode, + condition, + (data.set ? "S" : ""), + data.rd, + op_2); + case OpCode::TST: + case OpCode::TEQ: + case OpCode::CMP: + case OpCode::CMN: + return fmt::format( + "{}{} R{:d},{}", data.opcode, condition, data.rn, op_2); + default: + return fmt::format("{}{}{} R{:d},R{:d},{}", + data.opcode, + condition, + (data.set ? "S" : ""), + data.rd, + data.rn, + op_2); + } + }, [this](SoftwareInterrupt) { return fmt::format("SWI{}", condition); }, [this](CoprocessorDataTransfer& data) { std::string expression = fmt::format(",#{:d}", data.offset); diff --git a/src/cpu/psr.cc b/src/cpu/psr.cc index 30018c9..bbdf635 100644 --- a/src/cpu/psr.cc +++ b/src/cpu/psr.cc @@ -5,6 +5,16 @@ Psr::Psr(uint32_t raw) : psr(raw & PSR_CLEAR_RESERVED) {} +uint32_t +Psr::raw() const { + return psr; +} + +void +Psr::set_all(uint32_t raw) { + psr = raw & ~PSR_CLEAR_RESERVED; +} + Mode Psr::mode() const { return static_cast(psr & ~PSR_CLEAR_MODE); @@ -18,20 +28,20 @@ Psr::set_mode(Mode mode) { State Psr::state() const { - return static_cast(get_nth_bit(psr, 5)); + return static_cast(get_bit(psr, 5)); } void Psr::set_state(State state) { - chg_nth_bit(psr, 5, static_cast(state)); + chg_bit(psr, 5, static_cast(state)); } #define GET_SET_NTH_BIT_FUNCTIONS(name, n) \ bool Psr::name() const { \ - return get_nth_bit(psr, n); \ + return get_bit(psr, n); \ } \ void Psr::set_##name(bool val) { \ - chg_nth_bit(psr, n, val); \ + chg_bit(psr, n, val); \ } GET_SET_NTH_BIT_FUNCTIONS(fiq_disabled, 6) diff --git a/src/cpu/utility.cc b/src/cpu/utility.cc index 569a24d..cec5def 100644 --- a/src/cpu/utility.cc +++ b/src/cpu/utility.cc @@ -35,13 +35,45 @@ operator<<(std::ostream& os, const Condition cond) { return os; } +std::ostream& +operator<<(std::ostream& os, const OpCode opcode) { + +#define CASE(opcode) \ + case OpCode::opcode: \ + os << #opcode; \ + break; + + switch (opcode) { + CASE(AND) + CASE(EOR) + CASE(SUB) + CASE(RSB) + CASE(ADD) + CASE(ADC) + CASE(SBC) + CASE(RSC) + CASE(TST) + CASE(TEQ) + CASE(CMP) + CASE(CMN) + CASE(ORR) + CASE(MOV) + CASE(BIC) + CASE(MVN) + } + +#undef CASE + + return os; +} + uint32_t eval_shift(ShiftType shift_type, uint32_t value, uint8_t amount, bool& carry) { switch (shift_type) { case ShiftType::LSL: if (amount > 0 && amount <= 32) - carry = get_nth_bit(value, 32 - amount); + carry = get_bit(value, 32 - amount); else if (amount > 32) carry = 0; @@ -49,28 +81,28 @@ eval_shift(ShiftType shift_type, uint32_t value, uint8_t amount, bool& carry) { case ShiftType::LSR: if (amount > 0 && amount <= 32) - carry = get_nth_bit(value, amount - 1); + carry = get_bit(value, amount - 1); else if (amount > 32) carry = 0; else - carry = get_nth_bit(value, 31); + carry = get_bit(value, 31); return value >> amount; case ShiftType::ASR: if (amount > 0 && amount <= 32) - carry = get_nth_bit(value, amount - 1); + carry = get_bit(value, amount - 1); else - carry = get_nth_bit(value, 31); + carry = get_bit(value, 31); return static_cast(value) >> amount; case ShiftType::ROR: if (amount == 0) { bool old_carry = carry; - carry = get_nth_bit(value, 0); + carry = get_bit(value, 0); return (value >> 1) | (old_carry << 31); } else { - carry = get_nth_bit(value, (amount % 32 + 31) % 32); + carry = get_bit(value, (amount % 32 + 31) % 32); return std::rotr(value, amount); } } diff --git a/src/util/bits.hh b/src/util/bits.hh index 34c5c0f..d9e2236 100644 --- a/src/util/bits.hh +++ b/src/util/bits.hh @@ -7,32 +7,32 @@ using std::size_t; template inline bool -get_nth_bit(Int num, size_t n) { +get_bit(Int num, size_t n) { return (num >> n) & 1; } template inline void -set_nth_bit(Int& num, size_t n) { +set_bit(Int& num, size_t n) { num |= (1 << n); } template inline void -rst_nth_bit(Int& num, size_t n) { +rst_bit(Int& num, size_t n) { num &= ~(1 << n); } template inline void -chg_nth_bit(Int& num, size_t n, bool x) { +chg_bit(Int& num, size_t n, bool x) { num = (num & ~(1 << n)) | (x << n); } /// read range of bits from start to end inclusive template inline Int -get_bit_range(Int num, size_t start, size_t end) { +bit_range(Int num, size_t start, size_t end) { // NOTE: we do not require -1 if it is a signed integral Int left = std::numeric_limits::digits - (std::is_unsigned::value) - end;