3use crate::svp::{PmcWaitingFor, StatusRegister, Svp};
4use jgenesis_common::num::GetBit;
5use std::fmt::{Display, Formatter};

7#[derive(Debug, Clone, Copy, PartialEq, Eq)]
8enum AluOp {
9    Add,
10    Subtract,
11    Compare,
12    And,
13    Or,
14    ExclusiveOr,
15}
16
17impl AluOp {
18    fn from_opcode(opcode: u16) -> Option<Self> {
19        match opcode & 0xE000 {
20            0x2000 => Some(Self::Subtract),
21            0x6000 => Some(Self::Compare),
22            0x8000 => Some(Self::Add),
23            0xA000 => Some(Self::And),
24            0xC000 => Some(Self::Or),
25            0xE000 => Some(Self::ExclusiveOr),
26            _ => None,
27        }
28    }
29}
30
31#[derive(Debug, Clone, Copy, PartialEq, Eq)]
32enum AccumulateOp {
33    Zero,
34    Add,
35    Subtract,
36}
37
38#[derive(Debug, Clone, Copy, PartialEq, Eq)]
39enum Condition {
40    True,
41    Zero,
42    NotZero,
43    Negative,
44    NotNegative,
45}
46
47impl Condition {
48    fn from_opcode(opcode: u16) -> Self {
49        match opcode & 0x01F0 {
50            0x0000 => Self::True,
51            0x0050 => Self::NotZero,
52            0x0150 => Self::Zero,
53            0x0070 => Self::NotNegative,
54            0x0170 => Self::Negative,
55            _ => panic!("Invalid SVP opcode (invalid condition): {opcode:04X}"),
56        }
57    }
58
59    fn check(self, status: StatusRegister) -> bool {
60        match self {
61            Self::True => true,
62            Self::Zero => status.zero,
63            Self::NotZero => !status.zero,
64            Self::Negative => status.negative,
65            Self::NotNegative => !status.negative,
66        }
67    }
68}
69
70#[derive(Debug, Clone, Copy, PartialEq, Eq)]
71enum RamBank {
72    Zero,
73    One,
74}
75
76impl RamBank {
77    fn from_opcode(opcode: u16) -> Self {
78        if opcode.bit(8) { Self::One } else { Self::Zero }
79    }
80}
81
82impl Display for RamBank {
83    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
84        match self {
85            Self::Zero => write!(f, "RAM0"),
86            Self::One => write!(f, "RAM1"),
87        }
88    }
89}
90
91#[derive(Debug, Clone, Copy, PartialEq, Eq)]
92enum AddressingMode {
93    // d / s
94    GeneralRegister(u16),
95    // ri / rj
96    PointerRegister(RamBank, u16),
97    // (ri) / (rj)
98    Indirect { bank: RamBank, pointer: u16, modifier: u16 },
99    // ((ri)) / ((rj))
100    DoubleIndirect { bank: RamBank, pointer: u16, modifier: u16 },
101    // addr
102    Direct { bank: RamBank, address: u8 },
103    // imm
104    Immediate,
105    // simm
106    ShortImmediate(u8),
107    // (A)
108    AccumulatorIndirect,
109}
110
111impl AddressingMode {
112    const ACCUMULATOR_REGISTER: Self = Self::GeneralRegister(3);
113}
114
115impl Display for AddressingMode {
116    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
117        match self {
118            Self::GeneralRegister(0) => write!(f, "NULL"),
119            Self::GeneralRegister(1) => write!(f, "X"),
120            Self::GeneralRegister(2) => write!(f, "Y"),
121            Self::GeneralRegister(3) => write!(f, "AH"),
122            Self::GeneralRegister(4) => write!(f, "ST"),
123            Self::GeneralRegister(5) => write!(f, "STACK"),
124            Self::GeneralRegister(6) => write!(f, "PC"),
125            Self::GeneralRegister(7) => write!(f, "P"),
126            Self::GeneralRegister(8) => write!(f, "PM0/XSTStatus"),
127            Self::GeneralRegister(9) => write!(f, "PM1"),
128            Self::GeneralRegister(10) => write!(f, "PM2"),
129            Self::GeneralRegister(11) => write!(f, "PM3/XST"),
130            Self::GeneralRegister(12) => write!(f, "PM4"),
131            Self::GeneralRegister(14) => write!(f, "PMC"),
132            Self::GeneralRegister(15) => write!(f, "AL"),
133            Self::GeneralRegister(_) => write!(f, "(invalid register)"),
134            Self::PointerRegister(bank, pointer) => write!(f, "RIJ[{bank}, {pointer}]"),
135            Self::Indirect { bank, pointer, modifier } => {
136                write!(f, "(RIJ)[{bank}, {pointer}]/mod={modifier}")
137            }
138            Self::DoubleIndirect { bank, pointer, modifier } => {
139                write!(f, "((RIJ))[{bank}, {pointer}]/mod={modifier}")
140            }
141            Self::Direct { bank, address } => write!(f, "{bank}[{address:02X}]"),
142            Self::Immediate => write!(f, "#<imm>"),
143            Self::ShortImmediate(value) => write!(f, "#<{value:02X}>"),
144            Self::AccumulatorIndirect => write!(f, "(AH)"),
145        }
146    }
147}
148
149pub fn execute_instruction(svp: &mut Svp, rom: &[u16]) {
150    let opcode = fetch_operand(svp, rom);
151
152    log::trace!("PC={:04X}, opcode={opcode:04X}", svp.registers.pc.wrapping_sub(1));
153
154    // The first 8 bits are enough to distinguish between all opcodes
155    match opcode & 0xFF00 {
156        0x0000 => {
157            // ld d, s
158            ld_d_s(svp, rom, opcode);
159        }
160        0x0200 | 0x0300 => {
161            // ld d, (ri)
162            ld_d_ri_indirect(svp, rom, opcode);
163        }
164        0x0400 | 0x0500 => {
165            // ld (ri), s
166            ld_ri_s_indirect(svp, rom, opcode);
167        }
168        0x0600 | 0x0700 => {
169            // ld A, addr
170            ld_a_addr(svp, rom, opcode);
171        }
172        0x0800 => {
173            // ldi d, imm
174            ldi_d_imm(svp, rom, opcode);
175        }
176        0x0A00 | 0x0B00 => {
177            // ld d, ((ri))
178            ld_d_ri_double_indirect(svp, rom, opcode);
179        }
180        0x0C00 | 0x0D00 => {
181            // ldi (ri), imm
182            ldi_ri_imm(svp, rom, opcode);
183        }
184        0x0E00 | 0x0F00 => {
185            // ld addr, A
186            ld_addr_a(svp, rom, opcode);
187        }
188        0x1200 | 0x1300 => {
189            // ld d, ri
190            ld_d_ri(svp, rom, opcode);
191        }
192        0x1400 | 0x1500 => {
193            // ld ri, s
194            ld_ri_s(svp, rom, opcode);
195        }
196        0x1800..=0x1F00 => {
197            // ldi ri, simm
198            ldi_ri_simm(svp, rom, opcode);
199        }
200        0x3700 => {
201            // mpys (rj), (ri)
202            execute_multiply_accumulate(svp, opcode, AccumulateOp::Subtract);
203        }
204        0x4800 | 0x4900 => {
205            // call cond, addr
206            execute_call(svp, rom, opcode);
207        }
208        0x4A00 => {
209            // ld d, (a)
210            ld_d_a_indirect(svp, rom, opcode);
211        }
212        0x4C00 | 0x4D00 => {
213            // bra cond, addr
214            execute_bra(svp, rom, opcode);
215        }
216        0x9000 | 0x9100 => {
217            // mod cond, op
218            execute_mod(svp, opcode);
219        }
220        0x9700 => {
221            // mpya (rj), (ri)
222            execute_multiply_accumulate(svp, opcode, AccumulateOp::Add);
223        }
224        0xB700 => {
225            // mld (rj), (ri)
226            execute_multiply_accumulate(svp, opcode, AccumulateOp::Zero);
227        }
228        0xFF00 => {
229            // Treat as a no-op; do nothing
230        }
231        _ => {
232            // ALU ops; highest 3 bits determine op, next 5 bits determine addressing mode
233            execute_alu(svp, rom, opcode);
234        }
235    }
236}
237
238fn fetch_operand(svp: &mut Svp, rom: &[u16]) -> u16 {
239    let operand = svp.read_program_memory(svp.registers.pc, rom);
240    svp.registers.pc = svp.registers.pc.wrapping_add(1);
241    operand
242}
243
244fn execute_load(svp: &mut Svp, rom: &[u16], source: AddressingMode, dest: AddressingMode) {
245    log::trace!("  LD source={source}, dest={dest}");
246
247    match (source, dest) {
248        (AddressingMode::GeneralRegister(7), AddressingMode::GeneralRegister(3)) => {
249            // P to A; copy all 32 bits
250            svp.registers.accumulator = svp.registers.product();
251            log::trace!("  A = {:08X}", svp.registers.accumulator);
252        }
253        (
254            AddressingMode::GeneralRegister(0),
255            AddressingMode::GeneralRegister(register @ 8..=15),
256        ) => {
257            // Blind write; program PM register (if applicable) and reset PMC state
258            log::trace!("Blind write to register {register}");
259
260            if register <= 12 {
261                let pm_idx = (register - 8) as usize;
262                svp.registers.pm_write[pm_idx]
263                    .initialize(svp.registers.pmc.address, svp.registers.pmc.mode);
264
265                log::trace!(
266                    "Initialized PM{pm_idx} for writes: {:X?}",
267                    svp.registers.pm_write[pm_idx]
268                );
269            }
270
271            if register != 14 {
272                svp.registers.pmc.waiting_for = PmcWaitingFor::Address;
273            } else {
274                // Blind writes to PMC toggle state instead of resetting
275                svp.registers.pmc.waiting_for = svp.registers.pmc.waiting_for.toggle();
276            }
277        }
278        (
279            AddressingMode::GeneralRegister(register @ 8..=15),
280            AddressingMode::GeneralRegister(0),
281        ) => {
282            // Blind read; program PM register (if applicable) and reset PMC state
283            log::trace!("Blind read to register {register}");
284
285            if register <= 12 {
286                let pm_idx = (register - 8) as usize;
287                svp.registers.pm_read[pm_idx]
288                    .initialize(svp.registers.pmc.address, svp.registers.pmc.mode);
289
290                log::trace!(
291                    "Initialized PM{pm_idx} for reads: {:X?}",
292                    svp.registers.pm_read[pm_idx]
293                );
294            }
295
296            if register != 14 {
297                svp.registers.pmc.waiting_for = PmcWaitingFor::Address;
298            } else {
299                // Blind reads to PMC toggle state instead of resetting
300                svp.registers.pmc.waiting_for = svp.registers.pmc.waiting_for.toggle();
301            }
302        }
303        _ => {
304            // Normal 16-bit load
305            let value = read_addressing_mode(svp, rom, source);
306            write_addressing_mode(svp, dest, value);
307
308            log::trace!("  Wrote value {value:04X}");
309        }
310    }
311}
312
313fn execute_alu(svp: &mut Svp, rom: &[u16], opcode: u16) {
314    let Some(op) = AluOp::from_opcode(opcode) else {
315        panic!("Invalid SSP1601 opcode: {opcode:04X}");
316    };
317
318    let source = parse_alu_addressing_mode(opcode);
319
320    log::trace!("  ALU op={op:?}, source={source}");
321
322    // ALU operations are 32-bit, but most sources are 16-bit.
323    // If A or P is the source, use all 32 bits; otherwise shift the 16-bit value into the high
324    // word of a 32-bit value
325    let operand = match source {
326        AddressingMode::GeneralRegister(3) => svp.registers.accumulator,
327        AddressingMode::GeneralRegister(7) => svp.registers.product(),
328        _ => u32::from(read_addressing_mode(svp, rom, source)) << 16,
329    };
330
331    let accumulator = svp.registers.accumulator;
332    let result = match op {
333        AluOp::Add => accumulator.wrapping_add(operand),
334        AluOp::Subtract | AluOp::Compare => accumulator.wrapping_sub(operand),
335        AluOp::And => accumulator & operand,
336        AluOp::Or => accumulator | operand,
337        AluOp::ExclusiveOr => accumulator ^ operand,
338    };
339
340    update_flags(svp, result);
341    if op != AluOp::Compare {
342        svp.registers.accumulator = result;
343    }
344
345    log::trace!("  ALU result {result:08X}");
346}
347
348fn update_flags(svp: &mut Svp, accumulator: u32) {
349    svp.registers.status.zero = accumulator == 0;
350    svp.registers.status.negative = accumulator.bit(31);
351}
352
353fn execute_mod(svp: &mut Svp, opcode: u16) {
354    let condition = Condition::from_opcode(opcode);
355    if !condition.check(svp.registers.status) {
356        log::trace!("  MODIFY cond={condition:?}, condition false");
357        return;
358    }
359
360    // Lowest 3 bits determine operation
361    match opcode & 0x0007 {
362        0x0002 => {
363            // Arithmetic right shift
364            log::trace!("  ASR cond={condition:?}");
365
366            svp.registers.accumulator = ((svp.registers.accumulator as i32) >> 1) as u32;
367        }
368        0x0003 => {
369            // Left shift
370            log::trace!("  SL cond={condition:?}");
371
372            svp.registers.accumulator <<= 1;
373        }
374        0x0006 => {
375            // Negate
376            log::trace!("  NEG cond={condition:?}");
377
378            svp.registers.accumulator = (!svp.registers.accumulator).wrapping_add(1);
379        }
380        0x0007 => {
381            // Absolute value
382            log::trace!("  ABS cond={condition:?}");
383
384            if svp.registers.accumulator.bit(31) {
385                svp.registers.accumulator = (!svp.registers.accumulator).wrapping_add(1);
386            }
387        }
388        _ => panic!("Invalid SVP opcode: {opcode:04X}"),
389    }
390
391    update_flags(svp, svp.registers.accumulator);
392}
393
394fn execute_call(svp: &mut Svp, rom: &[u16], opcode: u16) {
395    let address = fetch_operand(svp, rom);
396    let condition = Condition::from_opcode(opcode);
397
398    log::trace!("  CALL cond={condition:?}, address={address:04X}");
399
400    if condition.check(svp.registers.status) {
401        svp.registers.stack.push(svp.registers.pc);
402        svp.registers.pc = address;
403    }
404}
405
406fn execute_bra(svp: &mut Svp, rom: &[u16], opcode: u16) {
407    let address = fetch_operand(svp, rom);
408    let condition = Condition::from_opcode(opcode);
409
410    log::trace!("  BRA cond={condition:?}, address={address:04X}");
411
412    if condition.check(svp.registers.status) {
413        svp.registers.pc = address;
414    }
415}
416
417fn execute_multiply_accumulate(svp: &mut Svp, opcode: u16, op: AccumulateOp) {
418    // Accumulation is performed before changing the multiply result via X and Y
419    match op {
420        AccumulateOp::Zero => {
421            svp.registers.accumulator = 0;
422        }
423        AccumulateOp::Add => {
424            svp.registers.accumulator =
425                svp.registers.accumulator.wrapping_add(svp.registers.product());
426        }
427        AccumulateOp::Subtract => {
428            svp.registers.accumulator =
429                svp.registers.accumulator.wrapping_sub(svp.registers.product());
430        }
431    }
432
433    update_flags(svp, svp.registers.accumulator);
434
435    // X is always set to a value from RAM0
436    let x_pointer = opcode & 0x03;
437    let x_modifier = (opcode >> 2) & 0x03;
438    let ram0_addr = read_pointer(svp, RamBank::Zero, x_pointer, x_modifier);
439    svp.registers.x = svp.ram0[ram0_addr as usize];
440
441    // Y is always set to a value from RAM1
442    let y_pointer = (opcode >> 4) & 0x03;
443    let y_modifier = (opcode >> 6) & 0x03;
444    let ram1_addr = read_pointer(svp, RamBank::One, y_pointer, y_modifier);
445    svp.registers.y = svp.ram1[ram1_addr as usize];
446
447    log::trace!(
448        "  MUL op={op:?}, X=RAM0[p{x_pointer}]/mod={x_modifier}, Y=RAM1[p{y_pointer}]/mod={y_modifier}"
449    );
450    log::trace!("  A={:08X}, P={:08X}", svp.registers.accumulator, svp.registers.product());
451}
452
453fn ld_d_s(svp: &mut Svp, rom: &[u16], opcode: u16) {
454    let source = AddressingMode::GeneralRegister(opcode & 0xF);
455    let dest = AddressingMode::GeneralRegister((opcode >> 4) & 0xF);
456    execute_load(svp, rom, source, dest);
457}
458
459fn ld_d_ri(svp: &mut Svp, rom: &[u16], opcode: u16) {
460    let bank = RamBank::from_opcode(opcode);
461    let pointer = opcode & 0x03;
462    let source = AddressingMode::PointerRegister(bank, pointer);
463
464    let dest = AddressingMode::GeneralRegister((opcode >> 4) & 0xF);
465
466    execute_load(svp, rom, source, dest);
467}
468
469fn ld_ri_s(svp: &mut Svp, rom: &[u16], opcode: u16) {
470    let bank = RamBank::from_opcode(opcode);
471    let pointer = opcode & 0x03;
472    let dest = AddressingMode::PointerRegister(bank, pointer);
473
474    let source = AddressingMode::GeneralRegister((opcode >> 4) & 0xF);
475
476    execute_load(svp, rom, source, dest);
477}
478
479fn ld_d_ri_indirect(svp: &mut Svp, rom: &[u16], opcode: u16) {
480    let bank = RamBank::from_opcode(opcode);
481    let pointer = opcode & 0x03;
482    let modifier = (opcode >> 2) & 0x03;
483    let source = AddressingMode::Indirect { bank, pointer, modifier };
484
485    let dest = AddressingMode::GeneralRegister((opcode >> 4) & 0xF);
486
487    execute_load(svp, rom, source, dest);
488}
489
490fn ld_ri_s_indirect(svp: &mut Svp, rom: &[u16], opcode: u16) {
491    let bank = RamBank::from_opcode(opcode);
492    let pointer = opcode & 0x03;
493    let modifier = (opcode >> 2) & 0x03;
494    let dest = AddressingMode::Indirect { bank, pointer, modifier };
495
496    let source = AddressingMode::GeneralRegister((opcode >> 4) & 0xF);
497
498    execute_load(svp, rom, source, dest);
499}
500
501fn ld_d_ri_double_indirect(svp: &mut Svp, rom: &[u16], opcode: u16) {
502    let bank = RamBank::from_opcode(opcode);
503    let pointer = opcode & 0x03;
504    let modifier = (opcode >> 2) & 0x03;
505    let source = AddressingMode::DoubleIndirect { bank, pointer, modifier };
506
507    let dest = AddressingMode::GeneralRegister((opcode >> 4) & 0xF);
508
509    execute_load(svp, rom, source, dest);
510}
511
512fn ld_a_addr(svp: &mut Svp, rom: &[u16], opcode: u16) {
513    let bank = RamBank::from_opcode(opcode);
514    let address = opcode as u8;
515    let source = AddressingMode::Direct { bank, address };
516
517    let dest = AddressingMode::ACCUMULATOR_REGISTER;
518
519    execute_load(svp, rom, source, dest);
520}
521
522fn ld_addr_a(svp: &mut Svp, rom: &[u16], opcode: u16) {
523    let bank = RamBank::from_opcode(opcode);
524    let address = opcode as u8;
525    let dest = AddressingMode::Direct { bank, address };
526
527    let source = AddressingMode::ACCUMULATOR_REGISTER;
528
529    execute_load(svp, rom, source, dest);
530}
531
532fn ldi_d_imm(svp: &mut Svp, rom: &[u16], opcode: u16) {
533    let source = AddressingMode::Immediate;
534    let dest = AddressingMode::GeneralRegister((opcode >> 4) & 0xF);
535    execute_load(svp, rom, source, dest);
536}
537
538fn ldi_ri_imm(svp: &mut Svp, rom: &[u16], opcode: u16) {
539    let bank = RamBank::from_opcode(opcode);
540    let pointer = opcode & 0x03;
541    let modifier = (opcode >> 2) & 0x03;
542    let dest = AddressingMode::Indirect { bank, pointer, modifier };
543
544    let source = AddressingMode::Immediate;
545
546    execute_load(svp, rom, source, dest);
547}
548
549fn ldi_ri_simm(svp: &mut Svp, rom: &[u16], opcode: u16) {
550    let source = AddressingMode::ShortImmediate(opcode as u8);
551
552    let bank = if opcode.bit(10) { RamBank::One } else { RamBank::Zero };
553    let pointer = (opcode >> 8) & 0x03;
554    let dest = AddressingMode::PointerRegister(bank, pointer);
555
556    execute_load(svp, rom, source, dest);
557}
558
559fn ld_d_a_indirect(svp: &mut Svp, rom: &[u16], opcode: u16) {
560    let source = AddressingMode::AccumulatorIndirect;
561    let dest = AddressingMode::GeneralRegister((opcode >> 4) & 0xF);
562    execute_load(svp, rom, source, dest);
563}
564
565fn parse_alu_addressing_mode(opcode: u16) -> AddressingMode {
566    match opcode & 0x1F00 {
567        0x0000 => {
568            // OP A, s
569            AddressingMode::GeneralRegister(opcode & 0xF)
570        }
571        0x0200 | 0x0300 => {
572            // OP A, (ri)
573            let bank = RamBank::from_opcode(opcode);
574            let pointer = opcode & 0x03;
575            let modifier = (opcode >> 2) & 0x03;
576            AddressingMode::Indirect { bank, pointer, modifier }
577        }
578        0x0600 | 0x0700 => {
579            // OP A, addr
580            let bank = RamBank::from_opcode(opcode);
581            let address = opcode as u8;
582            AddressingMode::Direct { bank, address }
583        }
584        0x0800 => {
585            // OPi A, imm
586            AddressingMode::Immediate
587        }
588        0x0A00 | 0x0B00 => {
589            // OP A, ((ri))
590            let bank = RamBank::from_opcode(opcode);
591            let pointer = opcode & 0x03;
592            let modifier = (opcode >> 2) & 0x03;
593            AddressingMode::DoubleIndirect { bank, pointer, modifier }
594        }
595        0x1200 | 0x1300 => {
596            // OP A, ri
597            let bank = RamBank::from_opcode(opcode);
598            let pointer = opcode & 0x03;
599            AddressingMode::PointerRegister(bank, pointer)
600        }
601        0x1800 => {
602            // OPi A, simm
603            let immediate_value = opcode as u8;
604            AddressingMode::ShortImmediate(immediate_value)
605        }
606        _ => panic!("Invalid SVP opcode: {opcode:04X}"),
607    }
608}
609
610fn read_addressing_mode(svp: &mut Svp, rom: &[u16], source: AddressingMode) -> u16 {
611    match source {
612        AddressingMode::GeneralRegister(register) => read_register(svp, rom, register),
613        AddressingMode::PointerRegister(bank, register) => match (bank, register) {
614            (RamBank::Zero, 0..=2) => svp.registers.ram0_pointers[register as usize].into(),
615            (RamBank::One, 0..=2) => svp.registers.ram1_pointers[register as usize].into(),
616            (RamBank::Zero | RamBank::One, 3) => 0,
617            _ => panic!("invalid pointer register: {register}"),
618        },
619        AddressingMode::Indirect { bank, pointer, modifier } => {
620            // Read a value from internal RAM using a pointer register
621            let ram_addr = read_pointer(svp, bank, pointer, modifier);
622            match bank {
623                RamBank::Zero => svp.ram0[ram_addr as usize],
624                RamBank::One => svp.ram1[ram_addr as usize],
625            }
626        }
627        AddressingMode::DoubleIndirect { bank, pointer, modifier } => {
628            // Read a value from program memory, with the address determined by an indirect read
629            // The address stored in internal RAM is incremented after the read
630            let ram_addr = read_pointer(svp, bank, pointer, modifier);
631            let ram = match bank {
632                RamBank::Zero => &mut svp.ram0,
633                RamBank::One => &mut svp.ram1,
634            };
635
636            let indirect_addr = ram[ram_addr as usize];
637            ram[ram_addr as usize] = indirect_addr.wrapping_add(1);
638
639            log::trace!("  Indirect addr = {indirect_addr:04X}");
640
641            svp.read_program_memory(indirect_addr, rom)
642        }
643        AddressingMode::Direct { bank, address } => match bank {
644            // Read a value from internal RAM using a direct address
645            RamBank::Zero => svp.ram0[address as usize],
646            RamBank::One => svp.ram1[address as usize],
647        },
648        AddressingMode::Immediate => {
649            // 16-bit immediate value, specified in the next word in program memory
650            let value = fetch_operand(svp, rom);
651            log::trace!("  Immediate value: {value:04X}");
652            value
653        }
654        AddressingMode::ShortImmediate(value) => {
655            // 8-bit immediate value embedded in the opcode, zero extended to 16 bits
656            value.into()
657        }
658        AddressingMode::AccumulatorIndirect => {
659            // Read program memory, using the high word of the accumulator as the address
660            let address = (svp.registers.accumulator >> 16) as u16;
661            svp.read_program_memory(address, rom)
662        }
663    }
664}
665
666fn write_addressing_mode(svp: &mut Svp, dest: AddressingMode, value: u16) {
667    match dest {
668        AddressingMode::GeneralRegister(register) => {
669            write_register(svp, register, value);
670        }
671        AddressingMode::PointerRegister(bank, pointer) => {
672            // Pointer registers 3/7 are not writable
673            if pointer < 3 {
674                match bank {
675                    RamBank::Zero => {
676                        svp.registers.ram0_pointers[pointer as usize] = value as u8;
677                    }
678                    RamBank::One => {
679                        svp.registers.ram1_pointers[pointer as usize] = value as u8;
680                    }
681                }
682            }
683        }
684        AddressingMode::Indirect { bank, pointer, modifier } => {
685            // Write a value to internal RAM using a pointer register
686            let ram_addr = read_pointer(svp, bank, pointer, modifier);
687            match bank {
688                RamBank::Zero => {
689                    svp.ram0[ram_addr as usize] = value;
690                }
691                RamBank::One => {
692                    svp.ram1[ram_addr as usize] = value;
693                }
694            }
695        }
696        AddressingMode::Direct { bank, address } => match bank {
697            // Write a value to internal RAM using a direct address
698            RamBank::Zero => {
699                svp.ram0[address as usize] = value;
700            }
701            RamBank::One => {
702                svp.ram1[address as usize] = value;
703            }
704        },
705        AddressingMode::DoubleIndirect { .. }
706        | AddressingMode::Immediate
707        | AddressingMode::ShortImmediate(..)
708        | AddressingMode::AccumulatorIndirect => panic!("Invalid write addressing mode: {dest:?}"),
709    }
710}
711
712#[allow(clippy::match_same_arms)]
713fn read_register(svp: &mut Svp, rom: &[u16], register: u16) -> u16 {
714    match register {
715        0 => {
716            // Dummy/null register; always reads $FFFF
717            0xFFFF
718        }
719        1 => {
720            // X register
721            svp.registers.x
722        }
723        2 => {
724            // Y register
725            svp.registers.y
726        }
727        3 => {
728            // Accumulator, high word
729            (svp.registers.accumulator >> 16) as u16
730        }
731        4 => {
732            // Status register
733            svp.registers.status.into()
734        }
735        5 => {
736            // Stack register; reads pop
737            svp.registers.stack.pop()
738        }
739        6 => {
740            // PC
741            svp.registers.pc
742        }
743        7 => {
744            // P register
745            (svp.registers.product() >> 16) as u16
746        }
747        8 => {
748            // PM0 / XST status
749            if svp.registers.status.st_bits_set() {
750                // PM0
751                pm_read(svp, rom, 0)
752            } else {
753                // XST status
754                svp.registers.xst.ssp_read_status()
755            }
756        }
757        9 => {
758            // PM1
759            pm_read(svp, rom, 1)
760        }
761        10 => {
762            // PM2
763            pm_read(svp, rom, 2)
764        }
765        11 => {
766            // PM3 / XST register
767            if svp.registers.status.st_bits_set() {
768                // PM3
769                pm_read(svp, rom, 3)
770            } else {
771                // XST register
772                svp.registers.xst.value
773            }
774        }
775        12 => {
776            // PM4
777            pm_read(svp, rom, 4)
778        }
779        13 => {
780            // Unknown; unused by SVP code
781            0xFFFF
782        }
783        14 => {
784            // PMC register
785            log::trace!("PMC read");
786            svp.registers.pmc.read()
787        }
788        15 => {
789            // Accumulator, low word
790            svp.registers.accumulator as u16
791        }
792        _ => panic!("Invalid SVP register number: {register}"),
793    }
794}
795
796#[allow(clippy::match_same_arms)]
797fn write_register(svp: &mut Svp, register: u16, value: u16) {
798    match register {
799        0 => {
800            // Dummy/null register; writes do nothing
801        }
802        1 => {
803            // X register
804            svp.registers.x = value;
805        }
806        2 => {
807            // Y register
808            svp.registers.y = value;
809        }
810        3 => {
811            // Accumulator, high word
812            svp.registers.accumulator =
813                (svp.registers.accumulator & 0x0000_FFFF) | (u32::from(value) << 16);
814        }
815        4 => {
816            // Status register
817            svp.registers.status.write(value);
818        }
819        5 => {
820            // Stack; writes push
821            svp.registers.stack.push(value);
822        }
823        6 => {
824            // PC
825            svp.registers.pc = value;
826        }
827        7 => {
828            // P register; writes do nothing because P always equals 2 * X * Y
829        }
830        8 => {
831            // PM0 / XST status
832            if svp.registers.status.st_bits_set() {
833                // PM0
834                pm_write(svp, 0, value);
835            } else {
836                // XST status; is this even writable? SVP code seems to write to it
837                svp.registers.xst.m68k_written = value.bit(1);
838                svp.registers.xst.ssp_written = value.bit(0);
839            }
840        }
841        9 => {
842            // PM1
843            pm_write(svp, 1, value);
844        }
845        10 => {
846            // PM2
847            pm_write(svp, 2, value);
848        }
849        11 => {
850            // PM3 / XST register
851            if svp.registers.status.st_bits_set() {
852                // PM3
853                pm_write(svp, 3, value);
854            } else {
855                // XST register
856                svp.registers.xst.ssp_write(value);
857            }
858        }
859        12 => {
860            // PM4
861            pm_write(svp, 4, value);
862        }
863        13 => {
864            // Unknown; unused by SVP
865        }
866        14 => {
867            // PMC
868            log::trace!("PMC write: {value:04X}");
869
870            svp.registers.pmc.write(value);
871        }
872        15 => {
873            // Accumulator, low word
874            svp.registers.accumulator =
875                (svp.registers.accumulator & 0xFFFF_0000) | u32::from(value);
876        }
877        _ => panic!("Invalid SVP register number: {register}"),
878    }
879}
880
881fn pm_read(svp: &mut Svp, rom: &[u16], pm_idx: usize) -> u16 {
882    log::trace!("PM{pm_idx} read");
883
884    let pm_register = &mut svp.registers.pm_read[pm_idx];
885    let address = pm_register.get_and_increment_address();
886
887    // Reading a PM register always updates the address/mode words stored in PMC
888    svp.registers.pmc.update_from(pm_register);
889
890    svp.read_external_memory(address, rom)
891}
892
893fn pm_write(svp: &mut Svp, pm_idx: usize, value: u16) {
894    log::trace!("PM{pm_idx} write {value:04X}");
895
896    let pm_register = &mut svp.registers.pm_write[pm_idx];
897    let address = pm_register.get_and_increment_address();
898    let overwrite_mode = pm_register.overwrite_mode;
899
900    // Writing a PM register always updates the address/mode words stored in PMC
901    svp.registers.pmc.update_from(pm_register);
902
903    if overwrite_mode {
904        // This is a read-modify-write, so verify that the SSP isn't attempting to write to ROM
905        if !(0x000000..=0x0FFFFF).contains(&address) {
906            let existing_value = svp.read_external_memory(address, &[]);
907
908            // Overwrite mode splits words into 4 nibbles and only writes the non-0 nibbles
909            let new_value = [0x000F, 0x00F0, 0x0F00, 0xF000]
910                .into_iter()
911                .map(|mask| if value & mask != 0 { value & mask } else { existing_value & mask })
912                .reduce(|a, b| a | b)
913                .unwrap();
914            svp.write_external_memory(address, new_value);
915        }
916    } else {
917        svp.write_external_memory(address, value);
918    }
919}
920
921fn read_pointer(svp: &mut Svp, bank: RamBank, pointer: u16, modifier: u16) -> u8 {
922    if pointer < 3 {
923        // Pointer register
924        let registers = match bank {
925            RamBank::Zero => &mut svp.registers.ram0_pointers,
926            RamBank::One => &mut svp.registers.ram1_pointers,
927        };
928
929        let ram_addr = registers[pointer as usize];
930        increment_pointer_register(
931            &mut registers[pointer as usize],
932            modifier,
933            svp.registers.status.loop_modulo(),
934        );
935        ram_addr
936    } else {
937        // "Fake" pointer register (p3/p7) used for access to internal RAM addresses 0-3
938        // Modifier bits are used as the address
939        modifier as u8
940    }
941}
942
943fn increment_pointer_register(register: &mut u8, modifier: u16, loop_modulo: u8) {
944    match modifier {
945        0 => {
946            // No auto-increment/decrement; do nothing
947        }
948        1 => {
949            // Auto-increment with no modulo
950            *register = (*register).wrapping_add(1);
951        }
952        2 => {
953            // Modulo decrement
954            *register = modulo_decrement(*register, loop_modulo);
955        }
956        3 => {
957            // Modulo increment
958            *register = modulo_increment(*register, loop_modulo);
959        }
960        _ => panic!("invalid pointer register modifier: {modifier}"),
961    }
962}
963
964fn modulo_increment(value: u8, modulo: u8) -> u8 {
965    let mask = modulo.wrapping_sub(1);
966    (value & !mask) | (value.wrapping_add(1) & mask)
967}
968
969fn modulo_decrement(value: u8, modulo: u8) -> u8 {
970    let mask = modulo.wrapping_sub(1);
971    (value & !mask) | (value.wrapping_sub(1) & mask)
972}