Multiplier-less fixed-point FM demodulation

fmdemod.c

/*
Multiplier-less fixed-point FM demodulation

Copyright 2020 Ahmet Inan <inan@aicodix.de>
*/

#include <stdio.h>

int int_real(int x)
{
	static int sum0, sum1, sum2, sum3, sum4, sum5;
	return sum5 += (sum4 += (sum3 += (sum2 += (sum1 += (sum0 += x)))));
}

int int_imag(int x)
{
	static int sum0, sum1, sum2, sum3, sum4, sum5;
	return sum5 += (sum4 += (sum3 += (sum2 += (sum1 += (sum0 += x)))));
}

int comb_real(int x)
{
	static int prv5, prv4, prv3, prv2, prv1, prv0;
	int tmp = x;
	tmp -= prv0; prv0 = x; x = tmp;
	tmp -= prv1; prv1 = x; x = tmp;
	tmp -= prv2; prv2 = x; x = tmp;
	tmp -= prv3; prv3 = x; x = tmp;
	tmp -= prv4; prv4 = x; x = tmp;
	tmp -= prv5; prv5 = x; x = tmp;
	return x;
}

int comb_imag(int x)
{
	static int prv5, prv4, prv3, prv2, prv1, prv0;
	int tmp = x;
	tmp -= prv0; prv0 = x; x = tmp;
	tmp -= prv1; prv1 = x; x = tmp;
	tmp -= prv2; prv2 = x; x = tmp;
	tmp -= prv3; prv3 = x; x = tmp;
	tmp -= prv4; prv4 = x; x = tmp;
	tmp -= prv5; prv5 = x; x = tmp;
	return x;
}

void cdc(short *real, short *imag, const short *input)
{
	const int growth = 6;

	int_real(input[0]);
	int_imag(0);

	real[0] = comb_real(int_real(0) >> growth);
	imag[0] = comb_imag(int_imag(-input[1]) >> growth);

	int_real(-input[2]);
	int_imag(0);

	real[1] = comb_real(int_real(0) >> growth);
	imag[1] = comb_imag(int_imag(input[3]) >> growth);
}

short cordic_atan2(short y, short x)
{
	static const int LUT[16] = {
		32768, 19344, 10221, 5188, 2604, 1303, 652, 326, 163, 81, 41, 20, 10, 5, 3, 1,
	};
	int angle = 0;
	int real = x << 1;
	int imag = y << 1;
	if (x < 0)
		real = -real;
	for (int i = 0; i < 16; ++i) {
		int re = real;
		int im = imag;
		if (imag < 0) {
			angle -= LUT[i];
			re -= imag >> i;
			im += real >> i;
		} else {
			angle += LUT[i];
			re += imag >> i;
			im -= real >> i;
		}
		real = re; imag = im;
	}
	if (x < 0)
		angle = 131072 - angle;
	return angle >> 2;
}

short fmd(short real, short imag)
{
	static short prev;
	short phase = cordic_atan2(imag, real);
	short diff = phase - prev;
	prev = phase;
	return diff;
}

short delay(short x)
{
	const short size = 1 << 5;
	static short buf[size];
	static short pos;
	short tmp = buf[pos];
	buf[pos] = x;
	if (++pos >= size)
		pos = 0;
	return tmp;
}

short sma(short x)
{
	static int sum;
	return (sum += x - delay(x)) >> 5;
}

int main()
{
	short input[4];
	while (fread(input, 2, 4, stdin) == 4) {
		short real[2], imag[2];
		cdc(real, imag, input);
		short output[2];
		for (int i = 0; i < 2; ++i)
			output[i] = sma(fmd(real[i], imag[i]));
		if (fwrite(&output, 2, 2, stdout) != 2)
			return 1;
	}
	return 0;
}