Proper implementation of set_fir() doesn't work

1 files changed, 41 insertions, 3 deletions

diff --git a/src/lib.rs b/src/lib.rs
index 0f9679a..000050a 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -28,10 +28,17 @@ const KNOWN_DEVICES: [(u16, u16, &str); 2] = [
     (0x0bda, 0x2838, "Generic RTL2832U OEM")
 ];
 
+const FIR_LENGTH: usize = 16;
+const FIR_DEFAULT: [i16; FIR_LENGTH] = [
+	-54, -36, -41, -40, -32, -14, 14, 53,	// 8 bit signed
+	101, 156, 215, 273, 327, 372, 404, 421	// 12 bit signed
+];
+
 pub struct RtlSdr<'a> {
     ctx: &'a libusb::Context,
     dev: Option<libusb::DeviceHandle<'a>>,
-    iface_id: u8
+    iface_id: u8,
+    fir: [i16; FIR_LENGTH]
 }
 
 impl<'a> RtlSdr<'a> {
@@ -40,7 +47,8 @@ impl<'a> RtlSdr<'a> {
         RtlSdr {
             ctx,
             dev: None,
-            iface_id: INTERFACE_ID
+            iface_id: INTERFACE_ID,
+            fir: FIR_DEFAULT
         }
     }
 
@@ -91,6 +99,36 @@ impl<'a> RtlSdr<'a> {
         self.demod_read_reg(handle, 0x0a, 0x01, 1)
     }
 
+    fn set_fir(&self, handle: &libusb::DeviceHandle) {
+        let fir: [i16; 20];
+
+        /* format: int8_t[8] */
+        for i in 0..8 {
+            let val = FIR_DEFAULT[i];
+            if val < -128 || val > 127 {
+                return;
+            }
+            fir[i] = val;
+        }
+
+        /* format: int12_t[8] */
+        for idx in 0..4 {
+            let i = idx * 2;
+            let val0 = FIR_DEFAULT[8+i];
+            let val1 = FIR_DEFAULT[8+i+1];
+            if val0 < -2048 || val0 > 2047 || val1 < -2048 || val1 > 2047 {
+                return;
+            }
+            fir[8+i*3/2] = val0 >> 4;
+            fir[8+i*3/2+1] = (val0 << 4) | ((val1 >> 8) & 0x0f);
+            fir[8+i*3/2+2] = val1;
+        }
+
+        for i in 0..20 {
+            self.demod_write_reg(handle, 1, 0x1c + i, fir[i], 1);
+        }
+    }
+
     fn init_baseband(&self, handle: &libusb::DeviceHandle) {
         // init USB
         self.write_reg(&handle, BLOCK_USBB, ADDR_USB_SYSCTL, 0x09, 1);
@@ -114,7 +152,7 @@ impl<'a> RtlSdr<'a> {
             self.demod_write_reg(&handle, 1, 0x16 + i, 0x00, 1);
         }
 
-        // rtlsdr_set_fir(&handle);
+        self.set_fir(&handle);
 
         // enable SDR mode, disable DAGC (bit 5)
         self.demod_write_reg(&handle, 0, 0x19, 0x05, 1);