Simple stereo resampler in C99, based on blargg's Fir_Resampler.(cpp|h)

Makefile

CFLAGS = -O2

OBJS = test.o resampler.o 

all: test spectrum.png

test : $(OBJS)
	$(CC) -o $@ $^

.c.o:
	$(CC) -c $(CFLAGS) $*.c -o $@

sweep16.raw:
	sox -V -r 16000 -c 1 -b 24 -L -e signed-integer -n $@ synth 8 sine 1:8000 vol -6dB

sweep32.raw:
	sox -V -r 32000 -c 1 -b 24 -L -e signed-integer -n $@ synth 8 sine 1:16000 vol -6dB

sweep48.raw:
	sox -V -r 48000 -c 1 -b 24 -L -e signed-integer -n $@ synth 8 sine 1:24000 vol -6dB

sweep96.raw:
	sox -V -r 96000 -c 1 -b 24 -L -e signed-integer -n $@ synth 8 sine 1:48000 vol -6dB

sweep_out_44.raw: test sweep16.raw sweep32.raw sweep48.raw sweep96.raw
	./test

spectrum.png: sweep_out_44.raw
	sox -b 24 -L -r 44100 -c 1 -e signed-integer $^ -n spectrogram -w Kaiser -z 180 -o $@

clean:
	rm -f $(OBJS) test sweep{16,32,48,96}.raw sweep_out_44.raw spectrum.png > /dev/null

resampler.c

#include "resampler.h"

#include <math.h>
#include <stdlib.h>
#include <string.h>

/* Copyright (C) 2004-2008 Shay Green.
   Copyright (C) 2015 Christopher Snowhill. This module is free software; you
can redistribute it and/or modify it under the terms of the GNU Lesser
General Public License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. This
module 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 Lesser General Public License for more
details. You should have received a copy of the GNU Lesser General Public
License along with this module; if not, write to the Free Software Foundation,
Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA */

#undef PI
#define PI 3.1415926535897932384626433832795029

enum { imp_scale = 0x7FFF };
typedef int16_t imp_t;
typedef int32_t imp_off_t; /* for max_res of 512 and impulse width of 32, end offsets must be 32 bits */

#if RESAMPLER_BITS == 16
typedef int32_t intermediate_t;
#elif RESAMPLER_BITS == 32
typedef int64_t intermediate_t;
#endif

static void gen_sinc( double rolloff, int width, double offset, double spacing, double scale,
		int count, imp_t* out )
{
	double angle;

	double const maxh = 256;
	double const step = PI / maxh * spacing;
	double const to_w = maxh * 2 / width;
	double const pow_a_n = pow( rolloff, maxh );
	scale /= maxh * 2;
	angle = (count / 2 - 1 + offset) * -step;

	while ( count-- )
	{
		double w;
		*out++ = 0;
		w = angle * to_w;
		if ( fabs( w ) < PI )
		{
			double rolloff_cos_a = rolloff * cos( angle );
			double num = 1 - rolloff_cos_a -
					pow_a_n * cos( maxh * angle ) +
					pow_a_n * rolloff * cos( (maxh - 1) * angle );
			double den = 1 - rolloff_cos_a - rolloff_cos_a + rolloff * rolloff;
			double sinc = scale * num / den - scale;

			out [-1] = (imp_t) (cos( w ) * sinc + sinc);
		}
		angle += step;
	}
}

enum { width = 32 };
enum { max_res = 512 };
enum { min_width = (width < 4 ? 4 : width) };
enum { adj_width = min_width / 4 * 4 + 2 };
enum { write_offset = adj_width };

enum { buffer_size = 128 };

typedef struct _resampler
{
	int width_;
	int rate_;
	int inptr;
	int infilled;
	int outptr;
	int outfilled;

	int latency;

	imp_t const* imp;
	imp_t impulses [max_res * (adj_width + 2 * (sizeof(imp_off_t) / sizeof(imp_t)))];
	sample_t buffer_in[buffer_size * 2];
	sample_t buffer_out[buffer_size];
} resampler;

void * resampler_create()
{
	resampler *r = (resampler *) malloc(sizeof(resampler));
	if (r) resampler_clear(r);
	return r;
}

void * resampler_dup(const void *_r)
{
    void *_t = (resampler *) malloc(sizeof(resampler));
    if (_t) resampler_dup_inplace(_t, _r);
    return _t;
}

void resampler_dup_inplace(void *_t, const void *_r)
{
	const resampler *r = (const resampler *)_r;
	resampler *t = (resampler *)_t;
	if (r && t)
	{
		memcpy(t, r, sizeof(resampler));
		t->imp = t->impulses + (r->imp - r->impulses);
	}
	else if (t)
	{
		resampler_clear(t);
	}
}

void resampler_destroy(void *r)
{
	free(r);
}

void resampler_clear(void *_r)
{
	resampler * r = (resampler *)_r;
	r->width_ = adj_width;
	r->inptr = 0;
	r->infilled = 0;
	r->outptr = 0;
	r->outfilled = 0;
	r->latency = 0;
	r->imp = r->impulses;

	resampler_set_rate(r, 1.0);
}

void resampler_set_rate( void *_r, double new_factor )
{
	int step; //const
	double filter; //const
	double fraction, pos;
    int n;

	resampler *rs = (resampler *)_r;
    imp_t* out;

	double const rolloff = 0.999;
	double const gain = 1.0;

	/* determine number of sub-phases that yield lowest error */
	double ratio_ = 0.0;
	int res = -1;
	{
		double least_error = 2;
		double pos = 0;
		int r;
		for ( r = 1; r <= max_res; r++ )
		{
			double nearest, error;
			pos += new_factor;
			nearest = floor( pos + 0.5 );
			error = fabs( pos - nearest );
			if ( error < least_error )
			{
				res = r;
				ratio_ = nearest / res;
				least_error = error;
			}
		}
	}
	rs->rate_ = ratio_;

	/* how much of input is used for each output sample */
	step = (int) floor( ratio_ );
	fraction = fmod( ratio_, 1.0 );

	filter = (ratio_ < 1.0) ? 1.0 : 1.0 / ratio_;
	pos = 0.0;
	/*int input_per_cycle = 0;*/
	out = rs->impulses;
	
	for ( n = res; --n >= 0; )
	{
        int cur_step;
        
		gen_sinc( rolloff, (int) (rs->width_ * filter + 1) & ~1, pos, filter,
				(double)(imp_scale * gain * filter), (int) rs->width_, out );
		out += rs->width_;

		cur_step = step;
		pos += fraction;
		if ( pos >= 0.9999999 )
		{
			pos -= 1.0;
			cur_step += 1;
		}

		((imp_off_t*)out)[0] = (cur_step - rs->width_ + 2) * sizeof (sample_t);
		((imp_off_t*)out)[1] = 2 * sizeof (imp_t) + 2 * sizeof (imp_off_t);
		out += 2 * (sizeof(imp_off_t) / sizeof(imp_t));
		/*input_per_cycle += cur_step;*/
	}
	/* last offset moves back to beginning of impulses*/
	((imp_off_t*)out) [-1] -= (char*) out - (char*) rs->impulses;

	rs->imp = rs->impulses;
}

int resampler_get_free(void *_r)
{
	resampler *r = (resampler *)_r;
	return buffer_size - r->infilled;
}

int resampler_get_min_fill(void *_r)
{
	resampler *r = (resampler *)_r;
	const int min_needed = write_offset + 1;
	const int latency = r->latency ? 0 : adj_width;
	int min_free = min_needed - r->infilled - latency;
	return min_free < 0 ? 0 : min_free;
}

void resampler_write_sample(void *_r, sample_t s)
{
	resampler *r = (resampler *)_r;

	if (!r->latency)
	{
	    int i;
		for ( i = 0; i < adj_width / 2; ++i)
		{
			r->buffer_in[r->inptr] = 0;
			r->buffer_in[buffer_size + r->inptr] = 0;
			r->inptr = (r->inptr + 1) % (buffer_size);
			r->infilled += 1;
		}
		r->latency = 1;
	}

	if (r->infilled < buffer_size)
	{
		r->buffer_in[r->inptr] = s;
		r->buffer_in[buffer_size + r->inptr + 0] = s;
		r->inptr = (r->inptr + 1) % (buffer_size);
		r->infilled += 1;
	}
}

#if defined(_MSC_VER) || defined(__GNUC__)
#define restrict __restrict
#endif

static const sample_t * resampler_inner_loop( resampler *r, sample_t** out_,
		sample_t const* out_end, sample_t const in [], int in_size )
{
	in_size -= write_offset;
	if ( in_size > 0 )
	{
		sample_t* restrict out = *out_;
		sample_t const* const in_end = in + in_size;
		imp_t const* imp = r->imp;

		do
		{
			int n;
			/* accumulate in extended precision*/
			int pt = imp [0];
			intermediate_t s = (intermediate_t)pt * (intermediate_t)(in [0]);
			if ( out >= out_end )
				break;
			for ( n = (adj_width - 2) / 2; n; --n )
			{
				pt = imp [1];
				s += (intermediate_t)pt * (intermediate_t)(in [1]);

				/* pre-increment more efficient on some RISC processors*/
				imp += 2;
				pt = imp [0];
				s += (intermediate_t)pt * (intermediate_t)(in [2]);
				in += 2;
			}
			pt = imp [1];
			s += (intermediate_t)pt * (intermediate_t)(in [1]);

			/* these two "samples" after the end of the impulse give the
			 * proper offsets to the next input sample and next impulse */
			in  = (sample_t const*) ((char const*) in  + ((imp_off_t*)(&imp [2]))[0]); /* some negative value */
			imp = (imp_t const*) ((char const*) imp + ((imp_off_t*)(&imp [2]))[1]); /* small positive or large negative */

			out [0] = (sample_t) (s >> 15);
			out += 1;
		}
		while ( in < in_end );

		r->imp = imp;
		*out_ = out;
	}
	return in;
}

#undef restrict

static int resampler_wrapper( resampler *r, sample_t out [], int* out_size,
		sample_t const in [], int in_size )
{
	sample_t* out_ = out;
	int result = resampler_inner_loop( r, &out_, out + *out_size, in, in_size ) - in;

	*out_size = out_ - out;
	return result;
}

static void resampler_fill( resampler *r )
{
	while (!r->outfilled && r->infilled)
	{
		int inread;

		int writepos = ( r->outptr + r->outfilled ) % (buffer_size);
		int writesize = (buffer_size) - writepos;
		if ( writesize > ( buffer_size - r->outfilled ) )
				writesize = buffer_size - r->outfilled;
		inread = resampler_wrapper(r, &r->buffer_out[writepos], &writesize, &r->buffer_in[buffer_size + r->inptr - r->infilled], r->infilled);
		r->infilled -= inread;
		r->outfilled += writesize;
		if (!inread)
			break;
	}
}

int resampler_get_avail(void *_r)
{
	resampler *r = (resampler *)_r;
	if (r->outfilled < 1 && r->infilled >= r->width_)
	  resampler_fill( r );
	return r->outfilled;
}

static void resampler_read_sample_internal( resampler *r, sample_t *s, int advance )
{
	if (r->outfilled < 1)
	  resampler_fill( r );
	if (r->outfilled < 1)
	{
		*s = 0;
		return;
	}
	*s = r->buffer_out[r->outptr];
	if (advance)
	{
		r->outptr = (r->outptr + 1) % (buffer_size);
		r->outfilled -= 1;
	}
}

void resampler_read_sample( void *_r, sample_t *s )
{
	resampler *r = (resampler *)_r;
	resampler_read_sample_internal(r, s, 1);
}

void resampler_peek_sample( void *_r, sample_t *s )
{
	resampler *r = (resampler *)_r;
	resampler_read_sample_internal(r, s, 0);
}

resampler.h

#ifndef _RESAMPLER_H_
#define _RESAMPLER_H_

/* Copyright (C) 2004-2008 Shay Green.
   Copyright (C) 2015 Christopher Snowhill. This module is free software; you
can redistribute it and/or modify it under the terms of the GNU Lesser
General Public License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. This
module 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 Lesser General Public License for more
details. You should have received a copy of the GNU Lesser General Public
License along with this module; if not, write to the Free Software Foundation,
Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA */

#define RESAMPLER_BITS 32
/* #define RESAMPLER_DECORATE */

#ifdef RESAMPLER_DECORATE
#undef PASTE
#undef EVALUATE
#define PASTE(a,b) a ## b
#define EVALUATE(a,b) PASTE(a,b)
#define resampler_create EVALUATE(RESAMPLER_DECORATE,_resampler_create)
#define resampler_dup EVALUATE(RESAMPLER_DECORATE,_resampler_dup)
#define resampler_dup_inplace EVALUATE(RESAMPLER_DECORATE,_resampler_dup_inplace)
#define resampler_destroy EVALUATE(RESAMPLER_DECORATE,_resampler_destroy)
#define resampler_clear EVALUATE(RESAMPLER_DECORATE,_resampler_clear)
#define resampler_set_rate EVALUATE(RESAMPLER_DECORATE,_resampler_set_rate)
#define resampler_get_free EVALUATE(RESAMPLER_DECORATE,_resampler_get_free)
#define resampler_get_min_fill EVALUATE(RESAMPLER_DECORATE,_resampler_get_min_fill)
#define resampler_write_sample EVALUATE(RESAMPLER_DECORATE,_resampler_write_sample)
#define resampler_get_avail EVALUATE(RESAMPLER_DECORATE,_resampler_get_avail)
#define resampler_read_sample EVALUATE(RESAMPLER_DECORATE,_resampler_read_sample)
#define resampler_peek_sample EVALUATE(RESAMPLER_DECORATE,_resampler_peek_sample)
#endif

#include <stdint.h>

#if RESAMPLER_BITS == 16
typedef int16_t sample_t;
#elif RESAMPLER_BITS == 32
typedef int32_t sample_t;
#else
#error Choose a bit depth!
#endif

#ifdef __cplusplus
extern "C" {
#endif

void * resampler_create();
void * resampler_dup(const void *);
void resampler_dup_inplace(void *, const void *);
void resampler_destroy(void *);

void resampler_clear(void *);

void resampler_set_rate( void *, double new_factor );

int resampler_get_free(void *);
int resampler_get_min_fill(void *);

void resampler_write_sample(void *, sample_t s);

int resampler_get_avail(void *);

void resampler_read_sample( void *, sample_t *s );
void resampler_peek_sample( void *, sample_t *s );

#ifdef __cplusplus
}
#endif

#endif

test.c

#include <stdio.h>
#include <stdint.h>

#include "resampler.h"

static inline int32_t read_24(const uint8_t * p)
{
  struct { int32_t sample:24; } sample;
  sample.sample = p[0] + (p[1] * 256) + (p[2] * 65536);
  return sample.sample;
}

static inline void write_24(FILE * f, int32_t sample)
{
  int8_t buffer[3];
  buffer[0] = (sample & 255);
  buffer[1] = (sample >> 8) & 255;
  buffer[2] = (sample >> 16) & 255;
  fwrite(buffer, 1, 3, f);
}

int main(int argc, char ** argv)
{
  int samples_free;
  size_t read;
  size_t i;
  size_t nf;

  void * resampler = resampler_create();

  FILE * f;
  FILE * g = fopen("sweep_out_44.raw", "wb");

  typedef struct sweep_info
  {
    const char * name;
    int freq;
  } sweep_info;

  const sweep_info files[4] = { { "sweep16.raw", 16000 },
                                { "sweep32.raw", 32000 },
                                { "sweep48.raw", 48000 },
                                { "sweep96.raw", 96000 } };

  for ( nf = 0; nf < 4; nf++ )
  {
    f = fopen( files[nf].name, "rb" );

    resampler_set_rate(resampler, (double)(files[nf].freq) / 44100.0);

    for (;;)
    {
      samples_free = resampler_get_free(resampler) / 2;
      {
        uint8_t samples[samples_free * 3];
        read = fread(samples, 3, samples_free, f);
        for (i = 0; i < read; ++i)
        {
          resampler_write_sample(resampler, read_24(samples + i * 3));
        }
      }

      while (resampler_get_avail(resampler))
      {
        sample_t sample;
        resampler_read_sample(resampler, &sample);
        if (sample > (1 << 23) - 1)
          sample = (1 << 23) - 1;
        else if (sample < -(1 << 23))
          sample = -(1 << 23);
        write_24(g, sample);
      }

      if (feof(f) && !resampler_get_avail(resampler))
        break;
    }
    fclose(f);
  }

  fclose(g);

  resampler_destroy(resampler);

  return 0;
}