fix(sound): Fix amplitude and properly emulate sound bias level

Former-commit-id: 92dec2c186b9e6f34ae5ce0ee167d6f1fa7730c8

src/core/sound/dsp.rs

@@ -1,36 +1,32 @@
-use crate::{AudioInterface, StereoSample};
+use crate::StereoSample;
 
 use serde::{Deserialize, Serialize};
 
 const PI: f32 = std::f32::consts::PI;
 
 pub trait Resampler {
-    fn push_sample(&mut self, s: StereoSample, audio: &mut dyn AudioInterface);
+    fn feed(&mut self, s: StereoSample<f32>, output: &mut Vec<StereoSample<f32>>);
 }
 
 #[derive(Serialize, Deserialize, Clone, Debug)]
 pub struct CosineResampler {
-    last_in_sample: StereoSample,
+    last_in_sample: StereoSample<f32>,
     phase: f32,
     pub in_freq: f32,
     out_freq: f32,
 }
 
-fn cosine_interpolation(y1: i16, y2: i16, phase: f32) -> i16 {
+fn cosine_interpolation(y1: f32, y2: f32, phase: f32) -> f32 {
-    let y1 = y1 as i32 as f32;
-    let y2 = y2 as i32 as f32;
-
     let mu2 = (1.0 - (PI * phase).cos()) / 2.0;
-
+    y2 * (1.0 - mu2) + y1 * mu2
-    (y2 * (1.0 - mu2) + y1 * mu2) as i16
 }
 
 impl Resampler for CosineResampler {
-    fn push_sample(&mut self, s: StereoSample, audio: &mut dyn AudioInterface) {
+    fn feed(&mut self, s: StereoSample<f32>, output: &mut Vec<StereoSample<f32>>) {
         while self.phase < 1.0 {
             let left = cosine_interpolation(self.last_in_sample.0, s.0, self.phase);
             let right = cosine_interpolation(self.last_in_sample.1, s.1, self.phase);
-            audio.push_sample((left, right));
+            output.push((left, right));
             self.phase += self.in_freq / self.out_freq;
         }
         self.phase = self.phase - 1.0;

src/core/sound/mod.rs

@@ -7,7 +7,7 @@ use serde::{Deserialize, Serialize};
 use super::dma::DmaController;
 use super::iodev::consts::*;
 use super::iodev::io_reg_string;
-use crate::AudioInterface;
+use crate::{AudioInterface, StereoSample};
 
 mod fifo;
 use fifo::SoundFifo;
@@ -98,7 +98,7 @@ pub struct SoundController {
     dma_sound: [DmaSoundChannel; 2],
 
     resampler: CosineResampler,
-    pub output_buffer: Vec<i16>,
+    output_buffer: Vec<StereoSample<f32>>,
 }
 
 impl SoundController {
@@ -133,7 +133,7 @@ impl SoundController {
             dma_sound: [Default::default(), Default::default()],
 
             resampler: resampler,
-            output_buffer: Vec::with_capacity(10000),
+            output_buffer: Vec::with_capacity(1024),
         }
     }
 
@@ -325,19 +325,31 @@ impl SoundController {
 
             // time to push a new sample!
 
-            let mut sample = [0; 2];
+            let mut sample = [0f32; 2];
 
             for channel in 0..=1 {
+                let mut dma_sample = 0;
                 for dma in &mut self.dma_sound {
                     if dma.is_stereo_channel_enabled(channel) {
-                        sample[channel] += dma.value as i16;
+                        let value = dma.value as i16;
+                        dma_sample += value * (2 << dma.volume_shift);
                     }
                 }
+
+                apply_bias(&mut dma_sample, self.sound_bias.bit_range(0..10) as i16);
+                sample[channel] = dma_sample as i32 as f32;
             }
 
+            let stereo_sample = (sample[0], sample[1]);
+            self.resampler.feed(stereo_sample, &mut self.output_buffer);
+
             let mut audio = audio_device.borrow_mut();
-            self.resampler
+            self.output_buffer.drain(..).for_each(|(left, right)| {
-                .push_sample((sample[0], sample[1]), &mut *audio);
+                audio.push_sample((
+                    (left.round() as i16) * (std::i16::MAX / 512),
+                    (right.round() as i16) * (std::i16::MAX / 512),
+                ));
+            });
         }
         if self.cycles_per_sample < *cycles_to_next_event {
             *cycles_to_next_event = self.cycles_per_sample;
@@ -345,6 +357,20 @@ impl SoundController {
     }
 }
 
+#[inline(always)]
+fn apply_bias(sample: &mut i16, level: i16) {
+    let mut s = *sample;
+    s += level;
+    // clamp
+    if s > 0x3ff {
+        s = 0x3ff;
+    } else if s < 0 {
+        s = 0;
+    }
+    s -= level;
+    *sample = s;
+}
+
 // TODO move
 fn cbit(idx: u8, value: bool) -> u16 {
     if value {

src/lib.rs

@@ -49,15 +49,18 @@ pub trait VideoInterface {
     fn render(&mut self, buffer: &[u32]) {}
 }
 
-pub type StereoSample = (i16, i16);
+pub type StereoSample<T> = (T, T);
 
 pub trait AudioInterface {
     fn get_sample_rate(&self) -> i32 {
         44100
     }
 
+    /// Pushes a stereo sample into the audio device
+    /// Sample should be normilized to siged 16bit values
+    /// Note: It is not guarentied that the sample will be played
     #[allow(unused_variables)]
-    fn push_sample(&mut self, samples: StereoSample) {}
+    fn push_sample(&mut self, samples: StereoSample<i16>) {}
 }
 
 pub trait InputInterface {

src/plat/sdl2/audio.rs

@@ -7,13 +7,13 @@ extern crate ringbuf;
 use ringbuf::{Consumer, Producer, RingBuffer};
 
 struct GbaAudioCallback {
-    consumer: Consumer<StereoSample>,
+    consumer: Consumer<StereoSample<i16>>,
     spec: AudioSpec,
 }
 
 pub struct Sdl2AudioPlayer {
     _device: AudioDevice<GbaAudioCallback>,
-    producer: Producer<StereoSample>,
+    producer: Producer<StereoSample<i16>>,
     freq: i32,
 }
 
@@ -25,8 +25,8 @@ impl AudioCallback for GbaAudioCallback {
 
         for i in 0..sample_count {
             if let Some((left, right)) = self.consumer.pop() {
-                out_samples[2 * i] = left * (1 << 4);
+                out_samples[2 * i] = left;
-                out_samples[2 * i + 1] = right * (1 << 4);
+                out_samples[2 * i + 1] = right;
             } else {
                 out_samples[2 * i] = self.spec.silence as i16;
                 out_samples[2 * i + 1] = self.spec.silence as i16;
@@ -40,7 +40,7 @@ impl AudioInterface for Sdl2AudioPlayer {
         self.freq
     }
 
-    fn push_sample(&mut self, sample: StereoSample) {
+    fn push_sample(&mut self, sample: StereoSample<i16>) {
         #![allow(unused_must_use)]
         self.producer.push(sample);
     }
@@ -57,7 +57,7 @@ pub fn create_audio_player(sdl: &sdl2::Sdl) -> Sdl2AudioPlayer {
 
     let mut freq = 0;
 
-    let mut producer: Option<Producer<StereoSample>> = None;
+    let mut producer: Option<Producer<StereoSample<i16>>> = None;
 
     let device = audio_subsystem
         .open_playback(None, &desired_spec, |spec| {
@@ -66,7 +66,7 @@ pub fn create_audio_player(sdl: &sdl2::Sdl) -> Sdl2AudioPlayer {
 
             // Create a thread-safe SPSC fifo
             let ringbuf_size = (spec.samples as usize) * 2;
-            let rb = RingBuffer::<StereoSample>::new(ringbuf_size);
+            let rb = RingBuffer::<StereoSample<i16>>::new(ringbuf_size);
             let (prod, cons) = rb.split();
 
             // move producer to the outer scope