diff --git a/InstructionSets/M68k/Executor.hpp b/InstructionSets/M68k/Executor.hpp index 9762540d6..4e6dff75d 100644 --- a/InstructionSets/M68k/Executor.hpp +++ b/InstructionSets/M68k/Executor.hpp @@ -44,7 +44,8 @@ template class Executor { void stop(); void set_pc(uint32_t); void add_pc(uint32_t); - void decline_branch(); + void decline_branch() {} + void did_update_status(); // TODO: ownership of this shouldn't be here. struct Registers { @@ -79,6 +80,7 @@ template class Executor { CPU::SlicedInt32 data_[8], address_[8]; CPU::SlicedInt32 stack_pointers_[2]; uint32_t instruction_address_; + int active_stack_pointer_ = 0; // A lookup table to ensure that A7 is adjusted by 2 rather than 1 in // postincrement and predecrement mode. diff --git a/InstructionSets/M68k/Implementation/ExecutorImplementation.hpp b/InstructionSets/M68k/Implementation/ExecutorImplementation.hpp index 969a70fe9..567e2eca0 100644 --- a/InstructionSets/M68k/Implementation/ExecutorImplementation.hpp +++ b/InstructionSets/M68k/Implementation/ExecutorImplementation.hpp @@ -24,6 +24,7 @@ template void Executor::reset() { // Establish: supervisor state, all interrupts blocked. status_.set_status(0b0010'0011'1000'0000); + did_update_status(); // Seed stack pointer and program counter. data_[7].l = bus_handler_.template read(0); @@ -212,7 +213,23 @@ void Executor::run_for_instructions(int count) { const auto opcode = read_pc(); const Preinstruction instruction = decoder_.decode(opcode); - // TODO: check privilege level. + if(!status_.is_supervisor_ && instruction.requires_supervisor()) { + raise_exception(8); + continue; + } + if(instruction.operation == Operation::Undefined) { + switch(opcode & 0xf000) { + default: + raise_exception(4); + continue; + case 0xa000: + raise_exception(10); + continue; + case 0xf000: + raise_exception(11); + continue; + } + } // Temporary storage. CPU::SlicedInt32 operand_[2]; @@ -328,7 +345,30 @@ void Executor::set_state(const Registers &state) { // TODO: flow control, all below here. template -void Executor::raise_exception(int) {} +void Executor::raise_exception(int index) { + const uint32_t address = index << 2; + + // Grab the status to store, then switch into supervisor mode. + const uint16_t status = status_.status(); + status_.is_supervisor_ = 1; + did_update_status(); + + // Push status and the program counter at instruction start. + bus_handler_.template write(address_[7].l - 4, instruction_address_); + bus_handler_.template write(address_[7].l - 6, status); + address_[7].l -= 6; + + // Fetch the new program counter. + program_counter_.l = bus_handler_.template read(address); +} + +template +void Executor::did_update_status() { + // Shuffle the stack pointers. + stack_pointers_[active_stack_pointer_] = address_[7]; + address_[7] = stack_pointers_[status_.is_supervisor_]; + active_stack_pointer_ = status_.is_supervisor_; +} template void Executor::stop() {} @@ -339,9 +379,6 @@ void Executor::set_pc(uint32_t) {} template void Executor::add_pc(uint32_t) {} -template -void Executor::decline_branch() {} - } }