3mod resampler;

5use crate::api;
6use crate::api::PceEmulatorConfig;
7use crate::psg::resampler::PsgResampler;
8use bincode::{Decode, Encode};
9use jgenesis_common::frontend::AudioOutput;
10use jgenesis_common::num::{GetBit, U16Ext};
11use std::sync::LazyLock;
12use std::{array, mem};

15pub const PSG_CLOCK_DIVIDER: u64 = 6;
16pub const PSG_FREQUENCY: f64 = api::MASTER_CLOCK_FREQUENCY / (PSG_CLOCK_DIVIDER as f64);

18const VOLUME_UPDATE_PERIOD_CYCLES: u16 = 256;

22static ATTENUATION_LOOKUP_TABLE: LazyLock<[f64; 32]> = LazyLock::new(|| {
23    let mut table = [0.0; 32];
24    table[0] = 1.0;
25
26    for i in 1..=29 {
27        table[i] = table[i - 1] * 10.0_f64.powf(-1.5 / 20.0);
28    }
29
30    table
31});

33#[derive(Debug, Clone, Encode, Decode)]
34struct NoiseGenerator {
35    enabled: bool,
36    lfsr: u32,
37    counter: u16,
38    counter_reload: u16,
39    current_sample: u8,
40}
41
42impl NoiseGenerator {
43    // https://web.archive.org/web/20080311065543/http://cgfm2.emuviews.com:80/blog/index.php
44    // Noise generator contains an 18-bit LFSR, initialized with only bit 0 set, taps bits 0 + 1 + 11 + 12 + 17
45
46    fn new() -> Self {
47        Self {
48            enabled: false,
49            lfsr: 1,
50            counter: 0x1F * 64,
51            counter_reload: 0x1F * 64,
52            current_sample: 0x1F,
53        }
54    }
55
56    fn write_r7(&mut self, value: u8) {
57        self.enabled = value.bit(7);
58
59        // Given a frequency value F, LFSR clocks every 64 * !F PSG cycles
60        self.counter_reload = 64 * u16::from(!value & 0x1F);
61    }
62
63    fn clock(&mut self) {
64        // In hardware there seems to be some sort of 6-bit divider; emulate that as the counter
65        // being 11-bit instead of 5-bit counter + 6-bit divider
66        self.counter = self.counter.wrapping_sub(1) & 0x7FF;
67        if self.counter == 0 {
68            self.counter = self.counter_reload;
69
70            // Noise generator always outputs either max sample or min sample based on the shifted-out bit
71            self.current_sample = if self.lfsr.bit(0) { 0x1F } else { 0x00 };
72
73            let new_bit = self.lfsr.bit(0)
74                ^ self.lfsr.bit(1)
75                ^ self.lfsr.bit(11)
76                ^ self.lfsr.bit(12)
77                ^ self.lfsr.bit(17);
78            self.lfsr = (self.lfsr >> 1) | (u32::from(new_bit) << 17);
79        }
80    }
81}
82
83#[derive(Debug, Clone, Encode, Decode)]
84struct PsgChannel {
85    idx: u8,
86    on: bool,
87    direct_da: bool,
88    wave_ram: [u8; 32],
89    wave_address: u8,
90    frequency: u16,
91    counter: u16,
92    amplitude: u8,
93    l_amplitude: u8,
94    r_amplitude: u8,
95    current_sample: u8,
96    noise: NoiseGenerator,
97    latched_attenuation_l: u8,
98    latched_attenuation_r: u8,
99}
100
101impl PsgChannel {
102    fn new(idx: u8) -> Self {
103        Self {
104            idx,
105            on: false,
106            direct_da: false,
107            wave_ram: array::from_fn(|_| 0),
108            wave_address: 0,
109            frequency: 0xFFF,
110            counter: 0xFFF,
111            amplitude: 0,
112            l_amplitude: 0,
113            r_amplitude: 0,
114            current_sample: 0,
115            noise: NoiseGenerator::new(),
116            latched_attenuation_l: 31,
117            latched_attenuation_r: 31,
118        }
119    }
120
121    fn clock(&mut self, lfo: &mut LowFrequencyOscillator, channel_2_sample: Option<u8>) {
122        if self.direct_da {
123            return;
124        }
125
126        if self.idx == 1 && lfo.enabled() && lfo.triggered {
127            // Manual implies that channel 2 is halted while the LFO is enabled and triggered
128            return;
129        }
130
131        if self.noise.enabled {
132            self.noise.clock();
133        }
134
135        // 12-bit frequency counter
136        self.counter = self.counter.wrapping_sub(1) & 0xFFF;
137        if self.counter == 0 {
138            // When LFO is enabled, channel 2 frequency modulates channel 1
139            let effective_frequency = if self.idx == 0
140                && lfo.enabled()
141                && let Some(sample) = channel_2_sample
142            {
143                lfo.modulate_frequency(self.frequency, sample)
144            } else {
145                self.frequency
146            };
147            self.counter = effective_frequency;
148
149            // When LFO is enabled, channel 2 frequency is multiplied by LFO frequency
150            let increment_wave_address = if self.idx == 1 && lfo.enabled() {
151                lfo.clock() == LfoClock::IncrementWaveAddress
152            } else {
153                true
154            };
155            if increment_wave_address {
156                self.wave_address = (self.wave_address + 1) & 0x1F;
157            }
158        }
159
160        self.current_sample = self.wave_ram[self.wave_address as usize];
161    }
162
163    fn current_output(&self) -> u8 {
164        if self.noise.enabled && !self.direct_da {
165            self.noise.current_sample
166        } else {
167            self.current_sample
168        }
169    }
170}
171
172#[derive(Debug, Clone, Copy, PartialEq, Eq)]
173enum LfoClock {
174    IncrementWaveAddress,
175    None,
176}
177
178#[derive(Debug, Clone, Encode, Decode)]
179struct LowFrequencyOscillator {
180    triggered: bool,
181    control: u8,
182    counter: u8,
183    frequency: u8,
184}
185
186impl LowFrequencyOscillator {
187    fn new() -> Self {
188        Self { triggered: false, control: 0, counter: 0xFF, frequency: 0xFF }
189    }
190
191    fn enabled(&self) -> bool {
192        // LFO is enabled whenever control bits are non-zero (R9 lowest two bits)
193        self.control != 0
194    }
195
196    fn clock(&mut self) -> LfoClock {
197        self.counter = self.counter.wrapping_sub(1);
198        if self.counter == 0 {
199            self.counter = self.frequency;
200            LfoClock::IncrementWaveAddress
201        } else {
202            LfoClock::None
203        }
204    }
205
206    fn modulate_frequency(&self, frequency: u16, channel_2_sample: u8) -> u16 {
207        debug_assert_ne!(self.control, 0, "modulate_frequency() called when LFO is disabled");
208
209        // Per manual, modulation range is +0x0F (sample 0x1F) to -0x10 (sample 0x00)
210        let frequency_delta = i16::from(channel_2_sample) - 0x10;
211
212        // 1 = No shift
213        // 2 = Left shift 2
214        // 3 = Left shift 4
215        let shift = 2 * (self.control - 1);
216
217        frequency.wrapping_add_signed(frequency_delta << shift) & 0xFFF
218    }
219}
220
221#[derive(Debug, Clone, Copy, PartialEq, Eq, Encode, Decode)]
222enum OutputChannel {
223    Right,
224    Left,
225}
226
227impl OutputChannel {
228    fn other(self) -> Self {
229        match self {
230            Self::Right => Self::Left,
231            Self::Left => Self::Right,
232        }
233    }
234}
235
236#[derive(Debug, Clone, Encode, Decode)]
237struct VolumeUpdateState {
238    update_needed: bool,
239    active: bool,
240    channel: u8,
241    output: OutputChannel,
242    cycles_till_next_update: u16,
243    latched_attenuation: u8,
244}
245
246impl VolumeUpdateState {
247    fn new() -> Self {
248        Self {
249            update_needed: false,
250            active: false,
251            channel: 0,
252            output: OutputChannel::Right,
253            cycles_till_next_update: VOLUME_UPDATE_PERIOD_CYCLES,
254            latched_attenuation: 0,
255        }
256    }
257}
258
259#[derive(Debug, Clone, Encode, Decode)]
260pub struct Huc6280Psg {
261    channels: [PsgChannel; 6],
262    selected_channel: u8,
263    l_main_amplitude: u8,
264    r_main_amplitude: u8,
265    lfo: LowFrequencyOscillator,
266    resampler: PsgResampler,
267    output_frequency: u64,
268    cycles: u64,
269    volume: VolumeUpdateState,
270}
271
272impl Huc6280Psg {
273    pub fn new(config: PceEmulatorConfig) -> Self {
274        let output_frequency = 48000;
275
276        Self {
277            channels: array::from_fn(|idx| PsgChannel::new(idx as u8)),
278            selected_channel: 0,
279            l_main_amplitude: 0,
280            r_main_amplitude: 0,
281            lfo: LowFrequencyOscillator::new(),
282            resampler: PsgResampler::new(config.audio_resampler, output_frequency),
283            output_frequency,
284            cycles: 0,
285            volume: VolumeUpdateState::new(),
286        }
287    }
288
289    pub fn step_to(&mut self, cycles: u64) {
290        while self.cycles < cycles {
291            self.clock();
292            self.cycles += PSG_CLOCK_DIVIDER;
293        }
294    }
295
296    pub fn clock(&mut self) {
297        let mut sample_l = 0.0;
298        let mut sample_r = 0.0;
299
300        let channel_2_sample = self.channels[1].on.then_some(self.channels[1].current_sample);
301
302        for channel in &mut self.channels {
303            if !channel.on {
304                continue;
305            }
306
307            channel.clock(&mut self.lfo, channel_2_sample);
308
309            // Per the official manual, total attenuation of 45 dB or higher results in silence
310            // Attenuation is in steps of 1.5 dB, so 30 = 45 dB
311            if channel.latched_attenuation_l >= 30 && channel.latched_attenuation_r >= 30 {
312                // Both of this channel's outputs are silent, skip sample calculations
313                continue;
314            }
315
316            // Center waveform at 0 for less poppy audio, and divide by 6 for number of channels
317            let channel_sample = channel.current_output();
318            let channel_sample = (f64::from(channel_sample) - 15.5) / 15.5 / 6.0;
319
320            if channel.latched_attenuation_l < 30 {
321                sample_l += channel_sample
322                    * ATTENUATION_LOOKUP_TABLE[channel.latched_attenuation_l as usize];
323            }
324
325            if channel.latched_attenuation_r < 30 {
326                sample_r += channel_sample
327                    * ATTENUATION_LOOKUP_TABLE[channel.latched_attenuation_r as usize];
328            }
329        }
330
331        self.resampler.collect([sample_l, sample_r]);
332
333        if self.volume.active {
334            self.progress_volume_update();
335        }
336    }
337
338    pub fn drain_output_buffer<A: AudioOutput>(
339        &mut self,
340        audio_output: &mut A,
341    ) -> Result<(), A::Err> {
342        while let Some([sample_l, sample_r]) = self.resampler.output_buffer_pop_front() {
343            audio_output.push_sample(sample_l, sample_r)?;
344        }
345
346        Ok(())
347    }
348
349    pub fn update_output_frequency(&mut self, output_frequency: u64) {
350        self.resampler.update_output_frequency(output_frequency as f64);
351        self.output_frequency = output_frequency;
352    }
353
354    pub fn reload_config(&mut self, config: PceEmulatorConfig) {
355        if config.audio_resampler != self.resampler.resampler_impl() {
356            self.resampler = PsgResampler::new(config.audio_resampler, self.output_frequency);
357        }
358    }
359
360    fn trigger_volume_update(&mut self) {
361        if !self.volume.active {
362            // Volume update is not running; start one now
363            self.volume.active = true;
364        } else {
365            // A volume update is already running, but another one is needed after it finishes
366            self.volume.update_needed = true;
367        }
368    }
369
370    fn progress_volume_update(&mut self) {
371        // Based on Mednafen's PSG volume update implementation; I don't think this behavior is
372        // documented anywhere else, but volume/amplitude updates apparently don't apply immediately
373        // Honey in the Sky depends on this to avoid static/buzzing noises caused by rapid volume changes
374
375        self.volume.cycles_till_next_update -= 1;
376        if self.volume.cycles_till_next_update == VOLUME_UPDATE_PERIOD_CYCLES - 1 {
377            // Latch attenuation on the first cycle of the 256-cycle update period
378            if let Some(channel) = self.channels.get(self.volume.channel as usize) {
379                // Channel amplitude is 0-31, -1 is 1.5 dB attenuation
380                // L/R amplitudes are 0-15, -1 is 3 dB attenuation
381                self.volume.latched_attenuation = match self.volume.output {
382                    OutputChannel::Right => {
383                        (31 - channel.amplitude)
384                            + 2 * (15 - channel.r_amplitude)
385                            + 2 * (15 - self.r_main_amplitude)
386                    }
387                    OutputChannel::Left => {
388                        (31 - channel.amplitude)
389                            + 2 * (15 - channel.l_amplitude)
390                            + 2 * (15 - self.l_main_amplitude)
391                    }
392                };
393            }
394        }
395
396        if self.volume.cycles_till_next_update != 0 {
397            return;
398        }
399        self.volume.cycles_till_next_update = VOLUME_UPDATE_PERIOD_CYCLES;
400
401        if let Some(channel) = self.channels.get_mut(self.volume.channel as usize) {
402            // Apply latched attenuation to channel at the end of the 256-cycle period
403            match self.volume.output {
404                OutputChannel::Right => {
405                    channel.latched_attenuation_r = self.volume.latched_attenuation;
406                }
407                OutputChannel::Left => {
408                    channel.latched_attenuation_l = self.volume.latched_attenuation;
409                }
410            }
411        }
412
413        self.volume.output = self.volume.output.other();
414        if self.volume.output == OutputChannel::Right {
415            // Per Mednafen the volume update goes through an 8-channel loop, even though there are
416            // only 6 channels; the last two iterations do nothing
417            self.volume.channel = (self.volume.channel + 1) & 7;
418            if self.volume.channel == 0 {
419                // End of update loop; start again if an amplitude register was written mid-update
420                self.volume.active = mem::take(&mut self.volume.update_needed);
421            }
422        }
423    }
424
425    // $1FE800-$1FE80F: PSG registers
426    pub fn write(&mut self, address: u32, value: u8) {
427        let address = address & 0xF;
428
429        log::trace!("PSG R{address} write: {value:02X} (ch {})", self.selected_channel);
430
431        if (2..=7).contains(&address) && self.selected_channel >= 6 {
432            // Per-channel register with an invalid channel
433            return;
434        }
435
436        if address == 7 && self.selected_channel < 4 {
437            // Invalid; only channels 5 and 6 support noise
438            return;
439        }
440
441        match address {
442            0 => {
443                // R0: Channel select
444                self.selected_channel = value & 7;
445
446                log::trace!("Selected channel: {}", self.selected_channel);
447            }
448            1 => {
449                // R1: Main amplitude
450                self.l_main_amplitude = value >> 4;
451                self.r_main_amplitude = value & 0xF;
452
453                log::trace!("L main amplitude: {}", self.l_main_amplitude);
454                log::trace!("R main amplitude: {}", self.r_main_amplitude);
455
456                self.trigger_volume_update();
457            }
458            2 => {
459                // R2: Frequency, low bits
460                let channel = &mut self.channels[self.selected_channel as usize];
461                channel.frequency.set_lsb(value);
462
463                log::trace!("Frequency: {}", channel.frequency);
464            }
465            3 => {
466                // R3: Frequency, high bits
467                let channel = &mut self.channels[self.selected_channel as usize];
468                channel.frequency.set_msb(value & 0xF);
469
470                log::trace!("Frequency: {}", channel.frequency);
471            }
472            4 => {
473                // R4: Channel on, direct D/A, channel amplitude
474                let channel = &mut self.channels[self.selected_channel as usize];
475
476                let prev_on = channel.on;
477                channel.on = value.bit(7);
478                channel.direct_da = value.bit(6);
479                channel.amplitude = value & 0x1F;
480
481                if !prev_on && channel.on {
482                    channel.counter = channel.frequency;
483                }
484
485                if channel.direct_da {
486                    channel.wave_address = 0;
487                }
488
489                log::trace!("Channel on: {}", channel.on);
490                log::trace!("Direct D/A: {}", channel.direct_da);
491                log::trace!("Channel amplitude: {}", channel.amplitude);
492
493                self.trigger_volume_update();
494            }
495            5 => {
496                // R5: L/R amplitude
497                let channel = &mut self.channels[self.selected_channel as usize];
498                channel.l_amplitude = value >> 4;
499                channel.r_amplitude = value & 0xF;
500
501                log::trace!("Channel L amplitude: {}", channel.l_amplitude);
502                log::trace!("Channel R amplitude: {}", channel.r_amplitude);
503
504                self.trigger_volume_update();
505            }
506            6 => {
507                // R6: Waveform data
508                let sample = value & 0x1F;
509
510                let channel = &mut self.channels[self.selected_channel as usize];
511                channel.current_sample = sample;
512
513                if !channel.direct_da {
514                    channel.wave_ram[channel.wave_address as usize] = sample;
515
516                    // TODO is this right? manual suggests increment is based purely on frequency
517                    // when CHON=1 and DDA=0
518                    if !channel.on {
519                        channel.wave_address = (channel.wave_address + 1) & 0x1F;
520                    }
521                }
522            }
523            7 => {
524                // R7: Noise enable and frequency
525                let channel = &mut self.channels[self.selected_channel as usize];
526                channel.noise.write_r7(value);
527
528                log::trace!("Noise enabled: {}", channel.noise.enabled);
529                log::trace!("Noise frequency: {}", value & 0x1F);
530            }
531            8 => {
532                // R8: LFO frequency
533                self.lfo.frequency = value;
534
535                log::trace!("LFO frequency: {}", self.lfo.frequency);
536            }
537            9 => {
538                // R9: LFO control
539                self.lfo.triggered = value.bit(7);
540                self.lfo.control = value & 3;
541
542                if self.lfo.enabled() && self.lfo.triggered {
543                    // Manual implies that triggering the LFO resets channel 2 and halts it
544                    self.channels[1].wave_address = 0;
545                    self.channels[1].current_sample = self.channels[1].wave_ram[0];
546                    self.channels[1].counter = self.channels[1].frequency;
547
548                    self.lfo.counter = self.lfo.frequency;
549                }
550
551                log::trace!("LFO triggered: {}", self.lfo.triggered);
552                log::trace!("LFO control: {}", self.lfo.control);
553            }
554            10..=15 => {} // Invalid addresses
555            _ => unreachable!("value & 0xF is always <= 15"),
556        }
557    }
558}