port more from osmocom

cellular/src/samplerate.rs

@@ -1,40 +1,149 @@
-struct SampleRate {
-    factor: f64,
-    filter_cutoff: f64,
-    down: DownSampler,
-    up: UpSampler,
+use crate::iir_filter::IIRFilter;
+
+type Sample = f64;
+
+#[derive(Clone)]
+pub struct SampleRate {
+    pub factor: f64,
+    pub filter_cutoff: f64,
+    pub down: DownSampler,
+    pub up: UpSampler,
 }
 
-struct DownSampler {
-    lp: IIRFilter,
-    last_sample: Sample,
-    in_index: f64,
+#[derive(Clone)]
+pub struct DownSampler {
+    pub lp: IIRFilter,
+    pub last_sample: Sample,
+    pub in_index: f64,
 }
 
-struct UpSampler {
-    lp: IIRFilter,
-    current_sample: Sample,
-    last_sample: Sample,
-    in_index: f64,
+#[derive(Clone)]
+pub struct UpSampler {
+    pub lp: IIRFilter,
+    pub current_sample: Sample,
+    pub last_sample: Sample,
+    pub in_index: f64,
 }
 
 impl SampleRate {
     pub fn init(&mut self, low_samplerate: f64, high_samplerate: f64, filter_cutoff: f64) -> Result<(), i32> {
-        // Implementation goes here
+        // clear state
+        self.factor = 0.0;
+        self.filter_cutoff = 0.0;
 
-        Ok(())
-        // or Err(error_code) if there's an error
-    }
+        self.factor = high_samplerate / low_samplerate;
+        if self.factor < 1.0 {
+            eprintln!("Software error: Low sample rate must be lower than high sample rate, aborting!");
+            return Err(-1);
+        }
 
-    pub fn downsample(&mut self, state: SampleRate, samples: Sample, input_num: i32) -> Result<(), i32> {
+        self.filter_cutoff = filter_cutoff;
+        if self.filter_cutoff > 0.0 {
+            self.up.lp.lowpass_init(filter_cutoff, high_samplerate as i32, 2);
+            self.down.lp.lowpass_init(filter_cutoff, high_samplerate as i32, 2);
+        }
         Ok(())
     }
 
-    pub fn upsample_input_num(&mut self, state: SampleRate, output_num: i32) -> Result<(), i32> {
-        Ok(())
+    pub fn downsample_input_num(&mut self, output_num: i32) -> i32 {
+        let factor = self.factor;
+        let mut in_index = self.down.in_index;
+        let mut idx = 0;
+
+        for _ in 0..output_num {
+            in_index += factor;
+            idx = in_index as i32;
+        }
+
+        idx
     }
 
-    pub fn upsample_output_num(&mut self, state: SampleRate, input_num: i32) -> Result<(), i32> {
+    pub fn downsample_output_num(&mut self, input_num: i32) -> i32 {
+        let factor = self.factor;
+        let mut in_index = self.down.in_index;
+        let mut output_num = 0;
+        let mut idx;
+
+        loop {
+            idx = in_index as i32;
+            if idx >= input_num {
+                break;
+            }
+            output_num += 1;
+            in_index += factor;
+        }
+
+        output_num
+    }
+
+    pub fn downsample(&mut self, samples: &mut [Sample], input_num: i32) -> i32 {
+        let factor = self.factor;
+        let mut output_num = 0;
+        let mut in_index = self.down.in_index;
+        let mut last_sample = self.down.last_sample;
+        let mut idx;
+        let mut diff;
+
+        // filter down if cutoff is set
+        if self.filter_cutoff > 0.0 {
+            self.down.lp.process(&mut samples[..input_num as usize]);
+        }
+
+        // allocate output buffer w/ safety margin
+        let output_size = ((input_num as f64) / factor + 0.5) as usize + 10;
+        let mut output = vec![0.0; output_size];
+
+        for i in 0.. {
+            idx = in_index as i32;
+            if idx >= input_num {
+                break;
+            }
+            diff = in_index - idx as f64;
+            if idx > 0 {
+                output[i] = samples[(idx-1) as usize] * (1.0 - diff) + samples[idx as usize] * diff;
+            } else {
+                output[i] = last_sample * (1.0 - diff) + samples[idx as usize] * diff;
+            }
+            output_num += 1;
+            in_index += factor;
+        }
+
+        // store last sample
+        if input_num > 0 {
+            self.down.last_sample = samples[(input_num -1) as usize];
+        }
+
+        // adjust in_index
+        in_index -= input_num as f64;
+        if (in_index as i32) < 0 {
+            in_index = 0.0;
+        }
+        self.down.in_index = in_index;
+
+        // copy output back to samples
+        for i in 0..output_num {
+            samples[i as usize] = output[i as usize];
+        }
+        
+        output_num
+    }
+
+    pub fn upsample_input_num(&mut self, output_num: i32) -> i32 {
+        let factor = 1.0 / self.factor;
+        let mut in_index = self.up.in_index;
+        let mut idx = 0;
+
+        for _ in 0..output_num {
+            in_index += factor;
+            if in_index >= 1.0 {
+                idx += 1;
+                in_index -= 1.0;
+            }
+        }
+        idx
+    }
+
+    pub fn upsample_output_num(&mut self, input_num: i32) -> i32 {
         Ok(())
     }