New common FSK implementation, replaces all individual implementations

21 files changed, 785 insertions, 1117 deletions

diff --git a/src/bnetz/bnetz.c b/src/bnetz/bnetz.c
index b014301..539e3c6 100644
--- a/src/bnetz/bnetz.c
+++ b/src/bnetz/bnetz.c
@@ -443,8 +443,6 @@ void bnetz_receive_telegramm(bnetz_t *bnetz, uint16_t telegramm, double level, d
 	struct impulstelegramm *it;
 	int digit = 0;
 
-	PDEBUG_CHAN(DFRAME, DEBUG_INFO, "Digit RX Level: %.0f%% Quality=%.0f\n", level * 100.0 + 0.5, quality * 100.0 + 0.5);
-
 	/* drop any telegramm that is too bad */
 	if (quality < 0.2)
 		return;
@@ -452,9 +450,11 @@ void bnetz_receive_telegramm(bnetz_t *bnetz, uint16_t telegramm, double level, d
 	it = bnetz_telegramm2digit(telegramm);
 	if (it) {
 		digit = it->digit;
-		PDEBUG(DBNETZ, (bnetz->sender.loopback) ? DEBUG_NOTICE : DEBUG_INFO, "Received telegramm '%s'.\n", it->description);
-	} else
-		PDEBUG(DBNETZ, DEBUG_DEBUG, "Received unknown telegramm digit '0x%04x'.\n", telegramm);
+		PDEBUG(DBNETZ, (bnetz->sender.loopback) ? DEBUG_NOTICE : DEBUG_INFO, "Received telegramm '%s' (RX Level: %.0f%% Quality=%.0f)\n", it->description, level * 100.0 + 0.5, quality * 100.0 + 0.5);
+	} else {
+		PDEBUG(DBNETZ, DEBUG_DEBUG, "Received unknown telegramm digit '0x%04x' (RX Level: %.0f%% Quality=%.0f) (might be radio noise)\n", telegramm, level * 100.0 + 0.5, quality * 100.0 + 0.5);
+		return;
+	}
 
 	if (bnetz->sender.loopback) {
 		if (digit >= '0' && digit <= '9') {
diff --git a/src/bnetz/bnetz.h b/src/bnetz/bnetz.h
index baca8a0..4d1b298 100644
--- a/src/bnetz/bnetz.h
+++ b/src/bnetz/bnetz.h
@@ -1,4 +1,4 @@
-#include "../common/goertzel.h"
+#include "../common/fsk.h"
 #include "../common/sender.h"
 
 /* fsk modes of transmission */
@@ -75,24 +75,20 @@ typedef struct bnetz {
 
 	/* dsp states */
 	enum dsp_mode		dsp_mode;		/* current mode: audio, durable tone 0 or 1, "Telegramm" */
-	goertzel_t		fsk_goertzel[2];	/* filter for fsk decoding */
-	int			samples_per_bit;	/* how many samples lasts one bit */
-	sample_t		*fsk_filter_spl;	/* array with samples_per_bit */
-	int			fsk_filter_pos;		/* current sample position in filter_spl */
-	int			fsk_filter_step;	/* number of samples for each analyzation */
-	int			fsk_filter_bit;		/* last bit, so we detect a bit change */
-	int			fsk_filter_sample;	/* count until it is time to sample bit */
-	uint16_t		fsk_filter_telegramm;	/* rx shift register for receiveing telegramm */
-	double			fsk_filter_quality[16];	/* quality of each bit in telegramm */
-	double			fsk_filter_level[16];	/* level of each bit in telegramm */
-	int			fsk_filter_qualidx;	/* index of quality array above */
+	fsk_t			fsk;			/* fsk modem instance */
+	uint16_t		rx_telegramm;		/* rx shift register for receiveing telegramm */
+	double			rx_telegramm_quality[16];/* quality of each bit in telegramm */
+	double			rx_telegramm_level[16];	/* level of each bit in telegramm */
+	int			rx_telegramm_qualidx;	/* index of quality array above */
 	int			tone_detected;		/* what tone has been detected */
 	int			tone_count;		/* how long has that tone been detected */
 	double			phaseshift65536[2];	/* how much the phase of sine wave changes per sample */
 	double			phase65536;		/* current phase */
-	int			telegramm;		/* set, if there is a valid telegram */
-	sample_t		*telegramm_spl;		/* 16 * samples_per_bit */
-	int			telegramm_pos;		/* current sample position in telegramm_spl */
+	const char		*tx_telegramm;		/* carries bits of one frame to transmit */
+	int			tx_telegramm_pos;
+	int			samples_per_chunk;	/* samples per loss detection interval */
+	sample_t		*chunk_spl;		/* chunk sample */
+	int			chunk_pos;		/* current received sample of chunk */
 
 	/* loopback test for latency */
 	int			loopback_count;		/* count digits from 0 to 9 */
diff --git a/src/bnetz/dsp.c b/src/bnetz/dsp.c
index e418531..5b6c393 100644
--- a/src/bnetz/dsp.c
+++ b/src/bnetz/dsp.c
@@ -29,12 +29,13 @@
 #include "../common/debug.h"
 #include "../common/timer.h"
 #include "../common/call.h"
+#include "../common/goertzel.h"
 #include "bnetz.h"
 #include "dsp.h"
 
 #define PI		3.1415927
 
-/* Notes on TX_PEAK_TONE level:
+/* Notes on TX_PEAK_FSK level:
  *
  * At 2000 Hz the deviation shall be 4 kHz, so with emphasis the deviation
  * at 1000 Hz would be theoretically 2 kHz. This is factor 0.714 below
@@ -45,52 +46,32 @@
 #define MAX_DEVIATION	4000.0
 #define MAX_MODULATION	3000.0
 #define DBM0_DEVIATION	2800.0	/* deviation of dBm0 at 1 kHz */
-#define TX_PEAK_TONE	(4000.0 / 2000.0 * 1000.0 / DBM0_DEVIATION)
+#define TX_PEAK_FSK	(4000.0 / 2000.0 * 1000.0 / DBM0_DEVIATION)
 #define MAX_DISPLAY	1.4	/* something above dBm0 */
-#define BIT_DURATION	0.010	/* bit length: 10 ms */
-#define FILTER_STEP	0.001	/* step every 1 ms */
+#define BIT_RATE	100.0
+#define BIT_ADJUST	0.5	/* full adjustment on bit change */
+#define F0		2070.0
+#define F1		1950.0
 #define METERING_HZ	2900	/* metering pulse frequency */
-
-#define TONE_DETECT_TH	70	/* 70 milliseconds to detect continuous tone */
+#define TONE_DETECT_TH	7	/* 70 milliseconds to detect continuous tone */
 
 /* carrier loss detection */
-#define LOSS_INTERVAL	1000	/* filter steps (milliseconds) for one second interval */
+#define CHUNK_DURATION	0.010	/* 10 ms */
+#define LOSS_INTERVAL	100	/* filter steps (milliseconds) for one second interval */
 #define LOSS_TIME	12	/* duration of signal loss before release */
 
-/* two signaling tones */
-static double fsk_bits[2] = {
-	2070.0,
-	1950.0,
-};
-
-/* table for fast sine generation */
-static sample_t dsp_sine[65536];
-
 /* global init for FSK */
 void dsp_init(void)
 {
-	int i;
-
-	PDEBUG(DDSP, DEBUG_DEBUG, "Generating sine table.\n");
-	for (i = 0; i < 65536; i++) {
-		dsp_sine[i] = sin((double)i / 65536.0 * 2.0 * PI) * TX_PEAK_TONE;
-	}
 }
 
+static int fsk_send_bit(void *inst);
+static void fsk_receive_bit(void *inst, int bit, double quality, double level);
+
 /* Init transceiver instance. */
 int dsp_init_sender(bnetz_t *bnetz)
 {
 	sample_t *spl;
-	int i;
-
-	if ((bnetz->sender.samplerate % (int)(1.0 / (double)BIT_DURATION))) {
-		PDEBUG(DDSP, DEBUG_ERROR, "Samples rate must be a multiple of %d (bits per second).\n", (int)(1.0 / (double)BIT_DURATION));
-		return -EINVAL;
-	}
-	if ((bnetz->sender.samplerate % (int)(1.0 / (double)FILTER_STEP))) {
-		PDEBUG(DDSP, DEBUG_ERROR, "Samples rate must be a multiple of %d (FSK probes per second).\n", (int)(1.0 / (double)FILTER_STEP));
-		return -EINVAL;
-	}
 
 	PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Init DSP for 'Sender'.\n");
 
@@ -99,32 +80,24 @@ int dsp_init_sender(bnetz_t *bnetz)
 
 	audio_init_loss(&bnetz->sender.loss, LOSS_INTERVAL, bnetz->sender.loss_volume, LOSS_TIME);
 
-	bnetz->samples_per_bit = bnetz->sender.samplerate * BIT_DURATION;
-	PDEBUG(DDSP, DEBUG_DEBUG, "Using %d samples per bit duration.\n", bnetz->samples_per_bit);
-	bnetz->fsk_filter_step = bnetz->sender.samplerate * FILTER_STEP;
-	PDEBUG(DDSP, DEBUG_DEBUG, "Using %d samples per filter step.\n", bnetz->fsk_filter_step);
-	spl = calloc(16, bnetz->samples_per_bit * sizeof(*spl));
-	if (!spl) {
-		PDEBUG(DDSP, DEBUG_ERROR, "No memory!\n");
-		return -ENOMEM;
-	}
-	bnetz->telegramm_spl = spl;
-	spl = calloc(1, bnetz->samples_per_bit * sizeof(*spl));
-	if (!spl) {
-		PDEBUG(DDSP, DEBUG_ERROR, "No memory!\n");
-		return -ENOMEM;
+	PDEBUG(DDSP, DEBUG_DEBUG, "Using FSK level of %.3f (%.3f KHz deviation @ 2000 Hz)\n", TX_PEAK_FSK, 4.0);
+
+	/* init fsk */
+	if (fsk_init(&bnetz->fsk, bnetz, fsk_send_bit, fsk_receive_bit, bnetz->sender.samplerate, BIT_RATE, F0, F1, TX_PEAK_FSK, 0, BIT_ADJUST) < 0) {
+		PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "FSK init failed!\n");
+		return -EINVAL;
 	}
-	bnetz->fsk_filter_spl = spl;
-	bnetz->fsk_filter_bit = -1;
 
 	bnetz->tone_detected = -1;
 
-	/* count symbols */
-	for (i = 0; i < 2; i++) {
-		audio_goertzel_init(&bnetz->fsk_goertzel[i], fsk_bits[i], bnetz->sender.samplerate);
-		bnetz->phaseshift65536[i] = 65536.0 / ((double)bnetz->sender.samplerate / fsk_bits[i]);
-		PDEBUG(DDSP, DEBUG_DEBUG, "phaseshift[%d] = %.4f (must be arround 64 at 8000hz)\n", i, bnetz->phaseshift65536[i]);
+	bnetz->samples_per_chunk = (double)bnetz->sender.samplerate * CHUNK_DURATION;
+	PDEBUG(DDSP, DEBUG_DEBUG, "Using %d samples per chunk duration.\n", bnetz->samples_per_chunk);
+	spl = calloc(bnetz->samples_per_chunk, sizeof(sample_t));
+	if (!spl) {
+		PDEBUG(DDSP, DEBUG_ERROR, "No memory!\n");
+		return -ENOMEM;
 	}
+	bnetz->chunk_spl = spl;
 
 	return 0;
 }
@@ -134,13 +107,11 @@ void dsp_cleanup_sender(bnetz_t *bnetz)
 {
 	PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Cleanup DSP for 'Sender'.\n");
 
-	if (bnetz->telegramm_spl) {
-		free(bnetz->telegramm_spl);
-		bnetz->telegramm_spl = NULL;
-	}
-	if (bnetz->fsk_filter_spl) {
-		free(bnetz->fsk_filter_spl);
-		bnetz->fsk_filter_spl = NULL;
+	fsk_cleanup(&bnetz->fsk);
+
+	if (bnetz->chunk_spl) {
+		free(bnetz->chunk_spl);
+		bnetz->chunk_spl = NULL;
 	}
 }
 
@@ -150,7 +121,7 @@ static void fsk_receive_tone(bnetz_t *bnetz, int bit, int goodtone, double level
 	/* lost tone because it is not good anymore or has changed */
 	if (!goodtone || bit != bnetz->tone_detected) {
 		if (bnetz->tone_count >= TONE_DETECT_TH) {
-			PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Lost %.0f Hz tone after %d ms.\n", fsk_bits[bnetz->tone_detected], bnetz->tone_count);
+			PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Lost F%d tone after %d ms.\n", bnetz->tone_detected, bnetz->tone_count);
 			bnetz_receive_tone(bnetz, -1);
 		}
 		if (goodtone)
@@ -167,106 +138,51 @@ static void fsk_receive_tone(bnetz_t *bnetz, int bit, int goodtone, double level
 	if (bnetz->tone_count >= TONE_DETECT_TH)
 		audio_reset_loss(&bnetz->sender.loss);
 	if (bnetz->tone_count == TONE_DETECT_TH) {
-		PDEBUG_CHAN(DDSP, DEBUG_INFO, "Detecting continuous tone: %.0f:Level=%3.0f%% Quality=%3.0f%%\n", fsk_bits[bnetz->tone_detected], level * 100.0, quality * 100.0);
+		PDEBUG_CHAN(DDSP, DEBUG_INFO, "Detecting continuous tone: F%d Level=%3.0f%% Quality=%3.0f%%\n", bnetz->tone_detected, level * 100.0, quality * 100.0);
+		/* must reset, so we will not get corrupt first digit */
+		bnetz->rx_telegramm = bnetz->tone_detected * 0xffff;
 		bnetz_receive_tone(bnetz, bnetz->tone_detected);
 	}
 }
 
-/* Collect 16 data bits (digit) and check for sync marc '01110'. */
-static void fsk_receive_bit(bnetz_t *bnetz, int bit, double level, double quality)
+/* Collect 16 data bits (digit) and check for sync mark '01110'. */
+static void fsk_receive_bit(void *inst, int bit, double quality, double level)
 {
+	bnetz_t *bnetz = (bnetz_t *)inst;
 	int i;
 
-	bnetz->fsk_filter_telegramm = (bnetz->fsk_filter_telegramm << 1) | bit;
-	bnetz->fsk_filter_quality[bnetz->fsk_filter_qualidx] = quality;
-	bnetz->fsk_filter_level[bnetz->fsk_filter_qualidx] = level;
-	if (++bnetz->fsk_filter_qualidx == 16)
-		bnetz->fsk_filter_qualidx = 0;
+	/* normalize FSK level */
+	level /= TX_PEAK_FSK;
+
+	/* continuous tone detection */
+	if (level > 0.10 && quality > 0.5) {
+		fsk_receive_tone(bnetz, bit, 1, level, quality);
+	} else
+		fsk_receive_tone(bnetz, bit, 0, level, quality);
+
+	/* collect bits */
+	if (level < 0.05)
+		return;
+	bnetz->rx_telegramm = (bnetz->rx_telegramm << 1) | bit;
+	bnetz->rx_telegramm_quality[bnetz->rx_telegramm_qualidx] = quality;
+	bnetz->rx_telegramm_level[bnetz->rx_telegramm_qualidx] = level;
+	if (++bnetz->rx_telegramm_qualidx == 16)
+		bnetz->rx_telegramm_qualidx = 0;
 
 	/* check if pattern 01110xxxxxxxxxxx matches */
-	if ((bnetz->fsk_filter_telegramm & 0xf800) != 0x7000)
+	if ((bnetz->rx_telegramm & 0xf800) != 0x7000)
 		return;
 
-	/* get worst bit and average level */
-	level = 0;
+	/* average level and quality */
+	level = quality = 0;
 	for (i = 0; i < 16; i++) {
-		if (bnetz->fsk_filter_quality[i] < quality)
-			quality = bnetz->fsk_filter_quality[i];
-		level = bnetz->fsk_filter_level[i];
+		level += bnetz->rx_telegramm_level[i];
+		quality += bnetz->rx_telegramm_quality[i];
 	}
+	level /= 16.0; quality /= 16.0;
 
 	/* send telegramm */
-	bnetz_receive_telegramm(bnetz, bnetz->fsk_filter_telegramm, level, quality);
-}
-
-//#define DEBUG_FILTER
-//#define DEBUG_QUALITY
-
-/* Filter one chunk of audio an detect tone, quality and loss of signal.
- * The chunk is a window of 10ms. This window slides over audio stream
- * and is processed every 1ms. (one step) */
-static inline void fsk_decode_step(bnetz_t *bnetz, int pos)
-{
-	double level, result[2], softbit, quality;
-	int max;
-	sample_t *spl;
-	int bit;
-
-	max = bnetz->samples_per_bit;
-	spl = bnetz->fsk_filter_spl;
-
-	level = audio_level(spl, max);
-
-	if (audio_detect_loss(&bnetz->sender.loss, level))
-		bnetz_loss_indication(bnetz);
-
-	audio_goertzel(bnetz->fsk_goertzel, spl, max, pos, result, 2);
-
-	/* calculate soft bit from both frequencies */
-	softbit = (result[1] / level - result[0] / level + 1.0) / 2.0;
-	/* scale it, since both filters overlap by some percent */
-#define MIN_QUALITY 0.08
-	softbit = (softbit - MIN_QUALITY) / (0.850 - MIN_QUALITY - MIN_QUALITY);
-	if (softbit > 1)
-		softbit = 1;
-	if (softbit < 0)
-		softbit = 0;
-#ifdef DEBUG_FILTER
-	printf("|%s", debug_amplitude(result[0]/level));
-	printf("|%s| low=%.3f high=%.3f level=%d\n", debug_amplitude(result[1]/level), result[0]/level, result[1]/level, (int)level);
-#endif
-	if (softbit > 0.5)
-		bit = 1;
-	else
-		bit = 0;
-
-//	quality = result[bit] / level;
-	if (softbit > 0.5)
-		quality = softbit * 2.0 - 1.0;
-	else
-		quality = 1.0 - softbit * 2.0;
-
-	// FIXME: better threshold
-	/* adjust level, so we get peak of sine curve */
-	if (level / 0.63 > 0.05 && (softbit > 0.75 || softbit < 0.25)) {
-		fsk_receive_tone(bnetz, bit, 1, level / 0.63662 / TX_PEAK_TONE, quality);
-	} else
-		fsk_receive_tone(bnetz, bit, 0, level / 0.63662 / TX_PEAK_TONE, quality);
-
-	if (bnetz->fsk_filter_bit != bit) {
-		/* if we have a bit change, reset sample counter to one half bit duration */
-		bnetz->fsk_filter_bit = bit;
-		bnetz->fsk_filter_sample = 5;
-	} else if (--bnetz->fsk_filter_sample == 0) {
-		/* if sample counter bit reaches 0, we reset sample counter to one bit duration */
-#ifdef DEBUG_QUALITY
-		printf("|%s| quality=%.2f ", debug_amplitude(softbit), quality);
-		printf("|%s|\n", debug_amplitude(quality);
-#endif
-		/* adjust level, so we get peak of sine curve */
-		fsk_receive_bit(bnetz, bit, level / 0.63662 / TX_PEAK_TONE, quality);
-		bnetz->fsk_filter_sample = 10;
-	}
+	bnetz_receive_telegramm(bnetz, bnetz->rx_telegramm, level, quality);
 }
 
 /* Process received audio stream from radio unit. */
@@ -274,24 +190,27 @@ void sender_receive(sender_t *sender, sample_t *samples, int length)
 {
 	bnetz_t *bnetz = (bnetz_t *) sender;
 	sample_t *spl;
-	int max, pos, step;
+	int max, pos;
+	double level;
 	int i;
 
 	/* write received samples to decode buffer */
-	max = bnetz->samples_per_bit;
-	pos = bnetz->fsk_filter_pos;
-	step = bnetz->fsk_filter_step;
-	spl = bnetz->fsk_filter_spl;
+	max = bnetz->samples_per_chunk;
+	pos = bnetz->chunk_pos;
+	spl = bnetz->chunk_spl;
 	for (i = 0; i < length; i++) {
 		spl[pos++] = samples[i];
-		if (pos == max)
+		if (pos == max) {
 			pos = 0;
-		/* if filter step has been reched */
-		if (!(pos % step)) {
-			fsk_decode_step(bnetz, pos);
+			level = audio_level(spl, max);
+			if (audio_detect_loss(&bnetz->sender.loss, level))
+				bnetz_loss_indication(bnetz);
 		}
 	}
-	bnetz->fsk_filter_pos = pos;
+	bnetz->chunk_pos = pos;
+
+	/* fsk/tone signal */
+	fsk_receive(&bnetz->fsk, samples, length);
 
 	if (bnetz->dsp_mode == DSP_MODE_AUDIO && bnetz->callref) {
 		int count;
@@ -311,84 +230,38 @@ void sender_receive(sender_t *sender, sample_t *samples, int length)
 		bnetz->sender.rxbuf_pos = 0;
 }
 
-static void fsk_tone(bnetz_t *bnetz, sample_t *samples, int length, int tone)
+static int fsk_send_bit(void *inst)
 {
-	double phaseshift, phase;
-	int i;
-
-	phase = bnetz->phase65536;
-	phaseshift = bnetz->phaseshift65536[tone];
-
-	for (i = 0; i < length; i++) {
-		*samples++ = dsp_sine[(uint16_t)phase];
-		phase += phaseshift;
-		if (phase >= 65536)
-			phase -= 65536;
-	}
-
-	bnetz->phase65536 = phase;
-}
+	bnetz_t *bnetz = (bnetz_t *)inst;
 
-static int fsk_telegramm(bnetz_t *bnetz, sample_t *samples, int length)
-{
-	sample_t *spl;
-	const char *telegramm;
-	int i, j;
-	double phaseshift, phase;
-	int count, max;
-
-next_telegramm:
-	if (!bnetz->telegramm) {
-		/* request telegramm */
-//		PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Request new 'Telegramm'.\n");
-		telegramm = bnetz_get_telegramm(bnetz);
-		if (!telegramm) {
-			PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Stop sending 'Telegramm'.\n");
-			return length;
-		}
-		bnetz->telegramm = 1;
-		bnetz->telegramm_pos = 0;
-		spl = bnetz->telegramm_spl;
-		/* render telegramm */
-		phase = bnetz->phase65536;
-		for (i = 0; i < 16; i++) {
-			phaseshift = bnetz->phaseshift65536[telegramm[i] == '1'];
-			for (j = 0; j < bnetz->samples_per_bit; j++) {
-				*spl++ = dsp_sine[(uint16_t)phase];
-				phase += phaseshift;
-				if (phase >= 65536)
-					phase -= 65536;
+	/* send frame bit (prio) */
+	switch (bnetz->dsp_mode) {
+	case DSP_MODE_TELEGRAMM:
+		if (!bnetz->tx_telegramm || bnetz->tx_telegramm_pos == 16) {
+			/* request frame */
+			bnetz->tx_telegramm = bnetz_get_telegramm(bnetz);
+			if (!bnetz->tx_telegramm) {
+				PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Stop sending 'Telegramm'.\n");
+				return -1;
 			}
+			bnetz->tx_telegramm_pos = 0;
 		}
-		bnetz->phase65536 = phase;
-	}
 
-	/* send audio from telegramm */
-	max = bnetz->samples_per_bit * 16;
-	count = max - bnetz->telegramm_pos;
-	if (count > length)
-		count = length;
-	spl = bnetz->telegramm_spl + bnetz->telegramm_pos;
-	for (i = 0; i < count; i++)
-		*samples++ = *spl++;
-	length -= count;
-	bnetz->telegramm_pos += count;
-	/* check for end of telegramm */
-	if (bnetz->telegramm_pos == max) {
-		bnetz->telegramm = 0;
-		/* we need more ? */
-		if (length)
-			goto next_telegramm;
+		return bnetz->tx_telegramm[bnetz->tx_telegramm_pos++];
+	case DSP_MODE_0:
+		return 0; /* F0 */
+	case DSP_MODE_1:
+		return 1; /* F1 */
+	default:
+		return -1; // should never happen
 	}
-
-	return length;
 }
 
 /* Provide stream of audio toward radio unit */
 void sender_send(sender_t *sender, sample_t *samples, int length)
 {
 	bnetz_t *bnetz = (bnetz_t *) sender;
-	int len;
+	int count;
 
 again:
 	switch (bnetz->dsp_mode) {
@@ -399,20 +272,15 @@ again:
 		jitter_load(&bnetz->sender.dejitter, samples, length);
 		break;
 	case DSP_MODE_0:
-		fsk_tone(bnetz, samples, length, 0);
-		break;
 	case DSP_MODE_1:
-		fsk_tone(bnetz, samples, length, 1);
-		break;
 	case DSP_MODE_TELEGRAMM:
-		/* Encode telegramm into audio stream. If telegramms have
+		/* Encode tone/frame into audio stream. If frames have
 		 * stopped, process again for rest of stream. */
-		len = fsk_telegramm(bnetz, samples, length);
-		if (len) {
-			samples += length - len;
-			length = len;
+		count = fsk_send(&bnetz->fsk, samples, length, 0);
+		samples += count;
+		length -= count;
+		if (length)
 			goto again;
-		}
 		break;
 	}
 }
@@ -441,8 +309,10 @@ const char *bnetz_dsp_mode_name(enum dsp_mode mode)
 void bnetz_set_dsp_mode(bnetz_t *bnetz, enum dsp_mode mode)
 {
 	/* reset telegramm */
-	if (mode == DSP_MODE_TELEGRAMM && bnetz->dsp_mode != mode)
-		bnetz->telegramm = 0;
+	if (mode == DSP_MODE_TELEGRAMM && bnetz->dsp_mode != mode) {
+		bnetz->tx_telegramm = 0;
+		fsk_tx_reset(&bnetz->fsk);
+	}
 	
 	PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "DSP mode %s -> %s\n", bnetz_dsp_mode_name(bnetz->dsp_mode), bnetz_dsp_mode_name(mode));
 	bnetz->dsp_mode = mode;
diff --git a/src/common/Makefile.am b/src/common/Makefile.am
index 92447dc..5b15507 100644
--- a/src/common/Makefile.am
+++ b/src/common/Makefile.am
@@ -24,7 +24,7 @@ libcommon_a_SOURCES = \
 	compandor.c \
 	fft.c \
 	fm_modulation.c \
-	ffsk.c \
+	fsk.c \
 	hagelbarger.c \
 	sender.c \
 	display_wave.c \
diff --git a/src/common/ffsk.c b/src/common/ffsk.c
deleted file mode 100644
index fdbf255..0000000
--- a/src/common/ffsk.c
+++ /dev/null
@@ -1,256 +0,0 @@
-/* FFSK audio processing (NMT / Radiocom 2000)
- *
- * (C) 2017 by Andreas Eversberg <jolly@eversberg.eu>
- * All Rights Reserved
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#define CHAN ffsk->channel
-
-#include <stdio.h>
-#include <stdint.h>
-#include <stdlib.h>
-#include <string.h>
-#include <errno.h>
-#include <math.h>
-#include "../common/sample.h"
-#include "../common/debug.h"
-#include "ffsk.h"
-
-#define PI			M_PI
-
-#define BIT_RATE		1200	/* baud rate */
-#define FILTER_STEPS		0.1	/* step every 1/12000 sec */
-
-/* two signaling tones */
-static double ffsk_freq[2] = {
-	1800.0,
-	1200.0,
-};
-
-static sample_t dsp_tone_bit[2][2][65536]; /* polarity, bit, phase */
-
-/* global init for FFSK */
-void ffsk_global_init(double peak_fsk)
-{
-	int i;
-	double s;
-
-	PDEBUG(DDSP, DEBUG_DEBUG, "Generating sine table for FFSK tones.\n");
-	for (i = 0; i < 65536; i++) {
-		s = sin((double)i / 65536.0 * 2.0 * PI);
-		/* bit(1) 1 cycle */
-		dsp_tone_bit[0][1][i] = s * peak_fsk;
-		dsp_tone_bit[1][1][i] = -s * peak_fsk;
-		/* bit(0) 1.5 cycles */
-		s = sin((double)i / 65536.0 * 3.0 * PI);
-		dsp_tone_bit[0][0][i] = s * peak_fsk;
-		dsp_tone_bit[1][0][i] = -s * peak_fsk;
-	}
-}
-
-/* Init FFSK */
-int ffsk_init(ffsk_t *ffsk, void *inst, void (*receive_bit)(void *inst, int bit, double quality, double level), int channel, int samplerate)
-{
-	sample_t *spl;
-	int i;
-
-	/* a symbol rate of 1200 Hz, times check interval of FILTER_STEPS */
-	if (samplerate < (double)BIT_RATE / (double)FILTER_STEPS) {
-		PDEBUG(DDSP, DEBUG_ERROR, "Sample rate must be at least 12000 Hz to process FSK+supervisory signal.\n");
-		return -EINVAL;
-	}
-
-	memset(ffsk, 0, sizeof(*ffsk));
-	ffsk->inst = inst;
-	ffsk->receive_bit = receive_bit;
-	ffsk->channel = channel;
-	ffsk->samplerate = samplerate;
-
-	ffsk->samples_per_bit = (double)ffsk->samplerate / (double)BIT_RATE;
-	ffsk->bits_per_sample = 1.0 / ffsk->samples_per_bit;
-	PDEBUG(DDSP, DEBUG_DEBUG, "Use %.4f samples for full bit duration @ %d.\n", ffsk->samples_per_bit, ffsk->samplerate);
-
-	/* allocate ring buffers, one bit duration */
-	ffsk->filter_size = floor(ffsk->samples_per_bit); /* buffer holds one bit (rounded down) */
-	spl = calloc(1, ffsk->filter_size * sizeof(*spl));
-	if (!spl) {
-		PDEBUG(DDSP, DEBUG_ERROR, "No memory!\n");
-		ffsk_cleanup(ffsk);
-		return -ENOMEM;
-	}
-	ffsk->filter_spl = spl;
-	ffsk->filter_bit = -1;
-
-	/* count symbols */
-	for (i = 0; i < 2; i++)
-		audio_goertzel_init(&ffsk->goertzel[i], ffsk_freq[i], ffsk->samplerate);
-	ffsk->phaseshift65536 = 65536.0 / ffsk->samples_per_bit;
-	PDEBUG(DDSP, DEBUG_DEBUG, "fsk_phaseshift = %.4f\n", ffsk->phaseshift65536);
-
-	return 0;
-}
-
-/* Cleanup transceiver instance. */
-void ffsk_cleanup(ffsk_t *ffsk)
-{
-	PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Cleanup DSP for Transceiver.\n");
-
-	if (ffsk->filter_spl) {
-		free(ffsk->filter_spl);
-		ffsk->filter_spl = NULL;
-	}
-}
-
-//#define DEBUG_MODULATOR
-//#define DEBUG_FILTER
-//#define DEBUG_QUALITY
-
-/* Filter one chunk of audio an detect tone, quality and loss of signal.
- * The chunk is a window of 1/1200s. This window slides over audio stream
- * and is processed every 1/12000s. (one step) */
-static inline void ffsk_decode_step(ffsk_t *ffsk, int pos)
-{
-	double level, result[2], softbit, quality;
-	int max;
-	sample_t *spl;
-	int bit;
-	
-	max = ffsk->filter_size;
-	spl = ffsk->filter_spl;
-
-	level = audio_level(spl, max);
-	/* limit level to prevent division by zero */
-	if (level < 0.001)
-		level = 0.001;
-
-	audio_goertzel(ffsk->goertzel, spl, max, pos, result, 2);
-
-	/* calculate soft bit from both frequencies */
-	softbit = (result[1] / level - result[0] / level + 1.0) / 2.0;
-//printf("%.3f: %.3f\n", level, softbit);
-	/* scale it, since both filters overlap by some percent */
-#define MIN_QUALITY 0.33
-	softbit = (softbit - MIN_QUALITY) / (1.0 - MIN_QUALITY - MIN_QUALITY);
-#ifdef DEBUG_FILTER
-//	printf("|%s", debug_amplitude(result[0]/level));
-//	printf("|%s| low=%.3f high=%.3f level=%d\n", debug_amplitude(result[1]/level), result[0]/level, result[1]/level, (int)level);
-	printf("|%s| softbit=%.3f\n", debug_amplitude(softbit), softbit);
-#endif
-	if (softbit > 1)
-		softbit = 1;
-	if (softbit < 0)
-		softbit = 0;
-	if (softbit > 0.5)
-		bit = 1;
-	else
-		bit = 0;
-
-	if (ffsk->filter_bit != bit) {
-		/* If we have a bit change, move sample counter towards one half bit duration.
-		 * We may have noise, so the bit change may be wrong or not at the correct place.
-		 * This can cause bit slips.
-		 * Therefore we change the sample counter only slightly, so bit slips may not
-		 * happen so quickly.
-		 * */
-#ifdef DEBUG_FILTER
-		puts("bit change");
-#endif
-		ffsk->filter_bit = bit;
-		if (ffsk->filter_sample < 5)
-			ffsk->filter_sample++;
-		if (ffsk->filter_sample > 5)
-			ffsk->filter_sample--;
-	} else if (--ffsk->filter_sample == 0) {
-		/* if sample counter bit reaches 0, we reset sample counter to one bit duration */
-#ifdef DEBUG_FILTER
-		puts("sample");
-#endif
-//		quality = result[bit] / level;
-		if (softbit > 0.5)
-			quality = softbit * 2.0 - 1.0;
-		else
-			quality = 1.0 - softbit * 2.0;
-#ifdef DEBUG_QUALITY
-		printf("|%s| quality=%.2f ", debug_amplitude(softbit), quality);
-		printf("|%s|\n", debug_amplitude(quality));
-#endif
-		/* adjust level, so a peak level becomes 100% */
-		ffsk->receive_bit(ffsk->inst, bit, quality, level / 0.63662);
-		ffsk->filter_sample = 10;
-	}
-}
-
-void ffsk_receive(ffsk_t *ffsk, sample_t *sample, int length)
-{
-	sample_t *spl;
-	int max, pos;
-	double step, bps;
-	int i;
-
-	/* write received samples to decode buffer */
-	max = ffsk->filter_size;
-	pos = ffsk->filter_pos;
-	step = ffsk->filter_step;
-	bps = ffsk->bits_per_sample;
-	spl = ffsk->filter_spl;
-	for (i = 0; i < length; i++) {
-#ifdef DEBUG_MODULATOR
-		printf("|%s|\n", debug_amplitude((double)samples[i] / 2333.0 /*fsk peak*/ / 2.0));
-#endif
-		/* write into ring buffer */
-		spl[pos++] = sample[i];
-		if (pos == max)
-			pos = 0;
-		/* if 1/10th of a bit duration is reached, decode buffer */
-		step += bps;
-		if (step >= FILTER_STEPS) {
-			step -= FILTER_STEPS;
-			ffsk_decode_step(ffsk, pos);
-		}
-	}
-	ffsk->filter_step = step;
-	ffsk->filter_pos = pos;
-}
-
-/* render frame */
-int ffsk_render_frame(ffsk_t *ffsk, const char *frame, int length, sample_t *sample)
-{
-	int bit, polarity;
-        double phaseshift, phase;
-	int count = 0, i;
-
-	polarity = ffsk->polarity;
-	phaseshift = ffsk->phaseshift65536;
-	phase = ffsk->phase65536;
-	for (i = 0; i < length; i++) {
-		bit = (frame[i] == '1');
-		do {
-			*sample++ = dsp_tone_bit[polarity][bit][(uint16_t)phase];
-			count++;
-			phase += phaseshift;
-		} while (phase < 65536.0);
-		phase -= 65536.0;
-		/* flip polarity when we have 1.5 sine waves */
-		if (bit == 0)
-			polarity = 1 - polarity;
-	}
-	ffsk->phase65536 = phase;
-	ffsk->polarity = polarity;
-
-	/* return number of samples created for frame */
-	return count;
-}
-
diff --git a/src/common/ffsk.h b/src/common/ffsk.h
deleted file mode 100644
index 84fc52a..0000000
--- a/src/common/ffsk.h
+++ /dev/null
@@ -1,27 +0,0 @@
-#include "../common/goertzel.h"
-
-typedef struct ffsk {
-	void			*inst;
-	void (*receive_bit)(void *inst, int bit, double quality, double level);
-	int			channel;		/* channel number */
-	int			samplerate;		/* current sample rate */
-	double			samples_per_bit;	/* number of samples for one bit (1200 Baud) */
-	double			bits_per_sample;	/* fraction of a bit per sample */
-	goertzel_t		goertzel[2];		/* filter for fsk decoding */
-	int			polarity;		/* current polarity state of bit */
-	sample_t		*filter_spl;		/* array to hold ring buffer for bit decoding */
-	int			filter_size;		/* size of ring buffer */
-	int			filter_pos;		/* position to write next sample */
-	double			filter_step;		/* counts bit duration, to trigger decoding every 10th bit */
-	int			filter_bit;		/* last bit state, so we detect a bit change */
-	int			filter_sample;		/* count until it is time to sample bit */
-	double			phaseshift65536;	/* how much the phase of fsk synbol changes per sample */
-	double			phase65536;		/* current phase */
-} ffsk_t;
-
-void ffsk_global_init(double peak_fsk);
-int ffsk_init(ffsk_t *ffsk, void *inst, void (*receive_bit)(void *inst, int bit, double quality, double level), int channel, int samplerate);
-void ffsk_cleanup(ffsk_t *ffsk);
-void ffsk_receive(ffsk_t *ffsk, sample_t *sample, int lenght);
-int ffsk_render_frame(ffsk_t *ffsk, const char *frame, int length, sample_t *sample);
-
diff --git a/src/common/fm_modulation.c b/src/common/fm_modulation.c
index aaf7e2c..2aa688a 100644
--- a/src/common/fm_modulation.c
+++ b/src/common/fm_modulation.c
@@ -23,13 +23,12 @@
 #include <string.h>
 #include <math.h>
 #include "sample.h"
-#include "iir_filter.h"
 #include "fm_modulation.h"
 
 //#define FAST_SINE
 
 /* init FM modulator */
-void fm_mod_init(fm_mod_t *mod, double samplerate, double offset, double amplitude)
+int fm_mod_init(fm_mod_t *mod, double samplerate, double offset, double amplitude)
 {
 	memset(mod, 0, sizeof(*mod));
 	mod->samplerate = samplerate;
@@ -42,17 +41,27 @@ void fm_mod_init(fm_mod_t *mod, double samplerate, double offset, double amplitu
 	mod->sin_tab = calloc(65536+16384, sizeof(*mod->sin_tab));
 	if (!mod->sin_tab) {
 		fprintf(stderr, "No mem!\n");
-		abort();
+		return -ENOMEM;
 	}
 
 	/* generate sine and cosine */
 	for (i = 0; i < 65536+16384; i++)
 		mod->sin_tab[i] = sin(2.0 * M_PI * (double)i / 65536.0) * amplitude;
 #endif
+
+	return 0;
 }
 
-/* do frequency modulation of samples and add them to existing buff */
-void fm_modulate(fm_mod_t *mod, sample_t *samples, int num, float *buff)
+void fm_mod_exit(fm_mod_t *mod)
+{
+	if (mod->sin_tab) {
+		free(mod->sin_tab);
+		mod->sin_tab = NULL;
+	}
+}
+
+/* do frequency modulation of samples and add them to existing baseband */
+void fm_modulate_complex(fm_mod_t *mod, sample_t *frequency, int length, float *baseband)
 {
 	double dev, rate, phase, offset;
 	int s, ss;
@@ -73,25 +82,25 @@ void fm_modulate(fm_mod_t *mod, sample_t *samples, int num, float *buff)
 #endif
 
 	/* modulate */
-	for (s = 0, ss = 0; s < num; s++) {
-		/* deviation is defined by the sample value and the offset */
-		dev = offset + samples[s];
+	for (s = 0, ss = 0; s < length; s++) {
+		/* deviation is defined by the frequency value and the offset */
+		dev = offset + frequency[s];
 #ifdef FAST_SINE
 		phase += 65536.0 * dev / rate;
 		if (phase < 0.0)
 			phase += 65536.0;
 		else if (phase >= 65536.0)
 			phase -= 65536.0;
-		buff[ss++] += cos_tab[(uint16_t)phase];
-		buff[ss++] += sin_tab[(uint16_t)phase];
+		baseband[ss++] += cos_tab[(uint16_t)phase];
+		baseband[ss++] += sin_tab[(uint16_t)phase];
 #else
 		phase += 2.0 * M_PI * dev / rate;
 		if (phase < 0.0)
 			phase += 2.0 * M_PI;
 		else if (phase >= 2.0 * M_PI)
 			phase -= 2.0 * M_PI;
-		buff[ss++] += cos(phase) * amplitude;
-		buff[ss++] += sin(phase) * amplitude;
+		baseband[ss++] += cos(phase) * amplitude;
+		baseband[ss++] += sin(phase) * amplitude;
 #endif
 	}
 
@@ -99,7 +108,7 @@ void fm_modulate(fm_mod_t *mod, sample_t *samples, int num, float *buff)
 }
 
 /* init FM demodulator */
-void fm_demod_init(fm_demod_t *demod, double samplerate, double offset, double bandwidth)
+int fm_demod_init(fm_demod_t *demod, double samplerate, double offset, double bandwidth)
 {
 	memset(demod, 0, sizeof(*demod));
 	demod->samplerate = samplerate;
@@ -119,21 +128,31 @@ void fm_demod_init(fm_demod_t *demod, double samplerate, double offset, double b
 	demod->sin_tab = calloc(65536+16384, sizeof(*demod->sin_tab));
 	if (!demod->sin_tab) {
 		fprintf(stderr, "No mem!\n");
-		abort();
+		return -ENOMEM;
 	}
 
 	/* generate sine and cosine */
 	for (i = 0; i < 65536+16384; i++)
 		demod->sin_tab[i] = sin(2.0 * M_PI * (double)i / 65536.0);
 #endif
+
+	return 0;
+}
+
+void fm_demod_exit(fm_demod_t *demod)
+{
+	if (demod->sin_tab) {
+		free(demod->sin_tab);
+		demod->sin_tab = NULL;
+	}
 }
 
-/* do frequency demodulation of buff and write them to samples */
-void fm_demodulate(fm_demod_t *demod, sample_t *samples, int num, float *buff)
+/* do frequency demodulation of baseband and write them to samples */
+void fm_demodulate_complex(fm_demod_t *demod, sample_t *frequency, int length, float *baseband, sample_t *I, sample_t *Q)
 {
 	double phase, rot, last_phase, dev, rate;
 	double _sin, _cos;
-	sample_t I[num], Q[num], i, q;
+	sample_t i, q;
 	int s, ss;
 #ifdef FAST_SINE
 	double *sin_tab, *cos_tab;
@@ -146,10 +165,10 @@ void fm_demodulate(fm_demod_t *demod, sample_t *samples, int num, float *buff)
 	sin_tab = demod->sin_tab;
 	cos_tab = demod->sin_tab + 16384;
 #endif
-	for (s = 0, ss = 0; s < num; s++) {
+	for (s = 0, ss = 0; s < length; s++) {
 		phase += rot;
-		i = buff[ss++];
-		q = buff[ss++];
+		i = baseband[ss++];
+		q = baseband[ss++];
 #ifdef FAST_SINE
 		if (phase < 0.0)
 			phase += 65536.0;
@@ -169,10 +188,66 @@ void fm_demodulate(fm_demod_t *demod, sample_t *samples, int num, float *buff)
 		Q[s] = i * _sin + q * _cos;
 	}
 	demod->phase = phase;
-	iir_process(&demod->lp[0], I, num);
-	iir_process(&demod->lp[1], Q, num);
+	iir_process(&demod->lp[0], I, length);
+	iir_process(&demod->lp[1], Q, length);
+	last_phase = demod->last_phase;
+	for (s = 0; s < length; s++) {
+		phase = atan2(Q[s], I[s]);
+		dev = (phase - last_phase) / 2 / M_PI;
+		last_phase = phase;
+		if (dev < -0.49)
+			dev += 1.0;
+		else if (dev > 0.49)
+			dev -= 1.0;
+		dev *= rate;
+		frequency[s] = dev;
+	}
+	demod->last_phase = last_phase;
+}
+
+void fm_demodulate_real(fm_demod_t *demod, sample_t *frequency, int length, sample_t *baseband, sample_t *I, sample_t *Q)
+{
+	double phase, rot, last_phase, dev, rate;
+	double _sin, _cos;
+	sample_t i;
+	int s, ss;
+#ifdef FAST_SINE
+	double *sin_tab, *cos_tab;
+#endif
+
+	rate = demod->samplerate;
+	phase = demod->phase;
+	rot = demod->rot;
+#ifdef FAST_SINE
+	sin_tab = demod->sin_tab;
+	cos_tab = demod->sin_tab + 16384;
+#endif
+	for (s = 0, ss = 0; s < length; s++) {
+		phase += rot;
+		i = baseband[ss++];
+#ifdef FAST_SINE
+		if (phase < 0.0)
+			phase += 65536.0;
+		else if (phase >= 65536.0)
+			phase -= 65536.0;
+		_sin = sin_tab[(uint16_t)phase];
+		_cos = cos_tab[(uint16_t)phase];
+#else
+		if (phase < 0.0)
+			phase += 2.0 * M_PI;
+		else if (phase >= 2.0 * M_PI)
+			phase -= 2.0 * M_PI;
+		_sin = sin(phase);
+		_cos = cos(phase);
+#endif
+		I[s] = i * _cos;
+		Q[s] = i * _sin;
+	}
+	demod->phase = phase;
+	iir_process(&demod->lp[0], I, length);
+	iir_process(&demod->lp[1], Q, length);
 	last_phase = demod->last_phase;
-	for (s = 0; s < num; s++) {
+	for (s = 0; s < length; s++) {
 		phase = atan2(Q[s], I[s]);
 		dev = (phase - last_phase) / 2 / M_PI;
 		last_phase = phase;
@@ -181,7 +256,7 @@ void fm_demodulate(fm_demod_t *demod, sample_t *samples, int num, float *buff)
 		else if (dev > 0.49)
 			dev -= 1.0;
 		dev *= rate;
-		samples[s] = dev;
+		frequency[s] = dev;
 	}
 	demod->last_phase = last_phase;
 }
diff --git a/src/common/fm_modulation.h b/src/common/fm_modulation.h
index 2cd571a..83e7db4 100644
--- a/src/common/fm_modulation.h
+++ b/src/common/fm_modulation.h
@@ -1,3 +1,4 @@
+#include "../common/iir_filter.h"
 
 typedef struct fm_mod {
 	double samplerate;	/* sample rate of in and out */
@@ -7,8 +8,9 @@ typedef struct fm_mod {
 	double *sin_tab;	/* sine/cosine table for modulation */
 } fm_mod_t;
 
-void fm_mod_init(fm_mod_t *mod, double samplerate, double offset, double amplitude);
-void fm_modulate(fm_mod_t *mod, sample_t *samples, int num, float *buff);
+int fm_mod_init(fm_mod_t *mod, double samplerate, double offset, double amplitude);
+void fm_mod_exit(fm_mod_t *mod);
+void fm_modulate_complex(fm_mod_t *mod, sample_t *frequency, int num, float *baseband);
 
 typedef struct fm_demod {
 	double samplerate;	/* sample rate of in and out */
@@ -19,6 +21,8 @@ typedef struct fm_demod {
 	double *sin_tab;	/* sine/cosine table rotation */
 } fm_demod_t;
 
-void fm_demod_init(fm_demod_t *demod, double samplerate, double offset, double bandwidth);
-void fm_demodulate(fm_demod_t *demod, sample_t *samples, int num, float *buff);
+int fm_demod_init(fm_demod_t *demod, double samplerate, double offset, double bandwidth);
+void fm_demod_exit(fm_demod_t *demod);
+void fm_demodulate_complex(fm_demod_t *demod, sample_t *frequency, int length, float *baseband, sample_t *I, sample_t *Q);
+void fm_demodulate_real(fm_demod_t *demod, sample_t *frequency, int length, sample_t *baseband, sample_t *I, sample_t *Q);
 
diff --git a/src/common/fsk.c b/src/common/fsk.c
new file mode 100644
index 0000000..fa0eaf8
--- /dev/null
+++ b/src/common/fsk.c
@@ -0,0 +1,293 @@
+/* FSK audio processing (coherent FSK modem)
+ *
+ * (C) 2017 by Andreas Eversberg <jolly@eversberg.eu>
+ * All Rights Reserved
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <math.h>
+#include "../common/sample.h"
+#include "../common/debug.h"
+#include "fsk.h"
+
+#define PI			M_PI
+
+/*
+ * fsk = instance of fsk modem
+ * inst = instance of user
+ * send_bit() = function to be called whenever a new bit has to be sent
+ * receive_bit() = function to be called whenever a new bit was received
+ * samplerate = samplerate
+ * bitrate = bits per second
+ * f0, f1 = two frequencies for bit 0 and bit 1
+ * level = level to modulate the frequencies
+ * coherent = use coherent modulation (FFSK)
+ * bitadjust = how much to adjust the sample clock when a bitchange was detected. (0 = nothing, don't use this, 0.5 full adjustment)
+ */
+int fsk_init(fsk_t *fsk, void *inst, int (*send_bit)(void *inst), void (*receive_bit)(void *inst, int bit, double quality, double level), int samplerate, double bitrate, double f0, double f1, double level, int coherent, double bitadjust)
+{
+	double bandwidth;
+	int i;
+	int rc;
+
+	PDEBUG(DDSP, DEBUG_DEBUG, "Setup FSK for Transceiver. (F0 = %.1f, F1 = %.1f, peak = %.1f)\n", f0, f1, level);
+
+	memset(fsk, 0, sizeof(*fsk));
+
+	/* gen sine table with deviation */
+	fsk->sin_tab = calloc(65536+16384, sizeof(*fsk->sin_tab));
+	if (!fsk->sin_tab) {
+		fprintf(stderr, "No mem!\n");
+		rc = -ENOMEM;
+		goto error;
+	}
+	for (i = 0; i < 65536; i++) 
+		fsk->sin_tab[i] = sin((double)i / 65536.0 * 2.0 * PI) * level;
+
+	fsk->inst = inst;
+	fsk->rx_bit = -1;
+	fsk->rx_bitadjust = bitadjust;
+	fsk->receive_bit = receive_bit;
+	fsk->tx_bit = -1;
+	fsk->level = level;
+	fsk->send_bit = send_bit;
+	fsk->f0_deviation = (f0 - f1) / 2.0;
+	fsk->f1_deviation = (f1 - f0) / 2.0;
+	if (f0 < f1) {
+		fsk->low_bit = 0;
+		fsk->high_bit = 1;
+	} else {
+		fsk->low_bit = 1;
+		fsk->high_bit = 0;
+	}
+
+	/* calculate bandwidth */
+	bandwidth = fabs(f0 - f1) * 2.0;
+
+	/* init fm demodulator */
+	rc = fm_demod_init(&fsk->demod, (double)samplerate, (f0 + f1) / 2.0, bandwidth);
+	if (rc < 0)
+		goto error;
+
+	fsk->bits_per_sample = (double)bitrate / (double)samplerate;
+	PDEBUG(DDSP, DEBUG_DEBUG, "Bitduration of %.4f bits per sample @ %d.\n", fsk->bits_per_sample, samplerate);
+
+	fsk->phaseshift65536[0] = f0 / (double)samplerate * 65536.0;
+	PDEBUG(DDSP, DEBUG_DEBUG, "phaseshift65536[0] = %.4f\n", fsk->phaseshift65536[0]);
+	fsk->phaseshift65536[1] = f1 / (double)samplerate * 65536.0;
+	PDEBUG(DDSP, DEBUG_DEBUG, "phaseshift65536[1] = %.4f\n", fsk->phaseshift65536[1]);
+
+	/* use coherent modulation, i.e. each bit has an integer number of
+	 * half waves and starts/ends at zero crossing
+	 */
+	if (coherent) {
+		double waves;
+
+		fsk->coherent = 1;
+		waves = (f0 / bitrate);
+		if (fabs(round(waves * 2) - (waves * 2)) > 0.001) {
+			fprintf(stderr, "Failed to set coherent mode, half waves of F0 does not fit exactly into one bit, please fix!\n");
+			abort();
+		}
+		fsk->cycles_per_bit65536[0] = waves * 65536.0;
+		waves = (f1 / bitrate);
+		if (fabs(round(waves * 2) - (waves * 2)) > 0.001) {
+			fprintf(stderr, "Failed to set coherent mode, half waves of F1 does not fit exactly into one bit, please fix!\n");
+			abort();
+		}
+		fsk->cycles_per_bit65536[1] = waves * 65536.0;
+	}
+
+	/* filter prevents emphasis to overshoot on bit change */
+	iir_lowpass_init(&fsk->tx_filter, 4000.0, samplerate, 2);
+
+	return 0;
+
+error:
+	fsk_cleanup(fsk);
+	return rc;
+}
+
+/* Cleanup transceiver instance. */
+void fsk_cleanup(fsk_t *fsk)
+{
+	PDEBUG(DDSP, DEBUG_DEBUG, "Cleanup FSK for Transceiver.\n");
+
+	if (fsk->sin_tab) {
+		free(fsk->sin_tab);
+		fsk->sin_tab = NULL;
+	}
+
+	fm_demod_exit(&fsk->demod);
+}
+
+//#define DEBUG_MODULATOR
+//#define DEBUG_FILTER
+
+/* Demodulates bits
+ *
+ * If bit is received, callback function send_bit() is called.
+ *
+ * We sample each bit 0.5 bits after polarity change.
+ *
+ * If we have a bit change, adjust sample counter towards one half bit duration.
+ * We may have noise, so the bit change may be wrong or not at the correct place.
+ * This can cause bit slips.
+ * Therefore we change the sample counter only slightly, so bit slips may not
+ * happen so quickly.
+ */
+void fsk_receive(fsk_t *fsk, sample_t *sample, int length)
+{
+	sample_t I[length], Q[length], frequency[length], f;
+	int i;
+	int bit;
+	double level, quality;
+
+	/* demod samples to offset arround center frequency */
+	fm_demodulate_real(&fsk->demod, frequency, length, sample, I, Q);
+
+	for (i = 0; i < length; i++) {
+		f = frequency[i];
+		if (f < 0)
+			bit = fsk->low_bit;
+		else
+			bit = fsk->high_bit;
+#ifdef DEBUG_FILTER
+			printf("|%s| %.3f\n", debug_amplitude(f / fabs(fsk->f0_deviation)), f / fabs(fsk->f0_deviation));
+#endif
+	
+
+		if (fsk->rx_bit != bit) {
+#ifdef DEBUG_FILTER
+			puts("bit change");
+#endif
+			fsk->rx_bit = bit;
+			if (fsk->rx_bitpos < 0.5) {
+				fsk->rx_bitpos += fsk->rx_bitadjust;
+				if (fsk->rx_bitpos > 0.5)
+					fsk->rx_bitpos = 0.5;
+			} else
+			if (fsk->rx_bitpos > 0.5) {
+				fsk->rx_bitpos -= fsk->rx_bitadjust;
+				if (fsk->rx_bitpos < 0.5)
+					fsk->rx_bitpos = 0.5;
+			}
+		}
+		/* if bit counter reaches 1, we substract 1 and sample the bit */
+		if (fsk->rx_bitpos >= 1.0) {
+			/* peak level is the length of I/Q vector
+			 * since we filter out the unwanted modulation product, the vector is only half of length */
+			level = sqrt(I[i] * I[i] + Q[i] * Q[i]) * 2.0;
+			/* quality is defined on how accurat the target frequency it hit
+			 * if it is hit close to the center or close to double deviation from center, quality is close to 0 */
+			if (bit == 0)
+				quality = 1.0 - fabs((f - fsk->f0_deviation) / fsk->f0_deviation);
+			else
+				quality = 1.0 - fabs((f - fsk->f1_deviation) / fsk->f1_deviation);
+			if (quality < 0)
+				quality = 0;
+#ifdef DEBUG_FILTER
+			printf("sample (level=%.3f, quality=%.3f)\n", level / fsk->level, quality);
+#endif
+			/* adjust the values, because this is best we can get from fm demodulator */
+			fsk->receive_bit(fsk->inst, bit, quality / 0.95, level);
+			fsk->rx_bitpos -= 1.0;
+		}
+		fsk->rx_bitpos += fsk->bits_per_sample;
+	}
+}
+
+/* modulate bits
+ *
+ * If first/next bit is required, callback function send_bit() is called.
+ * If there is no (more) data to be transmitted, the callback functions shall
+ * return -1. In this case, this function stops and returns the number of
+ * samples that have been rendered so far, if any.
+ *
+ * For coherent mode (FSK), we round the phase on every bit change to the
+ * next zero crossing. This prevents phase shifts due to rounding errors.
+ */
+int fsk_send(fsk_t *fsk, sample_t *sample, int length, int add)
+{
+	int count = 0;
+	double phase, phaseshift;
+
+	phase = fsk->tx_phase65536;
+
+	/* get next bit */
+	if (fsk->tx_bit < 0) {
+next_bit:
+		fsk->tx_bit = fsk->send_bit(fsk->inst);
+#ifdef DEBUG_MODULATOR
+		printf("bit change to %d\n", fsk->tx_bit);
+#endif
+		if (fsk->tx_bit < 0)
+			goto done;
+		/* correct phase when changing bit */
+		if (fsk->coherent) {
+			/* round phase to nearest zero crossing */
+			if (phase > 16384.0 && phase < 49152.0)
+				phase = 32768.0;
+			else
+				phase = 0;
+			/* set phase according to current position in bit */
+			phase += fsk->tx_bitpos * fsk->cycles_per_bit65536[fsk->tx_bit & 1];
+#ifdef DEBUG_MODULATOR
+			printf("phase %.3f bitpos=%.6f\n", phase, fsk->tx_bitpos);
+#endif
+		}
+	}
+
+	/* modulate bit */
+	phaseshift = fsk->phaseshift65536[fsk->tx_bit & 1];
+	while (count < length && fsk->tx_bitpos < 1.0) {
+		if (add)
+			sample[count++] += fsk->sin_tab[(uint16_t)phase];
+		else
+			sample[count++] = fsk->sin_tab[(uint16_t)phase];
+#ifdef DEBUG_MODULATOR
+		printf("|%s|\n", debug_amplitude(fsk->sin_tab[(uint16_t)phase] / fsk->level));
+#endif
+		phase += phaseshift;
+		if (phase >= 65536.0)
+			phase -= 65536.0;
+		fsk->tx_bitpos += fsk->bits_per_sample;
+	}
+	if (fsk->tx_bitpos >= 1.0) {
+		fsk->tx_bitpos -= 1.0;
+		goto next_bit;
+	}
+
+done:
+	fsk->tx_phase65536 = phase;
+
+	iir_process(&fsk->tx_filter, sample, count);
+
+	return count;
+}
+
+/* reset transmitter state, so we get a clean start */
+void fsk_tx_reset(fsk_t *fsk)
+{
+	fsk->tx_phase65536 = 0;
+	fsk->tx_bitpos = 0;
+	fsk->tx_bit = -1;
+}
+
diff --git a/src/common/fsk.h b/src/common/fsk.h
new file mode 100644
index 0000000..1a1009a
--- /dev/null
+++ b/src/common/fsk.h
@@ -0,0 +1,31 @@
+#include "../common/fm_modulation.h"
+
+typedef struct ffsk {
+	void		*inst;
+	int (*send_bit)(void *inst);
+	void (*receive_bit)(void *inst, int bit, double quality, double level);
+	fm_demod_t	demod;
+	iir_filter_t	tx_filter;
+	double		bits_per_sample;	/* fraction of a bit per sample */
+	double		*sin_tab;		/* sine table with correct peak level */
+	double		phaseshift65536[2];	/* how much the phase of fsk synbol changes per sample */
+	double		cycles_per_bit65536[2];	/* cacles of one bit */
+	double		tx_phase65536;		/* current transmit phase */
+	double		level;			/* level (amplitude) of signal */
+	int		coherent;		/* set, if coherent TX mode */
+	double		f0_deviation;		/* deviation of frequencies, relative to center */
+	double		f1_deviation;
+	int		low_bit, high_bit;	/* a low or high deviation means which bit? */
+	int		tx_bit;			/* current transmitting bit (-1 if not set) */
+	int		rx_bit;			/* current receiving bit (-1 if not yet measured) */
+	double		tx_bitpos;		/* current transmit position in bit */
+	double		rx_bitpos;		/* current receive position in bit (sampleclock) */
+	double		rx_bitadjust;		/* how much does a bit change cause the sample clock to be adjusted in phase */
+} fsk_t;
+
+int fsk_init(fsk_t *fsk, void *inst, int (*send_bit)(void *inst), void (*receive_bit)(void *inst, int bit, double quality, double level), int samplerate, double bitrate, double f0, double f1, double level, int coherent, double bitadjust);
+void fsk_cleanup(fsk_t *fsk);
+void fsk_receive(fsk_t *fsk, sample_t *sample, int length);
+int fsk_send(fsk_t *fsk, sample_t *sample, int length, int add);
+void fsk_tx_reset(fsk_t *fsk);
+
diff --git a/src/common/sdr.c b/src/common/sdr.c
index 7f465c4..41f78c8 100644
--- a/src/common/sdr.c
+++ b/src/common/sdr.c
@@ -26,7 +26,6 @@
 #include <pthread.h>
 #include <unistd.h>
 #include "sample.h"
-#include "iir_filter.h"
 #include "fm_modulation.h"
 #include "sender.h"
 #include "timer.h"
@@ -229,13 +228,17 @@ void *sdr_open(const char __attribute__((__unused__)) *audiodev, double *tx_freq
 			double tx_offset;
 			tx_offset = sdr->chan[c].tx_frequency - tx_center_frequency;
 			PDEBUG(DSDR, DEBUG_DEBUG, "Frequency #%d: TX offset: %.6f MHz\n", c, tx_offset / 1e6);
-			fm_mod_init(&sdr->chan[c].mod, samplerate, tx_offset, sdr->amplitude);
+			rc = fm_mod_init(&sdr->chan[c].mod, samplerate, tx_offset, sdr->amplitude);
+			if (rc < 0)
+				goto error;
 		}
 		if (sdr->paging_channel) {
 			double tx_offset;
 			tx_offset = sdr->chan[sdr->paging_channel].tx_frequency - tx_center_frequency;
 			PDEBUG(DSDR, DEBUG_DEBUG, "Paging Frequency: TX offset: %.6f MHz\n", tx_offset / 1e6);
-			fm_mod_init(&sdr->chan[sdr->paging_channel].mod, samplerate, tx_offset, sdr->amplitude);
+			rc = fm_mod_init(&sdr->chan[sdr->paging_channel].mod, samplerate, tx_offset, sdr->amplitude);
+			if (rc < 0)
+				goto error;
 		}
 		/* show gain */
 		PDEBUG(DSDR, DEBUG_INFO, "Using gain: TX %.1f dB\n", sdr_tx_gain);
@@ -286,7 +289,9 @@ void *sdr_open(const char __attribute__((__unused__)) *audiodev, double *tx_freq
 			double rx_offset;
 			rx_offset = sdr->chan[c].rx_frequency - rx_center_frequency;
 			PDEBUG(DSDR, DEBUG_DEBUG, "Frequency #%d: RX offset: %.6f MHz\n", c, rx_offset / 1e6);
-			fm_demod_init(&sdr->chan[c].demod, samplerate, rx_offset, bandwidth);
+			rc = fm_demod_init(&sdr->chan[c].demod, samplerate, rx_offset, bandwidth);
+			if (rc < 0)
+				goto error;
 		}
 		/* show gain */
 		PDEBUG(DSDR, DEBUG_INFO, "Using gain: RX %.1f dB\n", sdr_rx_gain);
@@ -513,7 +518,17 @@ void sdr_close(void *inst)
 		wave_destroy_record(&sdr->wave_tx_rec);
 		wave_destroy_playback(&sdr->wave_rx_play);
 		wave_destroy_playback(&sdr->wave_tx_play);
-		free(sdr->chan);
+		if (sdr->chan) {
+			int c;
+
+			for (c = 0; c < sdr->channels; c++) {
+				fm_mod_exit(&sdr->chan[c].mod);
+				fm_demod_exit(&sdr->chan[c].demod);
+			}
+			if (sdr->paging_channel)
+				fm_mod_exit(&sdr->chan[sdr->paging_channel].mod);
+			free(sdr->chan);
+		}
 		free(sdr);
 		sdr = NULL;
 	}
@@ -538,9 +553,9 @@ int sdr_write(void *inst, sample_t **samples, int num, enum paging_signal __attr
 		for (c = 0; c < channels; c++) {
 			/* switch to paging channel, if requested */
 			if (on[c] && sdr->paging_channel)
-				fm_modulate(&sdr->chan[sdr->paging_channel].mod, samples[c], num, buff);
+				fm_modulate_complex(&sdr->chan[sdr->paging_channel].mod, samples[c], num, buff);
 			else
-				fm_modulate(&sdr->chan[c].mod, samples[c], num, buff);
+				fm_modulate_complex(&sdr->chan[c].mod, samples[c], num, buff);
 		}
 	} else {
 		buff = (float *)samples;
@@ -603,6 +618,7 @@ int sdr_read(void *inst, sample_t **samples, int num, int channels)
 {
 	sdr_t *sdr = (sdr_t *)inst;
 	float buffer[num * 2], *buff = NULL;
+	sample_t I[num], Q[num];
 	int count = 0;
 	int c, s, ss;
 
@@ -675,7 +691,7 @@ int sdr_read(void *inst, sample_t **samples, int num, int channels)
 
 	if (channels) {
 		for (c = 0; c < channels; c++)
-			fm_demodulate(&sdr->chan[c].demod, samples[c], count, buff);
+			fm_demodulate_complex(&sdr->chan[c].demod, samples[c], count, buff, I, Q);
 	}
 
 	return count;
diff --git a/src/nmt/dms.c b/src/nmt/dms.c
index 8a27be0..4efa73a 100644
--- a/src/nmt/dms.c
+++ b/src/nmt/dms.c
@@ -286,15 +286,11 @@ static void dms_encode_dt(nmt_t *nmt, uint8_t d, uint8_t s, uint8_t n, uint8_t *
 	printf("\n");
 #endif
 
-	/* render wave form */
-	test_dms_frame(frame, 127); // used by test program
-	dms->frame_length = ffsk_render_frame(&nmt->ffsk, frame, 127, dms->frame_spl);
-	dms->frame_valid = 1;
-	dms->frame_pos = 0;
-	if (dms->frame_length > dms->frame_size) {
-		PDEBUG(DDMS, DEBUG_ERROR, "Frame exceeds buffer, please fix!\n");
-		abort();
-	}
+	/* store frame */
+	memcpy(dms->tx_frame, frame, 127);
+	dms->tx_frame_length = 127;
+	dms->tx_frame_pos = 0;
+	dms->tx_frame_valid = 1;
 }
 
 /* encode RR frame and schedule for next transmission */
@@ -334,29 +330,27 @@ static void dms_encode_rr(nmt_t *nmt, uint8_t d, uint8_t s, uint8_t n)
 	printf("\n");
 #endif
 
-	/* render wave form */
-	test_dms_frame(frame, 77); // used by test program
-	dms->frame_length = ffsk_render_frame(&nmt->ffsk, frame, 77, dms->frame_spl);
-	dms->frame_valid = 1;
-	dms->frame_pos = 0;
-	if (dms->frame_length > dms->frame_size) {
-		PDEBUG(DDMS, DEBUG_ERROR, "Frame exceeds buffer, please fix!\n");
-		abort();
-	}
+	/* store frame */
+	memcpy(dms->tx_frame, frame, 77);
+	dms->tx_frame_length = 77;
+	dms->tx_frame_pos = 0;
+	dms->tx_frame_valid = 1;
 }
 
 /* check if we have to transmit a frame and render it
  * also do nothing until a currently transmitted frame is completely
  * transmitted.
+ *
+ * this function is public, so it can be used by test routine.
  */
-static void trigger_frame_transmission(nmt_t *nmt)
+void trigger_frame_transmission(nmt_t *nmt)
 {
 	dms_t *dms = &nmt->dms;
 	struct dms_frame *dms_frame;
 	int i;
 
 	/* ongoing transmission, so we wait */
-	if (dms->frame_valid)
+	if (dms->tx_frame_valid)
 		return;
 
 	/* check for RR first, because high priority */
@@ -416,41 +410,21 @@ static void trigger_frame_transmission(nmt_t *nmt)
 }
 
 /* send data using FSK */
-int fsk_dms_frame(nmt_t *nmt, sample_t *samples, int length)
+int dms_send_bit(nmt_t *nmt)
 {
 	dms_t *dms = &nmt->dms;
-	sample_t *spl;
-	int i;
-	int count, max;
 
-next_frame:
-	/* check if no frame is currently transmitted */
-	if (dms->frame_length == 0) {
-		dms->frame_valid = 0;
+	if (!dms->tx_frame_valid)
+		return -1;
+
+	if (!dms->tx_frame_length || dms->tx_frame_pos == dms->tx_frame_length) {
+		dms->tx_frame_valid = 0;
 		trigger_frame_transmission(nmt);
-		if (!dms->frame_valid)
-			return length;
-	}
-	/* send audio from frame */
-	max = dms->frame_length;
-	count = max - dms->frame_pos;
-//printf("length = %d count=%d\n", length, count);
-	if (count > length)
-		count = length;
-	spl = dms->frame_spl + dms->frame_pos;
-	for (i = 0; i < count; i++) {
-		*samples++ = *spl++;
-	}
-	dms->frame_pos += count;
-	/* check for end of frame and stop */
-	if (dms->frame_pos == max) {
-		dms->frame_length = 0;
-		/* we need more ? */
-		if (length)
-			goto next_frame;
+		if (!dms->tx_frame_valid)
+			return -1;
 	}
 
-	return length;
+	return dms->tx_frame[dms->tx_frame_pos++];
 }
 
 /*
@@ -869,7 +843,7 @@ void dms_reset(nmt_t *nmt)
 	dms->rx_in_sync = 0;
 	memset(&dms->state, 0, sizeof(dms->state));
 
-	dms->frame_valid = 0;
+	dms->tx_frame_valid = 0;
 
 	while (dms->state.frame_list)
 		dms_frame_delete(nmt, dms->state.frame_list);
diff --git a/src/nmt/dms.h b/src/nmt/dms.h
index 1810e00..f70c9e1 100644
--- a/src/nmt/dms.h
+++ b/src/nmt/dms.h
@@ -24,11 +24,10 @@ struct dms_state {
 
 typedef struct dms {
 	/* DMS transmission */
-	int			frame_valid;		/* set, if there is a valid frame in sample buffer */
-	sample_t		*frame_spl;		/* 127 * fsk_bit_length */
-	int			frame_size;		/* total size of buffer */
-	int			frame_pos;		/* current sample position in frame_spl */
-	int			frame_length;		/* number of samples currently in frame_spl */
+	int			tx_frame_valid;		/* do we have or had a valid frame? */
+	char			tx_frame[127];		/* carries bits of one frame to transmit */
+	int			tx_frame_length;
+	int			tx_frame_pos;
 	uint16_t		rx_sync;		/* shift register to detect sync */
 	double			rx_sync_level[256];	/* level infos */
 	double			rx_sync_quality[256];	/* quality infos */
@@ -52,7 +51,7 @@ typedef struct dms {
 
 int dms_init_sender(nmt_t *nmt);
 void dms_cleanup_sender(nmt_t *nmt);
-int fsk_dms_frame(nmt_t *nmt, sample_t *samples, int length);
+int dms_send_bit(nmt_t *nmt);
 void fsk_receive_bit_dms(nmt_t *nmt, int bit, double quality, double level);
 void dms_reset(nmt_t *nmt);
 
@@ -60,5 +59,5 @@ void dms_send(nmt_t *nmt, const uint8_t *data, int length, int eight_bits);
 void dms_all_sent(nmt_t *nmt);
 void dms_receive(nmt_t *nmt, const uint8_t *data, int length, int eight_bits);
 
-void test_dms_frame(const char *frame, int length);
+void trigger_frame_transmission(nmt_t *nmt);
 
diff --git a/src/nmt/dsp.c b/src/nmt/dsp.c
index 0a1ba2d..d0063a8 100644
--- a/src/nmt/dsp.c
+++ b/src/nmt/dsp.c
@@ -59,7 +59,10 @@
 #define COMPANDOR_0DB		1.0	/* A level of 0dBm (1.0) shall be unaccected */
 #define TX_PEAK_FSK		(4200.0 / 1800.0 * 1000.0 / DBM0_DEVIATION)
 #define TX_PEAK_SUPER		(300.0 / 4015.0 * 1000.0 / DBM0_DEVIATION)
-#define BIT_RATE		1200
+#define BIT_RATE		1200.0
+#define BIT_ADJUST		0.1	/* how much do we adjust bit clock on frequency change */
+#define F0			1800.0
+#define F1			1200.0
 #define MAX_DISPLAY		1.4	/* something above dBm0 */
 #define DIALTONE_HZ		425.0	/* dial tone frequency */
 #define TX_PEAK_DIALTONE	0.5	/* dial tone peak FIXME */
@@ -81,7 +84,7 @@ static double super_freq[5] = {
 static sample_t dsp_sine_super[65536];
 static sample_t dsp_sine_dialtone[65536];
 
-/* global init for FFSK */
+/* global init for dsp */
 void dsp_init(void)
 {
 	int i;
@@ -95,17 +98,15 @@ void dsp_init(void)
 		/* dialtone sine */
 		dsp_sine_dialtone[i] = s * TX_PEAK_DIALTONE;
 	}
-
-	ffsk_global_init(TX_PEAK_FSK);
 }
 
+static int fsk_send_bit(void *inst);
 static void fsk_receive_bit(void *inst, int bit, double quality, double level);
 
 /* Init FSK of transceiver */
 int dsp_init_sender(nmt_t *nmt, double deviation_factor)
 {
 	sample_t *spl;
-	double samples_per_bit;
 	int i;
 
 	/* attack (3ms) and recovery time (13.5ms) according to NMT specs */
@@ -119,32 +120,12 @@ int dsp_init_sender(nmt_t *nmt, double deviation_factor)
 	PDEBUG(DDSP, DEBUG_DEBUG, "Using FSK level of %.3f (%.3f KHz deviation @ 1500 Hz)\n", TX_PEAK_FSK * deviation_factor, 3.5 * deviation_factor);
 	PDEBUG(DDSP, DEBUG_DEBUG, "Using Supervisory level of %.3f (%.3f KHz deviation @ 4015 Hz)\n", TX_PEAK_SUPER * deviation_factor, 0.3 * deviation_factor);
 
-	/* init ffsk */
-	if (ffsk_init(&nmt->ffsk, nmt, fsk_receive_bit, nmt->sender.kanal, nmt->sender.samplerate) < 0) {
-		PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "FFSK init failed!\n");
+	/* init fsk */
+	if (fsk_init(&nmt->fsk, nmt, fsk_send_bit, fsk_receive_bit, nmt->sender.samplerate, BIT_RATE, F0, F1, TX_PEAK_FSK, 1, BIT_ADJUST) < 0) {
+		PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "FSK init failed!\n");
 		return -EINVAL;
 	}
 
-	/* allocate transmit buffer for a complete frame, add 10 to be safe */
-
-	samples_per_bit = (double)nmt->sender.samplerate / (double)BIT_RATE;
-	nmt->frame_size = 166.0 * samples_per_bit + 10;
-	spl = calloc(nmt->frame_size, sizeof(*spl));
-	if (!spl) {
-		PDEBUG(DDSP, DEBUG_ERROR, "No memory!\n");
-		return -ENOMEM;
-	}
-	nmt->frame_spl = spl;
-
-	/* allocate DMS transmit buffer for a complete frame, add 10 to be safe */
-	nmt->dms.frame_size = 127.0 * samples_per_bit + 10;
-	spl = calloc(nmt->dms.frame_size, sizeof(*spl));
-	if (!spl) {
-		PDEBUG(DDSP, DEBUG_ERROR, "No memory!\n");
-		return -ENOMEM;
-	}
-	nmt->dms.frame_spl = spl;
-
 	/* allocate ring buffer for supervisory signal detection */
 	nmt->super_samples = (int)((double)nmt->sender.samplerate * SUPER_DURATION + 0.5);
 	spl = calloc(1, nmt->super_samples * sizeof(*spl));
@@ -179,16 +160,8 @@ void dsp_cleanup_sender(nmt_t *nmt)
 {
 	PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Cleanup DSP for Transceiver.\n");
 
-	ffsk_cleanup(&nmt->ffsk);
+	fsk_cleanup(&nmt->fsk);
 
-	if (nmt->frame_spl) {
-		free(nmt->frame_spl);
-		nmt->frame_spl = NULL;
-	}
-	if (nmt->dms.frame_spl) {
-		free(nmt->dms.frame_spl);
-		nmt->dms.frame_spl = NULL;
-	}
 	if (nmt->super_filter_spl) {
 		free(nmt->super_filter_spl);
 		nmt->super_filter_spl = NULL;
@@ -344,7 +317,8 @@ void sender_receive(sender_t *sender, sample_t *samples, int length)
 	}
 	nmt->super_filter_pos = pos;
 
-	ffsk_receive(&nmt->ffsk, samples, length);
+	/* fsk signal */
+	fsk_receive(&nmt->fsk, samples, length);
 
 	/* muting audio while receiving frame */
 	for (i = 0; i < length; i++) {
@@ -377,50 +351,31 @@ void sender_receive(sender_t *sender, sample_t *samples, int length)
 		nmt->sender.rxbuf_pos = 0;
 }
 
-static int fsk_frame(nmt_t *nmt, sample_t *samples, int length)
+static int fsk_send_bit(void *inst)
 {
+	nmt_t *nmt = (nmt_t *)inst;
 	const char *frame;
-	sample_t *spl;
-	int i;
-	int count, max;
-
-next_frame:
-	if (!nmt->frame_length) {
-		/* request frame */
-		frame = nmt_get_frame(nmt);
-		if (!frame) {
-			PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Stop sending frames.\n");
-			return length;
-		}
-		/* render frame */
-		nmt->frame_length = ffsk_render_frame(&nmt->ffsk, frame, 166, nmt->frame_spl);
-		nmt->frame_pos = 0;
-		if (nmt->frame_length > nmt->frame_size) {
-			PDEBUG_CHAN(DDSP, DEBUG_ERROR, "Frame exceeds buffer, please fix!\n");
-			abort();
+
+	/* send frame bit (prio) */
+	if (nmt->dsp_mode == DSP_MODE_FRAME) {
+		if (!nmt->tx_frame_length || nmt->tx_frame_pos == nmt->tx_frame_length) {
+			/* request frame */
+			frame = nmt_get_frame(nmt);
+			if (!frame) {
+				nmt->tx_frame_length = 0;
+				PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Stop sending frames.\n");
+				return -1;
+			}
+			memcpy(nmt->tx_frame, frame, 166);
+			nmt->tx_frame_length = 166;
+			nmt->tx_frame_pos = 0;
 		}
-	}
 
-	/* send audio from frame */
-	max = nmt->frame_length;
-	count = max - nmt->frame_pos;
-	if (count > length)
-		count = length;
-	spl = nmt->frame_spl + nmt->frame_pos;
-	for (i = 0; i < count; i++) {
-		*samples++ = *spl++;
-	}
-	length -= count;
-	nmt->frame_pos += count;
-	/* check for end of telegramm */
-	if (nmt->frame_pos == max) {
-		nmt->frame_length = 0;
-		/* we need more ? */
-		if (length)
-			goto next_frame;
+		return nmt->tx_frame[nmt->tx_frame_pos++];
 	}
 
-	return length;
+	/* send dms bit */
+	return dms_send_bit(nmt);
 }
 
 /* Generate audio stream with supervisory signal. Keep phase for next call of function. */
@@ -465,7 +420,7 @@ static void dial_tone(nmt_t *nmt, sample_t *samples, int length)
 void sender_send(sender_t *sender, sample_t *samples, int length)
 {
 	nmt_t *nmt = (nmt_t *) sender;
-	int len;
+	int count;
 
 again:
 	switch (nmt->dsp_mode) {
@@ -473,8 +428,8 @@ again:
 	case DSP_MODE_DTMF:
 		jitter_load(&nmt->sender.dejitter, samples, length);
 		/* send after dejitter, so audio is flushed */
-		if (nmt->dms.frame_valid) {
-			fsk_dms_frame(nmt, samples, length);
+		if (nmt->dms.tx_frame_valid) {
+			fsk_send(&nmt->fsk, samples, length, 0);
 			break;
 		}
 		if (nmt->supervisory)
@@ -489,15 +444,14 @@ again:
 	case DSP_MODE_FRAME:
 		/* Encode frame into audio stream. If frames have
 		 * stopped, process again for rest of stream. */
-		len = fsk_frame(nmt, samples, length);
+		count = fsk_send(&nmt->fsk, samples, length, 0);
 		/* special case: add supervisory signal to frame at loop test */
 		if (nmt->sender.loopback && nmt->supervisory)
-			super_encode(nmt, samples, length);
-		if (len) {
-			samples += length - len;
-			length = len;
+			super_encode(nmt, samples, count);
+		samples += count;
+		length -= count;
+		if (length)
 			goto again;
-		}
 		break;
 	}
 }
@@ -525,9 +479,11 @@ const char *nmt_dsp_mode_name(enum dsp_mode mode)
 
 void nmt_set_dsp_mode(nmt_t *nmt, enum dsp_mode mode)
 {
-	/* reset telegramm */
-	if (mode == DSP_MODE_FRAME && nmt->dsp_mode != mode)
-		nmt->frame_length = 0;
+	/* reset frame */
+	if (mode == DSP_MODE_FRAME && nmt->dsp_mode != mode) {
+		fsk_tx_reset(&nmt->fsk);
+		nmt->tx_frame_length = 0;
+	}
 
 	PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "DSP mode %s -> %s\n", nmt_dsp_mode_name(nmt->dsp_mode), nmt_dsp_mode_name(mode));
 	nmt->dsp_mode = mode;
diff --git a/src/nmt/main.c b/src/nmt/main.c
index d29ee62..396f7d0 100644
--- a/src/nmt/main.c
+++ b/src/nmt/main.c
@@ -427,6 +427,3 @@ fail:
 	return 0;
 }
 
-// dummy, will be replaced by DMS test program
-void test_dms_frame(const char __attribute__((unused)) *frame, int __attribute__((unused)) length) {}
-
diff --git a/src/nmt/nmt.c b/src/nmt/nmt.c
index 905e523..60c88f1 100644
--- a/src/nmt/nmt.c
+++ b/src/nmt/nmt.c
@@ -1532,7 +1532,7 @@ void nmt_receive_frame(nmt_t *nmt, const char *bits, double quality, double leve
 	frame_t frame;
 	int rc;
 
-	PDEBUG_CHAN(DDSP, DEBUG_INFO, "RX Level: %.0f%% Quality=%.0f\n", level * 100.0, quality * 100.0);
+	PDEBUG_CHAN(DDSP, DEBUG_INFO, "RX Level: %.0f%% Quality=%.0f%%\n", level * 100.0, quality * 100.0);
 
 	rc = decode_frame(nmt->sysinfo.system, &frame, bits, (nmt->sender.loopback) ? MTX_TO_XX : XX_TO_MTX, (nmt->state == STATE_MT_PAGING));
 	if (rc < 0) {
diff --git a/src/nmt/nmt.h b/src/nmt/nmt.h
index 3f9577c..ae871e6 100644
--- a/src/nmt/nmt.h
+++ b/src/nmt/nmt.h
@@ -2,7 +2,8 @@
 #include "../common/compandor.h"
 #include "../common/dtmf.h"
 #include "../common/call.h"
-#include "../common/ffsk.h"
+#include "../common/fsk.h"
+#include "../common/goertzel.h"
 #include "dms.h"
 #include "sms.h"
 
@@ -96,7 +97,7 @@ typedef struct nmt {
 
 	/* dsp states */
 	enum dsp_mode		dsp_mode;		/* current mode: audio, durable tone 0 or 1, paging */
-	ffsk_t			ffsk;			/* ffsk processing */
+	fsk_t			fsk;			/* fsk processing */
 	int			super_samples;		/* number of samples in buffer for supervisory detection */
 	goertzel_t		super_goertzel[5];	/* filter for supervisory decoding */
 	sample_t		*super_filter_spl;	/* array with sample buffer for supervisory detection */
@@ -112,15 +113,14 @@ typedef struct nmt {
 	int			rx_count;		/* next bit to receive */
 	double			rx_level[256];		/* level infos */
 	double			rx_quality[256];	/* quality infos */
-	sample_t		*frame_spl;		/* samples to store a complete rendered frame */
-	int			frame_size;		/* total size of sample buffer */
-	int			frame_length;		/* current length of data in sample buffer */
-	int			frame_pos;		/* current sample position in frame_spl */
 	uint64_t		rx_bits_count;		/* sample counter */
 	uint64_t		rx_bits_count_current;	/* sample counter of current frame */
 	uint64_t		rx_bits_count_last;	/* sample counter of last frame */
 	int			super_detected;		/* current detection state flag */
 	int			super_detect_count;	/* current number of consecutive detections/losses */
+	char			tx_frame[166];		/* carries bits of one frame to transmit */
+	int			tx_frame_length;
+	int			tx_frame_pos;
 
 	/* DMS/SMS states */
 	dms_t			dms;			/* DMS states */
diff --git a/src/r2000/dsp.c b/src/r2000/dsp.c
index 1a1c096..3b9dbd2 100644
--- a/src/r2000/dsp.c
+++ b/src/r2000/dsp.c
@@ -37,7 +37,8 @@
  *
  * Applies similar to NMT, read it there!
  *
- * I assume that the deviation at 1800 Hz (Bit 0) is +-1700 Hz.
+ * I assume that the deviation at 1500 Hz is +-1425 Hz. (according to R&S)
+ * This would lead to a deviation at 1800 Hz (Bit 0) about +-1700 Hz. (emphasis)
  *
  * Notes on TX_PEAK_SUPER level:
  *
@@ -49,44 +50,32 @@
 #define MAX_MODULATION		2550.0
 #define DBM0_DEVIATION		1500.0	/* deviation of dBm0 at 1 kHz */
 #define COMPANDOR_0DB		1.0	/* A level of 0dBm (1.0) shall be unaccected */
-#define TX_PEAK_FSK		(1700.0 / 1800.0 * 1000.0 / DBM0_DEVIATION) /* with emphasis */
+#define TX_PEAK_FSK		(1425.0 / 1500.0 * 1000.0 / DBM0_DEVIATION) /* with emphasis */
 #define TX_PEAK_SUPER		(300.0 / DBM0_DEVIATION) /* no emphasis */
-#define BIT_RATE		1200.0
-#define SUPER_RATE		50.0
+#define FSK_BIT_RATE		1200.0
+#define FSK_BIT_ADJUST		0.1	/* how much do we adjust bit clock on frequency change */
+#define FSK_F0			1800.0
+#define FSK_F1			1200.0
+#define SUPER_BIT_RATE		50.0
+#define SUPER_BIT_ADJUST	0.5	/* how much do we adjust bit clock on frequency change */
+#define SUPER_F0		136.0
+#define SUPER_F1		164.0
 #define FILTER_STEP		0.002	/* step every 2 ms */
 #define MAX_DISPLAY		1.4	/* something above dBm0 */
 
-/* two signaling tones */
-static double super_bits[2] = {
-	136.0,
-	164.0,
-};
-
-/* table for fast sine generation */
-static sample_t super_sine[65536];
-
-/* global init for FFSK */
+/* global init for FSK */
 void dsp_init(void)
 {
-	int i;
-
-	ffsk_global_init(TX_PEAK_FSK);
-
-	PDEBUG(DDSP, DEBUG_DEBUG, "Generating sine table.\n");
-	for (i = 0; i < 65536; i++) {
-		super_sine[i] = sin((double)i / 65536.0 * 2.0 * PI) * TX_PEAK_SUPER;
-	}
 }
 
+static int fsk_send_bit(void *inst);
 static void fsk_receive_bit(void *inst, int bit, double quality, double level);
+static int super_send_bit(void *inst);
+static void super_receive_bit(void *inst, int bit, double quality, double level);
 
 /* Init FSK of transceiver */
 int dsp_init_sender(r2000_t *r2000)
 {
-	sample_t *spl;
-	double fsk_samples_per_bit;
-	int i;
-
 	/* attack (3ms) and recovery time (13.5ms) according to NMT specs */
 	init_compandor(&r2000->cstate, 8000, 3.0, 13.5, COMPANDOR_0DB);
 
@@ -97,9 +86,9 @@ int dsp_init_sender(r2000_t *r2000)
 
 	PDEBUG(DDSP, DEBUG_DEBUG, "Using FSK level of %.3f\n", TX_PEAK_FSK);
 
-	/* init ffsk */
-	if (ffsk_init(&r2000->ffsk, r2000, fsk_receive_bit, r2000->sender.kanal, r2000->sender.samplerate) < 0) {
-		PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "FFSK init failed!\n");
+	/* init fsk */
+	if (fsk_init(&r2000->fsk, r2000, fsk_send_bit, fsk_receive_bit, r2000->sender.samplerate, FSK_BIT_RATE, FSK_F0, FSK_F1, TX_PEAK_FSK, 1, FSK_BIT_ADJUST) < 0) {
+		PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "FSK init failed!\n");
 		return -EINVAL;
 	}
 	if (r2000->sender.loopback)
@@ -107,43 +96,11 @@ int dsp_init_sender(r2000_t *r2000)
 	else
 		r2000->rx_max = 144;
 
-	/* allocate transmit buffer for a complete frame, add 10 to be safe */
-
-	fsk_samples_per_bit = (double)r2000->sender.samplerate / BIT_RATE;
-	r2000->frame_size = 208.0 * fsk_samples_per_bit + 10;
-	spl = calloc(r2000->frame_size, sizeof(*spl));
-	if (!spl) {
-		PDEBUG(DDSP, DEBUG_ERROR, "No memory!\n");
-		return -ENOMEM;
-	}
-	r2000->frame_spl = spl;
-
-	/* strange: better quality with window size of two bits */
-	r2000->super_samples_per_window = (double)r2000->sender.samplerate / SUPER_RATE * 2.0;
-	r2000->super_filter_step = (double)r2000->sender.samplerate * FILTER_STEP;
-	r2000->super_size = 20.0 * r2000->super_samples_per_window + 10;
-	PDEBUG(DDSP, DEBUG_DEBUG, "Using %d samples per filter step for supervisory signal.\n", r2000->super_filter_step);
-	spl = calloc(r2000->super_size, sizeof(*spl));
-	if (!spl) {
-		PDEBUG(DDSP, DEBUG_ERROR, "No memory!\n");
-		return -ENOMEM;
-	}
-	r2000->super_spl = spl;
-	spl = calloc(1, r2000->super_samples_per_window * sizeof(*spl));
-	if (!spl) {
-		PDEBUG(DDSP, DEBUG_ERROR, "No memory!\n");
-		return -ENOMEM;
-	}
-	r2000->super_filter_spl = spl;
-	r2000->super_filter_bit = -1;
-
-	/* count supervisory symbols */
-	for (i = 0; i < 2; i++) {
-		audio_goertzel_init(&r2000->super_goertzel[i], super_bits[i], r2000->sender.samplerate);
-		r2000->super_phaseshift65536[i] = 65536.0 / ((double)r2000->sender.samplerate / super_bits[i]);
-		PDEBUG(DDSP, DEBUG_DEBUG, "phaseshift[%d] = %.4f\n", i, r2000->super_phaseshift65536[i]);
+	/* init supervisorty fsk */
+	if (fsk_init(&r2000->super_fsk, r2000, super_send_bit, super_receive_bit, r2000->sender.samplerate, SUPER_BIT_RATE, SUPER_F0, SUPER_F1, TX_PEAK_SUPER, 0, SUPER_BIT_ADJUST) < 0) {
+		PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "FSK init failed!\n");
+		return -EINVAL;
 	}
-	r2000->super_bittime = SUPER_RATE / (double)r2000->sender.samplerate;
 
 	return 0;
 }
@@ -153,20 +110,8 @@ void dsp_cleanup_sender(r2000_t *r2000)
 {
 	PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Cleanup DSP for Transceiver.\n");
 
-	ffsk_cleanup(&r2000->ffsk);
-
-	if (r2000->frame_spl) {
-		free(r2000->frame_spl);
-		r2000->frame_spl = NULL;
-	}
-	if (r2000->super_spl) {
-		free(r2000->super_spl);
-		r2000->super_spl = NULL;
-	}
-	if (r2000->super_filter_spl) {
-		free(r2000->super_filter_spl);
-		r2000->super_filter_spl = NULL;
-	}
+	fsk_cleanup(&r2000->fsk);
+	fsk_cleanup(&r2000->super_fsk);
 }
 
 /* Check for SYNC bits, then collect data bits */
@@ -242,8 +187,9 @@ static void fsk_receive_bit(void *inst, int bit, double quality, double level)
 	r2000_receive_frame(r2000, r2000->rx_frame, quality, level);
 }
 
-static void super_receive_bit(r2000_t *r2000, int bit, double level, double quality)
+static void super_receive_bit(void *inst, int bit, double quality, double level)
 {
+	r2000_t *r2000 = (r2000_t *)inst;
 	int i;
 
 	/* normalize supervisory level */
@@ -272,108 +218,6 @@ static void super_receive_bit(r2000_t *r2000, int bit, double level, double qual
 	r2000_receive_super(r2000, (r2000->super_rx_word >> 1) & 0x7f, quality, level);
 }
 
-//#define DEBUG_FILTER
-//#define DEBUG_QUALITY
-
-/* demodulate supervisory signal
- * filter one chunk, that is 2ms long (1/10th of a bit) */
-static inline void super_decode_step(r2000_t *r2000, int pos)
-{
-	double level, result[2], softbit, quality;
-	int max;
-	sample_t *spl;
-	int bit;
-
-	max = r2000->super_samples_per_window;
-	spl = r2000->super_filter_spl;
-
-	level = audio_level(spl, max);
-
-	audio_goertzel(r2000->super_goertzel, spl, max, pos, result, 2);
-
-	/* calculate soft bit from both frequencies */
-	softbit = (result[1] / level - result[0] / level + 1.0) / 2.0;
-//	/* scale it, since both filters overlap by some percent */
-//#define MIN_QUALITY 0.08
-//	softbit = (softbit - MIN_QUALITY) / (0.850 - MIN_QUALITY - MIN_QUALITY);
-	if (softbit > 1)
-		softbit = 1;
-	if (softbit < 0)
-		softbit = 0;
-#ifdef DEBUG_FILTER
-	printf("|%s", debug_amplitude(result[0]/level));
-	printf("|%s| low=%.3f high=%.3f level=%d\n", debug_amplitude(result[1]/level), result[0]/level, result[1]/level, (int)level);
-#endif
-	if (softbit > 0.5)
-		bit = 1;
-	else
-		bit = 0;
-
-//	quality = result[bit] / level;
-	if (softbit > 0.5)
-		quality = softbit * 2.0 - 1.0;
-	else
-		quality = 1.0 - softbit * 2.0;
-
-	/* scale quality, because filters overlap */
-	quality /= 0.80;
-
-	if (r2000->super_filter_bit != bit) {
-#ifdef DEBUG_FILTER
-		puts("bit change");
-#endif
-		r2000->super_filter_bit = bit;
-#if 0
-		/* If we have a bit change, move sample counter towards one half bit duration.
-		 * We may have noise, so the bit change may be wrong or not at the correct place.
-		 * This can cause bit slips.
-		 * Therefore we change the sample counter only slightly, so bit slips may not
-		 * happen so quickly.
-		 */
-		if (r2000->super_filter_sample < 5)
-			r2000->super_filter_sample++;
-		if (r2000->super_filter_sample > 5)
-			r2000->super_filter_sample--;
-#else
-		/* directly center the sample position, because we don't have any sync sequence */ 
-		r2000->super_filter_sample = 5;
-#endif
-
-	} else if (--r2000->super_filter_sample == 0) {
-		/* if sample counter bit reaches 0, we reset sample counter to one bit duration */
-#ifdef DEBUG_QUALITY
-		printf("|%s| quality=%.2f ", debug_amplitude(softbit), quality);
-		printf("|%s|\n", debug_amplitude(quality);
-#endif
-		/* adjust level, so we get peak of sine curve */
-		super_receive_bit(r2000, bit, level / 0.63662, quality);
-		r2000->super_filter_sample = 10;
-	}
-}
-
-/* get audio chunk out of received stream */
-void super_receive(r2000_t *r2000, sample_t *samples, int length)
-{
-	sample_t *spl;
-	int max, pos, step;
-	int i;
-	/* write received samples to decode buffer */
-	max = r2000->super_samples_per_window;
-	pos = r2000->super_filter_pos;
-	step = r2000->super_filter_step;
-	spl = r2000->super_filter_spl;
-	for (i = 0; i < length; i++) {
-		spl[pos++] = samples[i];
-		if (pos == max)
-			pos = 0;
-		/* if filter step has been reched */
-		if (!(pos % step)) {
-			super_decode_step(r2000, pos);
-		}
-	}
-	r2000->super_filter_pos = pos;
-}
-
 /* Process received audio stream from radio unit. */
 void sender_receive(sender_t *sender, sample_t *samples, int length)
 {
@@ -390,14 +234,14 @@ void sender_receive(sender_t *sender, sample_t *samples, int length)
 	if (r2000->dsp_mode == DSP_MODE_AUDIO_TX
 	 || r2000->dsp_mode == DSP_MODE_AUDIO_TX_RX
 	 || r2000->sender.loopback)
-		super_receive(r2000, samples, length);
+		fsk_receive(&r2000->super_fsk, samples, length);
 
 	/* do de-emphasis */
 	if (r2000->de_emphasis)
 		de_emphasis(&r2000->estate, samples, length);
 
 	/* fsk signal */
-	ffsk_receive(&r2000->ffsk, samples, length);
+	fsk_receive(&r2000->fsk, samples, length);
 
 	/* we must have audio mode for both ways and a call */
 	if (r2000->dsp_mode == DSP_MODE_AUDIO_TX_RX
@@ -424,125 +268,43 @@ void sender_receive(sender_t *sender, sample_t *samples, int length)
 		r2000->sender.rxbuf_pos = 0;
 }
 
-static int fsk_frame(r2000_t *r2000, sample_t *samples, int length)
+static int fsk_send_bit(void *inst)
 {
+	r2000_t *r2000 = (r2000_t *)inst;
 	const char *frame;
-	sample_t *spl;
-	int i;
-	int count, max;
 
-next_frame:
-	if (!r2000->frame_length) {
-		/* request frame */
+	if (!r2000->tx_frame_length || r2000->tx_frame_pos == r2000->tx_frame_length) {
 		frame = r2000_get_frame(r2000);
 		if (!frame) {
+			r2000->tx_frame_length = 0;
 			PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Stop sending frames.\n");
-			return length;
-		}
-		/* render frame */
-		r2000->frame_length = ffsk_render_frame(&r2000->ffsk, frame, 208, r2000->frame_spl);
-		r2000->frame_pos = 0;
-		if (r2000->frame_length > r2000->frame_size) {
-			PDEBUG_CHAN(DDSP, DEBUG_ERROR, "Frame exceeds buffer, please fix!\n");
-			abort();
+			return -1;
 		}
+		memcpy(r2000->tx_frame, frame, 208);
+		r2000->tx_frame_length = 208;
+		r2000->tx_frame_pos = 0;
 	}
 
-	/* send audio from frame */
-	max = r2000->frame_length;
-	count = max - r2000->frame_pos;
-	if (count > length)
-		count = length;
-	spl = r2000->frame_spl + r2000->frame_pos;
-	for (i = 0; i < count; i++) {
-		*samples++ = *spl++;
-	}
-	length -= count;
-	r2000->frame_pos += count;
-	/* check for end of telegramm */
-	if (r2000->frame_pos == max) {
-		r2000->frame_length = 0;
-		/* we need more ? */
-		if (length)
-			goto next_frame;
-	}
-
-	return length;
+	return r2000->tx_frame[r2000->tx_frame_pos++];
 }
 
-static int super_render_frame(r2000_t *r2000, uint32_t word, sample_t *sample)
+static int super_send_bit(void *inst)
 {
-	double phaseshift, phase, bittime, bitpos;
-	int count = 0, i;
-
-	phase = r2000->super_phase65536;
-	bittime = r2000->super_bittime;
-	bitpos = r2000->super_bitpos;
-	for (i = 0; i < 20; i++) {
-		phaseshift = r2000->super_phaseshift65536[(word >> 19) & 1];
-		do {
-			*sample++ = super_sine[(uint16_t)phase];
-			count++;
-			phase += phaseshift;
-			if (phase >= 65536.0)
-				phase -= 65536.0;
-			bitpos += bittime;
-		} while (bitpos < 1.0);
-		bitpos -= 1.0;
-		word <<= 1;
-	}
-	r2000->super_phase65536 = phase;
-	bitpos = r2000->super_bitpos;
-
-	/* return number of samples created for frame */
-	return count;
-}
-
-static int super_frame(r2000_t *r2000, sample_t *samples, int length)
-{
-	sample_t *spl;
-	int i;
-	int count, max;
-
-next_frame:
-	if (!r2000->super_length) {
-		/* render supervisory rame */
-		PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "render word 0x%05x\n", r2000->super_tx_word);
-		r2000->super_length = super_render_frame(r2000, r2000->super_tx_word, r2000->super_spl);
-		r2000->super_pos = 0;
-		if (r2000->super_length > r2000->super_size) {
-			PDEBUG_CHAN(DDSP, DEBUG_ERROR, "Frame exceeds buffer, please fix!\n");
-			abort();
-		}
-	}
+	r2000_t *r2000 = (r2000_t *)inst;
 
-	/* send audio from frame */
-	max = r2000->super_length;
-	count = max - r2000->super_pos;
-	if (count > length)
-		count = length;
-	spl = r2000->super_spl + r2000->super_pos;
-	for (i = 0; i < count; i++) {
-		*samples++ += *spl++;
-	}
-	length -= count;
-	r2000->super_pos += count;
-	/* check for end of telegramm */
-	if (r2000->super_pos == max) {
-		r2000->super_length = 0;
-		/* we need more ? */
-		if (length)
-			goto next_frame;
+	if (!r2000->super_tx_word_length || r2000->super_tx_word_pos == r2000->super_tx_word_length) {
+		r2000->super_tx_word_length = 20;
+		r2000->super_tx_word_pos = 0;
 	}
 
-	return length;
+	return (r2000->super_tx_word >> (r2000->super_tx_word_length - (++r2000->super_tx_word_pos))) & 1;
 }
 
 /* Provide stream of audio toward radio unit */
 void sender_send(sender_t *sender, sample_t *samples, int length)
 {
 	r2000_t *r2000 = (r2000_t *) sender;
-	int len;
+	int count;
 
 again:
 	switch (r2000->dsp_mode) {
@@ -555,20 +317,25 @@ again:
 		/* do pre-emphasis */
 		if (r2000->pre_emphasis)
 			pre_emphasis(&r2000->estate, samples, length);
-		super_frame(r2000, samples, length);
+		/* add supervisory to sample buffer */
+		fsk_send(&r2000->super_fsk, samples, length, 1);
 		break;
 	case DSP_MODE_FRAME:
 		/* Encode frame into audio stream. If frames have
 		 * stopped, process again for rest of stream. */
-		len = fsk_frame(r2000, samples, length);
+		count = fsk_send(&r2000->fsk, samples, length, 0);
 		/* do pre-emphasis */
 		if (r2000->pre_emphasis)
-			pre_emphasis(&r2000->estate, samples, length - len);
-		if (len) {
-			samples += length - len;
-			length = len;
-			goto again;
+			pre_emphasis(&r2000->estate, samples, count);
+		/* special case: add supervisory signal to frame at loop test */
+		if (r2000->sender.loopback) {
+			/* add supervisory to sample buffer */
+			fsk_send(&r2000->super_fsk, samples, count, 1);
 		}
+		samples += count;
+		length -= count;
+		if (length)
+			goto again;
 		break;
 	}
 }
@@ -596,11 +363,13 @@ void r2000_set_dsp_mode(r2000_t *r2000, enum dsp_mode mode, int super)
 {
 	/* reset telegramm */
 	if (mode == DSP_MODE_FRAME && r2000->dsp_mode != mode) {
-		r2000->frame_length = 0;
+		r2000->tx_frame_length = 0;
+		fsk_tx_reset(&r2000->fsk);
 	}
 	if ((mode == DSP_MODE_AUDIO_TX || mode == DSP_MODE_AUDIO_TX_RX)
 	 && (r2000->dsp_mode != DSP_MODE_AUDIO_TX && r2000->dsp_mode != DSP_MODE_AUDIO_TX_RX)) {
-		r2000->super_length = 0;
+		r2000->super_tx_word_length = 0;
+		fsk_tx_reset(&r2000->super_fsk);
 	}
 
 	if (super >= 0) {
@@ -615,4 +384,3 @@ void r2000_set_dsp_mode(r2000_t *r2000, enum dsp_mode mode, int super)
 	r2000->dsp_mode = mode;
 }
 
-#warning fixme: high pass filter on tx side to prevent desturbance of supervisory signal
diff --git a/src/r2000/r2000.h b/src/r2000/r2000.h
index dbafcf2..6eb0bc5 100644
--- a/src/r2000/r2000.h
+++ b/src/r2000/r2000.h
@@ -1,7 +1,7 @@
 #include "../common/compandor.h"
 #include "../common/sender.h"
 #include "../common/call.h"
-#include "../common/ffsk.h"
+#include "../common/fsk.h"
 
 enum dsp_mode {
 	DSP_MODE_OFF,		/* no transmission */
@@ -78,7 +78,10 @@ typedef struct r2000 {
 
 	/* dsp states */
 	enum dsp_mode		dsp_mode;		/* current mode: audio, durable tone 0 or 1, paging */
-	ffsk_t			ffsk;			/* ffsk processing */
+	fsk_t			fsk;			/* fsk processing */
+	char			tx_frame[208];		/* carries bits of one frame to transmit */
+	int			tx_frame_length;
+	int			tx_frame_pos;
 	uint16_t		rx_sync;		/* shift register to detect sync */
 	int			rx_in_sync;		/* if we are in sync and receive bits */
 	int			rx_mute;		/* mute count down after sync */
@@ -87,33 +90,19 @@ typedef struct r2000 {
 	int			rx_count;		/* next bit to receive */
 	double			rx_level[256];		/* level infos */
 	double			rx_quality[256];	/* quality infos */
-	sample_t		*frame_spl;		/* samples to store a complete rendered frame */
-	int			frame_size;		/* total size of sample buffer */
-	int			frame_length;		/* current length of data in sample buffer */
-	int			frame_pos;		/* current sample position in frame_spl */
 	uint64_t		rx_bits_count;		/* sample counter */
 	uint64_t		rx_bits_count_current;	/* sample counter of current frame */
 	uint64_t		rx_bits_count_last;	/* sample counter of last frame */
 
 	/* supervisory dsp states */
-	goertzel_t		super_goertzel[2];	/* filter for fsk decoding */
-	int			super_samples_per_window;/* how many samples to analyze in one window */
-	sample_t		*super_filter_spl;	/* array with samples_per_bit */
-	int			super_filter_pos;	/* current sample position in filter_spl */
-	int			super_filter_step;	/* number of samples for each analyzation */
-	int			super_filter_bit;	/* last bit, so we detect a bit change */
-	int			super_filter_sample;	/* count until it is time to sample bit */
-	sample_t		*super_spl;		/* samples to store a complete rendered frame */
-	int			super_size;		/* total size of sample buffer */
-	int			super_length;		/* current length of data in sample buffer */
-	int			super_pos;		/* current sample position in frame_spl */
-	double			super_phaseshift65536[2];/* how much the phase of sine wave changes per sample */
-	double			super_phase65536;	/* current phase */
+	fsk_t			super_fsk;		/* fsk processing */
+	uint32_t		super_tx_word;		/* supervisory info to transmit */
+	int			super_tx_word_length;
+	int			super_tx_word_pos;
 	uint32_t		super_rx_word;		/* shift register for received supervisory info */
 	double			super_rx_level[20];	/* level infos */
 	double			super_rx_quality[20];	/* quality infos */
 	int			super_rx_index;		/* index for level and quality buffer */
-	uint32_t		super_tx_word;		/* supervisory info to transmit */
 double super_bittime;
 double super_bitpos;
 
diff --git a/src/test/test_dms.c b/src/test/test_dms.c
index c71f87c..c51c904 100644
--- a/src/test/test_dms.c
+++ b/src/test/test_dms.c
@@ -38,8 +38,7 @@ static const uint8_t test_null[][8] = {
 	{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 1 },
 };
 
-static char current_bits[1024], ack_bits[77];
-int current_bit_count;
+static char ack_bits[77];
 
 void dms_receive(nmt_t *nmt, const uint8_t *data, int length, int eight_bits)
 {
@@ -55,15 +54,6 @@ void dms_all_sent(nmt_t *nmt)
 {
 }
 
-/* receive bits from DMS */
-void test_dms_frame(const char *frame, int length)
-{
-	printf("(getting %d bits from DMS layer)\n", length);
-
-	memcpy(current_bits, frame, length);
-	current_bit_count = length;
-}
-
 nmt_t *alloc_nmt(void)
 {
 	nmt_t *nmt;
@@ -71,11 +61,6 @@ nmt_t *alloc_nmt(void)
 	nmt = calloc(sizeof(*nmt), 1);
 	nmt->sender.samplerate = 40 * 1200;
 	dms_init_sender(nmt);
-	ffsk_global_init(1.0);
-	ffsk_init(&nmt->ffsk, nmt, NULL, 1, nmt->sender.samplerate);
-	nmt->dms.frame_size = nmt->ffsk.samples_per_bit * 127 + 10;
-	nmt->dms.frame_spl = calloc(nmt->dms.frame_size, sizeof(nmt->dms.frame_spl[0]));
-
 	dms_reset(nmt);
 
 	return nmt;
@@ -84,7 +69,6 @@ nmt_t *alloc_nmt(void)
 void free_nmt(nmt_t *nmt)
 {
 	dms_cleanup_sender(nmt);
-	free(nmt->dms.frame_spl);
 	free(nmt);
 }
 
@@ -93,7 +77,6 @@ int main(void)
 	nmt_t *nmt;
 	dms_t *dms;
 	int i, j;
-	sample_t sample = 0;
 
 	debuglevel = DEBUG_DEBUG;
 	dms_allow_loopback = 1;
@@ -105,96 +88,96 @@ int main(void)
 
 	check_sequence = testsequence;
 	dms_send(nmt, (uint8_t *)testsequence, strlen(testsequence) + 1, 1);
-	assert(dms->frame_valid && current_bit_count == 127, "Expecting frame in queue with 127 bits");
+	assert(dms->tx_frame_valid && dms->tx_frame_length == 127, "Expecting frame in queue with 127 bits");
 	assert(dms->state.n_s == 1, "Expecting next frame to have sequence number 1");
 
 	printf("Pretend that frame has been sent\n");
-	dms->frame_length = 0;
-	fsk_dms_frame(nmt, &sample, 1);
+	dms->tx_frame_valid = 0;
+	trigger_frame_transmission(nmt);
 
-	assert(dms->frame_valid && current_bit_count == 127, "Expecting frame in queue with 127 bits");
+	assert(dms->tx_frame_valid && dms->tx_frame_length == 127, "Expecting frame in queue with 127 bits");
 	assert(dms->state.n_s == 0, "Expecting next frame to have sequence number 0 (cycles due to unacked RAND)");
 
 	printf("Pretend that frame has been sent\n");
-	dms->frame_length = 0;
-	fsk_dms_frame(nmt, &sample, 1);
+	dms->tx_frame_valid = 0;
+	trigger_frame_transmission(nmt);
 
-	assert(dms->frame_valid && current_bit_count == 127, "Expecting frame in queue with 127 bits");
+	assert(dms->tx_frame_valid && dms->tx_frame_length == 127, "Expecting frame in queue with 127 bits");
 	assert(dms->state.n_s == 1, "Expecting next frame to have sequence number 1");
 
 	/* send back ID */
 
 	printf("Sending back ID\n");
-	for (i = 0; i < current_bit_count; i++)
-		fsk_receive_bit_dms(nmt, current_bits[i] & 1, 1.0, 1.0);
+	for (i = 0; i < dms->tx_frame_length; i++)
+		fsk_receive_bit_dms(nmt, dms->tx_frame[i] & 1, 1.0, 1.0);
 
 	printf("Pretend that frame has been sent\n");
-	dms->frame_length = 0;
-	fsk_dms_frame(nmt, &sample, 1);
+	dms->tx_frame_valid = 0;
+	trigger_frame_transmission(nmt);
 
-	assert(dms->frame_valid && current_bit_count == 77, "Expecting frame in queue with 77 bits");
+	assert(dms->tx_frame_valid && dms->tx_frame_length == 77, "Expecting frame in queue with 77 bits");
 
 	printf("Pretend that frame has been sent\n");
-	dms->frame_length = 0;
-	fsk_dms_frame(nmt, &sample, 1);
+	dms->tx_frame_valid = 0;
+	trigger_frame_transmission(nmt);
 
-	assert(dms->frame_valid && current_bit_count == 127, "Expecting frame in queue with 127 bits");
+	assert(dms->tx_frame_valid && dms->tx_frame_length == 127, "Expecting frame in queue with 127 bits");
 	assert(dms->state.n_s == 0, "Expecting next frame to have sequence number 0");
 
 	/* send back RAND */
 	printf("Sending back RAND\n");
-	for (i = 0; i < current_bit_count; i++)
-		fsk_receive_bit_dms(nmt, current_bits[i] & 1, 1.0, 1.0);
+	for (i = 0; i < dms->tx_frame_length; i++)
+		fsk_receive_bit_dms(nmt, dms->tx_frame[i] & 1, 1.0, 1.0);
 
 	printf("Pretend that frame has been sent\n");
-	dms->frame_length = 0;
-	fsk_dms_frame(nmt, &sample, 1);
+	dms->tx_frame_valid = 0;
+	trigger_frame_transmission(nmt);
 
-	assert(dms->frame_valid && current_bit_count == 77, "Expecting frame in queue with 77 bits");
-	memcpy(ack_bits, current_bits, 77);
+	assert(dms->tx_frame_valid && dms->tx_frame_length == 77, "Expecting frame in queue with 77 bits");
+	memcpy(ack_bits, dms->tx_frame, 77);
 
 	/* check if DT frame will be sent now */
 
 	printf("Pretend that frame has been sent\n");
-	dms->frame_length = 0;
-	fsk_dms_frame(nmt, &sample, 1);
+	dms->tx_frame_valid = 0;
+	trigger_frame_transmission(nmt);
 
-	assert(dms->frame_valid && current_bit_count == 127, "Expecting frame in queue with 127 bits");
+	assert(dms->tx_frame_valid && dms->tx_frame_length == 127, "Expecting frame in queue with 127 bits");
 	assert(dms->state.n_s == 1, "Expecting next frame to have sequence number 1");
 
 	printf("Pretend that frame has been sent\n");
-	dms->frame_length = 0;
-	fsk_dms_frame(nmt, &sample, 1);
+	dms->tx_frame_valid = 0;
+	trigger_frame_transmission(nmt);
 
-	assert(dms->frame_valid && current_bit_count == 127, "Expecting frame in queue with 127 bits");
+	assert(dms->tx_frame_valid && dms->tx_frame_length == 127, "Expecting frame in queue with 127 bits");
 	assert(dms->state.n_s == 2, "Expecting next frame to have sequence number 2");
 
 	printf("Pretend that frame has been sent\n");
-	dms->frame_length = 0;
-	fsk_dms_frame(nmt, &sample, 1);
+	dms->tx_frame_valid = 0;
+	trigger_frame_transmission(nmt);
 
-	assert(dms->frame_valid && current_bit_count == 127, "Expecting frame in queue with 127 bits");
+	assert(dms->tx_frame_valid && dms->tx_frame_length == 127, "Expecting frame in queue with 127 bits");
 	assert(dms->state.n_s == 3, "Expecting next frame to have sequence number 3");
 
 	printf("Pretend that frame has been sent\n");
-	dms->frame_length = 0;
-	fsk_dms_frame(nmt, &sample, 1);
+	dms->tx_frame_valid = 0;
+	trigger_frame_transmission(nmt);
 
-	assert(dms->frame_valid && current_bit_count == 127, "Expecting frame in queue with 127 bits");
+	assert(dms->tx_frame_valid && dms->tx_frame_length == 127, "Expecting frame in queue with 127 bits");
 	assert(dms->state.n_s == 0, "Expecting next frame to have sequence number 0");
 
 	/* send back ack bitss */
 	printf("Sending back RR(2)\n");
-	memcpy(current_bits, ack_bits, 77);
-	current_bit_count = 77;
-	for (i = 0; i < current_bit_count; i++)
-		fsk_receive_bit_dms(nmt, current_bits[i] & 1, 1.0, 1.0);
+	memcpy(dms->tx_frame, ack_bits, 77);
+	dms->tx_frame_length = 77;
+	for (i = 0; i < dms->tx_frame_length; i++)
+		fsk_receive_bit_dms(nmt, dms->tx_frame[i] & 1, 1.0, 1.0);
 
 	printf("Pretend that frame has been sent\n");
-	dms->frame_length = 0;
-	fsk_dms_frame(nmt, &sample, 1);
+	dms->tx_frame_valid = 0;
+	trigger_frame_transmission(nmt);
 
-	assert(dms->frame_valid && current_bit_count == 127, "Expecting frame in queue with 127 bits");
+	assert(dms->tx_frame_valid && dms->tx_frame_length == 127, "Expecting frame in queue with 127 bits");
 	assert(dms->state.n_s == 3, "Expecting next frame to have sequence number 0");
 
 	ok();
@@ -203,11 +186,11 @@ int main(void)
 	printf("pipe through all data\n");
 	while (check_sequence[0])  {
 		printf("Sending back last received frame\n");
-		for (i = 0; i < current_bit_count; i++)
-			fsk_receive_bit_dms(nmt, current_bits[i] & 1, 1.0, 1.0);
+		for (i = 0; i < dms->tx_frame_length; i++)
+			fsk_receive_bit_dms(nmt, dms->tx_frame[i] & 1, 1.0, 1.0);
 		printf("Pretend that frame has been sent\n");
-		dms->frame_length = 0;
-		fsk_dms_frame(nmt, &sample, 1);
+		dms->tx_frame_valid = 0;
+		trigger_frame_transmission(nmt);
 	}
 
 	ok();
@@ -228,12 +211,12 @@ int main(void)
 	while (check_sequence[0])  {
 		if ((random() & 1)) {
 			printf("Sending back last received frame\n");
-			for (i = 0; i < current_bit_count; i++)
-				fsk_receive_bit_dms(nmt, current_bits[i] & 1, 1.0, 1.0);
+			for (i = 0; i < dms->tx_frame_length; i++)
+				fsk_receive_bit_dms(nmt, dms->tx_frame[i] & 1, 1.0, 1.0);
 		}
 		printf("Pretend that frame has been sent\n");
-		dms->frame_length = 0;
-		fsk_dms_frame(nmt, &sample, 1);
+		dms->tx_frame_valid = 0;
+		trigger_frame_transmission(nmt);
 	}
 
 	ok();
@@ -244,19 +227,19 @@ int main(void)
 
 	/* test zero termination */
 	for (j = 0; j < 4; j++) {
-		current_bit_count = 0;
+		dms->tx_frame_length = 0;
 		printf("zero-termination test: %d bytes in frame\n", test_null[j][7]);
 		dms_send(nmt, test_null[j], test_null[j][7], 1);
 		check_sequence = (char *)test_null[j];
 
-		while (current_bit_count) {
+		while (dms->tx_frame_length) {
 			printf("Sending back last received frame\n");
-			for (i = 0; i < current_bit_count; i++)
-				fsk_receive_bit_dms(nmt, current_bits[i] & 1, 1.0, 1.0);
-			current_bit_count = 0;
+			for (i = 0; i < dms->tx_frame_length; i++)
+				fsk_receive_bit_dms(nmt, dms->tx_frame[i] & 1, 1.0, 1.0);
+			dms->tx_frame_length = 0;
 			printf("Pretend that frame has been sent\n");
-			dms->frame_length = 0;
-			fsk_dms_frame(nmt, &sample, 1);
+			dms->tx_frame_valid = 0;
+			trigger_frame_transmission(nmt);
 		}
 		assert(check_length == test_null[j][7], "Expecting received length to match transmitted length");
 	}
diff --git a/src/test/test_performance.c b/src/test/test_performance.c
index 3f9cb40..577fc05 100644
--- a/src/test/test_performance.c
+++ b/src/test/test_performance.c
@@ -29,7 +29,7 @@ int tot_samples;
 
 
 #define SAMPLES 1000
-sample_t samples[SAMPLES];
+sample_t samples[SAMPLES], I[SAMPLES], Q[SAMPLES];
 float buff[SAMPLES * 2];
 fm_mod_t mod;
 fm_demod_t demod;
@@ -39,12 +39,12 @@ int main(void)
 {
 	fm_mod_init(&mod, 50000, 0, 0.333);
 	T_START()
-	fm_modulate(&mod, samples, SAMPLES, buff);
+	fm_modulate_complex(&mod, samples, SAMPLES, buff);
 	T_STOP("FM modulate", SAMPLES)
 
 	fm_demod_init(&demod, 50000, 0, 10000.0);
 	T_START()
-	fm_demodulate(&demod, samples, SAMPLES, buff);
+	fm_demodulate_complex(&demod, samples, SAMPLES, buff, I, Q);
 	T_STOP("FM demodulate", SAMPLES)
 
 	iir_lowpass_init(&lp, 10000.0 / 2.0, 50000, 1);