[UNTESTED] complete initial disassembler structure for ARM

Signed-off-by: Amneesh Singh <natto@weirdnatto.in>

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 uint8_t GPR_FIQ_FIRST     = 8;
-    static constexpr size_t GPR_SVC_FIRST     = 13;
+    static constexpr uint8_t GPR_SVC_FIRST     = 13;
-    static constexpr size_t GPR_ABT_FIRST     = 13;
+    static constexpr uint8_t GPR_ABT_FIRST     = 13;
-    static constexpr size_t GPR_IRQ_FIRST     = 13;
+    static constexpr uint8_t GPR_IRQ_FIRST     = 13;
-    static constexpr size_t GPR_UND_FIRST     = 13;
+    static constexpr uint8_t GPR_UND_FIRST     = 13;
-    static constexpr size_t GPR_SYS_USR_FIRST = 8;
+    static constexpr uint8_t GPR_SYS_USR_FIRST = 8;
 
     std::shared_ptr<Bus> bus;
     std::array<uint32_t, GPR_COUNT> 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<uint32_t, GPR_COUNT - GPR_FIQ_FIRST - 1> 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);
 };

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<Shift, uint32_t> 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,

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_RESERVED = 0xF00000FF;
-    static constexpr uint32_t PSR_CLEAR_MODE     = 0x0b00000;
+    static constexpr uint32_t PSR_CLEAR_MODE     = 0xFFFFFFE0;
 
     uint32_t psr;
 };

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<OpCode> : ostream_formatter {};
+
 enum class ShiftType {
     LSL = 0b00,
     LSR = 0b01,

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 <algorithm>
@@ -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<int32_t>(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<bool>(gpr[data.rd]));
+                cpsr.set_z(gpr[data.rd] == 0);
-                cpsr.set_n(get_nth_bit(gpr[data.rd], 31));
+                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<uint64_t>(gpr[data.rdlo])
                             : 0);
 
-                gpr[data.rdlo] = get_bit_range(eval, 0, 31);
+                gpr[data.rdlo] = bit_range(eval, 0, 31);
-                gpr[data.rdhi] = get_bit_range(eval, 32, 63);
+                gpr[data.rdhi] = bit_range(eval, 32, 63);
 
             } else {
                 int64_t eval =
@@ -210,23 +215,22 @@ Cpu::exec_arm(const ArmInstruction instruction) {
                                 static_cast<int64_t>(gpr[data.rdlo])
                             : 0);
 
-                gpr[data.rdlo] = get_bit_range(eval, 0, 31);
+                gpr[data.rdlo] = bit_range(eval, 0, 31);
-                gpr[data.rdhi] = get_bit_range(eval, 32, 63);
+                gpr[data.rdhi] = bit_range(eval, 32, 63);
             }
 
             if (data.set) {
-                cpsr.set_z(!(static_cast<bool>(gpr[data.rdhi]) ||
+                cpsr.set_z(gpr[data.rdhi] == 0 && gpr[data.rdlo] == 0);
-                             static_cast<bool>(gpr[data.rdlo])));
+                cpsr.set_n(get_bit(gpr[data.rdhi], 31));
-                cpsr.set_n(get_nth_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) {
+        [this, pc_error](ArmInstruction::SingleDataSwap& data) {
-            pc_warn(data.rm);
+            pc_error(data.rm);
-            pc_warn(data.rn);
+            pc_error(data.rn);
-            pc_warn(data.rd);
+            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);
 
+                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<uint32_t>(&data.operand)) {
+                op_2 = *immediate;
+            } else if (const Shift* shift = std::get_if<Shift>(&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));
+        debug(cur_pc);
-        log_info("{:#034b}", bus->read_word(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());
 
+        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;
         }
     }
+}

src/cpu/instruction.cc

@@ -1,8 +1,10 @@
 #include "cpu/instruction.hh"
+#include "cpu/utility.hh"
 #include "util/bits.hh"
+#include <iterator>
 
 ArmInstruction::ArmInstruction(uint32_t insn)
-  : condition(static_cast<Condition>(get_bit_range(insn, 28, 31))) {
+  : condition(static_cast<Condition>(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);
+        bool link       = get_bit(insn, 24);
-        uint32_t offset = get_bit_range(insn, 0, 23);
+        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 rm = bit_range(insn, 0, 3);
-        uint8_t rs = get_bit_range(insn, 8, 11);
+        uint8_t rs = bit_range(insn, 8, 11);
-        uint8_t rn = get_bit_range(insn, 12, 15);
+        uint8_t rn = bit_range(insn, 12, 15);
-        uint8_t rd = get_bit_range(insn, 16, 19);
+        uint8_t rd = bit_range(insn, 16, 19);
-        bool set   = get_nth_bit(insn, 20);
+        bool set   = get_bit(insn, 20);
-        bool acc   = get_nth_bit(insn, 21);
+        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 rm   = bit_range(insn, 0, 3);
-        uint8_t rs   = get_bit_range(insn, 8, 11);
+        uint8_t rs   = bit_range(insn, 8, 11);
-        uint8_t rdlo = get_bit_range(insn, 12, 15);
+        uint8_t rdlo = bit_range(insn, 12, 15);
-        uint8_t rdhi = get_bit_range(insn, 16, 19);
+        uint8_t rdhi = bit_range(insn, 16, 19);
-        bool set     = get_nth_bit(insn, 20);
+        bool set     = get_bit(insn, 20);
-        bool acc     = get_nth_bit(insn, 21);
+        bool acc     = get_bit(insn, 21);
-        bool uns     = get_nth_bit(insn, 22);
+        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 rm = bit_range(insn, 0, 3);
-        uint8_t rd = get_bit_range(insn, 12, 15);
+        uint8_t rd = bit_range(insn, 12, 15);
-        uint8_t rn = get_bit_range(insn, 16, 19);
+        uint8_t rn = bit_range(insn, 16, 19);
-        bool byte  = get_nth_bit(insn, 22);
+        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<uint16_t, Shift> offset;
-        uint8_t rd = get_bit_range(insn, 12, 15);
+        uint8_t rd = bit_range(insn, 12, 15);
-        uint8_t rn = get_bit_range(insn, 16, 19);
+        uint8_t rn = bit_range(insn, 16, 19);
-        bool load  = get_nth_bit(insn, 20);
+        bool load  = get_bit(insn, 20);
-        bool write = get_nth_bit(insn, 21);
+        bool write = get_bit(insn, 21);
-        bool byte  = get_nth_bit(insn, 22);
+        bool byte  = get_bit(insn, 22);
-        bool up    = get_nth_bit(insn, 23);
+        bool up    = get_bit(insn, 23);
-        bool pre   = get_nth_bit(insn, 24);
+        bool pre   = get_bit(insn, 24);
-        bool imm   = get_nth_bit(insn, 25);
+        bool imm   = get_bit(insn, 25);
 
         if (imm) {
-            uint8_t rm = get_bit_range(insn, 0, 3);
+            uint8_t rm = bit_range(insn, 0, 3);
-            bool reg   = get_nth_bit(insn, 4);
+            bool reg   = get_bit(insn, 4);
             ShiftType shift_type =
-              static_cast<ShiftType>(get_bit_range(insn, 5, 6));
+              static_cast<ShiftType>(bit_range(insn, 5, 6));
-            uint8_t operand = get_bit_range(insn, (reg ? 8 : 7), 11);
+            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<uint16_t>(get_bit_range(insn, 0, 11));
+            offset = static_cast<uint16_t>(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);
+        uint8_t offset = bit_range(insn, 0, 3);
-        bool half      = get_nth_bit(insn, 5);
+        bool half      = get_bit(insn, 5);
-        bool sign      = get_nth_bit(insn, 6);
+        bool sign      = get_bit(insn, 6);
-        uint8_t rd     = get_bit_range(insn, 12, 15);
+        uint8_t rd     = bit_range(insn, 12, 15);
-        uint8_t rn     = get_bit_range(insn, 16, 19);
+        uint8_t rn     = bit_range(insn, 16, 19);
-        bool load      = get_nth_bit(insn, 20);
+        bool load      = get_bit(insn, 20);
-        bool write     = get_nth_bit(insn, 21);
+        bool write     = get_bit(insn, 21);
-        bool imm       = get_nth_bit(insn, 22);
+        bool imm       = get_bit(insn, 22);
-        bool up        = get_nth_bit(insn, 23);
+        bool up        = get_bit(insn, 23);
-        bool pre       = get_nth_bit(insn, 24);
+        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<stringv, 2> 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);
+        data = BlockDataTransfer{ .regs  = regs,
-        uint8_t rn    = get_bit_range(insn, 16, 19);
+                                  .rn    = rn,
-        bool load     = get_nth_bit(insn, 20);
+                                  .load  = load,
-        bool write    = get_nth_bit(insn, 21);
+                                  .write = write,
-        bool s        = get_nth_bit(insn, 22);
+                                  .s     = s,
-        bool up       = get_nth_bit(insn, 23);
+                                  .up    = up,
-        bool pre      = get_nth_bit(insn, 24);
+                                  .pre   = pre };
 
-        // disassembly
+        // Data Processing
-        {
+    } else if ((insn & 0x0C000000) == 0x00000000) {
-            uint8_t lpu = load << 2 | pre << 1 | up;
+        uint8_t rd    = bit_range(insn, 12, 15);
-            std::string addr_mode;
+        uint8_t rn    = bit_range(insn, 16, 19);
+        bool set      = get_bit(insn, 20);
+        OpCode opcode = static_cast<OpCode>(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<Shift, uint32_t> operand;
 
-        // Software Interrupt
+            if (imm) {
-        // What to do here?
+                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<ShiftType>(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 offset = bit_range(insn, 0, 7);
-        uint8_t cpn    = get_bit_range(insn, 8, 11);
+        uint8_t cpn    = bit_range(insn, 8, 11);
-        uint8_t crd    = get_bit_range(insn, 12, 15);
+        uint8_t crd    = bit_range(insn, 12, 15);
-        uint8_t rn     = get_bit_range(insn, 16, 19);
+        uint8_t rn     = bit_range(insn, 16, 19);
-        bool load      = get_nth_bit(insn, 20);
+        bool load      = get_bit(insn, 20);
-        bool write     = get_nth_bit(insn, 21);
+        bool write     = get_bit(insn, 21);
-        bool len       = get_nth_bit(insn, 22);
+        bool len       = get_bit(insn, 22);
-        bool up        = get_nth_bit(insn, 23);
+        bool up        = get_bit(insn, 23);
-        bool pre       = get_nth_bit(insn, 24);
+        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 crm    = bit_range(insn, 0, 4);
-        uint8_t cp     = get_bit_range(insn, 5, 7);
+        uint8_t cp     = bit_range(insn, 5, 7);
-        uint8_t cpn    = get_bit_range(insn, 8, 11);
+        uint8_t cpn    = bit_range(insn, 8, 11);
-        uint8_t crd    = get_bit_range(insn, 12, 15);
+        uint8_t crd    = bit_range(insn, 12, 15);
-        uint8_t crn    = get_bit_range(insn, 16, 19);
+        uint8_t crn    = bit_range(insn, 16, 19);
-        uint8_t cp_opc = get_bit_range(insn, 20, 23);
+        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 crm    = bit_range(insn, 0, 4);
-        uint8_t cp     = get_bit_range(insn, 5, 7);
+        uint8_t cp     = bit_range(insn, 5, 7);
-        uint8_t cpn    = get_bit_range(insn, 8, 11);
+        uint8_t cpn    = bit_range(insn, 8, 11);
-        uint8_t rd     = get_bit_range(insn, 12, 15);
+        uint8_t rd     = bit_range(insn, 12, 15);
-        uint8_t crn    = get_bit_range(insn, 16, 19);
+        uint8_t crn    = bit_range(insn, 16, 19);
-        bool load      = get_nth_bit(insn, 20);
+        bool load      = get_bit(insn, 20);
-        uint8_t cp_opc = get_bit_range(insn, 21, 23);
+        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<uint32_t>(&data.operand)) {
+                op_2 = fmt::format("#{:d}", *operand);
+            } else if (const Shift* shift = std::get_if<Shift>(&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);

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<Mode>(psr & ~PSR_CLEAR_MODE);
@@ -18,20 +28,20 @@ Psr::set_mode(Mode mode) {
 
 State
 Psr::state() const {
-    return static_cast<State>(get_nth_bit(psr, 5));
+    return static_cast<State>(get_bit(psr, 5));
 }
 
 void
 Psr::set_state(State state) {
-    chg_nth_bit(psr, 5, static_cast<bool>(state));
+    chg_bit(psr, 5, static_cast<bool>(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)

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<int32_t>(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);
             }
     }

src/util/bits.hh

@@ -7,32 +7,32 @@ using std::size_t;
 
 template<std::integral Int>
 inline bool
-get_nth_bit(Int num, size_t n) {
+get_bit(Int num, size_t n) {
     return (num >> n) & 1;
 }
 
 template<std::integral Int>
 inline void
-set_nth_bit(Int& num, size_t n) {
+set_bit(Int& num, size_t n) {
     num |= (1 << n);
 }
 
 template<std::integral Int>
 inline void
-rst_nth_bit(Int& num, size_t n) {
+rst_bit(Int& num, size_t n) {
     num &= ~(1 << n);
 }
 
 template<std::integral Int>
 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<std::integral Int>
 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<Int>::digits - (std::is_unsigned<Int>::value) - end;