Add libraries from Osmocom-Analog

5 files changed, 438 insertions, 0 deletions

diff --git a/src/libdtmf/Makefile.am b/src/libdtmf/Makefile.am
new file mode 100644
index 0000000..f1df569
--- /dev/null
+++ b/src/libdtmf/Makefile.am
@@ -0,0 +1,7 @@
+AM_CPPFLAGS = -Wall -Wextra -g $(all_includes)
+
+noinst_LIBRARIES = libdtmf.a
+
+libdtmf_a_SOURCES = \
+	dtmf_encode.c \
+	dtmf_decode.c
diff --git a/src/libdtmf/dtmf_decode.c b/src/libdtmf/dtmf_decode.c
new file mode 100644
index 0000000..740d504
--- /dev/null
+++ b/src/libdtmf/dtmf_decode.c
@@ -0,0 +1,259 @@
+/* DTMF coder
+ *
+ * (C) 2016 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 <string.h>
+#include <math.h>
+#include "../libsample/sample.h"
+#include "dtmf_decode.h"
+
+//#define DEBUG
+
+#define level2db(level)	(20 * log10(level))
+#define db2level(db)	pow(10, (double)db / 20.0)
+
+#define DTMF_LOW_1	697.0
+#define DTMF_LOW_2	770.0
+#define DTMF_LOW_3	852.0
+#define DTMF_LOW_4	941.0
+#define DTMF_HIGH_1	1209.0
+#define DTMF_HIGH_2	1336.0
+#define DTMF_HIGH_3	1477.0
+#define DTMF_HIGH_4	1633.0
+
+static const char dtmf_digit[] = "     123A456B789C*0#D";
+
+#ifdef DEBUG
+const char *_debug_amplitude(double level)
+{
+        static char text[42];
+
+	strcpy(text, "                    :                    ");
+	if (level > 1.0)
+		level = 1.0;
+	if (level < -1.0)
+		level = -1.0;
+	text[20 + (int)(level * 20)] = '*';
+
+	return text;
+}
+#endif
+
+int dtmf_decode_init(dtmf_dec_t *dtmf, void *priv, void (*recv_digit)(void *priv, char digit, dtmf_meas_t *meas), int samplerate, double max_amplitude, double min_amplitude)
+{
+	int rc;
+
+	memset(dtmf, 0, sizeof(*dtmf));
+	dtmf->priv = priv;
+	dtmf->recv_digit = recv_digit;
+	dtmf->samplerate = samplerate;
+	dtmf->freq_margin = 1.03; /* 1.8 .. 3.5 % */
+	dtmf->max_amplitude = max_amplitude;
+	dtmf->min_amplitude = min_amplitude;
+	dtmf->forward_twist = db2level(4.0);
+	dtmf->reverse_twist = db2level(8.0);
+	dtmf->time_detect = (int)(0.025 * (double)samplerate);
+	dtmf->time_meas = (int)(0.015 * (double)samplerate);
+	dtmf->time_pause = (int)(0.010 * (double)samplerate);
+
+	/* init fm demodulator */
+	rc = fm_demod_init(&dtmf->demod_low, (double)samplerate, (DTMF_LOW_1 + DTMF_LOW_4) / 2.0, DTMF_LOW_4 - DTMF_LOW_1);
+	if (rc < 0)
+		goto error;
+	rc = fm_demod_init(&dtmf->demod_high, (double)samplerate, (DTMF_HIGH_1 + DTMF_HIGH_4) / 2.0, DTMF_HIGH_4 - DTMF_HIGH_1);
+	if (rc < 0)
+		goto error;
+
+	/* use fourth order (2 iter) filter, since it is as fast as second order (1 iter) filter */
+	iir_lowpass_init(&dtmf->freq_lp[0], 100.0, samplerate, 2);
+	iir_lowpass_init(&dtmf->freq_lp[1], 100.0, samplerate, 2);
+
+	return 0;
+
+error:
+	dtmf_decode_exit(dtmf);
+	return rc;
+}
+
+void dtmf_decode_exit(dtmf_dec_t *dtmf)
+{
+	fm_demod_exit(&dtmf->demod_low);
+	fm_demod_exit(&dtmf->demod_high);
+}
+
+void dtmf_decode_filter(dtmf_dec_t *dtmf, sample_t *samples, int length, sample_t *frequency_low, sample_t *frequency_high, sample_t *amplitude_low, sample_t *amplitude_high)
+{
+	sample_t I_low[length], Q_low[length];
+	sample_t I_high[length], Q_high[length];
+	int i;
+
+	fm_demodulate_real(&dtmf->demod_low, frequency_low, length, samples, I_low, Q_low);
+	fm_demodulate_real(&dtmf->demod_high, frequency_high, length, samples, I_high, Q_high);
+	/* peak amplitude is the length of I/Q vector
+	 * since we filter out the unwanted modulation product, the vector is only half of length */
+	for (i = 0; i < length; i++) {
+		amplitude_low[i] = sqrt(I_low[i] * I_low[i] + Q_low[i] * Q_low[i]) * 2.0;
+		amplitude_high[i] = sqrt(I_high[i] * I_high[i] + Q_high[i] * Q_high[i]) * 2.0;
+	}
+	iir_process(&dtmf->freq_lp[0], frequency_low, length);
+	iir_process(&dtmf->freq_lp[1], frequency_high, length);
+}
+void dtmf_decode(dtmf_dec_t *dtmf, sample_t *samples, int length)
+{
+	sample_t frequency_low[length], amplitude_low[length];
+	sample_t frequency_high[length], amplitude_high[length];
+	double margin, min_amplitude, max_amplitude, forward_twist, reverse_twist, f1, f2;
+	int time_detect, time_meas, time_pause;
+	int low = 0, high = 0;
+	char detected, digit;
+	int count;
+	int amplitude_ok, twist_ok;
+	int i;
+
+	margin = dtmf->freq_margin;
+	min_amplitude = dtmf->min_amplitude;
+	max_amplitude = dtmf->max_amplitude;
+	forward_twist = dtmf->forward_twist;
+	reverse_twist = dtmf->reverse_twist;
+	time_detect = dtmf->time_detect;
+	time_meas = dtmf->time_meas;
+	time_pause = dtmf->time_pause;
+	detected = dtmf->detected;
+	count = dtmf->count;
+
+	/* FM/AM demod */
+	dtmf_decode_filter(dtmf, samples, length, frequency_low, frequency_high, amplitude_low, amplitude_high);
+
+	for (i = 0; i < length; i++) {
+#ifdef DEBUG
+//		printf("%s %.5f\n", _debug_amplitude(samples[i]/2.0), samples[i]/2.0);
+#endif
+		/* get frequency of low frequencies, correct amplitude drop at cutoff point */
+		f1 = frequency_low[i] + (DTMF_LOW_1 + DTMF_LOW_4) / 2.0;
+		if (f1 >= DTMF_LOW_1 / margin && f1 <= DTMF_LOW_1 * margin) {
+			/* cutoff point */
+			amplitude_low[i] /= 0.7071;
+			low = 1;
+			f1 -= DTMF_LOW_1;
+		} else
+		if (f1 >= DTMF_LOW_2 / margin && f1 <= DTMF_LOW_2 * margin) {
+			amplitude_low[i] /= 1.0734;
+			low = 2;
+			f1 -= DTMF_LOW_2;
+		} else
+		if (f1 >= DTMF_LOW_3 / margin && f1 <= DTMF_LOW_3 * margin) {
+			amplitude_low[i] /= 1.0389;
+			low = 3;
+			f1 -= DTMF_LOW_3;
+		} else
+		if (f1 >= DTMF_LOW_4 / margin && f1 <= DTMF_LOW_4 * margin) {
+			/* cutoff point */
+			amplitude_low[i] /= 0.7071;
+			low = 4;
+			f1 -= DTMF_LOW_4;
+		} else
+			low = 0;
+		/* get frequency of high frequencies, correct amplitude drop at cutoff point */
+		f2 = frequency_high[i] + (DTMF_HIGH_1 + DTMF_HIGH_4) / 2.0;
+		if (f2 >= DTMF_HIGH_1 / margin && f2 <= DTMF_HIGH_1 * margin) {
+			/* cutoff point */
+			amplitude_high[i] /= 0.7071;
+			high = 1;
+			f2 -= DTMF_HIGH_1;
+		} else
+		if (f2 >= DTMF_HIGH_2 / margin && f2 <= DTMF_HIGH_2 * margin) {
+			amplitude_high[i] /= 1.0731;
+			high = 2;
+			f2 -= DTMF_HIGH_2;
+		} else
+		if (f2 >= DTMF_HIGH_3 / margin && f2 <= DTMF_HIGH_3 * margin) {
+			amplitude_high[i] /= 1.0372;
+			high = 3;
+			f2 -= DTMF_HIGH_3;
+		} else
+		if (f2 >= DTMF_HIGH_4 / margin && f2 <= DTMF_HIGH_4 * margin) {
+			/* cutoff point */
+			amplitude_high[i] /= 0.7071;
+			high = 4;
+			f2 -= DTMF_HIGH_4;
+		} else
+			high = 0;
+		digit = 0;
+		amplitude_ok = 0;
+		twist_ok = 0;
+		if (low && high) {
+			digit = dtmf_digit[low*4+high];
+			/* check for limits */
+			if (amplitude_low[i] <= max_amplitude && amplitude_low[i] >= min_amplitude && amplitude_high[i] <= max_amplitude && amplitude_high[i] >= min_amplitude) {
+				amplitude_ok = 1;
+#ifdef DEBUG
+				printf("%.1f %.1f (limits %.1f .. %.1f) %.1f\n", level2db(amplitude_low[i]), level2db(amplitude_high[i]), level2db(min_amplitude), level2db(max_amplitude), level2db(amplitude_high[i] / amplitude_low[i]));
+#endif
+				if (amplitude_high[i] / amplitude_low[i] <= forward_twist && amplitude_low[i] / amplitude_high[i] <= reverse_twist)
+					twist_ok = 1;
+			}
+		}
+
+		if (!detected) {
+			if (digit && amplitude_ok && twist_ok) {
+				if (count == 0) {
+					memset(&dtmf->meas, 0, sizeof(dtmf->meas));
+				}
+				if (count >= time_meas) {
+					dtmf->meas.frequency_low += f1;
+					dtmf->meas.frequency_high += f2;
+					dtmf->meas.amplitude_low += amplitude_low[i];
+					dtmf->meas.amplitude_high += amplitude_high[i];
+					dtmf->meas.count++;
+				}
+				count++;
+				if (count >= time_detect) {
+					detected = digit;
+					dtmf->meas.frequency_low /= dtmf->meas.count;
+					dtmf->meas.frequency_high /= dtmf->meas.count;
+					dtmf->meas.amplitude_low /= dtmf->meas.count;
+					dtmf->meas.amplitude_high /= dtmf->meas.count;
+					dtmf->meas.count = 1;
+					dtmf->recv_digit(dtmf->priv, digit, &dtmf->meas);
+				}
+			} else
+				count = 0;
+		} else {
+			if (!digit || digit != detected || !amplitude_ok || !twist_ok) {
+				count++;
+				if (count >= time_pause) {
+					detected = 0;
+#ifdef DEBUG
+					printf("lost!\n");
+#endif
+				}
+			} else
+				count = 0;
+		}
+#ifdef DEBUG
+		if (digit)
+			printf("DTMF tone='%c' diff frequency=%.1f %.1f amplitude=%.1f %.1f dB (%s) twist=%.1f dB (%s)\n", digit, f1, f2, level2db(amplitude_low[i]), level2db(amplitude_high[i]), (amplitude_ok) ? "OK" : "nok", level2db(amplitude_high[i] / amplitude_low[i]), (twist_ok) ? "OK" : "nok");
+#endif
+
+		dtmf->detected = detected;
+		dtmf->count = count;
+	}
+}
+
diff --git a/src/libdtmf/dtmf_decode.h b/src/libdtmf/dtmf_decode.h
new file mode 100644
index 0000000..7c5780f
--- /dev/null
+++ b/src/libdtmf/dtmf_decode.h
@@ -0,0 +1,35 @@
+#include "../libfm/fm.h"
+
+typedef struct ftmf_meas {
+	double		frequency_low;
+	double		frequency_high;
+	double		amplitude_low;
+	double		amplitude_high;
+	int		count;
+} dtmf_meas_t;
+
+typedef struct dtmf_dec {
+	void		*priv;
+	void		(*recv_digit)(void *priv, char digit, dtmf_meas_t *meas);
+	int		samplerate;		/* samplerate */
+	double		freq_margin;		/* +- limit of frequency deviation (percent) valid tone*/
+	double		min_amplitude;		/* minimum amplitude relative to 0 dBm */
+	double		max_amplitude;		/* maximum amplitude relative to 0 dBm */
+	double		forward_twist;		/* how much do higher frequencies are louder than lower frequencies */
+	double		reverse_twist;		/* how much do lower frequencies are louder than higher frequencies */
+	int		time_detect;
+	int		time_meas;
+	int		time_pause;
+	fm_demod_t	demod_low;		/* demodulator for low frequencies */
+	fm_demod_t	demod_high;		/* demodulator for high frequencies */
+	iir_filter_t	freq_lp[2];		/* low pass to filter the frequency result */
+	char		detected;		/* currently detected DTMF digit or 0 for no detection */
+	int		count;			/* counter to count detection or loss (pause) of signal */
+	dtmf_meas_t	meas;			/* measurements */
+} dtmf_dec_t;
+
+int dtmf_decode_init(dtmf_dec_t *dtmf, void *priv, void (*recv_digit)(void *priv, char digit, dtmf_meas_t *meas), int samplerate, double max_amplitude, double min_amplitude);
+void dtmf_decode_exit(dtmf_dec_t *dtmf);
+void dtmf_decode(dtmf_dec_t *dtmf, sample_t *samples, int length);
+void dtmf_decode_filter(dtmf_dec_t *dtmf, sample_t *samples, int length, sample_t *frequency_low, sample_t *frequency_high, sample_t *amplitude_low, sample_t *amplitude_high);
+
diff --git a/src/libdtmf/dtmf_encode.c b/src/libdtmf/dtmf_encode.c
new file mode 100644
index 0000000..53bd1b6
--- /dev/null
+++ b/src/libdtmf/dtmf_encode.c
@@ -0,0 +1,123 @@
+/* DTMF coder
+ *
+ * (C) 2016 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 <stdint.h>
+#include <string.h>
+#include <math.h>
+#include "../libsample/sample.h"
+#include "dtmf_encode.h"
+
+#define PI		M_PI
+
+#define PEAK_DTMF_LOW	0.2818	/* -11 dBm, relative to 0 dBm level */ 
+#define PEAK_DTMF_HIGH	0.3548	/* -9 dBm, relative to 0 dBm level */ 
+#define DTMF_DURATION	0.100	/* duration in seconds */
+
+static sample_t dsp_sine_dtmf_low[65536];
+static sample_t dsp_sine_dtmf_high[65536];
+
+void dtmf_encode_init(dtmf_enc_t *dtmf, int samplerate, double dBm_level)
+{
+	int i;
+
+	memset(dtmf, 0, sizeof(*dtmf));
+	dtmf->samplerate = samplerate;
+	dtmf->max = (int)((double)samplerate * DTMF_DURATION + 0.5);
+
+	// FIXME: do this globally and not per instance */
+	for (i = 0; i < 65536; i++) {
+		dsp_sine_dtmf_low[i] = sin((double)i / 65536.0 * 2.0 * PI) * PEAK_DTMF_LOW * dBm_level;
+		dsp_sine_dtmf_high[i] = sin((double)i / 65536.0 * 2.0 * PI) * PEAK_DTMF_HIGH * dBm_level;
+	}
+}
+
+/* set dtmf tone */
+void dtmf_encode_set_tone(dtmf_enc_t *dtmf, char tone)
+{
+	double f1, f2;
+
+	switch(tone) {
+		case '1': f1 = 697.0; f2 = 1209.0; break;
+		case '2': f1 = 697.0; f2 = 1336.0; break;
+		case '3': f1 = 697.0; f2 = 1477.0; break;
+	case'a':case 'A': f1 = 697.0; f2 = 1633.0; break;
+		case '4': f1 = 770.0; f2 = 1209.0; break;
+		case '5': f1 = 770.0; f2 = 1336.0; break;
+		case '6': f1 = 770.0; f2 = 1477.0; break;
+	case'b':case 'B': f1 = 770.0; f2 = 1633.0; break;
+		case '7': f1 = 852.0; f2 = 1209.0; break;
+		case '8': f1 = 852.0; f2 = 1336.0; break;
+		case '9': f1 = 852.0; f2 = 1477.0; break;
+	case'c':case 'C': f1 = 852.0; f2 = 1633.0; break;
+		case '*': f1 = 941.0; f2 = 1209.0; break;
+		case '0': f1 = 941.0; f2 = 1336.0; break;
+		case '#': f1 = 941.0; f2 = 1477.0; break;
+	case'd':case 'D': f1 = 941.0; f2 = 1633.0; break;
+	default:
+		dtmf->tone = 0;
+		return;
+	}
+	dtmf->tone = tone;
+	dtmf->pos = 0;
+	dtmf->phaseshift65536[0] = 65536.0 / ((double)dtmf->samplerate / f1);
+	dtmf->phaseshift65536[1] = 65536.0 / ((double)dtmf->samplerate / f2);
+}
+
+/* Generate audio stream from DTMF tone. Keep phase for next call of function. */
+void dtmf_encode(dtmf_enc_t *dtmf, sample_t *samples, int length)
+{
+        double *phaseshift, *phase;
+	int i, pos, max;
+
+	/* use silence, if no tone */
+	if (!dtmf->tone) {
+		memset(samples, 0, length * sizeof(*samples));
+		return;
+	}
+
+	phaseshift = dtmf->phaseshift65536;
+	phase = dtmf->phase65536;
+	pos = dtmf->pos;
+	max = dtmf->max;
+
+	for (i = 0; i < length; i++) {
+		*samples++ = dsp_sine_dtmf_low[(uint16_t)phase[0]]
+			   + dsp_sine_dtmf_high[(uint16_t)phase[1]];
+		phase[0] += phaseshift[0];
+		if (phase[0] >= 65536)
+			phase[0] -= 65536;
+		phase[1] += phaseshift[1];
+		if (phase[1] >= 65536)
+			phase[1] -= 65536;
+
+		/* tone ends */
+		if (++pos == max) {
+			dtmf->tone = 0;
+			break;
+		}
+	}
+	length -= i;
+
+	dtmf->pos = pos;
+
+	/* if tone ends, fill rest with silence */
+	if (length)
+		memset(samples, 0, length * sizeof(*samples));
+}
+
diff --git a/src/libdtmf/dtmf_encode.h b/src/libdtmf/dtmf_encode.h
new file mode 100644
index 0000000..15ac05e
--- /dev/null
+++ b/src/libdtmf/dtmf_encode.h
@@ -0,0 +1,14 @@
+
+typedef struct dtmf_enc {
+	int	samplerate;		/* samplerate */
+	char	tone;			/* current tone to be played */
+	int	pos;			/* sample counter for tone */
+	int	max;			/* max number of samples for tone duration */
+	double	phaseshift65536[2];	/* how much the phase of sine wave changes per sample */
+	double	phase65536[2];		/* current phase */
+} dtmf_enc_t;
+
+void dtmf_encode_init(dtmf_enc_t *dtmf, int samplerate, double dBm_level);
+void dtmf_encode_set_tone(dtmf_enc_t *dtmf, char tone);
+void dtmf_encode(dtmf_enc_t *dtmf, sample_t *samples, int length);
+