sdr-0.1.0.2: hs_sources/SDR/Filter.hs
{-# LANGUAGE RecordWildCards, FlexibleContexts, GADTs, ExistentialQuantification #-}
{-| FIR filtering, decimation and resampling.
FIR filters (and decimators, resamplers) work by taking successive dot products between the filter coefficients and the input data at increasing offsets. Sometimes the dot product fits entirely within one input buffer and other times it spans two input buffers (but never more because we assume that the filter length is less than the buffer size).
We divide the filtering code by these two cases. Each filter (or decimator, resampler) is described by a data structure such as `Filter` with two functions, one for filtering within a single buffer and one that crosses buffers.
The user must first create one of these data structures using the helper functions and pass this data structure to one of `firFilter`, `firDecimator`, or `firResampler` to create the `Pipe` that does the filtering. For example:
> decimatorStruct <- fastDecimatorC cpuInfo decimation coeffs
> let decimatorPipe :: Pipe (Vector (Complex Float)) (Vector (Complex Float)) IO ()
> decimatorPipe = firDecimator decimatorStruct outputSize
There are polymorphic Haskell only implementations of filtering, decimation and resampling, for example, `haskellFilter`. In addition, there are optimised C implementations that use SIMD instructions on x86 machines, such as `fastFilterR`. These are always specialized to either real or complex numbers. There are also even faster implementations specialized for the case where the filter coefficients are symmetric as in a linear phase filter such as `fastFilterSymR`.
The Haskell implementations are reasonably fast due to the Vector library and GHC's LLVM backend, however, if speed is important you are much better off with the C implementations.
In the future we may avoid the cross buffer filtering function by mapping the buffers consecutively in memory as (I believe) GNU Radio does.
An extensive benchmark suite exists in the /benchmarks subdirectory of this package.
-}
module SDR.Filter (
-- * Types
Filter(..),
Decimator(..),
Resampler(..),
-- * Helper Functions
-- ** Filters
haskellFilter,
-- *** Real Data
fastFilterCR,
fastFilterSSER,
fastFilterAVXR,
fastFilterR,
-- *** Complex Data
fastFilterCC,
fastFilterSSEC,
fastFilterAVXC,
fastFilterC,
-- *** Linear Phase Real Data
fastFilterSymSSER,
fastFilterSymAVXR,
fastFilterSymR,
-- ** Decimators
haskellDecimator,
-- *** Real Data
fastDecimatorCR,
fastDecimatorSSER,
fastDecimatorAVXR,
fastDecimatorR,
-- *** Complex Data
fastDecimatorCC,
fastDecimatorSSEC,
fastDecimatorAVXC,
fastDecimatorC,
-- *** Linear Phase Real Data
fastDecimatorSymSSER,
fastDecimatorSymAVXR,
fastDecimatorSymR,
-- ** Resamplers
haskellResampler,
-- *** Real Data
fastResamplerCR,
fastResamplerSSER,
fastResamplerAVXR,
fastResamplerR,
-- * Filter
firFilter,
-- * Decimate
firDecimator,
-- * Resample
firResampler,
-- * DC Blocking Filter
dcBlockingFilter
) where
import Data.Complex
import Control.Exception hiding (assert)
import qualified Data.Vector.Generic as VG
import qualified Data.Vector.Generic.Mutable as VGM
import qualified Data.Vector.Storable as VS
import Control.Monad.Primitive
import Pipes
import SDR.Util
import SDR.FilterInternal hiding (mkResampler)
import SDR.CPUID
{- | A `Filter` contains all of the information needed by the `filterr`
function to perform filtering. i.e. it contains the filter coefficients
and pointers to the functions to do the actual filtering.
-}
data Filter m v vm a = Filter {
numCoeffsF :: Int,
filterOne :: Int -> v a -> vm (PrimState m) a -> m (),
filterCross :: Int -> v a -> v a -> vm (PrimState m) a -> m ()
}
{- | A `Decimator` contains all of the information needed by the `decimate`
function to perform decimation i.e. it contains the filter coefficients
and pointers to the functions to do the actual decimation.
-}
data Decimator m v vm a = Decimator {
numCoeffsD :: Int,
decimationD :: Int,
decimateOne :: Int -> v a -> vm (PrimState m) a -> m (),
decimateCross :: Int -> v a -> v a -> vm (PrimState m) a -> m ()
}
{- | A `Resampler` contains all of the information needed by the `resample`
function to perform resampling i.e. it contains the filter coefficients
and pointers to the functions to do the actual resampling.
-}
data Resampler m v vm a = forall dat. Resampler {
numCoeffsR :: Int,
decimationR :: Int,
interpolationR :: Int,
startDat :: dat,
resampleOne :: dat -> Int -> v a -> vm (PrimState m) a -> m (dat, Int),
resampleCross :: dat -> Int -> v a -> v a -> vm (PrimState m) a -> m (dat, Int)
}
duplicate :: [a] -> [a]
duplicate = concat . map func
where func x = [x, x]
{-# INLINE haskellFilter #-}
-- | Returns a slow Filter data structure entirely implemented in Haskell
haskellFilter :: (PrimMonad m, Functor m, Num a, Mult a b, VG.Vector v a, VG.Vector v b, VGM.MVector vm a)
=> [b] -- ^ The filter coefficients
-> IO (Filter m v vm a) -- ^ The `Filter` data structure
haskellFilter coeffs = do
let vCoeffs = VG.fromList coeffs
evaluate vCoeffs
let filterOne = filterHighLevel vCoeffs
filterCross = filterCrossHighLevel vCoeffs
numCoeffsF = length coeffs
return $ Filter {..}
mkFilter :: Int
-> FilterRR
-> [Float]
-> IO (Filter IO VS.Vector VS.MVector Float)
mkFilter sizeMultiple filterFunc coeffs = do
let l = length coeffs
numCoeffsF = roundUp l sizeMultiple
diff = numCoeffsF - l
vCoeffs = VG.fromList $ coeffs ++ replicate diff 0
evaluate vCoeffs
let filterOne = filterFunc vCoeffs
filterCross = filterCrossHighLevel vCoeffs
return $ Filter {..}
-- | Returns a fast Filter data structure implemented in C. For filtering real data with real coefficients.
fastFilterCR :: [Float] -- ^ The filter coefficients
-> IO (Filter IO VS.Vector VS.MVector Float) -- ^ The `Filter` data structure
fastFilterCR = mkFilter 1 filterCRR
-- | Returns a fast Filter data structure implemented in C using SSE instructions. For filtering real data with real coefficients.
fastFilterSSER :: [Float] -- ^ The filter coefficients
-> IO (Filter IO VS.Vector VS.MVector Float) -- ^ The `Filter` data structure
fastFilterSSER = mkFilter 4 filterCSSERR
-- | Returns a fast Filter data structure implemented in C using AVX instructions. For filtering real data with real coefficients.
fastFilterAVXR :: [Float] -- ^ The filter coefficients
-> IO (Filter IO VS.Vector VS.MVector Float) -- ^ The `Filter` data structure
fastFilterAVXR = mkFilter 8 filterCAVXRR
-- | Returns a fast Filter data structure implemented in C using the fastest SIMD instruction set your processor supports. For filtering real data with real coefficients.
fastFilterR :: CPUInfo -- ^ The CPU's capabilities
-> [Float] -- ^ The filter coefficients
-> IO (Filter IO VS.Vector VS.MVector Float) -- ^ The `Filter` data structure
fastFilterR info = featureSelect info fastFilterCR [(hasAVX, fastFilterAVXR), (hasSSE42, fastFilterSSER)]
mkFilterC :: Int
-> FilterRC
-> [Float]
-> IO (Filter IO VS.Vector VS.MVector (Complex Float))
mkFilterC sizeMultiple filterFunc coeffs = do
let l = length coeffs
numCoeffsF = roundUp sizeMultiple l
diff = numCoeffsF - l
vCoeffs = VG.fromList $ duplicate $ coeffs ++ replicate diff 0
vCoeffs2 = VG.fromList $ coeffs ++ replicate diff 0
evaluate vCoeffs
let filterOne = filterFunc vCoeffs
filterCross = filterCrossHighLevel vCoeffs2
return $ Filter {..}
-- | Returns a fast Filter data structure implemented in C For filtering complex data with real coefficients.
fastFilterCC :: [Float] -- ^ The filter coefficients
-> IO (Filter IO VS.Vector VS.MVector (Complex Float)) -- ^ The `Filter` data structure
fastFilterCC = mkFilterC 1 filterCRC
-- | Returns a fast Filter data structure implemented in C using SSE instructions. For filtering complex data with real coefficients.
fastFilterSSEC :: [Float] -- ^ The filter coefficients
-> IO (Filter IO VS.Vector VS.MVector (Complex Float)) -- ^ The `Filter` data structure
fastFilterSSEC = mkFilterC 2 filterCSSERC
-- | Returns a fast Filter data structure implemented in C using AVX instructions. For filtering complex data with real coefficients.
fastFilterAVXC :: [Float] -- ^ The filter coefficients
-> IO (Filter IO VS.Vector VS.MVector (Complex Float)) -- ^ The `Filter` data structure
fastFilterAVXC = mkFilterC 4 filterCAVXRC
-- | Returns a fast Filter data structure implemented in C using the fastest SIMD instruction set your processor supports. For filtering complex data with real coefficients.
fastFilterC :: CPUInfo -- ^ The CPU's capabilities
-> [Float] -- ^ The filter coefficients
-> IO (Filter IO VS.Vector VS.MVector (Complex Float)) -- ^ The `Filter` data structure
fastFilterC info = featureSelect info fastFilterCC [(hasAVX, fastFilterAVXC), (hasSSE42, fastFilterSSEC)]
mkFilterSymR :: FilterRR
-> [Float]
-> IO (Filter IO VS.Vector VS.MVector Float)
mkFilterSymR filterFunc coeffs = do
let vCoeffs = VG.fromList coeffs
let vCoeffs2 = VG.fromList $ coeffs ++ reverse coeffs
evaluate vCoeffs
evaluate vCoeffs2
let filterOne = filterFunc vCoeffs
filterCross = filterCrossHighLevel vCoeffs2
numCoeffsF = length coeffs * 2
return $ Filter {..}
-- | Returns a fast Filter data structure implemented in C using SSE instructions. For filtering real data with real coefficients. For filters with symmetric coefficients, i.e. 'linear phase'. Coefficient length must be a multiple of 4.
fastFilterSymSSER :: [Float] -- ^ The first half of the filter coefficients
-> IO (Filter IO VS.Vector VS.MVector Float) -- ^ The `Filter` data structure
fastFilterSymSSER = mkFilterSymR filterCSSESymmetricRR
-- | Returns a fast Filter data structure implemented in C using AVX instructions. For filtering real data with real coefficients. For filters with symmetric coefficients, i.e. 'linear phase'. Coefficient length must be a multiple of 4.
fastFilterSymAVXR :: [Float] -- ^ The first half of the filter coefficients
-> IO (Filter IO VS.Vector VS.MVector Float) -- ^ The `Filter` data structure
fastFilterSymAVXR = mkFilterSymR filterCAVXSymmetricRR
-- | Returns a fast Filter data structure implemented in C using the fastest SIMD instruction set your processor supports. For filtering complex data with real coefficients. For filters with symmetric coefficients, i.e. 'linear phase'. Coefficient length must be a multiple of 4.
fastFilterSymR :: CPUInfo -- ^ The CPU's capabilities
-> [Float] -- ^ The filter coefficients
-> IO (Filter IO VS.Vector VS.MVector Float) -- ^ The `Filter` data structure
fastFilterSymR info = featureSelect info (error "At least SSE4.2 required") [(hasAVX, fastFilterSymAVXR), (hasSSE42, fastFilterSymSSER)]
{-# INLINE haskellDecimator #-}
-- | Returns a slow Decimator data structure entirely implemented in Haskell
haskellDecimator :: (PrimMonad m, Functor m, Num a, Mult a b, VG.Vector v a, VG.Vector v b, VGM.MVector vm a)
=> Int -- ^ The decimation factor
-> [b] -- ^ The filter coefficients
-> IO (Decimator m v vm a) -- ^ The `Decimator` data structure
haskellDecimator decimationD coeffs = do
let vCoeffs = VG.fromList coeffs
evaluate vCoeffs
let decimateOne = decimateHighLevel decimationD vCoeffs
decimateCross = decimateCrossHighLevel decimationD vCoeffs
numCoeffsD = length coeffs
return $ Decimator {..}
mkDecimator :: Int
-> DecimateRR
-> Int
-> [Float]
-> IO (Decimator IO VS.Vector VS.MVector Float)
mkDecimator sizeMultiple filterFunc decimationD coeffs = do
let l = length coeffs
numCoeffsD = roundUp l sizeMultiple
diff = numCoeffsD - l
vCoeffs = VG.fromList $ coeffs ++ replicate diff 0
evaluate vCoeffs
let decimateOne = filterFunc decimationD vCoeffs
decimateCross = decimateCrossHighLevel decimationD vCoeffs
return $ Decimator {..}
-- | Returns a fast Decimator data structure implemented in C. For decimating real data with real coefficients.
fastDecimatorCR :: Int -- ^ The decimation factor
-> [Float] -- ^ The filter coefficients
-> IO (Decimator IO VS.Vector VS.MVector Float) -- ^ The `Decimator` data structure
fastDecimatorCR = mkDecimator 1 decimateCRR
-- | Returns a fast Decimator data structure implemented in C using SSE instructions. For decimating real data with real coefficients.
fastDecimatorSSER :: Int -- ^ The decimation factor
-> [Float] -- ^ The filter coefficients
-> IO (Decimator IO VS.Vector VS.MVector Float) -- ^ The `Decimator` data structure
fastDecimatorSSER = mkDecimator 4 decimateCSSERR
-- | Returns a fast Decimator data structure implemented in C using AVX instructions. For decimating real data with real coefficients.
fastDecimatorAVXR :: Int -- ^ The decimation factor
-> [Float] -- ^ The filter coefficients
-> IO (Decimator IO VS.Vector VS.MVector Float) -- ^ The `Decimator` data structure
fastDecimatorAVXR = mkDecimator 8 decimateCAVXRR
-- | Returns a fast Decimator data structure implemented in C using the fastest SIMD instruction set your processor supports. For decimating real data with real coefficients.
fastDecimatorR :: CPUInfo -- ^ The CPU's capabilities
-> Int -- ^ The decimation factor
-> [Float] -- ^ The filter coefficients
-> IO (Decimator IO VS.Vector VS.MVector Float) -- ^ The `Decimator` data structure
fastDecimatorR info = featureSelect info fastDecimatorCR [(hasAVX, fastDecimatorAVXR), (hasSSE42, fastDecimatorSSER)]
mkDecimatorC :: Int
-> DecimateRC
-> Int
-> [Float]
-> IO (Decimator IO VS.Vector VS.MVector (Complex Float))
mkDecimatorC sizeMultiple filterFunc decimationD coeffs = do
let l = length coeffs
numCoeffsD = roundUp l sizeMultiple
diff = numCoeffsD - l
vCoeffs = VG.fromList $ duplicate $ coeffs ++ replicate diff 0
vCoeffs2 = VG.fromList $ coeffs ++ replicate diff 0
evaluate vCoeffs
let decimateOne = filterFunc decimationD vCoeffs
decimateCross = decimateCrossHighLevel decimationD vCoeffs2
return $ Decimator {..}
-- | Returns a fast Decimator data structure implemented in C. For decimating complex data with real coefficients.
fastDecimatorCC :: Int -- ^ The decimation factor
-> [Float] -- ^ The filter coefficients
-> IO (Decimator IO VS.Vector VS.MVector (Complex Float)) -- ^ The `Decimator` data structure
fastDecimatorCC = mkDecimatorC 1 decimateCRC
-- | Returns a fast Decimator data structure implemented in C using SSE instructions. For decimating complex data with real coefficients.
fastDecimatorSSEC :: Int -- ^ The decimation factor
-> [Float] -- ^ The filter coefficients
-> IO (Decimator IO VS.Vector VS.MVector (Complex Float)) -- ^ The `Decimator` data structure
fastDecimatorSSEC = mkDecimatorC 2 decimateCSSERC
-- | Returns a fast Decimator data structure implemented in C using AVX instructions. For decimating complex data with real coefficients.
fastDecimatorAVXC :: Int -- ^ The decimation factor
-> [Float] -- ^ The filter coefficients
-> IO (Decimator IO VS.Vector VS.MVector (Complex Float)) -- ^ The `Decimator` data structure
fastDecimatorAVXC = mkDecimatorC 4 decimateCAVXRC
-- | Returns a fast Decimator data structure implemented in C using the fastest SIMD instruction set your processor supports. For decimating complex data with real coefficients.
fastDecimatorC :: CPUInfo -- ^ The CPU's capabilities
-> Int -- ^ The decimation factor
-> [Float] -- ^ The filter coefficients
-> IO (Decimator IO VS.Vector VS.MVector (Complex Float)) -- ^ The `Decimator` data structure
fastDecimatorC info = featureSelect info fastDecimatorCC [(hasAVX, fastDecimatorAVXC), (hasSSE42, fastDecimatorSSEC)]
mkDecimatorSymR :: DecimateRR
-> Int
-> [Float]
-> IO (Decimator IO VS.Vector VS.MVector Float)
mkDecimatorSymR filterFunc decimationD coeffs = do
let vCoeffs = VG.fromList coeffs
let vCoeffs2 = VG.fromList $ coeffs ++ reverse coeffs
evaluate vCoeffs
evaluate vCoeffs2
let decimateOne = filterFunc decimationD vCoeffs
decimateCross = decimateCrossHighLevel decimationD vCoeffs2
numCoeffsD = length coeffs * 2
return $ Decimator {..}
-- | Returns a fast Decimator data structure implemented in C using SSE instructions. For decimating real data with real coefficients. For decimators with symmetric coefficients, i.e. 'linear phase'. Coefficient length must be a multiple of 4.
fastDecimatorSymSSER :: Int -- ^ The decimation factor
-> [Float] -- ^ The first half of the filter coefficients
-> IO (Decimator IO VS.Vector VS.MVector Float) -- ^ The `Decimator` data structure
fastDecimatorSymSSER = mkDecimatorSymR decimateCSSESymmetricRR
-- | Returns a fast Decimator data structure implemented in C using AVX instructions. For decimating real data with real coefficients. For decimators with symmetric coefficients, i.e. 'linear phase'. Coefficient length must be a multiple of 4.
fastDecimatorSymAVXR :: Int -- ^ The decimation factor
-> [Float] -- ^ The first half of the filter coefficients
-> IO (Decimator IO VS.Vector VS.MVector Float) -- ^ The `Decimator` data structure
fastDecimatorSymAVXR = mkDecimatorSymR decimateCAVXSymmetricRR
-- | Returns a fast Decimator data structure implemented in C using the fastest SIMD instruction set your processor supports. For decimating real data with real coefficients. For decimators with symmetric coefficients, i.e. 'linear phase'. Coefficient length must be a multiple of 4.
fastDecimatorSymR :: CPUInfo -- ^ The CPU's capabilities
-> Int -- ^ The decimation factor
-> [Float] -- ^ The filter coefficients
-> IO (Decimator IO VS.Vector VS.MVector Float) -- ^ The `Decimator` data structure
fastDecimatorSymR info = featureSelect info (error "at least AVX required") [(hasAVX, fastDecimatorSymAVXR), (hasSSE42, fastDecimatorSymSSER)]
{-# INLINE haskellResampler #-}
-- | Returns a slow Resampler data structure entirely implemented in Haskell
haskellResampler :: (PrimMonad m, Functor m, Num a, Mult a b, VG.Vector v a, VG.Vector v b, VGM.MVector vm a)
=> Int -- ^ The interpolation factor
-> Int -- ^ The decimation factor
-> [b] -- ^ The filter coefficients
-> IO (Resampler m v vm a) -- ^ The `Resampler` data structure
haskellResampler interpolationR decimationR coeffs = do
let vCoeffs = VG.fromList coeffs
evaluate vCoeffs
let resampleOne v w x y = func <$> resampleHighLevel interpolationR decimationR vCoeffs v w x y
resampleCross v w x y z = func <$> resampleCrossHighLevel interpolationR decimationR vCoeffs v w x y z
numCoeffsR = length coeffs
func x = (x, x)
startDat = 0
return $ Resampler {..}
mkResampler :: Int
-> ResampleRR
-> Int
-> Int
-> [Float]
-> IO (Resampler IO VS.Vector VS.MVector Float)
mkResampler sizeMultiple filterFunc interpolationR decimationR coeffs = do
let vCoeffs = VG.fromList coeffs
evaluate vCoeffs
resamp <- filterFunc interpolationR decimationR coeffs
let resampleOne v w x y = func1 <$> resamp (fst v) w x y
resampleCross (group, offset) count x y z = do
offset' <- resampleCrossHighLevel interpolationR decimationR vCoeffs offset count x y z
return (((group + count) `mod` interpolationR, offset'), offset')
numCoeffsR = roundUp (length coeffs) (interpolationR * sizeMultiple)
func1 group = let offset = interpolationR - 1 - ((interpolationR + group * decimationR - 1) `mod` interpolationR) in ((group, offset), offset)
startDat = (0, 0)
return $ Resampler {..}
-- | Returns a fast Resampler data structure implemented in C. For filtering real data with real coefficients.
fastResamplerCR :: Int -- ^ The interpolation factor
-> Int -- ^ The decimation factor
-> [Float] -- ^ The filter coefficients
-> IO (Resampler IO VS.Vector VS.MVector Float) -- ^ The `Resampler` data structure
fastResamplerCR = mkResampler 1 resampleCRR2
-- | Returns a fast Resampler data structure implemented in C using SSE instructions. For filtering real data with real coefficients.
fastResamplerSSER :: Int -- ^ The interpolation factor
-> Int -- ^ The decimation factor
-> [Float] -- ^ The filter coefficients
-> IO (Resampler IO VS.Vector VS.MVector Float) -- ^ The `Resampler` data structure
fastResamplerSSER = mkResampler 4 resampleCSSERR
-- | Returns a fast Resampler data structure implemented in C using AVX instructions. For filtering real data with real coefficients.
fastResamplerAVXR :: Int -- ^ The interpolation factor
-> Int -- ^ The decimation factor
-> [Float] -- ^ The filter coefficients
-> IO (Resampler IO VS.Vector VS.MVector Float) -- ^ The `Resampler` data structure
fastResamplerAVXR = mkResampler 8 resampleCAVXRR
-- | Returns a fast Resampler data structure implemented in C using the fastest SIMD instruction set your processor supports. For resampling real data with real coefficients.
fastResamplerR :: CPUInfo -- ^ The CPU's capabilities
-> Int -- ^ The interpolation factor
-> Int -- ^ The decimation factor
-> [Float] -- ^ The filter coefficients
-> IO (Resampler IO VS.Vector VS.MVector Float) -- ^ The `Resampler` data structure
fastResamplerR info = featureSelect info fastResamplerCR [(hasAVX, fastResamplerAVXR), (hasSSE42, fastResamplerSSER)]
data Buffer v a = Buffer {
buffer :: v a,
offset :: Int
}
space Buffer{..} = VGM.length buffer - offset
newBuffer :: (PrimMonad m, VGM.MVector vm a) => Int -> m (Buffer (vm (PrimState m)) a)
newBuffer size = do
buf <- VGM.new size
return $ Buffer buf 0
advanceOutBuf :: (PrimMonad m, VG.Vector v a) => Int -> Buffer (VG.Mutable v (PrimState m)) a -> Int -> Pipe b (v a) m (Buffer (VG.Mutable v (PrimState m)) a)
advanceOutBuf blockSizeOut buf@(Buffer bufOut offsetOut) count =
if count == space buf then do
bufOutF <- lift $ VG.unsafeFreeze bufOut
yield bufOutF
lift $ newBuffer blockSizeOut
else
return $ Buffer bufOut (offsetOut + count)
-- | My own assert implementation since the GHC one doesnt seem to work even with optimisations disabled and using -fno-ignore-asserts
assert loc False = error loc
assert loc True = return ()
--Filtering
{-# INLINE firFilter #-}
{-| Create a pipe that performs filtering -}
firFilter :: (PrimMonad m, Functor m, VG.Vector v a, Num a)
=> Filter m v (VG.Mutable v) a -- ^ The `Filter` data structure
-> Int -- ^ The output block size
-> Pipe (v a) (v a) m () -- ^ The `Pipe` that does the filtering
firFilter Filter{..} blockSizeOut = do
inBuf <- await
outBuf <- lift $ newBuffer blockSizeOut
simple inBuf outBuf
where
simple bufIn bufferOut@(Buffer bufOut offsetOut) = do
assert "filter 1" (VG.length bufIn >= numCoeffsF)
let count = min (VG.length bufIn - numCoeffsF + 1) (space bufferOut)
lift $ filterOne count bufIn (VGM.unsafeDrop offsetOut bufOut)
bufferOut' <- advanceOutBuf blockSizeOut bufferOut count
let bufIn' = VG.drop count bufIn
case VG.length bufIn' < numCoeffsF of
False -> simple bufIn' bufferOut'
True -> do
next <- await
crossover bufIn' next bufferOut'
crossover bufLast bufNext bufferOut@(Buffer bufOut offsetOut) = do
assert "filter 2" (VG.length bufLast < numCoeffsF)
assert "filter 3" (VG.length bufLast > 0)
let count = min (VG.length bufLast) (space bufferOut)
lift $ filterCross count bufLast bufNext (VGM.unsafeDrop offsetOut bufOut)
bufferOut' <- advanceOutBuf blockSizeOut bufferOut count
case VG.length bufLast == count of
True -> simple bufNext bufferOut'
False -> crossover (VG.drop count bufLast) bufNext bufferOut'
--Decimation
{-# INLINE firDecimator #-}
{-| Create a pipe that performs decimation -}
firDecimator :: (PrimMonad m, Functor m, VG.Vector v a, Num a)
=> Decimator m v (VG.Mutable v) a -- ^ The `Decimator` data structure
-> Int -- ^ The output block size
-> Pipe (v a) (v a) m () -- ^ The `Pipe` that does the decimation
firDecimator Decimator{..} blockSizeOut = do
inBuf <- await
outBuf <- lift $ newBuffer blockSizeOut
simple inBuf outBuf
where
simple bufIn bufferOut@(Buffer bufOut offsetOut) = do
assert "decimate 1" (VG.length bufIn >= numCoeffsD)
let count = min (((VG.length bufIn - numCoeffsD) `quot` decimationD) + 1) (space bufferOut)
lift $ decimateOne count bufIn (VGM.unsafeDrop offsetOut bufOut)
bufferOut' <- advanceOutBuf blockSizeOut bufferOut count
let bufIn' = VG.drop (count * decimationD) bufIn
case VG.length bufIn' < numCoeffsD of
False -> simple bufIn' bufferOut'
True -> do
next <- await
crossover bufIn' next bufferOut'
crossover bufLast bufNext bufferOut@(Buffer bufOut offsetOut) = do
assert "decimate 2" (VG.length bufLast < numCoeffsD)
assert "decimate 3" (VG.length bufLast > 0)
let count = min (VG.length bufLast `quotUp` decimationD) (space bufferOut)
lift $ decimateCross count bufLast bufNext (VGM.unsafeDrop offsetOut bufOut)
bufferOut' <- advanceOutBuf blockSizeOut bufferOut count
case VG.length bufLast <= count * decimationD of
True -> simple (VG.drop (count * decimationD - VG.length bufLast) bufNext) bufferOut'
False -> crossover (VG.drop (count * decimationD) bufLast) bufNext bufferOut'
{-
Rational Downsampling:
Input upsampled by 3: |**|**|**|**|**|**|**|**|**|**|**|
Output downsampled by 7: |******|******|******|******|*****
Consider here ^
Next output is here ^
Filter offset is 2
k is number of used inputs
filterOffset + k*interpolation = decimation + filterOffset'
where
k > 0
0 <= filterOffset, filterOffset' < interpolation
k*interpolation - filterOffset' = decimation - filterOffset
k*interpolation - filterOffset' - 1 = decimation - filterOffset - 1
(k-1) * interpolation + (interpolation - filterOffset' - 1) = decimation - filterOffset - 1
k = (decimation - filterOffset - 1) / interpolation + 1
filterOffset' = interpolation - 1 - (decimation - filterOffset - 1) % interpolation
Only works if decimation > interpolation
-}
{-
Rational Upsampling:
Input upsampled by 7: |******|******|******|******|*****
Output downsampled by 3: |**|**|**|**|**|**|**|**|**|**|**|
Consider Here ^
Next sample is ^
Filter offset is 4
filterOffset + k * interpolation = decimation + filterOffset'
where
k = {0, 1}
0 <= filterOffset, filterOffset' < interpolation
k * interpolation + (interpolation - filterOffset' - 1) = decimation - filterOffset + interpolation - 1
k = (decimation - filterOffset + interpolation - 1) / interpolation
============================
Or, equivalently,
k = 0 | filterOffset >= decimation
1 | otherwise
o = o - decimation + k * interpolation
-}
--Rational resampling
quotUp q d = (q + (d - 1)) `quot` d
{-# INLINE firResampler #-}
{-| Create a pipe that performs resampling -}
firResampler :: (PrimMonad m, VG.Vector v a, Num a)
=> Resampler m v (VG.Mutable v) a -- ^ The `Resampler` data structure
-> Int -- ^ The output block size
-> Pipe (v a) (v a) m () -- ^ The `Pipe` that does the resampling
firResampler Resampler{..} blockSizeOut = do
inBuf <- await
outBuf <- lift $ newBuffer blockSizeOut
simple inBuf outBuf startDat 0
where
simple bufIn bufferOut@(Buffer bufOut offsetOut) dat filterOffset = do
assert "resample 1" (VG.length bufIn * interpolationR >= numCoeffsR - filterOffset)
--available number of samples == interpolation * num_input
--required number of samples == decimation * (num_output - 1) + filter_length - filter_offset
let count = min (((VG.length bufIn * interpolationR - numCoeffsR + filterOffset) `quot` decimationR) + 1) (space bufferOut)
(dat, endOffset) <- lift $ resampleOne dat count bufIn (VGM.unsafeDrop offsetOut bufOut)
assert "resample 2" ((count * decimationR + endOffset - filterOffset) `rem` interpolationR == 0)
bufferOut' <- advanceOutBuf blockSizeOut bufferOut count
--samples no longer needed starting from filterOffset == count * decimation - filterOffset
--inputs lying in this region == (count * decimation - filterOffset) / interpolation (rounding up)
let usedInput = (count * decimationR - filterOffset) `quotUp` interpolationR
bufIn' = VG.drop usedInput bufIn
case VG.length bufIn' * interpolationR < numCoeffsR - endOffset of
False -> simple bufIn' bufferOut' dat endOffset
True -> do
next <- await
--TODO: why is this not needed in filter and decimator
case VG.length bufIn' == 0 of
True -> simple next bufferOut' dat endOffset
False -> crossover bufIn' next bufferOut' dat endOffset
crossover bufLast bufNext bufferOut@(Buffer bufOut offsetOut) dat filterOffset = do
assert "resample 3" (VG.length bufLast * interpolationR < numCoeffsR - filterOffset)
--outputsComputable is the number of outputs that need to be computed for the last buffer to no longer be needed
--outputsComputable * decimation == numInput * interpolation + filterOffset + k
let outputsComputable = (VG.length bufLast * interpolationR + filterOffset) `quotUp` decimationR
count = min outputsComputable (space bufferOut)
assert "resample 4" (count /= 0)
(dat, endOffset) <- lift $ resampleCross dat count bufLast bufNext (VGM.unsafeDrop offsetOut bufOut)
assert "resample 5" ((count * decimationR + endOffset - filterOffset) `rem` interpolationR == 0)
bufferOut' <- advanceOutBuf blockSizeOut bufferOut count
let inputUsed = (count * decimationR - filterOffset) `quotUp` interpolationR
case inputUsed >= VG.length bufLast of
True -> simple (VG.drop (inputUsed - VG.length bufLast) bufNext) bufferOut' dat endOffset
False -> crossover (VG.drop inputUsed bufLast) bufNext bufferOut' dat endOffset
-- | A DC blocking filter
dcBlockingFilter :: Pipe (VS.Vector Float) (VS.Vector Float) IO ()
dcBlockingFilter = func 0 0
where
func lastSample lastOutput = do
dat <- await
out <- lift $ VGM.new (VG.length dat)
(lastSample, lastOutput) <- lift $ dcBlocker (VG.length dat) lastSample lastOutput dat out
outF <- lift $ VG.unsafeFreeze out
yield outF
func lastSample lastOutput