resample.c

刻录光盘的程序

/* @(#)resample.c	1.2 99/12/19 Copyright 1998,1999 Heiko Eissfeldt */
#ifndef lint
static char     sccsid[] =
"@(#)resample.c	1.2 99/12/19 Copyright 1998,1999 Heiko Eissfeldt";

#endif
/* resampling module 
 *
 * The audio data has been read. Here are the
 * functions to ensure a correct continuation
 * of the output stream and to convert to a
 * lower sample rate.
 *
 */

#undef DEBUG_VOTE_ENDIANESS
#undef DEBUG_SHIFTS		/* simulate bad cdrom drives */
#undef DEBUG_MATCHING
#undef SHOW_JITTER
#undef CHECK_MEM

#include "config.h"
#include <timedefs.h>
#include <stdio.h>
#include <stdlib.h>
#if defined (HAVE_UNISTD_H) && (HAVE_UNISTD_H == 1)
#include <sys/types.h>
#include <unistd.h>
#endif
#include <standard.h>
#include <strdefs.h>
#include <limits.h>
#include <assert.h>
#include <math.h>

#include <scg/scsitransp.h>

#include "mytype.h"
#include "cdda2wav.h"
#include "interface.h"
#include "byteorder.h"
#include "ringbuff.h"
#include "resample.h"
#include "sndconfig.h"
#include "global.h"


int waitforsignal = 0;	/* flag: wait for any audio response */
int any_signal = 0;

short undersampling;	/* conversion factor */
short samples_to_do;	/* loop variable for conversion */
int Halved;		/* interpolate due to non integral divider */

static long lsum = 0, rsum = 0;	       /* accumulator for left/right channel */
static long ls2 = 0, rs2 = 0, ls3 = 0, rs3 = 0, auxl = 0, auxr = 0;

static const unsigned char *my_symmemmem	__PR((const unsigned char *HAYSTACK, const size_t HAYSTACK_LEN, const unsigned char *const NEEDLE, const size_t NEEDLE_LEN));
static const unsigned char *my_memmem	__PR((const unsigned char *HAYSTACK, const size_t HAYSTACK_LEN, const unsigned char *const NEEDLE, const size_t NEEDLE_LEN));
static const unsigned char *my_memrmem	__PR((const unsigned char *HAYSTACK, const size_t HAYSTACK_LEN, const unsigned char *const NEEDLE, const size_t NEEDLE_LEN));
static const unsigned char *sync_buffers	__PR((const unsigned char *const newbuf));
static long interpolate				__PR((long p1, long p2, long p3));
static void emit_sample				__PR((long lsumval, long rsumval, long channels));
static void change_endianness			__PR((UINT4 *pSam, unsigned int Samples));
static void swap_channels			__PR((UINT4 *pSam, unsigned int Samples));
static int guess_endianess			__PR((UINT4 *p, short *p2, unsigned int SamplesToDo));


#ifdef CHECK_MEM
static void
check_mem __PR((const unsigned char *p, unsigned long amount, const unsigned char *q, unsigned line, char *file));

static void check_mem(p, amount, q, line, file)
	const unsigned char *p;
	unsigned long amount;
	const unsigned char *q;
	unsigned line;
	char *file;
{
	if (p < q || p+amount > q + ENTRY_SIZE) {
		fprintf(stderr, "file %s, line %u: invalid buffer range (%p - %p), allowed is (%p - %p)\n",
			file,line,p, p+amount-1, q, q + ENTRY_SIZE-1);
		exit(1);
	}
}
#endif


#ifdef DEBUG_MATCHING
int memcmp(const void * a, const void * b, size_t c)
{
  return 1;
}
#endif

static const unsigned char *
my_symmemmem (HAYSTACK, HAYSTACK_LEN, NEEDLE, NEEDLE_LEN)
	const unsigned char * HAYSTACK;
	const size_t HAYSTACK_LEN;
	const unsigned char * const NEEDLE;
	const size_t NEEDLE_LEN;
{
  const unsigned char * const UPPER_LIMIT = HAYSTACK + HAYSTACK_LEN - NEEDLE_LEN - 1;
  const unsigned char * HAYSTACK2 = HAYSTACK-1;

  while (HAYSTACK <= UPPER_LIMIT) {
    if (memcmp(NEEDLE, HAYSTACK, NEEDLE_LEN) == 0) {
      return HAYSTACK;
    } else {
      if (memcmp(NEEDLE, HAYSTACK2, NEEDLE_LEN) == 0) {
        return HAYSTACK2;
      }
      HAYSTACK2--;
      HAYSTACK++;
    }
  }
#ifdef DEBUG_MATCHING
  HAYSTACK2++;
  HAYSTACK--;
  fprintf(stderr, "scompared %p-%p with %p-%p (%p)\n",
 	 NEEDLE, NEEDLE + NEEDLE_LEN-1,
	 HAYSTACK2, HAYSTACK + NEEDLE_LEN-1, HAYSTACK);
#endif
  return NULL;
}

static const unsigned char *
my_memmem (HAYSTACK, HAYSTACK_LEN, NEEDLE, NEEDLE_LEN)
	const unsigned char * HAYSTACK;
	const size_t HAYSTACK_LEN;
	const unsigned char * const NEEDLE;
	const size_t NEEDLE_LEN;
{
  const unsigned char * const UPPER_LIMIT = HAYSTACK + HAYSTACK_LEN - NEEDLE_LEN;

  while (HAYSTACK <= UPPER_LIMIT) {
    if (memcmp(NEEDLE, HAYSTACK, NEEDLE_LEN) == 0) {
      return HAYSTACK;
    } else {
      HAYSTACK++;
    }
  }
#ifdef DEBUG_MATCHING
  HAYSTACK--;
  fprintf(stderr, "fcompared %p-%p with %p-%p (%p)\n",
 	 NEEDLE, NEEDLE + NEEDLE_LEN-1,
	 HAYSTACK - HAYSTACK_LEN + NEEDLE_LEN, HAYSTACK + NEEDLE_LEN-1,
	 HAYSTACK);
#endif
  return NULL;
}

static const unsigned char *
my_memrmem (HAYSTACK, HAYSTACK_LEN, NEEDLE, NEEDLE_LEN)
	const unsigned char * HAYSTACK;
	const size_t HAYSTACK_LEN;
	const unsigned char * const NEEDLE;
	const size_t NEEDLE_LEN;
{
  const unsigned char * const LOWER_LIMIT = HAYSTACK - (HAYSTACK_LEN - 1);

  while (HAYSTACK >= LOWER_LIMIT) {
    if (memcmp(NEEDLE, HAYSTACK, NEEDLE_LEN) == 0) {
      return HAYSTACK;
    } else {
      HAYSTACK--;
    }
  }
#ifdef DEBUG_MATCHING
  HAYSTACK++;
  fprintf(stderr, "bcompared %p-%p with %p-%p (%p)\n",
 	 NEEDLE, NEEDLE + NEEDLE_LEN-1,
	 HAYSTACK, HAYSTACK + (HAYSTACK_LEN - 1),
	 HAYSTACK + (HAYSTACK_LEN - 1) - NEEDLE_LEN - 1);
#endif
  return NULL;
}

/* find continuation in new buffer */
static const unsigned char *
sync_buffers(newbuf)
	const unsigned char * const newbuf;
{
    const unsigned char *retval = newbuf;

    if (global.overlap != 0) {
      /* find position of SYNC_SIZE bytes 
	 of the old buffer in the new buffer */
      size_t haystack_len;
      const size_t needle_len = SYNC_SIZE;
      const unsigned char * const oldbuf = (const unsigned char *) (get_previous_read_buffer()->data);
      const unsigned char * haystack;
      const unsigned char * needle;

      /* compare the previous buffer with the new one
       *
       * 1. symmetrical search:
       *   look for the last SYNC_SIZE bytes of the previous buffer
       *   in the new buffer (from the optimum to the outer positions).
       *
       * 2. if the first approach did not find anything do forward search
       *   look for the last SYNC_SIZE bytes of the previous buffer
       *   in the new buffer (from behind the overlap to the end).
       *   
       */

      haystack_len = min((global.nsectors - global.overlap)*CD_FRAMESIZE_RAW
			 +SYNC_SIZE+1,
     			 global.overlap*CD_FRAMESIZE_RAW);
      /* expected here */
      haystack = newbuf + CD_FRAMESIZE_RAW*global.overlap - SYNC_SIZE;
      needle = oldbuf + CD_FRAMESIZE_RAW*global.nsectors - SYNC_SIZE; 

#ifdef DEBUG_MATCHING
	fprintf(stderr, "oldbuf    %p-%p  new %p-%p %u %u %u\n",
		oldbuf, oldbuf + CD_FRAMESIZE_RAW*global.nsectors - 1,
		newbuf, newbuf + CD_FRAMESIZE_RAW*global.nsectors - 1,
		CD_FRAMESIZE_RAW*global.nsectors, global.nsectors, global.overlap);
#endif

      retval = my_symmemmem(haystack, haystack_len, needle, needle_len);
      if (retval != NULL) {
	retval += SYNC_SIZE;
      } else {
	/* fallback to asymmetrical search */

	/* if there is no asymmetrical part left, return with 'not found' */
	if (2*global.overlap == global.nsectors) {
	  retval = NULL;
	} else if (2*global.overlap > global.nsectors) {
	  /* the asymmetrical part is in front, search backwards */
          haystack_len = (2*global.overlap-global.nsectors)*CD_FRAMESIZE_RAW;
          haystack = newbuf + haystack_len - 1;
          retval = my_memrmem(haystack, haystack_len, needle, needle_len);
	} else {
	  /* the asymmetrical part is at the end, search forward */
          haystack = newbuf + 2*(global.overlap*CD_FRAMESIZE_RAW - SYNC_SIZE);
          haystack_len = (global.nsectors-2*global.overlap)*CD_FRAMESIZE_RAW + 2*SYNC_SIZE;
          retval = my_memmem(haystack, haystack_len, needle, needle_len);
	}
        if (retval != NULL)
	  retval += SYNC_SIZE;
      }

#ifdef SHOW_JITTER
      if (retval) {
	fprintf(stderr,"%d\n",
		retval-(newbuf+global.overlap*CD_FRAMESIZE_RAW));
      } else {
	fprintf(stderr,"no match\n");
      }
#endif
    }

    return retval;
}

/* quadratic interpolation
 * p1, p3 span the interval 0 - 2. give interpolated value for 1/2 */
static long int 
interpolate( p1, p2, p3)
	long int p1;
	long int p2;
	long int p3;
{
  return (3L*p1 + 6L*p2 - p3)/8L;
}

static unsigned char *pStart;	/* running ptr defining end of output buffer */
static unsigned char *pDst;	/* start of output buffer */
/*
 * Write the filtered sample into the output buffer.
 */
static void 
emit_sample( lsumval, rsumval, channels )
	long lsumval;
	long rsumval;
	long channels;
{
    if (global.findminmax) {
       if (rsumval > global.maxamp[0]) global.maxamp[0] = rsumval;
       if (rsumval < global.minamp[0]) global.minamp[0] = rsumval;
       if (lsumval < global.minamp[1]) global.minamp[1] = lsumval;
       if (lsumval > global.maxamp[1]) global.maxamp[1] = lsumval;
    }
    /* convert to output format */
    if ( channels == 1 ) {
	short sum;       /* mono section */
	sum = ( lsumval + rsumval ) >> (global.sh_bits + 1);
	if ( global.sh_bits == 8 ) {
	    if ( ( (char) sum) != '\0' ) {
		if ( any_signal == 0 ) {
		    pStart = (unsigned char *) pDst;
		    any_signal = 1;
		}
	    }
	    *pDst++ = ( unsigned char ) sum + ( 1 << 7 );
	} else {
	    short * myptr = (short *) pDst;
	    if ( sum != 0 ) {
		if ( any_signal == 0 ) {
		    pStart = (unsigned char *) pDst;
		    any_signal = 1;
		}
	    }
	    *myptr = sum;
	    pDst += sizeof( short );
	}
    } else {
	/* stereo section */
	lsumval >>= global.sh_bits;
	rsumval >>= global.sh_bits;
	if ( global.sh_bits == 8 ) {
	    if ( (( char ) lsumval != '\0') || (( char ) rsumval != '\0')) {
		if ( any_signal == 0 ) {
		    pStart = (unsigned char *) pDst;
		    any_signal = 1;
		}
	    }
	    *pDst++ = ( unsigned char )( short ) lsumval + ( 1 << 7 );
	    *pDst++ = ( unsigned char )( short ) rsumval + ( 1 << 7 );
	} else {
	    short * myptr = (short *) pDst;
	    if ( (( short ) lsumval != 0) || (( short ) rsumval != 0)) {
		if ( any_signal == 0 ) {
		    pStart = (unsigned char *) pDst;
		    any_signal = 1;
		}
	    }
	    *myptr++ = ( short ) lsumval;
	    *myptr   = ( short ) rsumval;
	    pDst += 2*sizeof( short );
	}
    }
}

static void change_endianness(pSam, Samples)
	UINT4 *pSam;
	unsigned int Samples;
{
  UINT4 *pend = (pSam + Samples);

  /* type UINT4 may not be greater than the assumed biggest type */
#if (SIZEOF_LONG_INT < 4)
error type unsigned long is too small
#endif

#if (SIZEOF_LONG_INT == 4)

  unsigned long *plong = (unsigned long *)pSam;

  for (; plong < pend;) {
    *plong = ((*plong >> 8L) & UINT_C(0x00ff00ff)) |
             ((*plong << 8L) & UINT_C(0xff00ff00));
    plong++;
  }
#else  /* sizeof long unsigned > 4 bytes */
#if (SIZEOF_LONG_INT == 8)
#define INTEGRAL_LONGS (SIZEOF_LONG_INT-1UL)
  register unsigned long *plong;
  unsigned long *pend0 = (unsigned long *) (((unsigned long) pend) & ~ INTEGRAL_LONGS);

  if (((unsigned long) pSam) & INTEGRAL_LONGS) {
    *pSam = ((*pSam >> 8L) & UINT_C(0x00ff00ff)) |
            ((*pSam << 8L) & UINT_C(0xff00ff00));
    pSam++;
  }

  plong = (unsigned long *)pSam;

  for (; plong < pend0;) {
    *plong = ((*plong >> 8L) & ULONG_C(0x00ff00ff00ff00ff)) |
             ((*plong << 8L) & ULONG_C(0xff00ff00ff00ff00));
    plong++;
  }

  if (((unsigned long *) pend) != pend0) {
    UINT4 *pint = (UINT4 *) pend0;

    for (;pint < pend;) {
      *pint = ((*pint >> 8) & UINT_C(0x00ff00ff)) |
              ((*pint << 8) & UINT_C(0xff00ff00));
      pint++;
    }
  }
#else  /* sizeof long unsigned > 4 bytes but not 8 */
  {
    UINT4 *pint = pSam;

    for (;pint < pend;) {
      *pint = ((*pint >> 8) & UINT_C(0x00ff00ff)) |
              ((*pint << 8) & UINT_C(0xff00ff00));
      pint++;
    }
  }
#endif
#endif
}

static void swap_channels(pSam, Samples)
	UINT4 *pSam;
	unsigned int Samples;
{
  UINT4 *pend = (pSam + Samples);

  /* type UINT4 may not be greater than the assumed biggest type */
#if (SIZEOF_LONG_INT < 4)
error type unsigned long is too small
#endif

#if (SIZEOF_LONG_INT == 4)

  unsigned long *plong = (unsigned long *)pSam;

  for (; plong < pend;) {
    *plong = ((*plong >> 16L) & UINT_C(0x0000ffff)) |
             ((*plong << 16L) & UINT_C(0xffff0000));
    plong++;
  }
#else  /* sizeof long unsigned > 4 bytes */
#if (SIZEOF_LONG_INT == 8)
#define INTEGRAL_LONGS (SIZEOF_LONG_INT-1UL)
  register unsigned long *plong;
  unsigned long *pend0 = (unsigned long *) (((unsigned long) pend) & ~ INTEGRAL_LONGS);

  if (((unsigned long) pSam) & INTEGRAL_LONGS) {
    *pSam = ((*pSam >> 16L) & UINT_C(0x0000ffff)) |
            ((*pSam << 16L) & UINT_C(0xffff0000));
    pSam++;
  }

  plong = (unsigned long *)pSam;

  for (; plong < pend0;) {
    *plong = ((*plong >> 16L) & ULONG_C(0x0000ffff0000ffff)) |
             ((*plong << 16L) & ULONG_C(0xffff0000ffff0000));
    plong++;
  }

  if (((unsigned long *) pend) != pend0) {
    UINT4 *pint = (UINT4 *) pend0;

    for (;pint < pend;) {
      *pint = ((*pint >> 16L) & UINT_C(0x0000ffff)) |
              ((*pint << 16L) & UINT_C(0xffff0000));
      pint++;
    }
  }
#else  /* sizeof long unsigned > 4 bytes but not 8 */
  {
    UINT4 *pint = pSam;

    for (;pint < pend;) {
      *pint = ((*pint >> 16L) & UINT_C(0x0000ffff)) |
              ((*pint << 16L) & UINT_C(0xffff0000));
      pint++;
    }
  }
#endif
#endif
}

#ifdef	ECHO_TO_SOUNDCARD
static long ReSampleBuffer			__PR((unsigned char *p, unsigned char *newp, long samples));
static long ReSampleBuffer( p, newp, samples)
	unsigned char *p;
	unsigned char *newp;
	long samples;
{
	double idx=0;
	long    di=0,si=0;

	if (global.playback_rate == 100.0) {
		memcpy(newp, p, 4* samples);
		di = samples;
	} else while( si < samples ){