CDSPBlockConvolver.h

Go to the documentation of this file.

//$ nobt
//$ nocpp

 
#ifndef R8B_CDSPBLOCKCONVOLVER_INCLUDED
#define R8B_CDSPBLOCKCONVOLVER_INCLUDED
 
#include "CDSPFIRFilter.h"
#include "CDSPProcessor.h"
 
namespace r8b {

 
class CDSPBlockConvolver : public CDSPProcessor
{
public:
 
    CDSPBlockConvolver( CDSPFIRFilter& aFilter, const int aUpFactor,
        const int aDownFactor, const double PrevLatency = 0.0,
        const bool aDoConsumeLatency = true )
        : Filter( &aFilter )
        , UpFactor( aUpFactor )
        , DownFactor( aDownFactor )
        , BlockLen2( 2 << Filter -> getBlockLenBits() )
        , DoConsumeLatency( aDoConsumeLatency )
    {
        R8BASSERT( UpFactor > 0 );
        R8BASSERT( DownFactor > 0 );
        R8BASSERT( PrevLatency >= 0.0 );
 
        int fftinBits;
        UpShift = getBitOccupancy( UpFactor ) - 1;
 
        if(( 1 << UpShift ) == UpFactor )
        {
            fftinBits = Filter -> getBlockLenBits() + 1 - UpShift;
            PrevInputLen = ( Filter -> getKernelLen() - 1 + UpFactor - 1 ) /
                UpFactor;
 
            InputLen = BlockLen2 - PrevInputLen * UpFactor;
        }
        else
        {
            UpShift = -1;
            fftinBits = Filter -> getBlockLenBits() + 1;
            PrevInputLen = Filter -> getKernelLen() - 1;
            InputLen = BlockLen2 - PrevInputLen;
        }
 
        OutOffset = ( Filter -> isZeroPhase() ? Filter -> getLatency() : 0 );
        LatencyFrac = Filter -> getLatencyFrac() + PrevLatency * UpFactor;
        Latency = (int) LatencyFrac;
        const int InLatency = Latency + Filter -> getLatency() - OutOffset;
        LatencyFrac -= Latency;
        LatencyFrac /= DownFactor;
 
        Latency += InputLen + Filter -> getLatency();
 
        int fftoutBits;
        InputDelay = 0;
        DownSkipInit = 0;
        DownShift = getBitOccupancy( DownFactor ) - 1;
 
        if(( 1 << DownShift ) == DownFactor )
        {
            fftoutBits = Filter -> getBlockLenBits() + 1 - DownShift;
 
            if( DownFactor > 1 )
            {
                if( UpShift > 0 )
                {
                    // This case never happens in practice due to mutual
                    // exclusion of "power of 2" DownFactor and UpFactor
                    // values.
 
                    R8BASSERT( UpShift == 0 );
                }
                else
                {
                    // Make sure InputLen is divisible by DownFactor.
 
                    const int ilc = InputLen & ( DownFactor - 1 );
                    PrevInputLen += ilc;
                    InputLen -= ilc;
                    Latency -= ilc;
 
                    // Correct InputDelay for input and filter's latency.
 
                    const int lc = InLatency & ( DownFactor - 1 );
 
                    if( lc > 0 )
                    {
                        InputDelay = DownFactor - lc;
                    }
 
                    if( !DoConsumeLatency )
                    {
                        Latency /= DownFactor;
                    }
                }
            }
        }
        else
        {
            fftoutBits = Filter -> getBlockLenBits() + 1;
            DownShift = -1;
 
            if( !DoConsumeLatency && DownFactor > 1 )
            {
                DownSkipInit = Latency % DownFactor;
                Latency /= DownFactor;
            }
        }
 
        R8BASSERT( Latency >= 0 );
 
        fftin = new CDSPRealFFTKeeper( fftinBits );
 
        if( fftoutBits == fftinBits )
        {
            fftout = fftin;
        }
        else
        {
            ffto2 = new CDSPRealFFTKeeper( fftoutBits );
            fftout = ffto2;
        }
 
        WorkBlocks.alloc( BlockLen2 * 2 + PrevInputLen );
        CurInput = &WorkBlocks[ 0 ];
        CurOutput = &WorkBlocks[ BlockLen2 ]; // CurInput and
            // CurOutput are address-aligned.
        PrevInput = &WorkBlocks[ BlockLen2 * 2 ];
 
        clear();
 
        R8BCONSOLE( "CDSPBlockConvolver: flt_len=%i in_len=%i io=%i/%i "
            "fft=%i/%i latency=%i\n", Filter -> getKernelLen(), InputLen,
            UpFactor, DownFactor, (*fftin) -> getLen(), (*fftout) -> getLen(),
            getLatency() );
    }
 
    virtual int getInLenBeforeOutPos( int ReqOutPos ) const
    {
        if( Next != R8B_NULL )
        {
            ReqOutPos = Next -> getInLenBeforeOutPos( ReqOutPos );
        }
 
        return( (int) (( Latency + (double) ReqOutPos * DownFactor ) /
            UpFactor + LatencyFrac * DownFactor / UpFactor ));
    }
 
    virtual int getLatency() const
    {
        return( DoConsumeLatency ? 0 : Latency );
    }
 
    virtual double getLatencyFrac() const
    {
        return( LatencyFrac );
    }
 
    virtual int getMaxOutLen( const int MaxInLen ) const
    {
        R8BASSERT( MaxInLen >= 0 );
 
        return(( MaxInLen * UpFactor + DownFactor - 1 ) / DownFactor );
    }
 
    virtual void clear()
    {
        memset( &PrevInput[ 0 ], 0,
            (size_t) PrevInputLen * sizeof( PrevInput[ 0 ]));
 
        if( DoConsumeLatency )
        {
            LatencyLeft = Latency;
        }
        else
        {
            LatencyLeft = 0;
 
            if( DownShift > 0 )
            {
                memset( &CurOutput[ 0 ], 0,
                    (size_t) ( BlockLen2 >> DownShift ) *
                    sizeof( CurOutput[ 0 ]));
            }
            else
            {
                memset( &CurOutput[ BlockLen2 - OutOffset ], 0,
                    (size_t) OutOffset * sizeof( CurOutput[ 0 ]));
 
                memset( &CurOutput[ 0 ], 0,
                    (size_t) ( InputLen - OutOffset ) *
                    sizeof( CurOutput[ 0 ]));
            }
        }
 
        memset( CurInput, 0, (size_t) InputDelay * sizeof( CurInput[ 0 ]));
 
        InDataLeft = InputLen - InputDelay;
        UpSkip = 0;
        DownSkip = DownSkipInit;
    }
 
    virtual int process( double* ip, int l0, double*& op0 )
    {
        R8BASSERT( l0 >= 0 );
        R8BASSERT( UpFactor / DownFactor <= 1 || ip != op0 || l0 == 0 );
 
        double* op = op0;
        int l = l0 * UpFactor;
        l0 = 0;
 
        while( l > 0 )
        {
            const int Offs = InputLen - InDataLeft;
 
            if( l < InDataLeft )
            {
                InDataLeft -= l;
 
                if( UpShift >= 0 )
                {
                    memcpy( &CurInput[ Offs >> UpShift ], ip,
                        (size_t) ( l >> UpShift ) * sizeof( CurInput[ 0 ]));
                }
                else
                {
                    copyUpsample( ip, &CurInput[ Offs ], l );
                }
 
                copyToOutput( Offs - OutOffset, op, l, l0 );
                break;
            }
 
            const int b = InDataLeft;
            l -= b;
            InDataLeft = InputLen;
            int ilu;
 
            if( UpShift >= 0 )
            {
                const int bu = b >> UpShift;
                memcpy( &CurInput[ Offs >> UpShift ], ip,
                    (size_t) bu * sizeof( CurInput[ 0 ]));
 
                ip += bu;
                ilu = InputLen >> UpShift;
            }
            else
            {
                copyUpsample( ip, &CurInput[ Offs ], b );
                ilu = InputLen;
            }
 
            const size_t pil = (size_t) PrevInputLen * sizeof( CurInput[ 0 ]);
            memcpy( &CurInput[ ilu ], PrevInput, pil );
            memcpy( PrevInput, &CurInput[ ilu - PrevInputLen ], pil );
 
            realfft_t* CurInputFFT = (*fftin) -> forward( CurInput );
            realfft_t dsn = 0;
 
            if( UpShift > 0 || DownShift > 0 )
            {
                CurInputFFT = (*fftin) -> reorderForward( CurInputFFT,
                    ( (*fftin) -> getLen() > (*fftout) -> getLen() ?
                    (*fftin) -> getWorkBuf() : (*fftout) -> getWorkBuf() ));
 
                if( UpShift > 0 )
                {
                    mirrorInputSpectrum( CurInputFFT );
                }
 
                if( DownShift > 0 )
                {
                    const int z = BlockLen2 >> DownShift;
                    dsn = Filter -> calcDownShiftNyquist( CurInputFFT[ z ],
                        CurInputFFT[ z + 1 ]);
                }
 
                CurInputFFT = (*fftout) -> reorderInverse( CurInputFFT,
                    CurInput );
            }
 
            if( Filter -> isZeroPhase() )
            {
                (*fftout) -> multiplyBlocksZP( Filter -> getKernelBlock(),
                    CurInputFFT );
            }
            else
            {
                (*fftout) -> multiplyBlocks( Filter -> getKernelBlock(),
                    CurInputFFT );
            }
 
            if( DownShift > 0 )
            {
                (*fftout) -> setBinNyquist( CurInputFFT, dsn );
            }
 
            (*fftout) -> inverse( CurInputFFT );
 
            copyToOutput( Offs - OutOffset, op, b, l0 );
 
            double* const tmp = CurInput;
            CurInput = CurOutput;
            CurOutput = tmp;
        }
 
        return( l0 );
    }
 
private:
    CRefKeeper< CDSPFIRFilter > Filter; 
    CPtrKeeper< CDSPRealFFTKeeper > fftin; 
    CPtrKeeper< CDSPRealFFTKeeper > ffto2; 
    CDSPRealFFTKeeper* fftout; 
    int UpFactor; 
    int DownFactor; 
    int BlockLen2; 
    int OutOffset; 
    int PrevInputLen; 
    int InputLen; 
    double LatencyFrac; 
    int Latency; 
    int UpShift; 
    int DownShift; 
    int InputDelay; 
    double* PrevInput; 
    double* CurInput; 
    double* CurOutput; 
    int InDataLeft; 
    int LatencyLeft; 
    int UpSkip; 
    int DownSkip; 
    int DownSkipInit; 
    CFixedBuffer< double > WorkBlocks; 
    bool DoConsumeLatency; 

 
    void copyUpsample( double*& ip0, double* op, int l0 )
    {
        int b = min( UpSkip, l0 );
 
        if( b != 0 )
        {
            UpSkip -= b;
            l0 -= b;
 
            *op = 0.0;
            op++;
 
            while( --b != 0 )
            {
                *op = 0.0;
                op++;
            }
        }
 
        double* ip = ip0;
        const int upf = UpFactor;
        int l = l0 / upf;
        int lz = l0 - l * upf;
 
        if( upf == 3 )
        {
            while( l != 0 )
            {
                op[ 0 ] = *ip;
                op[ 1 ] = 0.0;
                op[ 2 ] = 0.0;
                ip++;
                op += upf;
                l--;
            }
        }
        else
        if( upf == 5 )
        {
            while( l != 0 )
            {
                op[ 0 ] = *ip;
                op[ 1 ] = 0.0;
                op[ 2 ] = 0.0;
                op[ 3 ] = 0.0;
                op[ 4 ] = 0.0;
                ip++;
                op += upf;
                l--;
            }
        }
        else
        {
            const size_t zc = (size_t) ( upf - 1 ) * sizeof( op[ 0 ]);
 
            while( l != 0 )
            {
                *op = *ip;
                ip++;
 
                memset( op + 1, 0, zc );
                op += upf;
                l--;
            }
        }
 
        if( lz != 0 )
        {
            *op = *ip;
            ip++;
            op++;
 
            UpSkip = upf - lz;
 
            while( --lz != 0 )
            {
                *op = 0.0;
                op++;
            }
        }
 
        ip0 = ip;
    }

 
    void copyToOutput( int Offs, double*& op0, int b, int& l0 )
    {
        if( Offs < 0 )
        {
            if( Offs + b <= 0 )
            {
                Offs += BlockLen2;
            }
            else
            {
                copyToOutput( Offs + BlockLen2, op0, -Offs, l0 );
                b += Offs;
                Offs = 0;
            }
        }
 
        if( LatencyLeft != 0 )
        {
            if( LatencyLeft >= b )
            {
                LatencyLeft -= b;
                return;
            }
 
            Offs += LatencyLeft;
            b -= LatencyLeft;
            LatencyLeft = 0;
        }
 
        const int df = DownFactor;
 
        if( DownShift > 0 )
        {
            int Skip = Offs & ( df - 1 );
 
            if( Skip > 0 )
            {
                Skip = df - Skip;
                b -= Skip;
                Offs += Skip;
            }
 
            if( b > 0 )
            {
                b = ( b + df - 1 ) >> DownShift;
                memcpy( op0, &CurOutput[ Offs >> DownShift ],
                    (size_t) b * sizeof( op0[ 0 ]));
 
                op0 += b;
                l0 += b;
            }
        }
        else
        {
            if( df > 1 )
            {
                const double* ip = &CurOutput[ Offs + DownSkip ];
                int l = ( b + df - 1 - DownSkip ) / df;
                DownSkip += l * df - b;
 
                double* op = op0;
                l0 += l;
                op0 += l;
 
                while( l > 0 )
                {
                    *op = *ip;
                    ip += df;
                    op++;
                    l--;
                }
            }
            else
            {
                memcpy( op0, &CurOutput[ Offs ],
                    (size_t) b * sizeof( op0[ 0 ]));
 
                op0 += b;
                l0 += b;
            }
        }
    }

 
    template< typename T >
    void mirrorInputSpectrum( T* const p )
    {
        const int bl1 = BlockLen2 >> UpShift;
        const int bl2 = bl1 * 2;
        int i;
 
        for( i = bl1 + 2; i < bl2; i += 2 )
        {
            p[ i ] = p[ bl2 - i ];
            p[ i + 1 ] = -p[ bl2 - i + 1 ];
        }
 
        p[ bl1 ] = p[ 1 ];
        p[ bl1 + 1 ] = (T) 0;
        p[ 1 ] = p[ 0 ];
 
        for( i = 1; i < UpShift; i++ )
        {
            const int z = bl1 << i;
            memcpy( &p[ z ], p, (size_t) z * sizeof( p[ 0 ]));
            p[ z + 1 ] = (T) 0;
        }
    }
};
 
} // namespace r8b
 
#endif // R8B_CDSPBLOCKCONVOLVER_INCLUDED