3use crate::bus::BusInterface;
4use crate::instructions::rn;
5use crate::{SP, Sh2};

7fn i12(opcode: u16) -> i16 {
8    ((opcode as i16) << 4) >> 4
9}
10
11macro_rules! impl_conditional_branch {
12    ($name:ident $(, $not:tt)?) => {
13        pub(crate) fn $name(&mut self, opcode: u16, bus: &mut impl BusInterface) {
14            if $($not)? self.registers.sr.t {
15                let disp = i32::from(opcode as i8) << 1;
16                self.registers.pc = self.registers.next_pc.wrapping_add(disp as u32);
17                self.registers.next_pc = self.registers.pc.wrapping_add(2);
18
19                bus.increment_cycle_counter(2);
20            }
21        }
22    }
23}
24
25macro_rules! impl_delayed_conditional_branch {
26    ($name:ident $(, $not:tt)?) => {
27        pub(crate) fn $name(&mut self, opcode: u16, bus: &mut impl BusInterface) {
28            if $($not)? self.registers.sr.t {
29                let disp = i32::from(opcode as i8) << 1;
30                self.registers.next_pc = self.registers.next_pc.wrapping_add(disp as u32);
31                self.registers.next_op_in_delay_slot = true;
32
33                bus.increment_cycle_counter(1);
34            }
35        }
36    }
37}
38
39impl Sh2 {
40    // JMP @Rm
41    // Unconditional jump
42    pub(crate) fn jmp(&mut self, opcode: u16, bus: &mut impl BusInterface) {
43        let n = rn(opcode);
44        self.registers.next_pc = self.registers.gpr[n];
45        self.registers.next_op_in_delay_slot = true;
46
47        bus.increment_cycle_counter(1);
48    }
49
50    // JSR @Rm
51    // Jump to subroutine
52    pub(crate) fn jsr(&mut self, opcode: u16, bus: &mut impl BusInterface) {
53        self.registers.pr = self.registers.next_pc;
54        self.jmp(opcode, bus);
55    }
56
57    // BRA label
58    // Unconditional branch
59    pub(crate) fn bra(&mut self, opcode: u16, bus: &mut impl BusInterface) {
60        let disp = i12(opcode) << 1;
61        self.registers.next_pc = self.registers.next_pc.wrapping_add(disp as u32);
62        self.registers.next_op_in_delay_slot = true;
63
64        bus.increment_cycle_counter(1);
65    }
66
67    // BRAF Rm
68    // Unconditional branch far
69    pub(crate) fn braf(&mut self, opcode: u16, bus: &mut impl BusInterface) {
70        let n = rn(opcode);
71        self.registers.next_pc = self.registers.next_pc.wrapping_add(self.registers.gpr[n]);
72        self.registers.next_op_in_delay_slot = true;
73
74        bus.increment_cycle_counter(1);
75    }
76
77    // BSR label
78    // Branch to subroutine
79    pub(crate) fn bsr(&mut self, opcode: u16, bus: &mut impl BusInterface) {
80        self.registers.pr = self.registers.next_pc;
81        self.bra(opcode, bus);
82    }
83
84    // BSRF Rm
85    // Branch to subroutine far
86    pub(crate) fn bsrf(&mut self, opcode: u16, bus: &mut impl BusInterface) {
87        self.registers.pr = self.registers.next_pc;
88        self.braf(opcode, bus);
89    }
90
91    // BF label
92    // Branch if false
93    impl_conditional_branch!(bf, !);
94
95    // BT label
96    // Branch if true
97    impl_conditional_branch!(bt);
98
99    // BF/S label
100    // Branch if false with delay slot
101    impl_delayed_conditional_branch!(bf_s, !);
102
103    // BT/S label
104    // Branch if true with delay slot
105    impl_delayed_conditional_branch!(bt_s);
106
107    // RTS
108    // Return from subroutine
109    pub(crate) fn rts(&mut self, bus: &mut impl BusInterface) {
110        self.registers.next_pc = self.registers.pr;
111        self.registers.next_op_in_delay_slot = true;
112
113        bus.increment_cycle_counter(1);
114    }
115
116    // RTE
117    // Return from exception
118    pub(crate) fn rte(&mut self, bus: &mut impl BusInterface) {
119        self.registers.next_pc = self.read_longword(self.registers.gpr[SP], bus);
120        self.registers.next_op_in_delay_slot = true;
121        self.registers.gpr[SP] = self.registers.gpr[SP].wrapping_add(4);
122
123        self.registers.sr = self.read_longword(self.registers.gpr[SP], bus).into();
124        self.registers.gpr[SP] = self.registers.gpr[SP].wrapping_add(4);
125    }
126
127    // TRAPA #imm
128    // Trap always
129    pub(crate) fn trapa(&mut self, opcode: u16, bus: &mut impl BusInterface) {
130        self.registers.gpr[SP] = self.registers.gpr[SP].wrapping_sub(4);
131        self.write_longword(self.registers.gpr[SP], self.registers.sr.into(), bus);
132
133        self.registers.gpr[SP] = self.registers.gpr[SP].wrapping_sub(4);
134        self.write_longword(self.registers.gpr[SP], self.registers.next_pc, bus);
135
136        let vector_number = opcode & 0xFF;
137        let vector_addr = self.registers.vbr.wrapping_add((vector_number << 2).into());
138        self.registers.pc = self.read_longword(vector_addr, bus);
139        self.registers.next_pc = self.registers.pc.wrapping_add(2);
140
141        bus.increment_cycle_counter(7);
142    }
143}