Implement RST instruction

src/cpu/instructions/control.rs

@@ -9,6 +9,9 @@ pub enum ControlFlowInstruction {
     Call {
         addr: u16,
     },
+    RestartAtAddress {
+        addr: u16,
+    },
     CallIfFlagMatches {
         flag: register::Flag,
         value: u8,

src/cpu/parse/control.rs

@@ -24,8 +24,12 @@ impl OpcodeParser for Parser {
 }
 
 fn parse_unconditional_control_operation(data: &View) -> ParseResult {
-    let opcode = parse::get_opcode_from_data(data);
+    parse_unconditional_immediate_based_control_flow_operation(data)
+        .or_parse(parse_restart_instruction)
+}
 
+fn parse_unconditional_immediate_based_control_flow_operation(data: &View) -> ParseResult {
+    let opcode = parse::get_opcode_from_data(data);
     match opcode {
         0xC3 => Ok(build_immediate_parameterized_control_flow_data(
             data,
@@ -39,6 +43,17 @@ fn parse_unconditional_control_operation(data: &View) -> ParseResult {
     }
 }
 
+fn parse_restart_instruction(opcode: u8) -> ParseResult {
+    if !(matches!(opcode & 0xF0, 0xC0..=0xF0) && matches!(opcode & 0x0F, 0x07 | 0x0F)) {
+        return Err(parse::Error::UnknownOpcode(opcode));
+    }
+
+    let addr = u16::from((opcode & 0xF0) - 0xC0) | u16::from((opcode & 0x0F) - 0x07);
+    let control_flow_instruction = ControlFlowInstruction::RestartAtAddress { addr };
+
+    Ok((Instruction::ControlFlow(control_flow_instruction), 1))
+}
+
 fn parse_conditional_control_operation(data: &View) -> ParseResult {
     let opcode = parse::get_opcode_from_data(data);
 
@@ -56,7 +71,9 @@ fn parse_conditional_control_operation(data: &View) -> ParseResult {
     }
 }
 
-fn conditional_control_flow_type_for_opcode(opcode: u8) -> Result<ConditionalControlFlowType, parse::Error> {
+fn conditional_control_flow_type_for_opcode(
+    opcode: u8,
+) -> Result<ConditionalControlFlowType, parse::Error> {
     match opcode {
         0xC2 | 0xD2 | 0xCA | 0xDA => Ok(ConditionalControlFlowType::Jump),
         0xC4 | 0xD4 | 0xCC | 0xDC => Ok(ConditionalControlFlowType::Call),

src/cpu/run/control.rs

@@ -12,6 +12,7 @@ impl Run for ControlFlowInstruction {
 
                 Ok(Cycles(16))
             }
+
             Self::JumpToImmediateIfFlagMatches { flag, value, addr } => {
                 if processor.registers.get_flag_bit(flag) == value {
                     processor.registers.set_single_16bit_register(
@@ -23,10 +24,12 @@ impl Run for ControlFlowInstruction {
                     Ok(Cycles(12))
                 }
             }
+
             Self::Call { addr } => {
                 do_call(processor, addr)?;
                 Ok(Cycles(24))
             }
+
             Self::CallIfFlagMatches { flag, value, addr } => {
                 if processor.registers.get_flag_bit(flag) == value {
                     do_call(processor, addr)?;
@@ -35,6 +38,21 @@ impl Run for ControlFlowInstruction {
                     Ok(Cycles(12))
                 }
             }
+
+            Self::RestartAtAddress { addr } => {
+                let current_pc = processor
+                    .registers
+                    .get_single_16bit_register(register::SingleSixteenBit::ProgramCounter);
+
+                // We know the rest instruction to be 1 byte. A bit of a hack, but eh.
+                processor.push_16bit_value_to_stack(current_pc.wrapping_add(1))?;
+
+                processor
+                    .registers
+                    .set_single_16bit_register(register::SingleSixteenBit::ProgramCounter, addr);
+
+                Ok(Cycles(16))
+            }
         }
     }
 }
@@ -45,7 +63,7 @@ fn do_call(processor: &mut Processor, to_addr: u16) -> Result<(), super::Error>
         .get_single_16bit_register(register::SingleSixteenBit::ProgramCounter);
 
     // We know the call instruction to be 3 bytes. A bit of a hack, but eh.
-    processor.push_16bit_value_to_stack(current_pc + 3)?;
+    processor.push_16bit_value_to_stack(current_pc.wrapping_add(3))?;
 
     processor
         .registers

tests/cpu/control.rs

@@ -52,6 +52,53 @@ fn test_call_pushes_pc_onto_stack() {
     assert_eq!(old_sp - 2, processor.registers.stack_pointer);
 }
 
+#[test_case(0xC7, 0x00)]
+#[test_case(0xCF, 0x08)]
+#[test_case(0xD7, 0x10)]
+#[test_case(0xDF, 0x18)]
+#[test_case(0xE7, 0x20)]
+#[test_case(0xEF, 0x28)]
+#[test_case(0xF7, 0x30)]
+#[test_case(0xFF, 0x38)]
+fn test_restart_adjusts_program_counter_to_encoded_value(opcode: u8, addr: u8) {
+    let mut processor = Processor::default();
+
+    let data = [opcode, 0x13];
+    let (ins, extra_data) = Instruction::from_data(&data).expect("could not parse instruction");
+
+    assert_eq!(extra_data, &[0x13]);
+
+    processor.run_instruction(ins);
+
+    assert_eq!(u16::from(addr), processor.registers.program_counter);
+}
+
+#[test_case(0xC7)]
+#[test_case(0xCF)]
+#[test_case(0xD7)]
+#[test_case(0xDF)]
+#[test_case(0xE7)]
+#[test_case(0xEF)]
+#[test_case(0xF7)]
+#[test_case(0xFF)]
+fn test_restart_adjusts_pushes_current_program_counter_to_stack(opcode: u8) {
+    let mut processor = Processor::default();
+    processor.registers.program_counter = 0xDEAD;
+
+    let data = [opcode, 0x13];
+    let (ins, extra_data) = Instruction::from_data(&data).expect("could not parse instruction");
+
+    assert_eq!(extra_data, &[0x13]);
+
+    let old_sp = processor.registers.stack_pointer;
+    processor.run_instruction(ins);
+
+    assert_eq!(0xDE, processor.memory.get((old_sp - 1).into()).unwrap());
+    // 0xAD + 1, which is the size of the RST instruction
+    assert_eq!(0xAE, processor.memory.get((old_sp - 2).into()).unwrap());
+    assert_eq!(old_sp - 2, processor.registers.stack_pointer);
+}
+
 #[test_case(0xCC, Flag::Zero, 1; "CALL Z")]
 #[test_case(0xDC, Flag::Carry, 1; "CALL C")]
 #[test_case(0xC4, Flag::Zero, 0; "CALL NZ")]