resample.c

一个开源SIP协议栈

	  t += (((RES_WORD)*Hdp)*a)>>Na; /* t is now interp'd filter coeff */
	  t *= *Xp;		/* Mult coeff by input sample */
	  if (t & 1<<(Nhxn-1))	/* Round, if needed */
	    t += 1<<(Nhxn-1);
	  t >>= Nhxn;		/* Leave some guard bits, but come back some */
	  v += t;			/* The filter output */
	  Ho += dhb;		/* IR step */
	  Xp += Inc;		/* Input signal step. NO CHECK ON BOUNDS */
      }
    else 
      while ((Hp = &Imp[Ho>>Na]) < End) {
	  t = *Hp;		/* Get IR sample */
	  t *= *Xp;		/* Mult coeff by input sample */
	  if (t & 1<<(Nhxn-1))	/* Round, if needed */
	    t += 1<<(Nhxn-1);
	  t >>= Nhxn;		/* Leave some guard bits, but come back some */
	  v += t;			/* The filter output */
	  Ho += dhb;		/* IR step */
	  Xp += Inc;		/* Input signal step. NO CHECK ON BOUNDS */
      }
    return(v);
}

/* Sampling rate up-conversion only subroutine;
 * Slightly faster than down-conversion;
 */
static int SrcUp(const RES_HWORD X[], RES_HWORD Y[], double pFactor, 
		 RES_UHWORD nx, RES_UHWORD pNwing, RES_UHWORD pLpScl,
		 const RES_HWORD pImp[], const RES_HWORD pImpD[], RES_BOOL Interp)
{
    const RES_HWORD *xp;
    RES_HWORD *Ystart, *Yend;
    RES_WORD v;
    
    double dt;                  /* Step through input signal */ 
    RES_UWORD dtb;                  /* Fixed-point version of Dt */
    RES_UWORD time = 0;
    RES_UWORD endTime;              /* When time reaches EndTime, return to user */
    
    dt = 1.0/pFactor;            /* Output sampling period */
    dtb = dt*(1<<Np) + 0.5;     /* Fixed-point representation */
    
    Ystart = Y;
    Yend = Ystart + (unsigned)(nx * pFactor);
    endTime = time + (1<<Np)*(RES_WORD)nx;
    while (time < endTime)
    {
	xp = &X[time>>Np];      /* Ptr to current input sample */
	/* Perform left-wing inner product */
	v = 0;
	v = FilterUp(pImp, pImpD, pNwing, Interp, xp, (RES_HWORD)(time&Pmask),-1);

	/* Perform right-wing inner product */
	v += FilterUp(pImp, pImpD, pNwing, Interp, xp+1,  (RES_HWORD)((-time)&Pmask),1);

	v >>= Nhg;		/* Make guard bits */
	v *= pLpScl;		/* Normalize for unity filter gain */
	*Y++ = WordToHword(v,NLpScl);   /* strip guard bits, deposit output */
	time += dtb;		/* Move to next sample by time increment */
    }
    return (Y - Ystart);        /* Return the number of output samples */
}


/* Sampling rate conversion subroutine */

static int SrcUD(const RES_HWORD X[], RES_HWORD Y[], double pFactor, 
		 RES_UHWORD nx, RES_UHWORD pNwing, RES_UHWORD pLpScl,
		 const RES_HWORD pImp[], const RES_HWORD pImpD[], RES_BOOL Interp)
{
    const RES_HWORD *xp;
    RES_HWORD *Ystart, *Yend;
    RES_WORD v;
    
    double dh;                  /* Step through filter impulse response */
    double dt;                  /* Step through input signal */
    RES_UWORD time = 0;
    RES_UWORD endTime;          /* When time reaches EndTime, return to user */
    RES_UWORD dhb, dtb;         /* Fixed-point versions of Dh,Dt */
    
    dt = 1.0/pFactor;            /* Output sampling period */
    dtb = dt*(1<<Np) + 0.5;     /* Fixed-point representation */
    
    dh = MIN(Npc, pFactor*Npc);  /* Filter sampling period */
    dhb = dh*(1<<Na) + 0.5;     /* Fixed-point representation */
    
    Ystart = Y;
    Yend = Ystart + (unsigned)(nx * pFactor);
    endTime = time + (1<<Np)*(RES_WORD)nx;
    while (time < endTime)
    {
	xp = &X[time>>Np];	/* Ptr to current input sample */
	v = FilterUD(pImp, pImpD, pNwing, Interp, xp, (RES_HWORD)(time&Pmask),
		     -1, dhb);	/* Perform left-wing inner product */
	v += FilterUD(pImp, pImpD, pNwing, Interp, xp+1, (RES_HWORD)((-time)&Pmask),
		      1, dhb);	/* Perform right-wing inner product */
	v >>= Nhg;		/* Make guard bits */
	v *= pLpScl;		/* Normalize for unity filter gain */
	*Y++ = WordToHword(v,NLpScl);   /* strip guard bits, deposit output */
	time += dtb;		/* Move to next sample by time increment */
    }
    return (Y - Ystart);        /* Return the number of output samples */
}


/* ***************************************************************************
 *
 * PJMEDIA RESAMPLE 
 *
 * ***************************************************************************
 */

struct pjmedia_resample
{
    double	 factor;	/* Conversion factor = rate_out / rate_in.  */
    pj_bool_t	 large_filter;	/* Large filter?			    */
    pj_bool_t	 high_quality;	/* Not fast?				    */
    unsigned	 xoff;		/* History and lookahead size, in samples   */
    unsigned	 frame_size;	/* Samples per frame.			    */
    pj_int16_t	*buffer;	/* Input buffer.			    */
};


PJ_DEF(pj_status_t) pjmedia_resample_create( pj_pool_t *pool,
					     pj_bool_t high_quality,
					     pj_bool_t large_filter,
					     unsigned rate_in,
					     unsigned rate_out,
					     unsigned samples_per_frame,
					     pjmedia_resample **p_resample)
{
    pjmedia_resample *resample;

    PJ_ASSERT_RETURN(pool && p_resample && rate_in &&
		     rate_out && samples_per_frame, PJ_EINVAL);

    resample = pj_pool_alloc(pool, sizeof(pjmedia_resample));
    PJ_ASSERT_RETURN(resample, PJ_ENOMEM);

    /*
     * If we're downsampling, always use the fast algorithm since it seems
     * to yield the same quality.
     */
    if (rate_out < rate_in) {
	//no this is not a good idea. It sounds pretty good with speech,
	//but very poor with background noise etc.
	//high_quality = 0;
    }

#if !defined(PJMEDIA_HAS_LARGE_FILTER) || PJMEDIA_HAS_LARGE_FILTER==0
    /*
     * If large filter is excluded in the build, then prevent application
     * from using it.
     */
    if (high_quality && large_filter) {
	large_filter = PJ_FALSE;
	PJ_LOG(5,(THIS_FILE, 
		  "Resample uses small filter because large filter is "
		  "disabled"));
    }
#endif

#if !defined(PJMEDIA_HAS_SMALL_FILTER) || PJMEDIA_HAS_SMALL_FILTER==0
    /*
     * If small filter is excluded in the build and application wants to
     * use it, then drop to linear conversion.
     */
    if (high_quality && large_filter == 0) {
	high_quality = PJ_FALSE;
	PJ_LOG(4,(THIS_FILE, 
		  "Resample uses linear because small filter is disabled"));
    }
#endif

    resample->factor = rate_out * 1.0 / rate_in;
    resample->large_filter = large_filter;
    resample->high_quality = high_quality;
    resample->frame_size = samples_per_frame;

    if (high_quality) {
	unsigned size;

	/* This is a bug in xoff calculation, thanks Stephane Lussier
	 * of Macadamian dot com.
	 *   resample->xoff = large_filter ? 32 : 6;
	 */
	if (large_filter)
	    resample->xoff = (LARGE_FILTER_NMULT + 1) / 2.0  *  
			     MAX(1.0, 1.0/resample->factor);
	else
	    resample->xoff = (SMALL_FILTER_NMULT + 1) / 2.0  *  
			     MAX(1.0, 1.0/resample->factor);


	size = (samples_per_frame + 2*resample->xoff) * sizeof(pj_int16_t);
	resample->buffer = pj_pool_alloc(pool, size);
	PJ_ASSERT_RETURN(resample->buffer, PJ_ENOMEM);

	pjmedia_zero_samples(resample->buffer, resample->xoff*2);


    } else {
	resample->xoff = 0;
    }

    *p_resample = resample;

    PJ_LOG(5,(THIS_FILE, "resample created: %s qualiy, %s filter, in/out "
			  "rate=%d/%d", 
			  (high_quality?"high":"low"),
			  (large_filter?"large":"small"),
			  rate_in, rate_out));
    return PJ_SUCCESS;
}



PJ_DEF(void) pjmedia_resample_run( pjmedia_resample *resample,
				   const pj_int16_t *input,
				   pj_int16_t *output )
{
    PJ_ASSERT_ON_FAIL(resample, return);

    if (resample->high_quality) {
	pj_int16_t *dst_buf;
	const pj_int16_t *src_buf;

	/* Okay chaps, here's how we do resampling.
	 *
	 * The original resample algorithm requires xoff samples *before* the
	 * input buffer as history, and another xoff samples *after* the
	 * end of the input buffer as lookahead. Since application can only
	 * supply framesize buffer on each run, PJMEDIA needs to arrange the
	 * buffer to meet these requirements.
	 *
	 * So here comes the trick.
	 *
	 * First of all, because of the history and lookahead requirement, 
	 * resample->buffer need to accomodate framesize+2*xoff samples in its
	 * buffer. This is done when the buffer is created.
	 *
	 * On the first run, the input frame (supplied by application) is
	 * copied to resample->buffer at 2*xoff position. The first 2*xoff
	 * samples are initially zeroed (in the initialization). The resample
	 * algorithm then invoked at resample->buffer+xoff ONLY, thus giving
	 * it one xoff at the beginning as zero, and one xoff at the end
	 * as the end of the original input. The resample algorithm will see
	 * that the first xoff samples in the input as zero.
	 *
	 * So here's the layout of resample->buffer on the first run.
	 *
	 * run 0 
	 *     +------+------+--------------+
	 *     | 0000 | 0000 |  frame0...   |
	 *     +------+------+--------------+
	 *     ^      ^      ^              ^
         *     0    xoff  2*xoff       size+2*xoff 
         *
	 * (Note again: resample algorithm is called at resample->buffer+xoff)
	 *
	 * At the end of the run, 2*xoff samples from the end of 
	 * resample->buffer are copied to the beginning of resample->buffer.
	 * The first xoff part of this will be used as history for the next
	 * run, and the second xoff part of this is actually the start of
	 * resampling for the next run.
	 *
	 * And the first run completes, the function returns.
	 *
	 * 
	 * On the next run, the input frame supplied by application is again
	 * copied at 2*xoff position in the resample->buffer, and the 
	 * resample algorithm is again invoked at resample->buffer+xoff 
	 * position. So effectively, the resample algorithm will start its
	 * operation on the last xoff from the previous frame, and gets the
	 * history from the last 2*xoff of the previous frame, and the look-
	 * ahead from the last xoff of current frame.
	 *
	 * So on this run, the buffer layout is:
	 *
	 * run 1
	 *     +------+------+--------------+
	 *     | frm0 | frm0 |  frame1...   |
	 *     +------+------+--------------+
	 *     ^      ^      ^              ^
         *     0    xoff  2*xoff       size+2*xoff 
	 *
	 * As you can see from above diagram, the resampling algorithm is
	 * actually called from the last xoff part of previous frame (frm0).
	 *
	 * And so on the process continues for the next frame, and the next,
	 * and the next, ...
	 *
	 */
	dst_buf = resample->buffer + resample->xoff*2;
	pjmedia_copy_samples(dst_buf, input, resample->frame_size);
	    
	if (resample->factor >= 1) {

	    if (resample->large_filter) {
		SrcUp(resample->buffer + resample->xoff, output,
		      resample->factor, resample->frame_size,
		      LARGE_FILTER_NWING, LARGE_FILTER_SCALE,
		      LARGE_FILTER_IMP, LARGE_FILTER_IMPD,
		      PJ_TRUE);
	    } else {
		SrcUp(resample->buffer + resample->xoff, output,
		      resample->factor, resample->frame_size,
		      SMALL_FILTER_NWING, SMALL_FILTER_SCALE,
		      SMALL_FILTER_IMP, SMALL_FILTER_IMPD,
		      PJ_TRUE);
	    }

	} else {

	    if (resample->large_filter) {

		SrcUD( resample->buffer + resample->xoff, output,
		       resample->factor, resample->frame_size,
		       LARGE_FILTER_NWING, 
		       LARGE_FILTER_SCALE * resample->factor + 0.5,
		       LARGE_FILTER_IMP, LARGE_FILTER_IMPD,
		       PJ_TRUE);

	    } else {

		SrcUD( resample->buffer + resample->xoff, output,
		       resample->factor, resample->frame_size,
		       SMALL_FILTER_NWING, 
		       SMALL_FILTER_SCALE * resample->factor + 0.5,
		       SMALL_FILTER_IMP, SMALL_FILTER_IMPD,
		       PJ_TRUE);

	    }

	}

	dst_buf = resample->buffer;
	src_buf = input + resample->frame_size - resample->xoff*2;
	pjmedia_copy_samples(dst_buf, src_buf, resample->xoff * 2);

    } else {
	SrcLinear( input, output, resample->factor, resample->frame_size);
    }
}

PJ_DEF(unsigned) pjmedia_resample_get_input_size(pjmedia_resample *resample)
{
    PJ_ASSERT_RETURN(resample != NULL, 0);
    return resample->frame_size;
}