shithub: riscv

ref: 37e65b331bdf98f409be1bd2fbb55aec158c7877
dir: /sys/src/libpcm/conv.c/

View raw version
#include <u.h>
#include <libc.h>
#include <pcm.h>

typedef struct Chan Chan;
typedef struct Pcmconv Pcmconv;

struct Chan
{
	ulong	ρ;	/* factor */

	ulong	t;	/* time */

	ulong	tΔ;	/* output step */
	ulong	lΔ;	/* filter step */
	ulong	le;	/* filter end */

	int	u;	/* unity scale */
	int	*h;	/* filter coefficients */
	int	*hΔ;	/* coefficient deltas for interpolation */

	int	wx;	/* extra samples */
	int	ix;	/* buffer index */
	int	nx;	/* buffer size */
	int	*x;	/* the buffer */
};

struct Pcmconv
{
	Pcmdesc	idesc;
	Pcmdesc	odesc;
	void	(*iconv)(int *, uchar *, int, int, int);
	void	(*oconv)(int *, uchar *, int, int, int);
	int	*in;
	int	*out;
	uchar	*buf;
	int	nbuf;
	ulong	prnd;
	int	flags;
	Chan	ch[];
};

enum {
	Nl	= 8,		/* 2^Nl samples per zero crossing in fir */
	Nη	= 8,		/* phase bits for filter interpolation */
	Np	= Nl+Nη,	/* phase bits (fract of fixed point) */
	One	= 1<<Np,
	Fcopy	= 1<<0,
	Nin	= 2048,
};

#define MAXINT	((int)(~0UL>>1))
#define MININT	(MAXINT+1)

#define clip(v)	((v) > MAXINT ? MAXINT : ((v) < MININT ? MININT : (v)))

static int
chaninit(Chan *c, int irate, int orate, int count, uintptr caller)
{
	static int h[] = {
#include "fir.h"
	};
	static int hΔ[nelem(h)], init = 0;
	int n;

	c->ρ = ((uvlong)orate<<Np)/irate;
	if(c->ρ == One)
		goto Done;

	c->tΔ = ((uvlong)irate<<Np)/orate;
	c->lΔ = 1<<(Nl+Nη);
	c->le = nelem(h)<<Nη;
	c->wx = 1 + (c->le / c->lΔ);
	c->u = 13128;	/* unity scale factor for fir */
	if(c->ρ < One){
		c->u *= c->ρ;
		c->u >>= Np;
		c->lΔ *= c->ρ;
		c->lΔ >>= Np;
		c->wx *= c->tΔ;
		c->wx >>= Np;
	}
	if(!init){
		for(n=0; n<nelem(hΔ)-1; n++)
			hΔ[n] = h[n+1] - h[n];
		init = 1;
	}
	c->h = h;
	c->hΔ = hΔ;
	c->ix = c->wx;
	c->t = c->ix<<Np;
	c->nx = c->wx*2 + count;
	c->x = mallocz(sizeof(c->x[0]) * c->nx, 1);
	if(c->x == nil){
		werrstr("memory");
		return -1;
	}
	setmalloctag(c->x, caller);
	count += c->nx; /* account for buffer accumulation */
Done:
	return ((uvlong)count * c->ρ) >> Np;
}

static int
filter(Chan *c)
{
	ulong l, lΔ, le, p, i;
	int *x, *h, *hΔ, a;
	vlong v;

	v = 0;

	h = c->h;
	hΔ = c->hΔ;
	lΔ = c->lΔ;
	le = c->le;

	/* left side */
	x = &c->x[c->t>>Np];
	p = c->t & ((1<<Np)-1);
	l = c->ρ < One ? (c->ρ * p)>>Np : p;
	while(l < le){
		i = l >> Nη;
		a = l & ((1<<Nη)-1);
		l += lΔ;
		a *= hΔ[i];
		a >>= Nη;
		a += h[i];
		v += (vlong)*(--x) * a;
	}

	/* right side */
	x = &c->x[c->t>>Np];
	p = (One - p) & ((1<<Np)-1);
	l = c->ρ < One ? (c->ρ * p)>>Np : p;
	if(p == 0) /* skip h[0] as it was already been summed above if p == 0 */
		l += c->lΔ;
	while(l < le){
		i = l >> Nη;
		a = l & ((1<<Nη)-1);
		l += lΔ;
		a *= hΔ[i];
		a >>= Nη;
		a += h[i];
		v += (vlong)*x++ * a;
	}

	/* scale */
	v >>= 2;
	v *= c->u;
	v >>= 27;

	return clip(v);
}

static int*
resample(Chan *c, int *x, int *y, int count)
{
	ulong e;
	int i, n;

	if(c->ρ == One){
		/* no rate conversion needed */
		if(count > 0)
			memmove(y, x, count*sizeof(int));
		return y+count;
	}

	if(count == 0){
		/* stuff zeros to drain last samples out */
		while(c->ix < 2*c->wx)
			c->x[c->ix++] = 0;
	}

	do {
		/* fill buffer */
		for(i = c->ix, n = c->nx; count > 0 && i < n; count--, i++)
			c->x[i] = *x++;
		c->ix = i;

		/* need at least 2*wx samples in buffer for filter */
		if(i < 2*c->wx)
			break;

		/* process buffer */
		e = (i - c->wx)<<Np;
		while(c->t < e){
			*y++ = filter(c);
			c->t += c->tΔ;
		}

		/* check if we'r at the end of buffer and wrap it */
		e = c->t >> Np;
		if(e >= (c->nx - c->wx)){
			c->t -= e << Np;
			c->t += c->wx << Np;
			n = c->t >> Np;
			n -= c->wx;
			e -= c->wx;
			i -= e;
			if(i > 0){
				memmove(c->x + n, c->x + e, i*sizeof(int));
				n += i;
			}
			c->ix = n;
		}
	} while(count > 0);

	return y;
}

static ulong
dither(int *y, int ibits, int obits, int count, ulong prnd)
{
	if(ibits >= 32 || obits >= ibits)
		return prnd;

	while(count--){
		prnd = (prnd*0x19660dL + 0x3c6ef35fL) & 0xffffffffL;
		*y = clip((vlong)*y + ((int)prnd >> ibits));
		y++;
	}
	return prnd;
}

static void
mixin(int *y, int *x, int count)
{
	while(count--){
		*y = clip((vlong)*y + *x++);
		y++;
	}
}

static void
siconv(int *dst, uchar *src, int bits, int skip, int count)
{
	int i, v, s, b;

	b = (bits+7)/8;
	s = sizeof(int)*8-bits;
	while(count--){
		v = 0;
		i = b;
		switch(b){
		case 4:
			v = src[--i];
		case 3:
			v = (v<<8) | src[--i];
		case 2:
			v = (v<<8) | src[--i];
		case 1:
			v = (v<<8) | src[--i];
		}
		*dst++ = v << s;
		src += skip;
	}
}

static void
Siconv(int *dst, uchar *src, int bits, int skip, int count)
{
	int i, v, s, b;

	b = (bits+7)/8;
	s = sizeof(int)*8-bits;
	while(count--){
		v = 0;
		i = 0;
		switch(b){
		case 4:
			v = src[i++];
		case 3:
			v = (v<<8) | src[i++];
		case 2:
			v = (v<<8) | src[i++];
		case 1:
			v = (v<<8) | src[i];
		}
		*dst++ = v << s;
		src += skip;
	}
}

static void
uiconv(int *dst, uchar *src, int bits, int skip, int count)
{
	int i, s, b;
	uint v;

	b = (bits+7)/8;
	s = sizeof(uint)*8-bits;
	while(count--){
		v = 0;
		i = b;
		switch(b){
		case 4:
			v = src[--i];
		case 3:
			v = (v<<8) | src[--i];
		case 2:
			v = (v<<8) | src[--i];
		case 1:
			v = (v<<8) | src[--i];
		}
		*dst++ = (v << s) - (~0UL>>1);
		src += skip;
	}
}

static void
Uiconv(int *dst, uchar *src, int bits, int skip, int count)
{
	int i, s, b;
	uint v;

	b = (bits+7)/8;
	s = sizeof(uint)*8-bits;
	while(count--){
		v = 0;
		i = 0;
		switch(b){
		case 4:
			v = src[i++];
		case 3:
			v = (v<<8) | src[i++];
		case 2:
			v = (v<<8) | src[i++];
		case 1:
			v = (v<<8) | src[i];
		}
		*dst++ = (v << s) - (~0UL>>1);
		src += skip;
	}
}

static void
ficonv(int *dst, uchar *src, int bits, int skip, int count)
{
	if(bits == 32){
		while(count--){
			float f;

			f = *((float*)src), src += skip;
			if(f > 1.0)
				*dst++ = MAXINT;
			else if(f < -1.0)
				*dst++ = MININT;
			else
				*dst++ = f*((float)MAXINT);
		}
	} else {
		while(count--){
			double d;

			d = *((double*)src), src += skip;
			if(d > 1.0)
				*dst++ = MAXINT;
			else if(d < -1.0)
				*dst++ = MININT;
			else
				*dst++ = d*((double)MAXINT);
		}
	}
}

static void
aiconv(int *dst, uchar *src, int, int skip, int count)
{
	int t, seg;
	uchar a;

	while(count--){
		a = *src, src += skip;
		a ^= 0x55;
		t = (a & 0xf) << 4;
		seg = (a & 0x70) >> 4;
		switch(seg){
		case 0:
			t += 8;
			break;
		case 1:
			t += 0x108;
			break;
		default:
			t += 0x108;
			t <<= seg - 1;
		}
		t = (a & 0x80) ? t : -t;
		*dst++ = t << (sizeof(int)*8 - 16);
	}
}

static void
µiconv(int *dst, uchar *src, int, int skip, int count)
{
	int t;
	uchar u;

	while(count--){
		u = *src, src += skip;
		u = ~u;
		t = ((u & 0xf) << 3) + 0x84;
		t <<= (u & 0x70) >> 4;
		t = u & 0x80 ? 0x84 - t: t - 0x84;
		*dst++ = t << (sizeof(int)*8 - 16);
	}
}

static void
soconv(int *src, uchar *dst, int bits, int skip, int count)
{
	int i, v, s, b;

	b = (bits+7)/8;
	s = sizeof(int)*8-bits;
	while(count--){
		v = *src++ >> s;
		i = 0;
		switch(b){
		case 4:
			dst[i++] = v, v >>= 8;
		case 3:
			dst[i++] = v, v >>= 8;
		case 2:
			dst[i++] = v, v >>= 8;
		case 1:
			dst[i] = v;
		}
		dst += skip;
	}
}

static void
Soconv(int *src, uchar *dst, int bits, int skip, int count)
{
	int i, v, s, b;

	b = (bits+7)/8;
	s = sizeof(int)*8-bits;
	while(count--){
		v = *src++ >> s;
		i = b;
		switch(b){
		case 4:
			dst[--i] = v, v >>= 8;
		case 3:
			dst[--i] = v, v >>= 8;
		case 2:
			dst[--i] = v, v >>= 8;
		case 1:
			dst[--i] = v;
		}
		dst += skip;
	}
}

static void
uoconv(int *src, uchar *dst, int bits, int skip, int count)
{
	int i, s, b;
	uint v;

	b = (bits+7)/8;
	s = sizeof(uint)*8-bits;
	while(count--){
		v = ((~0UL>>1) + *src++) >> s;
		i = 0;
		switch(b){
		case 4:
			dst[i++] = v, v >>= 8;
		case 3:
			dst[i++] = v, v >>= 8;
		case 2:
			dst[i++] = v, v >>= 8;
		case 1:
			dst[i] = v;
		}
		dst += skip;
	}
}

static void
Uoconv(int *src, uchar *dst, int bits, int skip, int count)
{
	int i, s, b;
	uint v;

	b = (bits+7)/8;
	s = sizeof(uint)*8-bits;
	while(count--){
		v = ((~0UL>>1) + *src++) >> s;
		i = b;
		switch(b){
		case 4:
			dst[--i] = v, v >>= 8;
		case 3:
			dst[--i] = v, v >>= 8;
		case 2:
			dst[--i] = v, v >>= 8;
		case 1:
			dst[--i] = v;
		}
		dst += skip;
	}
}

static void
foconv(int *src, uchar *dst, int bits, int skip, int count)
{
	if(bits == 32){
		while(count--){
			*((float*)dst) = *src++ / ((float)MAXINT);
			dst += skip;
		}
	} else {
		while(count--){
			*((double*)dst) = *src++ / ((double)MAXINT);
			dst += skip;
		}
	}
}

Pcmconv	*
allocpcmconv(Pcmdesc *i, Pcmdesc *o)
{
	uintptr caller;
	Pcmconv *c;
	int k, nout;

	if(i->channels < 1){
		werrstr("invalid number of input channels: %d", i->channels);
		return nil;
	}
	if(o->channels < 1){
		werrstr("invalid number of output channels: %d", o->channels);
		return nil;
	}

	caller = getcallerpc(&i);
	c = mallocz(sizeof(*c) + i->channels*sizeof(Chan) + i->framesz, 1);
	if(c == nil){
Nomem:
		werrstr("memory");
		goto Err;
	}
	setmalloctag(c, caller);
	memcpy(&c->idesc, i, sizeof(*i));
	memcpy(&c->odesc, o, sizeof(*o));
	c->buf = (uchar*)&c->ch[i->channels+1];

	switch(i->fmt){
	case L's': c->iconv = siconv; break;
	case L'S': c->iconv = Siconv; break;
	case L'u': c->iconv = uiconv; break;
	case L'U': c->iconv = Uiconv; break;
	case L'f': c->iconv = ficonv; break;
	case L'a': c->iconv = aiconv; break;
	case L'µ': c->iconv = µiconv; break;
	default:
		werrstr("unsupported input format: %C", i->fmt);
		goto Err;
	}

	switch(o->fmt){
	case L's': c->oconv = soconv; break;
	case L'S': c->oconv = Soconv; break;
	case L'u': c->oconv = uoconv; break;
	case L'U': c->oconv = Uoconv; break;
	case L'f': c->oconv = foconv; break;
	default:
		werrstr("unsupported output format: %C", o->fmt);
		goto Err;
	}

	/* if same format, just copy */
	c->flags = (
		i->rate == o->rate
		&& i->bits == o->bits
		&& i->channels == o->channels
		&& i->framesz == o->framesz
		&& i->fmt == o->fmt
	) ? Fcopy : 0;

	c->in = malloc(sizeof(int) * Nin);
	if(c->in == nil)
		goto Nomem;
	setmalloctag(c->in, caller);

	nout = 0;
	for(k=0; k < i->channels; k++){
		nout = chaninit(&c->ch[k], i->rate, o->rate, Nin, caller);
		if(nout < 0)
			goto Err;
	}

	/* out is also used for mixing before resampling, so needs to be max(Nin, nout) */
	c->out = malloc(sizeof(int) * (nout > Nin ? nout : Nin));
	if(c->out == nil)
		goto Nomem;
	setmalloctag(c->out, caller);

	return c;
Err:
	freepcmconv(c);
	return nil;
}

void
freepcmconv(Pcmconv *c)
{
	int k;

	if(c == nil)
		return;
	free(c->in);
	free(c->out);
	for(k=0; k < c->idesc.channels; k++)
		free(c->ch[k].x);
	free(c);
}

static int
conv(Pcmconv *c, uchar *in, uchar *out0, int insz)
{
	uchar *out;
	Pcmdesc *i, *o;
	int k, n, m, nin;

	i = &c->idesc;
	o = &c->odesc;
	out = out0;

	nin = insz / i->framesz;
	for(;;){
		if((n = nin) > Nin)
			n = Nin;
		nin -= n;

		c->iconv(c->in, in, i->bits, i->framesz, n);
		if(i->channels > o->channels){
			for(k=1; k<i->channels; k++){
				c->iconv(c->out, in + k*((i->bits+7)/8), i->bits, i->framesz, n);
				mixin(c->in, c->out, n);
			}
		}
		c->prnd = dither(c->in, i->abits, o->abits, n, c->prnd);
		m = resample(&c->ch[0], c->in, c->out, n) - c->out;
		if(m < 1){
			if(n == 0)
				break;
		} else
			c->oconv(c->out, out, o->bits, o->framesz, m);
		if(i->channels == o->channels){
			for(k=1; k<i->channels; k++){
				c->iconv(c->in, in + k*((i->bits+7)/8), i->bits, i->framesz, n);
				c->prnd = dither(c->in, i->abits, o->abits, n, c->prnd);
				resample(&c->ch[k], c->in, c->out, n);
				if(m > 0)
					c->oconv(c->out, out + k*((o->bits+7)/8), o->bits, o->framesz, m);
			}
		} else if(m > 0){
			for(k=1; k<o->channels; k++)
				c->oconv(c->out, out + k*((o->bits+7)/8), o->bits, o->framesz, m);
		}
		if(m > 0)
			out += m * o->framesz;
		if(n == 0)
			break;
		in += n * i->framesz;
	}

	return out - (uchar*)out0;
}

int
pcmconv(Pcmconv *c, void *in0, void *out0, int insz)
{
	uchar *in, *out;
	Pcmdesc *i;
	int n;

	if(c->flags & Fcopy){
		memmove(out0, in0, insz);
		return insz;
	}
	i = &c->idesc;
	in = in0;
	out = out0;
	if(c->nbuf > 0){
		n = i->framesz - c->nbuf;
		if(n > insz)
			n = insz;
		memcpy(c->buf+c->nbuf, in, n);
		c->nbuf += n;
		in += n;
		insz -= n;
		if(c->nbuf < i->framesz)
			return 0;
		out += conv(c, c->buf, out, i->framesz);
		c->nbuf = 0;
	}

	n = insz % i->framesz;
	insz -= n;
	out += conv(c, in, out, insz);
	if(n > 0){
		memcpy(c->buf, in+insz, n);
		c->nbuf += n;
	}

	return out - (uchar*)out0;
}

int
pcmratio(Pcmconv *c, int insz)
{
	int outsz, fsz;

	fsz = c->idesc.framesz;
	if(insz < fsz)
		goto Bad;
	insz = (insz + fsz)/fsz;
	outsz = ((uvlong)insz * c->odesc.framesz * ((uvlong)c->odesc.rate<<Np)/c->idesc.rate) >> Np;
	if(outsz >= c->odesc.framesz)
		return outsz;
Bad:
	werrstr("invalid buffer size: %d", insz);
	return -1;
}