apps: Add demo application gmr1_rx

This currently try to lock to any FCCH alignement
and process the BCCH to wireshark

Signed-off-by: Sylvain Munaut <tnt@246tNt.com>

1 files changed, 405 insertions, 0 deletions

diff --git a/src/gmr1_rx.c b/src/gmr1_rx.c
new file mode 100644
index 0000000..ae5337a
--- /dev/null
+++ b/src/gmr1_rx.c
@@ -0,0 +1,405 @@
+/* GMR-1 Demo RX application */
+
+/* (C) 2011 by Sylvain Munaut <tnt@246tNt.com>
+ * All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Affero 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 Affero General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <complex.h>
+#include <errno.h>
+#include <math.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+
+#include <osmocom/core/bits.h>
+#include <osmocom/core/gsmtap.h>
+#include <osmocom/core/gsmtap_util.h>
+
+#include <osmocom/sdr/cfile.h>
+#include <osmocom/sdr/cxvec.h>
+#include <osmocom/sdr/cxvec_math.h>
+
+#include <osmocom/gmr1/gsmtap.h>
+#include <osmocom/gmr1/l1/bcch.h>
+#include <osmocom/gmr1/sdr/defs.h>
+#include <osmocom/gmr1/sdr/fcch.h>
+#include <osmocom/gmr1/sdr/pi4cxpsk.h>
+#include <osmocom/gmr1/sdr/nb.h>
+
+
+#define START_DISCARD	8000
+
+
+static struct gsmtap_inst *g_gti;
+
+
+struct chan_desc {
+	struct cfile *bcch;
+	int sps;
+	int align;
+	float freq_err;
+};
+
+
+/* Helpers ---------------------------------------------------------------- */
+
+static inline float
+to_ms(struct chan_desc *cd, int s)
+{
+	return (1000.0f * (float)s) / (cd->sps * GMR1_SYM_RATE);
+}
+
+static inline float
+to_hz(float f_rps)
+{
+	return (GMR1_SYM_RATE * f_rps) / (2.0f * M_PIf);
+}
+
+static inline float
+to_db(float v)
+{
+	return 10.0f * log10f(v);
+}
+
+static int
+win_map(struct osmo_cxvec *win, struct cfile *cf, int begin, int len)
+{
+	if ((begin + len) > cf->len)
+		return -1;
+
+	osmo_cxvec_init_from_data(win, &cf->data[begin], len);
+
+	return 0;
+}
+
+static int
+burst_map(struct osmo_cxvec *burst, struct chan_desc *cd,
+          struct gmr1_pi4cxpsk_burst *burst_type, int tn, int win)
+{
+	int begin, len;
+	int etoa;
+
+	etoa  = win >> 1;
+	begin = cd->align + (cd->sps * tn * 39) - etoa;
+	len   = (burst_type->len * cd->sps) + win;
+
+	if ((begin + len) > cd->bcch->len)
+		return -1;
+
+	osmo_cxvec_init_from_data(burst, &cd->bcch->data[begin], len);
+
+	return etoa;
+}
+
+static float
+burst_energy(struct osmo_cxvec *burst)
+{
+	int i;
+	float e = 0.0f;
+	for (i=0; i<burst->len; i++)
+		e += osmo_normsqf(burst->data[i]);
+	e /= burst->len;
+	return e;
+}
+
+
+/* Procesing -------------------------------------------------------------- */
+
+static int
+fcch_single_init(struct chan_desc *cd)
+{
+	struct osmo_cxvec _win, *win = &_win;
+	int rv, toa;
+
+	/* FCCH rough detection in the first 330 ms */
+	rv = win_map(win, cd->bcch, cd->align, (330 * GMR1_SYM_RATE * cd->sps) / 1000);
+	if (rv) {
+		fprintf(stderr, "[!] Not enough samples\n");
+		return rv;
+	}
+
+	rv = gmr1_fcch_rough(win, cd->sps, 0.0f, &toa);
+	if (rv) {
+		fprintf(stderr, "[!] Error during FCCH rough acquisition (%d)\n", rv);
+		return rv;
+	}
+
+	cd->align += toa;
+
+	/* Fine FCCH detection*/
+	win_map(win, cd->bcch, cd->align, GMR1_FCCH_SYMS * cd->sps);
+
+	rv = gmr1_fcch_fine(win, cd->sps, 0.0f, &toa, &cd->freq_err);
+	if (rv) {
+		fprintf(stderr, "[!] Error during FCCH fine acquisition (%d)\n", rv);
+		return rv;
+	}
+
+	cd->align += toa;
+
+	/* Done */
+	return 0;
+}
+
+typedef int (*fcch_multi_cb_t)(struct chan_desc *cd);
+
+static int
+fcch_multi_process(struct chan_desc *cd, fcch_multi_cb_t cb)
+{
+	struct osmo_cxvec _win, *win = &_win;
+	int base_align, mtoa[16];
+	int i, j, rv, n_fcch;
+	float ref_snr, ref_freq_err;
+
+	fprintf(stderr, "[+] FCCH multi acquisition\n");
+
+	/* Multi FCCH detection (need 650 ms of signals) */
+	base_align = cd->align - GMR1_FCCH_SYMS * cd->sps;
+
+	rv = win_map(win, cd->bcch, base_align, (650 * GMR1_SYM_RATE * cd->sps) / 1000);
+	if (rv) {
+		fprintf(stderr, "[!] Not enough samples\n");
+		return rv;
+	}
+
+	rv = gmr1_fcch_rough_multi(win, cd->sps, -cd->freq_err, mtoa, 16);
+	if (rv < 0) {
+		fprintf(stderr, "[!] Error during FCCH rough mutli-acquisition (%d)\n", rv);
+		return rv;
+	}
+
+	n_fcch = rv;
+
+	/* Check each of them for validity */
+	for (i=0, j=0; i<n_fcch; i++) {
+		float freq_err, e_fcch, e_cich, snr;
+		int toa;
+
+		/* Perform fine acquisition */
+		win_map(win, cd->bcch, base_align + mtoa[i], GMR1_FCCH_SYMS * cd->sps);
+
+		rv = gmr1_fcch_fine(win, cd->sps, -cd->freq_err, &toa, &freq_err);
+		if (rv) {
+			fprintf(stderr, "[!] Error during FCCH fine acquisition (%d)\n", rv);
+			return rv;
+		}
+
+		/* Compute SNR (comparing energy with neighboring CICH) */
+		win_map(win, cd->bcch,
+			base_align + mtoa[i] + toa + 5 * cd->sps,
+			(117 - 10) * cd->sps
+		);
+
+		e_fcch = burst_energy(win);
+
+		win_map(win, cd->bcch,
+			base_align + mtoa[i] + toa + (5 + 117) * cd->sps,
+			(117 - 10) * cd->sps
+		);
+
+		e_cich = burst_energy(win);
+
+		snr = e_fcch / e_cich;
+
+		/* Check against strongest */
+		if (i==0) {
+			/* This _is_ the reference */
+			ref_snr = snr;
+			ref_freq_err = freq_err;
+		} else {
+			/* Check if SNR is 'good enough' */
+			if (snr < 2.0f)
+				continue;
+
+			if (snr < (ref_snr / 6.0f))
+				continue;
+
+			/* Check if frequency error is not too "off" */
+			if (to_hz(fabs(ref_freq_err - freq_err)) > 500.0f)
+				continue;
+		}
+
+		/* Debug print */
+		fprintf(stderr, "[.]  Potential FCCH @%d (%.3f ms). [snr = %.1f dB, freq_err = %.1f Hz]\n",
+			base_align + mtoa[i] + toa,
+			to_ms(cd, base_align + mtoa[i] + toa),
+			to_db(snr),
+			to_hz(freq_err + cd->freq_err)
+		);
+
+		/* Save it */
+		mtoa[j++] = mtoa[i] + toa;
+	}
+
+	n_fcch = j;
+
+	/* Now process each survivor */
+	for (i=0; i<n_fcch; i++) {
+		struct chan_desc _cdl, *cdl = &_cdl;
+
+		memcpy(cdl, cd, sizeof(struct chan_desc));
+		cdl->align = base_align + mtoa[i];
+
+		rv = cb(cdl);
+		if (rv)
+			break;
+	}
+
+	return rv;
+}
+
+static int
+rx_bcch(struct chan_desc *cd)
+{
+	struct osmo_cxvec _burst, *burst = &_burst;
+	sbit_t ebits[424];
+	uint8_t l2[24];
+	float freq_err, toa;
+	int rv, crc, conv, e_toa;
+
+	/* Demodulate burst */
+	e_toa = burst_map(burst, cd, &gmr1_bcch_burst, 0, 20 * cd->sps);
+	if (e_toa < 0)
+		return e_toa;
+
+	rv = gmr1_pi4cxpsk_demod(
+		&gmr1_bcch_burst,
+		burst, cd->sps, -cd->freq_err,
+		ebits, NULL, &toa, &freq_err
+	);
+
+	if (rv)
+		return rv;
+
+	/* Decode burst */
+	crc = gmr1_bcch_decode(l2, ebits, &conv);
+
+	fprintf(stderr, "crc=%d, conv=%d\n", crc, conv);
+
+	/* If burst turned out OK, use data to align channel */
+	if (!crc) {
+		cd->align += ((int)roundf(toa)) - e_toa;
+		cd->freq_err += freq_err / 4.0f;
+	}
+
+	/* Send to GSMTap if correct */
+	if (!crc)
+		gsmtap_sendmsg(g_gti, gmr1_gsmtap_makemsg(GSMTAP_GMR1_BCCH, l2, 24));
+
+	return 0;
+}
+
+static int
+process_bcch(struct chan_desc *cd)
+{
+	int frame_len;
+	int rfn, sirfn;
+
+	fprintf(stderr, "[+] Processing BCCH @%d (%.3f ms). [freq_err = %.1f Hz]\n",
+		cd->align, to_ms(cd, cd->align), to_hz(cd->freq_err));
+
+	/* Process frame by frame */
+	frame_len = cd->sps * 24 * 39;
+	rfn = 0;
+
+	while (1) {
+		/* Debug */
+		fprintf(stderr, "[-]  RFN: %d (%f ms)\n", rfn, to_ms(cd, cd->align));
+
+		/* SI relative frame number inside an hyperframe */
+		sirfn = rfn % 64;
+
+		/* BCCH */
+		if (sirfn % 8 == 2)
+		{
+			fprintf(stderr, "[.]   BCCH\n");
+			rx_bcch(cd);
+		}
+
+		/* Next frame */
+		rfn++;
+		cd->align += frame_len;
+
+		/* Stop if we don't have a complete frame */
+		if ((cd->align + frame_len) > cd->bcch->len)
+			break;
+	}
+
+	return 0;
+}
+
+
+/* Main ------------------------------------------------------------------- */
+
+int main(int argc, char *argv[])
+{
+	struct chan_desc _cd, *cd = &_cd;
+	int rv=0;
+
+	/* Init channel description */
+	cd->bcch = NULL;
+	cd->align = START_DISCARD;
+	cd->freq_err = 0.0f;
+
+	/* Arg check */
+	if (argc != 3) {
+		fprintf(stderr, "Usage: %s sps bcch.cfile\n", argv[0]);
+		return -EINVAL;
+	}
+
+	cd->sps = atoi(argv[1]);
+
+	if (cd->sps < 1 || cd->sps > 16) {
+		fprintf(stderr, "[!] sps must be withing [1,16]\n");
+		return -EINVAL;
+	}
+
+	cd->bcch = cfile_load(argv[2]);
+	if (!cd->bcch) {
+		fprintf(stderr, "[!] Failed to load fcch input file\n");
+		rv = -EIO;
+		goto err;
+	}
+
+	/* Init GSMTap */
+	g_gti = gsmtap_source_init("127.0.0.1", GSMTAP_UDP_PORT, 0);
+	gsmtap_source_add_sink(g_gti);
+
+	/* Use best FCCH for inital sync / freq error */
+	rv = fcch_single_init(cd);
+	if (rv) {
+		fprintf(stderr, "[!] Failed to acquired primary FCCH\n");
+		goto err;
+	}
+
+	fprintf(stderr, "[+] Primary FCCH found @%d (%.3f ms). [freq_err = %.1f Hz]\n",
+		cd->align, to_ms(cd, cd->align), to_hz(cd->freq_err));
+
+	/* Detect all 'visible' FCCH and process them */
+	rv = fcch_multi_process(cd, process_bcch);
+	if (rv)
+		goto err;
+
+	/* Done ! */
+	rv = 0;
+
+	/* Clean up */
+err:
+	if (cd->bcch)
+		cfile_release(cd->bcch);
+
+	return rv;
+}