Audio.cpp

/* 
 *      Copyright (C) 2003-2005 Gabest
 *      http://www.gabest.org
 *
 *  This Program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2, or (at your option)
 *  any later version.
 *   
 *  This Program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 *  GNU General Public License for more details.
 *   
 *  You should have received a copy of the GNU General Public License
 *  along with GNU Make; see the file COPYING.  If not, write to
 *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA. 
 *  http://www.gnu.org/copyleft/gpl.html
 *
 */

// originally from virtualdub

#include "stdafx.h"
#include "Audio.h"

static long audio_pointsample_8(void *dst, void *src, long accum, long samp_frac, long cnt) {
        unsigned char *d = (unsigned char *)dst;
        unsigned char *s = (unsigned char *)src;

        do {
                *d++ = s[accum>>19];
                accum += samp_frac;
        } while(--cnt);

        return accum;
}

static long audio_pointsample_16(void *dst, void *src, long accum, long samp_frac, long cnt) {
        unsigned short *d = (unsigned short *)dst;
        unsigned short *s = (unsigned short *)src;

        do {
                *d++ = s[accum>>19];
                accum += samp_frac;
        } while(--cnt);

        return accum;
}

static long audio_pointsample_32(void *dst, void *src, long accum, long samp_frac, long cnt) {
        unsigned long *d = (unsigned long *)dst;
        unsigned long *s = (unsigned long *)src;

        do {
                *d++ = s[accum>>19];
                accum += samp_frac;
        } while(--cnt);

        return accum;
}

static long audio_downsample_mono8(void *dst, void *src, long *filter_bank, int filter_width, long accum, long samp_frac, long cnt) {
        unsigned char *d = (unsigned char *)dst;
        unsigned char *s = (unsigned char *)src;

        do {
                long sum = 0;
                int w;
                long *fb_ptr;
                unsigned char *s_ptr;

                w = filter_width;
                fb_ptr = filter_bank + filter_width * ((accum>>11)&0xff);
                s_ptr = s + (accum>>19);
                do {
                        sum += *fb_ptr++ * (int)*s_ptr++;
                } while(--w);

                if (sum < 0)
                        *d++ = 0;
                else if (sum > 0x3fffff)
                        *d++ = 0xff;
                else
                        *d++ = ((sum + 0x2000)>>14);

                accum += samp_frac;
        } while(--cnt);

        return accum;
}

static long audio_downsample_mono16(void *dst, void *src, long *filter_bank, int filter_width, long accum, long samp_frac, long cnt) {
        signed short *d = (signed short *)dst;
        signed short *s = (signed short *)src;

        do {
                long sum = 0;
                int w;
                long *fb_ptr;
                signed short *s_ptr;

                w = filter_width;
                fb_ptr = filter_bank + filter_width * ((accum>>11)&0xff);
                s_ptr = s + (accum>>19);
                do {
                        sum += *fb_ptr++ * (int)*s_ptr++;
                } while(--w);

                if (sum < -0x20000000)
                        *d++ = -0x8000;
                else if (sum > 0x1fffffff)
                        *d++ = 0x7fff;
                else
                        *d++ = ((sum + 0x2000)>>14);

                accum += samp_frac;
        } while(--cnt);

        return accum;
}


static int permute_index(int a, int b) {
        return (b-(a>>8)-1) + (a&255)*b;
}

static void make_downsample_filter(long *filter_bank, int filter_width, long samp_frac) {
        int i, j, v;
        double filt_max;
        double filtwidth_frac;

        filtwidth_frac = samp_frac/2048.0;

        filter_bank[filter_width-1] = 0;

        filt_max = (16384.0 * 524288.0) / samp_frac;

        for(i=0; i<128*filter_width; i++) {
                int y = 0;
                double d = i / filtwidth_frac;

                if (d<1.0)
                        y = (int)(0.5 + filt_max*(1.0 - d));

                filter_bank[permute_index(128*filter_width + i, filter_width)]
                        = filter_bank[permute_index(128*filter_width - i, filter_width)]
                        = y;
        }

        // Normalize the filter to correct for integer roundoff errors

        for(i=0; i<256*filter_width; i+=filter_width) {
                v=0;
                for(j=0; j<filter_width; j++)
                        v += filter_bank[i+j];

//              _RPT2(0,"error[%02x] = %04x\n", i/filter_width, 0x4000 - v);

                v = (0x4000 - v)/filter_width;
                for(j=0; j<filter_width; j++)
                        filter_bank[i+j] += v;
        }

//      _CrtCheckMemory();
}

AudioStreamResampler::AudioStreamResampler(int bps, long org_rate, long new_rate, bool fHighQuality)
{
        samp_frac = 0x80000;

        this->bps = bps;

        if(bps == 1)
        {
                ptsampleRout = audio_pointsample_8;
                dnsampleRout = audio_downsample_mono8;
        }
        else if(bps >= 2)
        {
                ptsampleRout = audio_pointsample_16;
                dnsampleRout = audio_downsample_mono16;
        }
        else
        {
                return;
        }

// org_rate > new_rate!
        samp_frac = MulDiv(org_rate, 0x80000, new_rate);

        holdover = 0;
        filter_bank = NULL;
        filter_width = 1;
        accum = 0;

        // If this is a high-quality downsample, allocate memory for the filter bank

        if(fHighQuality)
        {
                if(samp_frac>0x80000)
                {
                        // HQ downsample: allocate filter bank

                        filter_width = ((samp_frac + 0x7ffff)>>19)<<1 <<1;

                        if(!(filter_bank = new long[filter_width * 256]))
                        {
                                filter_width = 1;
                                return;
                        }

                        make_downsample_filter(filter_bank, filter_width, samp_frac);

                        // Clear lower samples

                        memset(cbuffer, bps >= 2 ? 0 : 0x80, bps*filter_width);

                        holdover = filter_width/2;
                }
        }
}

AudioStreamResampler::~AudioStreamResampler()
{
        delete [] filter_bank;
}

long AudioStreamResampler::Downsample(void* input, long samplesin, void* output, long samplesout)
{
        long lActualSamples = 0;

        // Downsampling is even worse because we have overlap to the left and to the
        // right of the interpolated point.
        //
        // We need (n/2) points to the left and (n/2-1) points to the right.

        while(samplesin > 0 && samplesout > 0)
        {
                long srcSamples, dstSamples;
                int nhold;

                // Figure out how many source samples we need.
                //
                // To do this, compute the highest fixed-point accumulator we'll reach.
                // Truncate that, and add the filter width.  Then subtract however many
                // samples are sitting at the bottom of the buffer.

                srcSamples = (long)(((__int64)samp_frac*(samplesout-1) + accum) >> 19) + filter_width - holdover;

                // Don't exceed the buffer (BUFFER_SIZE - holdover).

                if(srcSamples > BUFFER_SIZE - holdover)
                        srcSamples = BUFFER_SIZE - holdover;

                // Read into buffer.

                srcSamples = min(srcSamples, samplesin);
                if(!srcSamples) break;

                memcpy((char*)cbuffer + holdover*bps, (char*)input, srcSamples*bps);
                input = (void *)((char *)input + srcSamples*bps);

                // Figure out how many destination samples we'll get out of what we
                // read.  We'll have (srcSamples+holdover) bytes, so the maximum
                // fixed-pt accumulator we can hit is
                // (srcSamples+holdover-filter_width)<<16 + 0xffff.

                dstSamples = (((__int64)(srcSamples+holdover-filter_width)<<19) + 0x7ffff - accum) / samp_frac + 1;

                if(dstSamples > samplesout)
                        dstSamples = samplesout;

                if(dstSamples >= 1)
                {
                        if(filter_bank)
                                accum = dnsampleRout(output, cbuffer, filter_bank, filter_width, accum, samp_frac, dstSamples);
                        else
                                accum = ptsampleRout(output, cbuffer, accum, samp_frac, dstSamples);

                        output = (void *)((char *)output + bps * dstSamples);
                        lActualSamples += dstSamples;
                        samplesout -= dstSamples;
                }

                // We're "shifting" the new samples down to the bottom by discarding
                // all the samples in the buffer, so adjust the fixed-pt accum
                // accordingly.

                accum -= ((srcSamples+holdover)<<19);

                // Oops, did we need some of those?
                //
                // If accum=0, we need (n/2) samples back.  accum>=0x10000 is fewer,
                // accum<0 is more.

                nhold = - (accum>>19);

//              _ASSERT(nhold<=(filter_width/2));

                if (nhold>0) {
                        memmove(cbuffer, (char *)cbuffer+bps*(srcSamples+holdover-nhold), bps*nhold);
                        holdover = nhold;
                        accum += nhold<<19;
                } else
                        holdover = 0;

//              _ASSERT(accum>=0);
        }

        int Bytes = lActualSamples * bps;

        return lActualSamples;
}

---