3use crate::Sh2;
4use crate::bus::BusInterface;
5use crate::instructions::{rm, rn};
6use jgenesis_common::num::SignBit;

8macro_rules! impl_compare {
9    ($name:ident, |$rn:ident| $compare:expr) => {
10        pub(crate) fn $name(&mut self, opcode: u16) {
11            let $rn = self.registers.gpr[rn(opcode)];
12            self.registers.sr.t = $compare;
13        }
14    };
15    ($name:ident, |$rm:ident, $rn:ident| $compare:expr) => {
16        pub(crate) fn $name(&mut self, opcode: u16) {
17            let $rm = self.registers.gpr[rm(opcode)];
18            let $rn = self.registers.gpr[rn(opcode)];
19            self.registers.sr.t = $compare;
20        }
21    };
22}
23
24impl Sh2 {
25    // ADD Rm, Rn
26    // Addition
27    pub(crate) fn add_rm_rn(&mut self, opcode: u16) {
28        let m = rm(opcode);
29        let n = rn(opcode);
30        self.registers.gpr[n] = self.registers.gpr[n].wrapping_add(self.registers.gpr[m]);
31    }
32
33    // ADD #imm, Rn
34    // Addition with immediate operand
35    pub(crate) fn add_imm_rn(&mut self, opcode: u16) {
36        let n = rn(opcode);
37        let imm = opcode as i8;
38        self.registers.gpr[n] = self.registers.gpr[n].wrapping_add(imm as u32);
39    }
40
41    // ADDC Rm, Rn
42    // Addition with carry
43    pub(crate) fn addc(&mut self, opcode: u16) {
44        let m = rm(opcode);
45        let n = rn(opcode);
46
47        let (partial_sum, carry1) = self.registers.gpr[m].overflowing_add(self.registers.gpr[n]);
48        let (sum, carry2) = partial_sum.overflowing_add(self.registers.sr.t.into());
49
50        self.registers.gpr[n] = sum;
51        self.registers.sr.t = carry1 || carry2;
52    }
53
54    // ADDV Rm, Rn
55    // Addition with signed overflow check
56    pub(crate) fn addv(&mut self, opcode: u16) {
57        let m = rm(opcode);
58        let n = rn(opcode);
59
60        let source_sign = self.registers.gpr[m].sign_bit();
61        let destination_sign = self.registers.gpr[n].sign_bit();
62        self.registers.gpr[n] = self.registers.gpr[n].wrapping_add(self.registers.gpr[m]);
63
64        // Signed overflow occurs when the operands have the same sign and the sum has a different sign
65        let sum_sign = self.registers.gpr[n].sign_bit();
66        self.registers.sr.t = source_sign == destination_sign && source_sign != sum_sign;
67    }
68
69    // SUB Rm, Rn
70    // Subtraction
71    pub(crate) fn sub_rm_rn(&mut self, opcode: u16) {
72        let m = rm(opcode);
73        let n = rn(opcode);
74
75        self.registers.gpr[n] = self.registers.gpr[n].wrapping_sub(self.registers.gpr[m]);
76    }
77
78    // SUBC Rm, Rn
79    // Subtraction with carry
80    pub(crate) fn subc(&mut self, opcode: u16) {
81        let m = rm(opcode);
82        let n = rn(opcode);
83
84        let (partial_diff, borrow1) = self.registers.gpr[n].overflowing_sub(self.registers.gpr[m]);
85        let (difference, borrow2) = partial_diff.overflowing_sub(self.registers.sr.t.into());
86
87        self.registers.gpr[n] = difference;
88        self.registers.sr.t = borrow1 || borrow2;
89    }
90
91    // SUBV Rm, Rn
92    // Subtraction with signed underflow check
93    pub(crate) fn subv(&mut self, opcode: u16) {
94        let m = rm(opcode);
95        let n = rn(opcode);
96
97        let source_sign = self.registers.gpr[m].sign_bit();
98        let dest_sign = self.registers.gpr[n].sign_bit();
99        self.registers.gpr[n] = self.registers.gpr[n].wrapping_sub(self.registers.gpr[m]);
100
101        // Signed overflow occurs when the operands have different signs and the difference sign does
102        // not match the left operand's sign
103        let difference_sign = self.registers.gpr[n].sign_bit();
104        self.registers.sr.t = source_sign != dest_sign && difference_sign != dest_sign;
105    }
106
107    // NEG Rm, Rn
108    // Negate
109    pub(crate) fn neg(&mut self, opcode: u16) {
110        let m = rm(opcode);
111        let n = rn(opcode);
112        self.registers.gpr[n] = 0_u32.wrapping_sub(self.registers.gpr[m]);
113    }
114
115    // NEGC Rm, Rn
116    // Negate with carry
117    pub(crate) fn negc(&mut self, opcode: u16) {
118        let m = rm(opcode);
119        let n = rn(opcode);
120
121        let (partial_diff, borrow1) = 0_u32.overflowing_sub(self.registers.gpr[m]);
122        let (difference, borrow2) = partial_diff.overflowing_sub(self.registers.sr.t.into());
123        self.registers.gpr[n] = difference;
124        self.registers.sr.t = borrow1 || borrow2;
125    }
126
127    // CMP/EQ Rm, Rn
128    // Set the T flag if Rm = Rn
129    impl_compare!(cmp_eq_rm_rn, |rm, rn| rm == rn);
130
131    // CMP/EQ #imm, R0
132    // Set the T flag if R0 = #imm
133    pub(crate) fn cmp_eq_imm_r0(&mut self, opcode: u16) {
134        let imm = opcode as i8;
135        self.registers.sr.t = self.registers.gpr[0] == imm as u32;
136    }
137
138    // CMP/GE Rm, Rn
139    // Set the T flag if Rn >= Rm (signed)
140    impl_compare!(cmp_ge, |rm, rn| (rn as i32) >= (rm as i32));
141
142    // CMP/GT Rm, Rn
143    // Set the T flag if Rn > Rm (signed)
144    impl_compare!(cmp_gt, |rm, rn| (rn as i32) > (rm as i32));
145
146    // CMP/HI Rm, Rn
147    // Set the T flag if Rn > Rm (unsigned)
148    impl_compare!(cmp_hi, |rm, rn| rn > rm);
149
150    // CMP/HS Rm, Rn
151    // Set the T flag if Rn >= Rm (unsigned)
152    impl_compare!(cmp_hs, |rm, rn| rn >= rm);
153
154    // CMP/PL Rn
155    // Set the T flag if Rn > 0
156    impl_compare!(cmp_pl, |rn| (rn as i32) > 0);
157
158    // CMP/PZ Rn
159    // Set the T flag if Rn >= 0
160    impl_compare!(cmp_pz, |rn| (rn as i32) >= 0);
161
162    // CMP/STR Rm, Rn
163    // Set the T flag if any individual byte is equal in Rm and Rn
164    pub(crate) fn cmp_str(&mut self, opcode: u16) {
165        let m = rm(opcode);
166        let n = rn(opcode);
167
168        let xor = self.registers.gpr[m] ^ self.registers.gpr[n];
169        self.registers.sr.t = (xor & 0xFF == 0)
170            || ((xor >> 8) & 0xFF == 0)
171            || ((xor >> 16) & 0xFF == 0)
172            || ((xor >> 24) & 0xFF == 0);
173    }
174
175    // EXTS.B Rm, Rn
176    // Sign extend byte
177    pub(crate) fn exts_b(&mut self, opcode: u16) {
178        let m = rm(opcode);
179        let n = rn(opcode);
180        self.registers.gpr[n] = self.registers.gpr[m] as i8 as u32;
181    }
182
183    // EXTS.W Rm, Rn
184    // Sign extend word
185    pub(crate) fn exts_w(&mut self, opcode: u16) {
186        let m = rm(opcode);
187        let n = rn(opcode);
188        self.registers.gpr[n] = self.registers.gpr[m] as i16 as u32;
189    }
190
191    // EXTU.B Rm, Rn
192    // Zero extend byte
193    pub(crate) fn extu_b(&mut self, opcode: u16) {
194        let m = rm(opcode);
195        let n = rn(opcode);
196        self.registers.gpr[n] = self.registers.gpr[m] & 0xFF;
197    }
198
199    // EXTU.W Rm, Rn
200    // Zero extend word
201    pub(crate) fn extu_w(&mut self, opcode: u16) {
202        let m = rm(opcode);
203        let n = rn(opcode);
204        self.registers.gpr[n] = self.registers.gpr[m] & 0xFFFF;
205    }
206
207    // DT Rn
208    // Decrement and test
209    pub(crate) fn dt(&mut self, opcode: u16) {
210        let n = rn(opcode);
211        self.registers.gpr[n] = self.registers.gpr[n].wrapping_sub(1);
212        self.registers.sr.t = self.registers.gpr[n] == 0;
213    }
214
215    // MUL.L Rm, Rn
216    // 32-bit x 32-bit -> 32-bit multiplication
217    pub(crate) fn mul(&mut self, opcode: u16) {
218        let m = rm(opcode);
219        let n = rn(opcode);
220        self.registers.macl = self.registers.gpr[m].wrapping_mul(self.registers.gpr[n]);
221    }
222
223    // MULS.W Rm, Rn
224    // Signed 16-bit x 16-bit -> 32-bit multiplication
225    pub(crate) fn muls(&mut self, opcode: u16) {
226        let m = rm(opcode);
227        let n = rn(opcode);
228
229        let operand_l: i32 = (self.registers.gpr[m] as i16).into();
230        let operand_r: i32 = (self.registers.gpr[n] as i16).into();
231        self.registers.macl = (operand_l * operand_r) as u32;
232    }
233
234    // MULU.W Rm, Rn
235    // Unsigned 16-bit x 16-bit -> 32-bit multiplication
236    pub(crate) fn mulu(&mut self, opcode: u16) {
237        let m = rm(opcode);
238        let n = rn(opcode);
239
240        let operand_l = self.registers.gpr[m] & 0xFFFF;
241        let operand_r = self.registers.gpr[n] & 0xFFFF;
242        self.registers.macl = operand_l * operand_r;
243    }
244
245    // DMULS.L Rm, Rn
246    // Signed 32-bit x 32-bit -> 64-bit multiplication
247    pub(crate) fn dmuls(&mut self, opcode: u16) {
248        let m = rm(opcode);
249        let n = rn(opcode);
250
251        let operand_l: i64 = (self.registers.gpr[m] as i32).into();
252        let operand_r: i64 = (self.registers.gpr[n] as i32).into();
253        self.registers.set_mac(operand_l * operand_r);
254    }
255
256    // DMULU Rm, Rn
257    // Unsigned 32-bit x 32-bit -> 64-bit multiplication
258    pub(crate) fn dmulu(&mut self, opcode: u16) {
259        let m = rm(opcode);
260        let n = rn(opcode);
261
262        let product = u64::from(self.registers.gpr[m]) * u64::from(self.registers.gpr[n]);
263        self.registers.macl = product as u32;
264        self.registers.mach = (product >> 32) as u32;
265    }
266
267    // MAC.W @Rm+, @Rn+
268    // Multiply and accumulate with word operands
269    pub(crate) fn mac_w(&mut self, opcode: u16, bus: &mut impl BusInterface) {
270        let m = rm(opcode);
271        let n = rn(opcode);
272
273        let operand_l = self.read_word(self.registers.gpr[m], bus) as i16;
274        self.registers.gpr[m] = self.registers.gpr[m].wrapping_add(2);
275
276        let operand_r = self.read_word(self.registers.gpr[n], bus) as i16;
277        self.registers.gpr[n] = self.registers.gpr[n].wrapping_add(2);
278
279        let product = i64::from(operand_l) * i64::from(operand_r);
280
281        if self.registers.sr.s {
282            // 16-bit x 16-bit + 32-bit -> 32-bit, with saturation
283            let sum = i64::from(self.registers.macl as i32) + product;
284            self.registers.macl = sum.clamp(i32::MIN.into(), i32::MAX.into()) as u32;
285            // TODO set overflow bit in MACH? manual suggests that only SH-1 does this
286        } else {
287            // 16-bit x 16-bit + 64-bit -> 64-bit
288            let sum = product.wrapping_add(self.registers.mac());
289            self.registers.set_mac(sum);
290        }
291    }
292
293    // MAC.L @Rm+, @Rn+
294    // Multiply and accumulate with longword operands
295    pub(crate) fn mac_l(&mut self, opcode: u16, bus: &mut impl BusInterface) {
296        let m = rm(opcode);
297        let n = rn(opcode);
298
299        let operand_l = self.read_longword(self.registers.gpr[m], bus) as i32;
300        self.registers.gpr[m] = self.registers.gpr[m].wrapping_add(4);
301
302        let operand_r = self.read_longword(self.registers.gpr[n], bus) as i32;
303        self.registers.gpr[n] = self.registers.gpr[n].wrapping_add(4);
304
305        let product = i64::from(operand_l) * i64::from(operand_r);
306        let product_sum = product.wrapping_add(self.registers.mac());
307
308        if self.registers.sr.s {
309            // Saturate to signed 48-bit
310            let clamped = product_sum.clamp(-(1 << 47), (1 << 47) - 1);
311            self.registers.set_mac(clamped);
312        } else {
313            self.registers.set_mac(product_sum);
314        }
315    }
316
317    // DIV0U
318    // Initialization step for unsigned division
319    pub(crate) fn div0u(&mut self) {
320        self.registers.sr.m = false;
321        self.registers.sr.q = false;
322        self.registers.sr.t = false;
323    }
324
325    // DIV0S Rm, Rn
326    // Initialization step for signed division
327    pub(crate) fn div0s(&mut self, opcode: u16) {
328        let divisor = self.registers.gpr[rm(opcode)];
329        let dividend = self.registers.gpr[rn(opcode)];
330
331        self.registers.sr.m = divisor.sign_bit();
332        self.registers.sr.q = dividend.sign_bit();
333        self.registers.sr.t = self.registers.sr.m != self.registers.sr.q;
334    }
335
336    // DIV1 Rm, Rn
337    // Division single step
338    pub(crate) fn div1(&mut self, opcode: u16) {
339        let m = rm(opcode);
340        let n = rn(opcode);
341
342        let divisor = self.registers.gpr[m];
343        let mut dividend = self.registers.gpr[n];
344
345        let prev_sign_bit = dividend.sign_bit();
346        dividend = (dividend << 1) | u32::from(self.registers.sr.t);
347
348        let prev_dividend = dividend;
349        let overflowed = if self.registers.sr.q == self.registers.sr.m {
350            dividend = dividend.wrapping_sub(divisor);
351            dividend > prev_dividend
352        } else {
353            dividend = dividend.wrapping_add(divisor);
354            dividend < prev_dividend
355        };
356
357        self.registers.sr.q = overflowed ^ prev_sign_bit ^ self.registers.sr.m;
358        self.registers.sr.t = self.registers.sr.q == self.registers.sr.m;
359        self.registers.gpr[n] = dividend;
360    }
361}
362
363#[cfg(test)]
364mod tests {
365    use super::*;
366
367    fn mn_opcode(rm: u16, rn: u16) -> u16 {
368        (rm << 4) | (rn << 8)
369    }
370
371    #[test]
372    fn unsigned_division() {
373        let mut cpu = Sh2::new(String::new());
374
375        cpu.registers.gpr[0] = 100000;
376        cpu.registers.gpr[1] = 300 << 16;
377
378        cpu.div0u();
379
380        let opcode = mn_opcode(1, 0);
381        for _ in 0..16 {
382            cpu.div1(opcode);
383        }
384
385        cpu.registers.gpr[0] = (cpu.registers.gpr[0] << 1) | u32::from(cpu.registers.sr.t);
386
387        assert_eq!(cpu.registers.gpr[0] & 0xFFFF, 333);
388    }
389
390    #[test]
391    fn signed_division() {
392        let mut cpu = Sh2::new(String::new());
393
394        for _ in 0..100 {
395            let dividend: i16 = rand::random();
396            let mut divisor = rand::random::<i16>() >> 7;
397            while divisor == 0 {
398                divisor = rand::random();
399            }
400
401            cpu.registers.gpr[0] = (divisor as u32) << 16;
402            cpu.registers.gpr[1] = dividend as u32;
403
404            cpu.registers.gpr[3] = cpu.registers.gpr[1];
405            cpu.rotcl(3 << 8);
406            cpu.subc(mn_opcode(2, 1));
407
408            cpu.div0s(mn_opcode(0, 1));
409            for _ in 0..16 {
410                cpu.div1(mn_opcode(0, 1));
411            }
412
413            cpu.registers.gpr[1] = (cpu.registers.gpr[1] as i16) as u32;
414
415            cpu.rotcl(1 << 8);
416            cpu.addc(mn_opcode(2, 1));
417
418            let quotient = cpu.registers.gpr[1] as i16;
419            assert_eq!(quotient, dividend / divisor);
420        }
421    }
422}