aboutsummaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
Diffstat
-rw-r--r--.gitignore1
-rw-r--r--include/osmocom/gmr1/gsmtap.h2
-rw-r--r--src/Makefile.am7
-rw-r--r--src/gmr1_rx.c10
-rw-r--r--src/gmr1_rx_live.c1469
-rw-r--r--src/gsmtap.c6
-rw-r--r--src/ringbuf.c141
-rw-r--r--src/ringbuf.h86
-rw-r--r--src/sa_file.c115
-rw-r--r--src/sa_file.h30
-rw-r--r--src/sampbuf.c358
-rw-r--r--src/sampbuf.h111
12 files changed, 2328 insertions, 8 deletions
diff --git a/.gitignore b/.gitignore
index 3047cf1..572a7a6 100644
--- a/.gitignore
+++ b/.gitignore
@@ -34,3 +34,4 @@ Doxyfile
doc/
src/gmr1_rx
src/gmr1_gen_mat
+src/gmr1_rx_live
diff --git a/include/osmocom/gmr1/gsmtap.h b/include/osmocom/gmr1/gsmtap.h
index 0c1e727..96cd250 100644
--- a/include/osmocom/gmr1/gsmtap.h
+++ b/include/osmocom/gmr1/gsmtap.h
@@ -34,7 +34,7 @@ struct msgb;
struct msgb *gmr1_gsmtap_makemsg(
- uint8_t chan_type, uint32_t fn, uint8_t tn,
+ uint8_t chan_type, uint16_t arfcn, uint32_t fn, uint8_t tn,
const uint8_t *l2, int len);
diff --git a/src/Makefile.am b/src/Makefile.am
index 08f14ff..601ea86 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -3,13 +3,18 @@ SUBDIRS = codec l1 sdr
AM_CPPFLAGS = $(all_includes) -I$(top_srcdir)/include -I$(top_builddir)
AM_CFLAGS = -Wall $(LIBOSMOCORE_CFLAGS) $(LIBOSMODSP_CFLAGS)
-bin_PROGRAMS = gmr1_rx gmr1_rach_gen gmr1_gen_mat gmr1_ambe_decode
+bin_PROGRAMS = gmr1_rx gmr1_rx_live gmr1_rach_gen gmr1_gen_mat gmr1_ambe_decode
gmr1_rx_SOURCES = gmr1_rx.c gsmtap.c
gmr1_rx_LDADD = $(top_builddir)/src/l1/libgmr1-l1.a \
$(top_builddir)/src/sdr/libgmr1-sdr.a \
$(LIBOSMOCORE_LIBS) $(LIBOSMODSP_LIBS) $(FFTW3F_LIBS)
+gmr1_rx_live_SOURCES = gmr1_rx_live.c gsmtap.c ringbuf.c sa_file.c sampbuf.c
+gmr1_rx_live_LDADD = $(top_builddir)/src/l1/libgmr1-l1.a \
+ $(top_builddir)/src/sdr/libgmr1-sdr.a \
+ $(LIBOSMOCORE_LIBS) $(LIBOSMODSP_LIBS) $(FFTW3F_LIBS)
+
gmr1_rach_gen_SOURCES = gmr1_rach_gen.c
gmr1_rach_gen_LDADD = $(top_builddir)/src/l1/libgmr1-l1.a \
$(top_builddir)/src/sdr/libgmr1-sdr.a \
diff --git a/src/gmr1_rx.c b/src/gmr1_rx.c
index 8bce7bb..3120005 100644
--- a/src/gmr1_rx.c
+++ b/src/gmr1_rx.c
@@ -308,7 +308,7 @@ rx_tch9(struct chan_desc *cd)
if (!crc)
gsmtap_sendmsg(g_gti, gmr1_gsmtap_makemsg(
GSMTAP_GMR1_TCH9 | GSMTAP_GMR1_FACCH,
- cd->fn, cd->tch9_state.tn, l2, 38));
+ 0, cd->fn, cd->tch9_state.tn, l2, 38));
} else { /* TCH9 */
uint8_t l2[60];
int i, s = 0;
@@ -327,7 +327,7 @@ rx_tch9(struct chan_desc *cd)
/* Forward to GSMTap (no CRC to validate :( ) */
gsmtap_sendmsg(g_gti, gmr1_gsmtap_makemsg(
GSMTAP_GMR1_TCH9,
- cd->fn, cd->tch9_state.tn, l2, 60));
+ 0, cd->fn, cd->tch9_state.tn, l2, 60));
/* Save to file */
{
@@ -422,7 +422,7 @@ _rx_tch3_facch_flush(struct chan_desc *cd)
if (!crc)
gsmtap_sendmsg(g_gti, gmr1_gsmtap_makemsg(
GSMTAP_GMR1_TCH3 | GSMTAP_GMR1_FACCH,
- cd->fn-3, st->tn, l2, 10));
+ 0, cd->fn-3, st->tn, l2, 10));
/* Parse for assignement */
if (!crc && facch3_is_ass_cmd_1(l2))
@@ -789,7 +789,7 @@ rx_bcch(struct chan_desc *cd, float *energy)
if (!crc)
gsmtap_sendmsg(g_gti, gmr1_gsmtap_makemsg(
GSMTAP_GMR1_BCCH,
- cd->fn, cd->sa_bcch_stn, l2, 24));
+ 0, cd->fn, cd->sa_bcch_stn, l2, 24));
return 0;
}
@@ -841,7 +841,7 @@ rx_ccch(struct chan_desc *cd, float min_energy)
if (!crc)
gsmtap_sendmsg(g_gti, gmr1_gsmtap_makemsg(
GSMTAP_GMR1_CCCH,
- cd->fn, cd->sa_bcch_stn, l2, 24));
+ 0, cd->fn, cd->sa_bcch_stn, l2, 24));
return 0;
}
diff --git a/src/gmr1_rx_live.c b/src/gmr1_rx_live.c
new file mode 100644
index 0000000..b3254d9
--- /dev/null
+++ b/src/gmr1_rx_live.c
@@ -0,0 +1,1469 @@
+/* GMR-1 Demo RX live application */
+
+/* (C) 2012 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 <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+#include <osmocom/core/gsmtap.h>
+#include <osmocom/core/gsmtap_util.h>
+
+#include <osmocom/dsp/cxvec.h>
+#include <osmocom/dsp/cxvec_math.h>
+
+#include <osmocom/gmr1/gsmtap.h>
+#include <osmocom/gmr1/l1/a5.h>
+#include <osmocom/gmr1/l1/bcch.h>
+#include <osmocom/gmr1/l1/ccch.h>
+#include <osmocom/gmr1/l1/facch3.h>
+#include <osmocom/gmr1/l1/facch9.h>
+#include <osmocom/gmr1/l1/tch3.h>
+#include <osmocom/gmr1/l1/tch9.h>
+#include <osmocom/gmr1/l1/interleave.h>
+#include <osmocom/gmr1/sdr/defs.h>
+#include <osmocom/gmr1/sdr/dkab.h>
+#include <osmocom/gmr1/sdr/fcch.h>
+#include <osmocom/gmr1/sdr/pi4cxpsk.h>
+#include <osmocom/gmr1/sdr/nb.h>
+
+#include "sampbuf.h"
+#include "sa_file.h"
+
+
+struct app_state
+{
+ /* Sample source */
+ struct sample_buf *buf;
+
+ int arfcn[MAX_CHANS];
+ char *filename[MAX_CHANS];
+
+ /* Params */
+ int n_chans;
+ int sps;
+
+ /* GSMTap */
+ struct gsmtap_inst *gti;
+};
+
+#define TCH3_MARGIN 10
+#define TCH9_MARGIN 50
+#define BCCH_MARGIN 100
+
+
+/* ------------------------------------------------------------------------ */
+
+static int
+win_map(struct osmo_cxvec *win,
+ float complex *data, int data_len,
+ int begin, int win_len)
+{
+ if ((begin + win_len) > data_len)
+ return -1;
+
+ osmo_cxvec_init_from_data(win, &data[begin], win_len);
+
+ return 0;
+}
+
+static int
+burst_map(struct osmo_cxvec *burst,
+ float complex *data, int data_len, int base_align, int sps,
+ struct gmr1_pi4cxpsk_burst *burst_type, int tn, int win)
+{
+ int begin, len;
+ int etoa;
+
+ etoa = win >> 1;
+ begin = base_align + (sps * tn * 39) - etoa;
+ len = (burst_type->len * sps) + win;
+
+ if ((begin < 0) || ((begin + len) > data_len))
+ return -EIO;
+
+ osmo_cxvec_init_from_data(burst, &data[begin], len);
+
+ return etoa;
+}
+
+static float
+burst_energy(struct osmo_cxvec *burst)
+{
+ int i;
+ int b = (burst->len >> 5); /* exclude the borders */
+ float e = 0.0f;
+ for (i=b; i<burst->len-b; i++)
+ e += osmo_normsqf(burst->data[i]);
+ e /= burst->len;
+ return e;
+}
+
+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);
+}
+
+
+/* TCH9 ------------------------------------------------------------------- */
+
+struct tch9_sink_params {
+ struct app_state *as;
+ int chan_id;
+ uint32_t fn;
+ uint64_t align;
+ float freq_err;
+ int tn;
+ int ciph;
+ uint8_t kc[8];
+};
+
+struct tch9_sink_priv {
+ /* App */
+ struct app_state *as;
+ int chan_id;
+
+ /* Alignement time/freq */
+ uint32_t fn;
+ uint64_t align;
+ int align_err;
+ float freq_err;
+
+ int tn;
+
+ int aligned;
+
+ /* End detection */
+ int bad_crc;
+
+ /* Cipher */
+ int ciph;
+ uint8_t kc[8];
+
+ /* Interleaver */
+ struct gmr1_interleaver il;
+
+ /* Output data */
+ FILE *data;
+};
+
+static int
+tch9_sink_init(struct sample_actor *sa, void *params_ptr)
+{
+ struct tch9_sink_priv *priv = sa->priv;
+ struct tch9_sink_params *params = params_ptr;
+
+ priv->as = params->as;
+ priv->chan_id = params->chan_id;
+
+ priv->fn = params->fn;
+ priv->align = params->align;
+ priv->freq_err = params->freq_err;
+
+ priv->tn = params->tn;
+
+ priv->ciph = params->ciph;
+ memcpy(priv->kc, params->kc, 8);
+
+ gmr1_interleaver_init(&priv->il, 3, 648);
+
+ return 0;
+}
+
+static void
+tch9_sink_fini(struct sample_actor *sa)
+{
+ struct tch9_sink_priv *priv = sa->priv;
+
+ gmr1_interleaver_fini(&priv->il);
+
+ if (priv->data)
+ fclose(priv->data);
+}
+
+static int
+tch9_sink_work(struct sample_actor *sa,
+ float complex *data, unsigned int data_len)
+{
+ struct tch9_sink_priv *priv = sa->priv;
+ struct osmo_cxvec _burst, *burst = &_burst;
+ int sps, base_align, frame_len;
+ int e_toa, rv, sync_id, crc, conv;
+ sbit_t ebits[662], bits_sacch[10], bits_status[4];
+ ubit_t ciph[658];
+ float toa, freq_err;
+
+ /* Get quick params */
+ sps = priv->as->sps;
+ frame_len = sps * 39 * 24;
+ base_align = sps * TCH9_MARGIN;
+
+ /* If not aligned ... do that first */
+ if (!priv->aligned) {
+ /* Basically we want to have :
+ * |#|Frame|#| with the # being margin.
+ */
+
+ uint64_t target = priv->align - sps * TCH9_MARGIN;
+ int discard;
+
+ if (target < sa->time) {
+ target += frame_len;
+ priv->fn += 1;
+ priv->align += frame_len;
+ }
+
+ discard = target - sa->time;
+
+ if (discard > data_len)
+ return data_len;
+
+ priv->aligned = 1;
+
+ return discard;
+ }
+
+ /* Check we have enough data (frame_len) */
+ if (data_len < (frame_len + 2*TCH9_MARGIN))
+ return 0;
+
+ /* Map potential burst */
+ e_toa = burst_map(burst, data, data_len, base_align, sps,
+ &gmr1_nt9_burst, priv->tn, sps + (sps/2));
+ if (e_toa < 0)
+ return e_toa;
+
+ /* Demodulate burst */
+ rv = gmr1_pi4cxpsk_demod(
+ &gmr1_nt9_burst,
+ burst, sps, -priv->freq_err,
+ ebits, &sync_id, &toa, &freq_err
+ );
+ if (rv < 0)
+ return rv;
+
+ fprintf(stderr, "[.] %s\n", sync_id ? "TCH9" : "FACCH9");
+ fprintf(stderr, "toa=%.1f, sync_id=%d\n", toa, sync_id);
+
+ /* Track frequency */
+ priv->freq_err += freq_err;
+
+ /* Process depending on type */
+ if (!sync_id) { /* FACCH9 */
+ uint8_t l2[38];
+
+ /* Generate cipher stream */
+ gmr1_a5(priv->ciph, priv->kc, priv->fn, 658, ciph, NULL);
+
+ /* Decode */
+ crc = gmr1_facch9_decode(l2, bits_sacch, bits_status, ebits, ciph, &conv);
+ fprintf(stderr, "crc=%d, conv=%d\n", crc, conv);
+
+ /* Send to GSMTap if correct */
+ if (!crc)
+ gsmtap_sendmsg(priv->as->gti, gmr1_gsmtap_makemsg(
+ GSMTAP_GMR1_TCH9 | GSMTAP_GMR1_FACCH,
+ priv->as->arfcn[priv->chan_id],
+ priv->fn, priv->tn, l2, 38));
+
+ /* Detect end */
+ if (crc)
+ if (priv->bad_crc++ > 10)
+ return -1;
+ } else { /* TCH9 */
+ uint8_t l2[60];
+ int i, s = 0;
+
+ /* Generate cipher stream */
+ gmr1_a5(priv->ciph, priv->kc, priv->fn, 658, ciph, NULL);
+
+ for (i=0; i<662; i++)
+ s += ebits[i] < 0 ? -ebits[i] : ebits[i];
+ s /= 662;
+
+ /* Decode */
+ gmr1_tch9_decode(l2, bits_sacch, bits_status, ebits, GMR1_TCH9_9k6, ciph, &priv->il, &conv);
+ fprintf(stderr, "fn=%d, conv9=%d, avg=%d\n", priv->fn, conv, s);
+
+ /* Forward to GSMTap (no CRC to validate :( ) */
+ gsmtap_sendmsg(priv->as->gti, gmr1_gsmtap_makemsg(
+ GSMTAP_GMR1_TCH9,
+ priv->as->arfcn[priv->chan_id],
+ priv->fn, priv->tn, l2, 60));
+
+ /* Save to file */
+ {
+ if (!priv->data) {
+ char fname[256];
+ sprintf(fname, "/tmp/gmr1_csd_%d_%d_%d.dat", priv->as->arfcn[priv->chan_id], priv->tn, priv->fn);
+ priv->data = fopen(fname, "wb");
+ }
+ fwrite(l2, 60, 1, priv->data);
+ }
+ }
+
+ /* Accumulate alignement error */
+ fprintf(stderr, "toa=%f | %d %d %d\n", toa, e_toa, ((int)roundf(toa)) - e_toa, priv->align_err);
+ priv->align_err += ((int)roundf(toa)) - e_toa;
+
+ /* Take align_err into account */
+ if (priv->align_err > 4) {
+ frame_len++;
+ priv->align_err -= 4;
+ fprintf(stderr, ">>>> REALIGN +++ %d\n", priv->align_err);
+ } else if (priv->align_err < -4) {
+ frame_len--;
+ priv->align_err += 4;
+ fprintf(stderr, ">>>> REALIGN --- %d\n", priv->align_err);
+ }
+
+ /* Done, go to next frame */
+ priv->fn += 1;
+
+ return frame_len;
+}
+
+const struct sample_actor_desc tch9_sink = {
+ .init = tch9_sink_init,
+ .fini = tch9_sink_fini,
+ .work = tch9_sink_work,
+ .priv_size = sizeof(struct tch9_sink_priv),
+};
+
+
+/* TCH3 ------------------------------------------------------------------- */
+
+struct tch3_sink_params {
+ struct app_state *as;
+ int chan_id;
+ uint32_t fn;
+ uint64_t align;
+ float freq_err;
+ int tn;
+ int dkab_pos;
+ float ref_energy;
+};
+
+struct tch3_sink_priv {
+ /* App */
+ struct app_state *as;
+ int chan_id;
+
+ /* Alignement time/freq */
+ uint32_t fn;
+ uint64_t align;
+ int align_err;
+ float freq_err;
+
+ int tn;
+ int dkab_pos;
+
+ int aligned;
+
+ /* Energy thresholds */
+ float energy_dkab;
+ float energy_burst;
+
+ int weak_cnt;
+
+ /* FAACH state */
+ sbit_t ebits[104*4];
+ uint32_t bi_fn[4];
+ int sync_id;
+ int burst_cnt;
+
+ int followed;
+
+ /* Cipher */
+ int ciph;
+ uint8_t kc[8];
+
+ /* Output data */
+ FILE *data;
+};
+
+static int
+tch3_sink_init(struct sample_actor *sa, void *params_ptr)
+{
+ struct tch3_sink_priv *priv = sa->priv;
+ struct tch3_sink_params *params = params_ptr;
+
+ priv->as = params->as;
+ priv->chan_id = params->chan_id;
+
+ priv->fn = params->fn;
+ priv->align = params->align;
+ priv->freq_err = params->freq_err;
+
+ priv->tn = params->tn;
+ priv->dkab_pos = params->dkab_pos;
+
+ priv->energy_burst = params->ref_energy * 0.75f;
+ priv->energy_dkab = priv->energy_burst / 8.0f; /* ~ 8 times less pwr */
+
+ priv->weak_cnt = 0;
+
+ priv->ciph = 0;
+
+ return 0;
+}
+
+static void
+tch3_sink_fini(struct sample_actor *sa)
+{
+ struct tch3_sink_priv *priv = sa->priv;
+
+ if (priv->data)
+ fclose(priv->data);
+}
+
+static int
+_rx_tch3_dkab(struct sample_actor *sa, struct osmo_cxvec *burst, float *toa)
+{
+ struct tch3_sink_priv *priv = sa->priv;
+ sbit_t ebits[8];
+ int rv;
+
+ fprintf(stderr, "[.] DKAB\n");
+
+ rv = gmr1_dkab_demod(burst, priv->as->sps, -priv->freq_err, priv->dkab_pos, ebits, toa);
+
+ fprintf(stderr, "toa=%f\n", *toa);
+
+ return rv;
+}
+
+static inline int
+_facch3_is_ass_cmd_1(const uint8_t *l2)
+{
+ return (l2[3] == 0x06) && (l2[4] == 0x2e);
+}
+
+static void
+_facch3_ass_cmd_1_parse(const uint8_t *l2, int *arfcn, int *rx_tn)
+{
+ *rx_tn = ((l2[5] & 0x03) << 3) | (l2[6] >> 5);
+ *arfcn = ((l2[6] & 0x1f) << 6) | (l2[7] >> 2);
+}
+
+static int
+_rx_tch3_facch_flush(struct sample_actor *sa)
+{
+ struct tch3_sink_priv *priv = sa->priv;
+ ubit_t ciph[96*4];
+ uint8_t l2[10];
+ ubit_t sbits[8*4];
+ int sps, base_align;
+ int i, crc, conv;
+
+ /* Get quick params */
+ sps = priv->as->sps;
+ base_align = sps * TCH3_MARGIN;
+
+ /* Cipher stream */
+ for (i=0; i<4; i++)
+ gmr1_a5(priv->ciph, priv->kc, priv->bi_fn[i], 96, ciph+(96*i), NULL);
+
+ /* Decode the burst */
+ crc = gmr1_facch3_decode(l2, sbits, priv->ebits, ciph, &conv);
+
+ fprintf(stderr, "crc=%d, conv=%d\n", crc, conv);
+
+ /* Retry with ciphering ? */
+#if 0
+ if (!st->ciph && crc) {
+ ciph = _ciph;
+ for (i=0; i<4; i++)
+ gmr1_a5(1, cd->kc, st->bi_fn[i], 96, ciph+(96*i), NULL);
+
+ crc = gmr1_facch3_decode(l2, sbits, st->ebits, ciph, &conv);
+
+ fprintf(stderr, "crc=%d, conv=%d\n", crc, conv);
+
+ if (!crc)
+ st->ciph = 1;
+ }
+#endif
+
+ /* Send to GSMTap if correct */
+ if (!crc)
+ gsmtap_sendmsg(priv->as->gti, gmr1_gsmtap_makemsg(
+ GSMTAP_GMR1_TCH3 | GSMTAP_GMR1_FACCH,
+ priv->as->arfcn[priv->chan_id],
+ priv->fn-3, priv->tn, l2, 10));
+
+ /* Parse for assignement */
+ if (!crc && _facch3_is_ass_cmd_1(l2) && !priv->followed)
+ {
+ struct tch9_sink_params p;
+ struct sample_actor *nsa;
+ int arfcn, tn;
+ int i;
+
+ /* Extract TN & ARFCN */
+ _facch3_ass_cmd_1_parse(l2, &arfcn, &tn);
+
+ /* Debug print */
+ fprintf(stderr, "[+] TCH9 assigned on ARFCN %d TN %d\n",
+ arfcn, tn);
+
+ /* Find matching channel ID */
+ for (i=0; i<priv->as->n_chans; i++)
+ if (priv->as->arfcn[i] == arfcn)
+ break;
+
+ if (i == priv->as->n_chans) {
+ fprintf(stderr, "No data stream available for that ARFCN\n");
+ goto nofollow;
+ }
+
+ /* Start TCH9 task */
+ p.as = priv->as;
+ p.chan_id = i;
+ p.fn = priv->fn;
+ p.align = sa->time + base_align;
+ p.freq_err = priv->freq_err;
+ p.tn = tn;
+ p.ciph = priv->ciph;
+ memcpy(p.kc, priv->kc, 8);
+
+ nsa = sbuf_add_consumer(priv->as->buf, p.chan_id, &tch9_sink, &p);
+ if (!nsa) {
+ fprintf(stderr, "[!] Failed to create TCH9 sink for stream #%d\n", p.chan_id);
+ return -ENOMEM;
+ }
+
+ /* Stop current TCH3 task */
+ /* FIXME should only happen later, ass message spans several
+ * FACCH3 ... */
+ priv->followed = 1;
+ }
+nofollow:
+
+ /* Clear state */
+ priv->sync_id ^= 1;
+ priv->burst_cnt = 0;
+ memset(priv->bi_fn, 0xff, sizeof(uint32_t) * 4);
+ memset(priv->ebits, 0x00, sizeof(sbit_t) * 104 * 4);
+
+ /* Done */
+ return 0;
+}
+
+static int
+_rx_tch3_facch(struct sample_actor *sa, struct osmo_cxvec *burst, float *toa)
+{
+ struct tch3_sink_priv *priv = sa->priv;
+ sbit_t ebits[104];
+ int rv, bi, sync_id;
+
+ /* Burst index */
+ bi = priv->fn & 3;
+
+ /* Debug */
+ fprintf(stderr, "[.] FACCH3 (bi=%d)\n", bi);
+
+ /* Demodulate burst */
+ rv = gmr1_pi4cxpsk_demod(
+ &gmr1_nt3_facch_burst,
+ burst, priv->as->sps, -priv->freq_err,
+ ebits, &sync_id, toa, NULL
+ );
+
+ fprintf(stderr, "toa=%.1f, sync_id=%d\n", *toa, sync_id);
+
+ /* Does this burst belong with previous ones ? */
+ if (sync_id != priv->sync_id)
+ _rx_tch3_facch_flush(sa);
+
+ /* Store this burst */
+ memcpy(&priv->ebits[104*bi], ebits, sizeof(sbit_t) * 104);
+ priv->sync_id = sync_id;
+ priv->bi_fn[bi] = priv->fn;
+ priv->burst_cnt += 1;
+
+ /* Is it time to flush ? */
+ if (priv->burst_cnt == 4)
+ _rx_tch3_facch_flush(sa);
+
+ return rv;
+}
+
+static int
+_rx_tch3_speech(struct sample_actor *sa, struct osmo_cxvec *burst, float *toa)
+{
+ struct tch3_sink_priv *priv = sa->priv;
+ sbit_t ebits[212];
+ ubit_t sbits[4], ciph[208];
+ uint8_t frame0[10], frame1[10];
+ int rv, conv[2];
+
+ /* Debug */
+ fprintf(stderr, "[.] TCH3\n");
+
+ /* Demodulate burst */
+ rv = gmr1_pi4cxpsk_demod(
+ &gmr1_nt3_speech_burst,
+ burst, priv->as->sps, -priv->freq_err,
+ ebits, NULL, toa, NULL
+ );
+ if (rv < 0)
+ return rv;
+
+ /* Decode it */
+ gmr1_a5(priv->ciph, priv->kc, priv->fn, 208, ciph, NULL);
+
+ gmr1_tch3_decode(frame0, frame1, sbits, ebits, ciph, 0, &conv[0], &conv[1]);
+
+ /* More debug */
+ fprintf(stderr, "toa=%.1f\n", *toa);
+ fprintf(stderr, "conv=%3d,%3d\n", conv[0], conv[1]);
+ fprintf(stderr, "frame0=%s\n", osmo_hexdump_nospc(frame0, 10));
+ fprintf(stderr, "frame1=%s\n", osmo_hexdump_nospc(frame1, 10));
+
+ /* Save to file */
+ {
+ if (!priv->data) {
+ char fname[256];
+ sprintf(fname, "/tmp/gmr1_speech_%d_%d_%d.dat", priv->as->arfcn[priv->chan_id], priv->tn, priv->fn);
+ priv->data = fopen(fname, "wb");
+ }
+ fwrite(frame0, 10, 1, priv->data);
+ fwrite(frame1, 10, 1, priv->data);
+ }
+
+ return 0;
+}
+
+static int
+tch3_sink_work(struct sample_actor *sa,
+ float complex *data, unsigned int data_len)
+{
+ static struct gmr1_pi4cxpsk_burst *burst_types[] = {
+ &gmr1_nt3_facch_burst,
+ &gmr1_nt3_speech_burst,
+ NULL
+ };
+
+ struct tch3_sink_priv *priv = sa->priv;
+ struct osmo_cxvec _burst, *burst = &_burst;
+ int sps, base_align, frame_len;
+ int e_toa, btid, sid;
+ float be, det, toa;
+ int rv;
+
+ /* Get quick params */
+ sps = priv->as->sps;
+ frame_len = sps * 39 * 24;
+ base_align = sps * TCH3_MARGIN;
+
+ /* If not aligned ... do that first */
+ if (!priv->aligned) {
+ /* Basically we want to have :
+ * |#|Frame|#| with the # being margin.
+ */
+
+ uint64_t target = priv->align - sps * TCH3_MARGIN;
+ int discard;
+
+ if (target < sa->time) {
+ target += frame_len;
+ priv->fn += 1;
+ priv->align += frame_len;
+ }
+
+ discard = target - sa->time;
+
+ if (discard > data_len)
+ return data_len;
+
+ priv->aligned = 1;
+
+ return discard;
+ }
+
+ /* Check we have enough data (frame_len) */
+ if (data_len < (frame_len + 2*TCH3_MARGIN))
+ return 0;
+
+ /* Map potential burst (use FACCH3 as reference) */
+ e_toa = burst_map(burst, data, data_len, base_align, sps,
+ &gmr1_nt3_facch_burst, priv->tn, sps + sps/2);
+ if (e_toa < 0)
+ return e_toa;
+
+ /* Burst energy (and check for DKAB) */
+ be = burst_energy(burst);
+
+ det = (priv->energy_dkab + priv->energy_burst) / 4.0f;
+
+ if (be < det) {
+ rv = _rx_tch3_dkab(sa, burst, &toa);
+
+ if (rv < 0)
+ return rv;
+ else if (rv == 1) {
+ if (priv->weak_cnt++ > 8) {
+ fprintf(stderr, "END @%d\n", priv->fn);
+ return -1;
+ }
+ } else {
+ priv->energy_dkab =
+ (0.1f * be) +
+ (0.9f * priv->energy_dkab);
+ }
+
+ goto done;
+ } else
+ priv->weak_cnt = 0;
+
+ priv->energy_burst =
+ (0.1f * be) +
+ (0.9f * priv->energy_burst);
+
+ /* Detect burst type */
+ rv = gmr1_pi4cxpsk_detect(
+ burst_types, (float)e_toa,
+ burst, sps, -priv->freq_err,
+ &btid, &sid, &toa
+ );
+ if (rv < 0)
+ return rv;
+
+ /* Delegate appropriately */
+ if (btid == 0)
+ rv = _rx_tch3_facch(sa, burst, &toa);
+ else
+ rv = _rx_tch3_speech(sa, burst, &toa);
+
+ if (rv < 0)
+ return rv;
+
+done:
+ /* Accumulate alignement error */
+ fprintf(stderr, "toa=%f | %d %d %d\n", toa, e_toa, ((int)roundf(toa)) - e_toa, priv->align_err);
+ priv->align_err += ((int)roundf(toa)) - e_toa;
+
+ /* Take align_err into account */
+ if (priv->align_err > 4) {
+ frame_len++;
+ priv->align_err -= 4;
+ fprintf(stderr, ">>>> REALIGN +++ %d\n", priv->align_err);
+ } else if (priv->align_err < -4) {
+ frame_len--;
+ priv->align_err += 4;
+ fprintf(stderr, ">>>> REALIGN --- %d\n", priv->align_err);
+ }
+
+ /* Done, go to next frame */
+ priv->fn += 1;
+
+ return frame_len;
+}
+
+const struct sample_actor_desc tch3_sink = {
+ .init = tch3_sink_init,
+ .fini = tch3_sink_fini,
+ .work = tch3_sink_work,
+ .priv_size = sizeof(struct tch3_sink_priv),
+};
+
+
+/* BCCH / CCCH ------------------------------------------------------------ */
+
+struct bcch_sink_params {
+ struct app_state *as;
+ int chan_id;
+ uint64_t align;
+ float freq_err;
+};
+
+struct bcch_sink_priv {
+ struct app_state *as;
+ int chan_id;
+
+ uint64_t align;
+ int align_err;
+ float freq_err;
+
+ uint32_t fn;
+ int sa_sirfn_delay;
+ int sa_bcch_stn;
+
+ float bcch_energy;
+ int bcch_err;
+
+ int la_arfcn;
+ int la_tn;
+ int la_dkab_pos;
+
+ int aligned;
+};
+
+static int
+bcch_sink_init(struct sample_actor *sa, void *params_ptr)
+{
+ struct bcch_sink_priv *priv = sa->priv;
+ struct bcch_sink_params *params = params_ptr;
+
+ priv->as = params->as;
+ priv->chan_id = params->chan_id;
+
+ priv->align = params->align;
+ priv->freq_err = params->freq_err;
+
+ return 0;
+}
+
+static void
+bcch_sink_fini(struct sample_actor *sa)
+{
+ /* struct bcch_sink_priv *priv = sa->priv; */
+
+ /* Nothing to do */
+}
+
+static int
+_bcch_tdma_align(struct bcch_sink_priv *priv, uint8_t *l2)
+{
+ int sa_sirfn_delay, sa_bcch_stn;
+ int superframe_num, multiframe_num, mffn_high_bit;
+ int fn;
+
+ /* Check if it's a SI1 */
+ if ((l2[0] & 0xf8) != 0x08)
+ return 0;
+
+ /* Check if it contains a Seg 2A bis */
+ if ((l2[9] & 0xfc) != 0x80)
+ return 0;
+
+ /* Retrieve SA_SIRFN_DELAY, SA_BCCH_STN,
+ * Superframe number, Multiframe number, MFFN high bit */
+ sa_sirfn_delay = (l2[10] >> 3) & 0x0f;
+ sa_bcch_stn = ((l2[10] << 2) & 0x1c) | (l2[11] >> 6);
+
+ superframe_num = ((l2[11] & 0x3f) << 7) | (l2[12] >> 1);
+ multiframe_num = ((l2[12] & 0x01) << 1) | (l2[13] >> 7);
+ mffn_high_bit = ((l2[13] & 0x40) >> 6);
+
+ /* Compute frame number */
+ fn = (superframe_num << 6) |
+ (multiframe_num << 4) |
+ (mffn_high_bit << 3) |
+ ((2 + sa_sirfn_delay) & 7);
+
+ /* Fix SDR alignement */
+ priv->align_err += (priv->sa_bcch_stn - sa_bcch_stn) * 39 * priv->as->sps;
+
+ /* Align TDMA */
+ priv->fn = fn;
+ priv->sa_sirfn_delay = sa_sirfn_delay;
+ priv->sa_bcch_stn = sa_bcch_stn;
+
+ return 0;
+}
+
+static int
+_rx_bcch(struct sample_actor *sa,
+ float complex *data, unsigned int data_len)
+{
+ struct bcch_sink_priv *priv = sa->priv;
+ struct osmo_cxvec _burst, *burst = &_burst;
+ sbit_t ebits[424];
+ uint8_t l2[24];
+ float freq_err, toa;
+ int sps, base_align;
+ int rv, crc, conv, e_toa;
+
+ /* Get quick params */
+ sps = priv->as->sps;
+ base_align = sps * BCCH_MARGIN;
+
+ /* Debug */
+ fprintf(stderr, "[.] BCCH\n");
+
+ /* Demodulate burst */
+ e_toa = burst_map(burst, data, data_len, base_align, sps,
+ &gmr1_bcch_burst, priv->sa_bcch_stn, 20 * sps);
+ if (e_toa < 0)
+ return e_toa;
+
+ rv = gmr1_pi4cxpsk_demod(
+ &gmr1_bcch_burst,
+ burst, sps, -priv->freq_err,
+ ebits, NULL, &toa, &freq_err
+ );
+
+ if (rv)
+ return rv;
+
+ /* Measure energy as a reference */
+ priv->bcch_energy = burst_energy(burst);
+
+ /* 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) {
+ /* SDR alignement */
+ priv->align_err += ((int)roundf(toa)) - e_toa;
+ priv->freq_err += freq_err;
+
+ /* Acquire TDMA alignement */
+ _bcch_tdma_align(priv, l2);
+ }
+
+ /* Count errors */
+ if (!crc)
+ priv->bcch_err = 0;
+ else
+ priv->bcch_err++;
+
+ /* Send to GSMTap if correct */
+ if (!crc)
+ gsmtap_sendmsg(priv->as->gti, gmr1_gsmtap_makemsg(
+ GSMTAP_GMR1_BCCH,
+ priv->as->arfcn[priv->chan_id],
+ priv->fn, priv->sa_bcch_stn, l2, 24));
+
+ return 0;
+}
+
+static inline int
+_ccch_is_imm_ass(const uint8_t *l2)
+{
+ return (l2[1] == 0x06) && (l2[2] == 0x3f);
+}
+
+static void
+_ccch_imm_ass_parse(const uint8_t *l2, int *arfcn, int *rx_tn, int *p)
+{
+ *p = (l2[8] & 0xfc) >> 2;
+ *rx_tn = ((l2[8] & 0x03) << 3) | (l2[9] >> 5);
+ *arfcn = ((l2[9] & 0x1f) << 6) | (l2[10] >> 2);
+}
+
+static int
+_rx_ccch(struct sample_actor *sa,
+ float complex *data, unsigned int data_len)
+{
+ struct bcch_sink_priv *priv = sa->priv;
+ struct osmo_cxvec _burst, *burst = &_burst;
+ sbit_t ebits[432];
+ uint8_t l2[24];
+ int sps, base_align;
+ int rv, crc, conv, e_toa;
+
+ /* Get quick params */
+ sps = priv->as->sps;
+ base_align = sps * BCCH_MARGIN;
+
+ /* Map potential burst */
+ e_toa = burst_map(burst, data, data_len, base_align, sps,
+ &gmr1_dc6_burst, priv->sa_bcch_stn, 10 * sps);
+ if (e_toa < 0)
+ return e_toa;
+
+ /* Energy detection */
+ if (burst_energy(burst) < (priv->bcch_energy / 2.0f))
+ return 0; /* Nothing to do */
+
+ /* Debug */
+ fprintf(stderr, "[.] CCCH\n");
+
+ /* Demodulate burst */
+ rv = gmr1_pi4cxpsk_demod(
+ &gmr1_dc6_burst,
+ burst, sps, -priv->freq_err,
+ ebits, NULL, NULL, NULL
+ );
+
+ if (rv)
+ return rv;
+
+ /* Decode burst */
+ crc = gmr1_ccch_decode(l2, ebits, &conv);
+
+ fprintf(stderr, "crc=%d, conv=%d\n", crc, conv);
+
+ /* Check for IMM.ASS */
+ if (!crc && _ccch_is_imm_ass(l2)) {
+ struct tch3_sink_params p;
+ struct sample_actor *nsa;
+ int arfcn, tn, dkab_pos;
+ int i;
+
+ /* Parse ASS */
+ _ccch_imm_ass_parse(l2, &arfcn, &tn, &dkab_pos);
+
+ /* Quick & Dirty check for dupes */
+ if ((priv->la_arfcn == arfcn) &&
+ (priv->la_tn == tn) &&
+ (priv->la_dkab_pos == dkab_pos))
+ goto nofollow;
+
+ priv->la_arfcn = arfcn;
+ priv->la_tn = tn;
+ priv->la_dkab_pos = dkab_pos;
+
+ /* Debug print */
+ fprintf(stderr, "[+] TCH3 assigned on ARFCN %d TN %d DKAB %d\n",
+ arfcn, tn, dkab_pos);
+
+ /* Find matching channel ID */
+ for (i=0; i<priv->as->n_chans; i++)
+ if (priv->as->arfcn[i] == arfcn)
+ break;
+
+ if (i == priv->as->n_chans) {
+ fprintf(stderr, "No data stream available for that ARFCN\n");
+ goto nofollow;
+ }
+
+ /* Start TCH3 task */
+ p.as = priv->as;
+ p.chan_id = i;
+ p.fn = priv->fn;
+ p.align = sa->time + base_align;
+ p.freq_err = priv->freq_err;
+ p.tn = tn;
+ p.dkab_pos = dkab_pos;
+ p.ref_energy = priv->bcch_energy / 2.0f;
+
+ nsa = sbuf_add_consumer(priv->as->buf, p.chan_id, &tch3_sink, &p);
+ if (!nsa) {
+ fprintf(stderr, "[!] Failed to create TCH3 sink for stream #%d\n", p.chan_id);
+ return -ENOMEM;
+ }
+ }
+nofollow:
+
+ /* Send to GSMTap if correct */
+ if (!crc)
+ gsmtap_sendmsg(priv->as->gti, gmr1_gsmtap_makemsg(
+ GSMTAP_GMR1_CCCH,
+ priv->as->arfcn[priv->chan_id],
+ priv->fn, priv->sa_bcch_stn, l2, 24));
+
+ return 0;
+}
+
+static int
+bcch_sink_work(struct sample_actor *sa,
+ float complex *data, unsigned int data_len)
+{
+ struct bcch_sink_priv *priv = sa->priv;
+ int sps, base_align, frame_len, sirfn;
+
+ /* Get quick params */
+ sps = priv->as->sps;
+ frame_len = sps * 39 * 24;
+ base_align = sps * BCCH_MARGIN;
+
+ /* If not aligned ... do that first */
+ if (!priv->aligned) {
+ /* Basically we want to have :
+ * |#|Frame0|Frame1|#| with the # being
+ * margin blocks
+ */
+
+ uint64_t target = priv->align - sps * BCCH_MARGIN;
+ int discard;
+
+ if (target < sa->time) {
+ target += frame_len;
+ priv->align += frame_len;
+ }
+
+ discard = target - sa->time;
+
+ if (discard > data_len)
+ return data_len;
+
+ priv->aligned = 1;
+
+ return discard;
+ }
+
+ /* Check we have enough data (2*BCCH_MARGIN + 2*frame_len) */
+ if (data_len < (2*BCCH_MARGIN*sps + 2*frame_len))
+ return 0;
+
+ /* Debug print */
+ fprintf(stderr, "[-] FN: %6d (@%d:%llu)\n",
+ priv->fn, priv->chan_id, (long long unsigned int)(sa->time + base_align));
+
+ /* SI relative frame number inside an hyperframe */
+ sirfn = (priv->fn - priv->sa_sirfn_delay) & 63;
+
+ /* BCCH */
+ if (sirfn % 8 == 2)
+ _rx_bcch(sa, data, data_len);
+
+ if (priv->bcch_err > 10)
+ return -1;
+
+ /* CCCH */
+ if ((sirfn % 8 != 0) && (sirfn % 8 != 2))
+ _rx_ccch(sa, data, data_len);
+
+ /* Next frame */
+ priv->fn += 1;
+
+ frame_len += priv->align_err;
+ priv->align_err = 0;
+
+ return frame_len;
+}
+
+const struct sample_actor_desc bcch_sink = {
+ .init = bcch_sink_init,
+ .fini = bcch_sink_fini,
+ .work = bcch_sink_work,
+ .priv_size = sizeof(struct bcch_sink_priv),
+};
+
+
+/* FCCH ------------------------------------------------------------------- */
+
+#define START_DISCARD 8000
+
+
+struct fcch_sink_params {
+ struct app_state *as;
+ int chan_id;
+};
+
+struct fcch_sink_priv {
+ struct app_state *as;
+ int chan_id;
+
+ enum {
+ FCCH_STATE_SINGLE = 0,
+ FCCH_STATE_MULTI = 1,
+ } state;
+
+ float freq_err;
+};
+
+static int
+fcch_sink_init(struct sample_actor *sa, void *params_ptr)
+{
+ struct fcch_sink_priv *priv = sa->priv;
+ struct fcch_sink_params *params = params_ptr;
+
+ priv->as = params->as;
+ priv->chan_id = params->chan_id;
+
+ return 0;
+}
+
+static void
+fcch_sink_fini(struct sample_actor *sa)
+{
+ /* struct fcch_sink_priv *priv = sa->priv; */
+
+ /* Nothing to do */
+}
+
+static int
+_fcch_sink_work_single(struct sample_actor *sa,
+ float complex *data, unsigned int data_len)
+{
+ struct fcch_sink_priv *priv = sa->priv;
+ struct osmo_cxvec _win, *win = &_win;
+ int sps, win_len, base_align, toa;
+ int rv;
+
+ /* Params */
+ sps = priv->as->sps;
+ base_align = START_DISCARD;
+
+ /* Get large enough window (330 ms) */
+ win_len = ((330 * GMR1_SYM_RATE * sps) / 1000);
+
+ rv = win_map(win, data, data_len, base_align, win_len);
+ if (rv < 0)
+ return 0; /* Not enough data yet */
+
+ /* FCCH rough retect */
+ rv = gmr1_fcch_rough(win, sps, 0.0f, &toa);
+ if (rv < 0) {
+ fprintf(stderr, "[!] Error during FCCH rough acquisition (%d)\n", rv);
+ return rv;
+ }
+
+ /* Fine FCCH detection */
+ win_map(win, data, data_len, base_align + toa, GMR1_FCCH_SYMS * sps);
+
+ rv = gmr1_fcch_fine(win, sps, 0.0f, &toa, &priv->freq_err);
+ if (rv < 0) {
+ fprintf(stderr, "[!] Error during FCCH fine acquisition (%d)\n", rv);
+ return rv;
+ }
+
+ base_align += toa;
+
+ /* Debug print */
+ fprintf(stderr, "[+] Primary FCCH found @%d:%d [freq_err = %.1f Hz]\n",
+ priv->chan_id, base_align, to_hz(priv->freq_err));
+
+ /* Take a safety margin for next step */
+ base_align -= GMR1_FCCH_SYMS * sps;
+ if (base_align < 0)
+ base_align = 0;
+
+ /* Next step is multi */
+ priv->state = FCCH_STATE_MULTI;
+
+ /* Done. We discard what we won't use */
+ return base_align;
+}
+
+static int
+_fcch_sink_work_multi(struct sample_actor *sa,
+ float complex *data, unsigned int len)
+{
+ struct fcch_sink_priv *priv = sa->priv;
+ struct osmo_cxvec _win, *win = &_win;
+ int win_len, sps, mtoa[16], n_fcch;
+ float ref_snr = 0.0f, ref_freq_err = 0.0f;
+ int rv, i, j;
+
+ /* Params */
+ sps = priv->as->sps;
+
+ /* Get large enough window */
+ win_len = ((650 * GMR1_SYM_RATE * sps) / 1000);
+
+ rv = win_map(win, data, len, 0, win_len);
+ if (rv < 0)
+ return 0; /* Not enough data yet */
+
+ /* Multi FCCH detection */
+ rv = gmr1_fcch_rough_multi(win, sps, -priv->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, data, len,
+ mtoa[i], GMR1_FCCH_SYMS * sps);
+
+ rv = gmr1_fcch_fine(win, sps, -priv->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, data, len,
+ mtoa[i] + toa + 5 * sps,
+ (117 - 10) * sps);
+
+ e_fcch = burst_energy(win);
+
+ win_map(win, data, len,
+ mtoa[i] + toa + (5 + 117) * sps,
+ (117 - 10) * 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:%d [snr = %.1f dB, freq_err = %.1f Hz]\n",
+ priv->chan_id,
+ (int)(sa->time + mtoa[i] + toa),
+ to_db(snr),
+ to_hz(freq_err + priv->freq_err)
+ );
+
+ /* Save it */
+ mtoa[j++] = mtoa[i] + toa;
+ }
+
+ n_fcch = j;
+
+ /* Create processing tasks for survivors */
+ for (i=0; i<n_fcch; i++) {
+ struct bcch_sink_params p = {
+ .as = priv->as,
+ .chan_id = priv->chan_id,
+ .align = sa->time + mtoa[i],
+ .freq_err = priv->freq_err,
+ };
+ sa = sbuf_add_consumer(priv->as->buf, priv->chan_id, &bcch_sink, &p);
+ if (!sa) {
+ fprintf(stderr, "[!] Failed to create BCCH sink for stream #%d\n", i);
+ return -ENOMEM;
+ }
+ }
+
+ /* All done here */
+ return -1;
+}
+
+static int
+fcch_sink_work(struct sample_actor *sa,
+ float complex *data, unsigned int len)
+{
+ struct fcch_sink_priv *priv = sa->priv;
+
+ if (priv->state == FCCH_STATE_SINGLE)
+ return _fcch_sink_work_single(sa, data, len);
+ else if (priv->state == FCCH_STATE_MULTI)
+ return _fcch_sink_work_multi(sa, data, len);
+ else
+ return -EINVAL;
+}
+
+const struct sample_actor_desc fcch_sink = {
+ .init = fcch_sink_init,
+ .fini = fcch_sink_fini,
+ .work = fcch_sink_work,
+ .priv_size = sizeof(struct fcch_sink_priv),
+};
+
+
+/* Main ------------------------------------------------------------------- */
+
+int main(int argc, char *argv[])
+{
+ struct app_state _as, *as = &_as;
+ int rv = 0, i;
+
+ /* Init app state */
+ memset(as, 0x00, sizeof(struct app_state));
+
+ /* Args */
+ if (argc < 3) {
+ fprintf(stderr, "Usage: %s sps arfcn1:file1 [arfcn2:file2] ...\n", argv[0]);
+ return -EINVAL;
+ }
+
+ as->n_chans = argc - 2;
+
+ as->sps = atoi(argv[1]);
+ if (as->sps < 1 || as->sps > 16) {
+ fprintf(stderr, "[!] sps must be withing [1,16]\n");
+ return -EINVAL;
+ }
+
+ /* Init GSMTap */
+ as->gti = gsmtap_source_init("127.0.0.1", GSMTAP_UDP_PORT, 0);
+ gsmtap_source_add_sink(as->gti);
+
+ /* Buffer */
+ as->buf = sbuf_alloc(as->n_chans);
+ if (!as->buf) {
+ rv = -ENOMEM;
+ goto err;
+ }
+
+ /* Parse arguments */
+ for (i=0; i<as->n_chans; i++)
+ {
+ char *d;
+
+ d = strchr(argv[i+2], ':');
+ if (!d) {
+ fprintf(stderr, "[!] Arguments must be of the form arfcn:filename\n");
+ rv = -EINVAL;
+ goto err;
+ }
+
+ *d = '\0';
+
+ as->arfcn[i] = atoi(argv[i+2]);
+ as->filename[i] = d+1;
+ }
+
+ /* Create all the sources */
+ for (i=0; i<as->n_chans; i++) {
+ struct sample_actor *sa;
+ sa = sbuf_set_producer(as->buf, i, &sa_file_src, as->filename[i]);
+ if (!sa) {
+ fprintf(stderr, "[!] Failed to create source for stream #%d\n", i);
+ rv = -EIO;
+ goto err;
+ }
+ }
+
+ /* Attribute single 'FCCH detect' sink to each channel */
+ for (i=0; i<as->n_chans; i++) {
+ struct sample_actor *sa;
+ struct fcch_sink_params p = { .as = as, .chan_id = i };
+ sa = sbuf_add_consumer(as->buf, i, &fcch_sink, &p);
+ if (!sa) {
+ fprintf(stderr, "[!] Failed to create FCCH sink for stream #%d\n", i);
+ rv = -ENOMEM;
+ goto err;
+ }
+ }
+
+ /* Go forth and process ! */
+ sbuf_work(as->buf);
+
+ /* Done ! */
+ rv = 0;
+
+ /* Clean up */
+err:
+ sbuf_free(as->buf);
+
+ return rv;
+}
diff --git a/src/gsmtap.c b/src/gsmtap.c
index 3bfeff0..25c9351 100644
--- a/src/gsmtap.c
+++ b/src/gsmtap.c
@@ -37,11 +37,14 @@
/*! \brief Helper to build GSM tap message with GMR-1 payload
* \param[in] chan_type Type of channel (one of GSMTAP_GMR1_xxx)
+ * \param[in] arfcn ARFCN
+ * \param[in] fn Frame number
+ * \param[in] tn Timeslot number
* \param[in] l2 Packet of L2 data to encapsulate
* \param[in] len Length of the l2 data in bytes
*/
struct msgb *
-gmr1_gsmtap_makemsg(uint8_t chan_type, uint32_t fn, uint8_t tn,
+gmr1_gsmtap_makemsg(uint8_t chan_type, uint16_t arfcn, uint32_t fn, uint8_t tn,
const uint8_t *l2, int len)
{
struct msgb *msg;
@@ -56,6 +59,7 @@ gmr1_gsmtap_makemsg(uint8_t chan_type, uint32_t fn, uint8_t tn,
gh->version = GSMTAP_VERSION;
gh->hdr_len = sizeof(*gh)/4;
gh->type = GSMTAP_TYPE_GMR1_UM;
+ gh->arfcn = htons(arfcn);
gh->timeslot = tn;
gh->sub_slot = 0;
gh->snr_db = 0;
diff --git a/src/ringbuf.c b/src/ringbuf.c
new file mode 100644
index 0000000..64604fb
--- /dev/null
+++ b/src/ringbuf.c
@@ -0,0 +1,141 @@
+/* Ring Buffer implementation */
+
+/* (C) 2012 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 <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <sys/types.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+
+#include "ringbuf.h"
+
+
+static int
+_osmo_rb_create_file(off_t len)
+{
+ char path[] = "/dev/shm/osmo-rb-XXXXXX";
+ mode_t oum;
+ int fd, rv;
+
+ /* Make sure only user can use temp file */
+ oum = umask(0777);
+
+ /* Create temporary file */
+ fd = mkstemp(path);
+ if (fd < 0)
+ return -errno;
+
+ /* Restore umask */
+ umask(oum);
+
+ /* Remove file */
+ rv = unlink(path);
+ if (rv) {
+ close(fd);
+ return -errno;
+ }
+
+ /* Adjust size */
+ rv = ftruncate(fd, len);
+ if (rv < 0) {
+ close(fd);
+ return -errno;
+ }
+
+ return fd;
+}
+
+static void *
+_osmo_rb_mmap_file(int fd, off_t len)
+{
+ void *base, *p;
+
+ base = mmap(NULL, len << 1, PROT_NONE, MAP_ANONYMOUS | MAP_PRIVATE, -1, 0);
+ if (base == MAP_FAILED)
+ return NULL;
+
+ p = mmap(base, len, PROT_READ | PROT_WRITE, MAP_FIXED | MAP_SHARED, fd, 0);
+ if (p != base)
+ return NULL;
+
+ p = mmap(base + len, len, PROT_READ | PROT_WRITE, MAP_FIXED | MAP_SHARED, fd, 0);
+ if (p != (base + len))
+ return NULL;
+
+ return base;
+}
+
+int
+osmo_rb_init(struct osmo_ringbuf *rb, unsigned int len)
+{
+ int fd, rv = 0;
+
+ memset(rb, 0x00, sizeof(struct osmo_ringbuf));
+
+ rb->len = len;
+
+ fd = _osmo_rb_create_file(len);
+ if (fd < 0)
+ return fd;
+
+ rb->base = _osmo_rb_mmap_file(fd, len);
+ if (!rb->base)
+ rv = -errno;
+
+ close(fd);
+
+ return rv;
+}
+
+void
+osmo_rb_deinit(struct osmo_ringbuf *rb)
+{
+ munmap(rb->base, rb->len);
+ munmap(rb->base + rb->len, rb->len);
+ munmap(rb->base, rb->len << 1);
+}
+
+struct osmo_ringbuf *
+osmo_rb_alloc(unsigned int len)
+{
+ struct osmo_ringbuf *rb;
+
+ rb = malloc(sizeof(struct osmo_ringbuf));
+ if (!rb)
+ return NULL;
+
+ if (osmo_rb_init(rb, len)) {
+ free(rb);
+ return NULL;
+ }
+
+ return rb;
+}
+
+void
+osmo_rb_free(struct osmo_ringbuf *rb)
+{
+ if (rb) {
+ osmo_rb_deinit(rb);
+ free(rb);
+ }
+}
diff --git a/src/ringbuf.h b/src/ringbuf.h
new file mode 100644
index 0000000..bd7060a
--- /dev/null
+++ b/src/ringbuf.h
@@ -0,0 +1,86 @@
+/* Ring Buffer implementation */
+
+/* (C) 2012 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/>.
+ */
+
+#ifndef __OSMO_RINGBUF_H__
+#define __OSMO_RINGBUF_H__
+
+
+struct osmo_ringbuf
+{
+ /* Memory zone */
+ void *base; /*!< Base pointer */
+ unsigned int len; /*!< Length in bytes */
+
+ /* Pointers */
+ unsigned int ri; /*!< Read index */
+ unsigned int wi; /*!< Write index */
+};
+
+
+int osmo_rb_init(struct osmo_ringbuf *rb, unsigned int len);
+void osmo_rb_deinit(struct osmo_ringbuf *rb);
+struct osmo_ringbuf *osmo_rb_alloc(unsigned int len);
+void osmo_rb_free(struct osmo_ringbuf *rb);
+
+
+static inline void
+osmo_rb_clear(struct osmo_ringbuf *rb)
+{
+ rb->wi = 0;
+ rb->ri = 0;
+}
+
+static inline unsigned int
+osmo_rb_used_bytes(struct osmo_ringbuf *rb)
+{
+ return (rb->wi - rb->ri + rb->len) % rb->len;
+}
+
+static inline unsigned int
+osmo_rb_free_bytes(struct osmo_ringbuf *rb)
+{
+ return rb->len - osmo_rb_used_bytes(rb) - 1;
+}
+
+static inline void *
+osmo_rb_write_ptr(struct osmo_ringbuf *rb)
+{
+ return rb->base + rb->wi;
+}
+
+static inline void *
+osmo_rb_read_ptr(struct osmo_ringbuf *rb)
+{
+ return rb->base + rb->ri;
+}
+
+static inline void
+osmo_rb_write_advance(struct osmo_ringbuf *rb, unsigned int bytes)
+{
+ rb->wi = (rb->wi + bytes) % rb->len;
+}
+
+static inline void
+osmo_rb_read_advance(struct osmo_ringbuf *rb, unsigned int bytes)
+{
+ rb->ri = (rb->ri + bytes) % rb->len;
+}
+
+
+#endif /* __OSMO_RINGBUF_H__ */
diff --git a/src/sa_file.c b/src/sa_file.c
new file mode 100644
index 0000000..37e910e
--- /dev/null
+++ b/src/sa_file.c
@@ -0,0 +1,115 @@
+/* File source/sink for use with Sample Buffer */
+
+/* (C) 2012 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 <errno.h>
+#include <stdio.h>
+
+#include "sampbuf.h"
+
+
+#define MAX_CHUNK_SIZE (1 << 14) /* 128k samples = 1 Mo */
+
+
+struct sa_file_priv {
+ FILE *fh;
+};
+
+static int
+sa_file_src_init(struct sample_actor *sc, void *params)
+{
+ struct sa_file_priv *p = sc->priv;
+ const char *filename = params;
+
+ p->fh = fopen(filename, "rb");
+ if (!p->fh) {
+ fprintf(stderr, "[!] File Source: Failed to open input file '%s'\n", filename);
+ perror("fopen");
+ return -errno;
+ }
+
+ return 0;
+}
+
+static int
+sa_file_sink_init(struct sample_actor *sc, void *params)
+{
+ struct sa_file_priv *p = sc->priv;
+ const char *filename = params;
+
+ p->fh = fopen(filename, "wb");
+ if (!p->fh) {
+ fprintf(stderr, "[!] File Sink: Failed to open output file '%s'\n", filename);
+ perror("fopen");
+ return -errno;
+ }
+
+ return 0;
+}
+
+static void
+sa_file_fini(struct sample_actor *sc)
+{
+ struct sa_file_priv *p = sc->priv;
+ fclose(p->fh);
+}
+
+static int
+sa_file_src_work(struct sample_actor *sc,
+ float complex *data, unsigned int len)
+{
+ struct sa_file_priv *p = sc->priv;
+ size_t rv;
+
+ if (len > MAX_CHUNK_SIZE)
+ len = MAX_CHUNK_SIZE;
+
+ rv = fread(data, sizeof(float complex), len, p->fh);
+
+ return rv > 0 ? rv : -1;
+}
+
+static int
+sa_file_sink_work(struct sample_actor *sc,
+ float complex *data, unsigned int len)
+{
+ struct sa_file_priv *p = sc->priv;
+ size_t rv;
+
+ if (len > MAX_CHUNK_SIZE)
+ len = MAX_CHUNK_SIZE;
+
+ rv = fwrite(data, sizeof(float complex), len, p->fh);
+
+ return rv > 0 ? rv : -1;
+}
+
+
+const struct sample_actor_desc sa_file_src = {
+ .init = sa_file_src_init,
+ .fini = sa_file_fini,
+ .work = sa_file_src_work,
+ .priv_size = sizeof(struct sa_file_priv),
+};
+
+const struct sample_actor_desc sa_file_sink = {
+ .init = sa_file_sink_init,
+ .fini = sa_file_fini,
+ .work = sa_file_sink_work,
+ .priv_size = sizeof(struct sa_file_priv),
+};
diff --git a/src/sa_file.h b/src/sa_file.h
new file mode 100644
index 0000000..e65301a
--- /dev/null
+++ b/src/sa_file.h
@@ -0,0 +1,30 @@
+/* File source/sink for use with Sample Buffer */
+
+/* (C) 2012 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/>.
+ */
+
+#ifndef __SA_FILE_H__
+#define __SA_FILE_H__
+
+
+#include "sampbuf.h"
+
+extern const struct sample_actor_desc sa_file_src;
+extern const struct sample_actor_desc sa_file_sink;
+
+
+#endif /* __SA_FILE_H__ */
diff --git a/src/sampbuf.c b/src/sampbuf.c
new file mode 100644
index 0000000..93db857
--- /dev/null
+++ b/src/sampbuf.c
@@ -0,0 +1,358 @@
+/* Sample Buffer with producer / consummer model */
+
+/* (C) 2012 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 <stdint.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+#include <osmocom/core/linuxlist.h>
+
+#include "ringbuf.h"
+#include "sampbuf.h"
+
+
+/* ------------------------------------------------------------------------ */
+/* Sample Actor */
+/* ------------------------------------------------------------------------ */
+
+struct sample_actor *
+sact_alloc(const struct sample_actor_desc *desc, void *params)
+{
+ struct sample_actor *sact;
+ int rv;
+
+ sact = calloc(1, sizeof(struct sample_actor));
+ if (!sact)
+ return NULL;
+
+ INIT_LLIST_HEAD(&sact->list);
+
+ sact->desc = desc;
+
+ if (desc->priv_size > 0) {
+ sact->priv = calloc(1, desc->priv_size);
+ if (!sact->priv)
+ goto err;
+ }
+
+ rv = sact->desc->init(sact, params);
+ if (rv)
+ goto err;
+
+ return sact;
+
+err:
+ free(sact->priv);
+ free(sact);
+ return NULL;
+}
+
+void
+sact_free(struct sample_actor *sact)
+{
+ if (sact) {
+ sact->desc->fini(sact);
+ if (sact->desc->priv_size > 0)
+ free(sact->priv);
+ free(sact);
+ }
+}
+
+
+/* ------------------------------------------------------------------------ */
+/* Sample Buffer */
+/* ------------------------------------------------------------------------ */
+
+struct sample_buf *
+sbuf_alloc(int n_chans)
+{
+ struct sample_buf *sbuf;
+ int i;
+
+ sbuf = calloc(1, sizeof(struct sample_buf));
+ if (!sbuf)
+ return NULL;
+
+ sbuf->n_chans = n_chans;
+
+ for (i=0; i<n_chans; i++)
+ {
+ INIT_LLIST_HEAD(&sbuf->consumers[i]);
+
+ sbuf->rb[i] = osmo_rb_alloc(RB_LEN);
+ if (!sbuf->rb[i]) {
+ sbuf_free(sbuf);
+ return NULL;
+ }
+ }
+
+ return sbuf;
+}
+
+void
+sbuf_free(struct sample_buf *sbuf)
+{
+ int i;
+
+ if (!sbuf)
+ return;
+
+ for (i=0; i<sbuf->n_chans; i++)
+ {
+ sact_free(sbuf->producer[i]);
+
+ /* FIXME release consumers */
+
+ osmo_rb_free(sbuf->rb[i]);
+ }
+
+ free(sbuf);
+}
+
+
+struct sample_actor *
+sbuf_set_producer(struct sample_buf *sbuf, int chan_id,
+ const struct sample_actor_desc *desc, void *params)
+{
+ struct sample_actor *sact = NULL;
+
+ sact_free(sbuf->producer[chan_id]);
+
+ if (desc) {
+ sact = sact_alloc(desc, params);
+ if (!sact)
+ return NULL;
+
+ sact->time = sbuf->chan_wtime[chan_id];
+ }
+
+ sbuf->producer[chan_id] = sact;
+
+ return sact;
+}
+
+struct sample_actor *
+sbuf_add_consumer(struct sample_buf *sbuf, int chan_id,
+ const struct sample_actor_desc *desc, void *params)
+{
+ struct sample_actor *sact;
+
+ sact = sact_alloc(desc, params);
+ if (!sact)
+ return NULL;
+
+ sact->time = sbuf->chan_rtime[chan_id];
+
+ llist_add(&sact->list, &sbuf->consumers[chan_id]);
+
+ return sact;
+}
+
+
+#define WORK_CHUNK (1 << 17) /* 128k samples */
+
+static int
+_sbuf_chan_produce(struct sample_buf *sbuf, int chan_id)
+{
+ struct sample_actor *sact;
+ float complex *data;
+ int rv, free;
+
+ /* Check free space */
+ free = osmo_rb_free_bytes(sbuf->rb[chan_id]) / sizeof(float complex);
+ if (free < WORK_CHUNK)
+ return 0;
+
+ /* Get producer */
+ sact = sbuf->producer[chan_id];
+ if (!sact)
+ return 0;
+
+ /* Get where to write */
+ data = osmo_rb_write_ptr(sbuf->rb[chan_id]);
+
+ /* Do some work */
+ rv = sact->desc->work(sact, data, WORK_CHUNK);
+
+ /* If nothing was done, return directly */
+ if (!rv)
+ return 0;
+
+ /* If < 0, then this producer is done */
+ if (rv < 0) {
+ sbuf_set_producer(sbuf, chan_id, NULL, NULL);
+ return 0;
+ }
+
+ /* Update state */
+ osmo_rb_write_advance(sbuf->rb[chan_id], sizeof(float complex) * rv);
+
+ sact->time += rv;
+ sbuf->chan_wtime[chan_id] += rv;
+
+ return 1;
+}
+
+static int
+_sbuf_produce(struct sample_buf *sbuf)
+{
+ int i;
+ int work_done = 0;
+
+ for (i=0; i<sbuf->n_chans; i++)
+ work_done |= _sbuf_chan_produce(sbuf, i);
+
+ return work_done;
+}
+
+static int
+_sbuf_chan_consume(struct sample_buf *sbuf, int chan_id)
+{
+ struct sample_actor *sact, *tmp;
+ float complex *data;
+ uint64_t rtime;
+ int used, rv;
+ int work_done = 0;
+
+ /* Check available data */
+ used = osmo_rb_used_bytes(sbuf->rb[chan_id]) / sizeof(float complex);
+
+ /* Get where to write & matchine timestamp */
+ data = osmo_rb_read_ptr(sbuf->rb[chan_id]);
+ rtime = sbuf->chan_rtime[chan_id];
+
+ /* Scan all consumers */
+ llist_for_each_entry_safe(sact, tmp, &sbuf->consumers[chan_id], list)
+ {
+ int adv = sact->time - rtime;
+
+ /* Can we do anything ? */
+ if (used == adv)
+ continue;
+
+ /* Do some work */
+ rv = sact->desc->work(sact, &data[adv], used - adv);
+
+ /* If nothing was done, ... next */
+ if (!rv)
+ continue;
+
+ /* If < 0, then this consumer is done */
+ if (rv < 0) {
+ llist_del(&sact->list);
+ sact_free(sact);
+ continue;
+ }
+
+ /* Update state */
+ sact->time += rv;
+
+ work_done = 1;
+ }
+
+ /* If we did no work and no producer left, we remove all consumers */
+ if (!work_done && !sbuf->producer[chan_id]) {
+ llist_for_each_entry_safe(sact, tmp, &sbuf->consumers[chan_id], list)
+ {
+ llist_del(&sact->list);
+ sact_free(sact);
+ }
+ }
+
+ return work_done;
+}
+
+static int
+_sbuf_consume(struct sample_buf *sbuf)
+{
+ int i, found;
+ uint64_t rtime;
+ int work_done = 0;
+
+ /* Consume data */
+ for (i=0; i<sbuf->n_chans; i++)
+ work_done |= _sbuf_chan_consume(sbuf, i);
+
+ /* Find time up to where we can discard */
+ found = 0;
+
+ for (i=0; i<sbuf->n_chans; i++)
+ {
+ struct sample_actor *sact, *tmp;
+
+ llist_for_each_entry_safe(sact, tmp, &sbuf->consumers[i], list)
+ {
+ if (!found || (rtime > sact->time)) {
+ rtime = sact->time;
+ found = 1;
+ }
+ }
+ }
+
+ /* Actually discard */
+ for (i=0; i<sbuf->n_chans; i++) {
+ int discard_bytes = (rtime - sbuf->chan_rtime[i]) * sizeof(float complex);
+ if (osmo_rb_used_bytes(sbuf->rb[i]) >= discard_bytes)
+ osmo_rb_read_advance(sbuf->rb[i], discard_bytes);
+ else
+ osmo_rb_clear(sbuf->rb[i]);
+ sbuf->chan_rtime[i] = rtime;
+ }
+
+ return work_done;
+}
+
+void
+sbuf_work(struct sample_buf *sbuf)
+{
+ int i, rv;
+ int has_produced, has_consumed;
+ int has_producers, has_consumers;
+
+ while (1) {
+ /* Produce / Consume */
+ has_produced = _sbuf_produce(sbuf);
+
+ has_consumed = 0;
+ do {
+ rv = _sbuf_consume(sbuf);
+ has_consumed |= rv;
+ } while (rv);
+
+ /* Check if there is any producers left */
+ has_producers = 0;
+ for (i=0; i<sbuf->n_chans; i++)
+ has_producers |= (sbuf->producer[i] != NULL);
+
+ /* Check if there is any consumer left */
+ for (i=0; i<sbuf->n_chans; i++)
+ if (!llist_empty(&sbuf->consumers[i]))
+ break;
+ has_consumers = (i != sbuf->n_chans);
+
+ /* Check exit conditions */
+ if (!has_consumers)
+ break;
+
+ if (!has_consumed && !has_producers)
+ break;
+ }
+}
diff --git a/src/sampbuf.h b/src/sampbuf.h
new file mode 100644
index 0000000..d7fe23b
--- /dev/null
+++ b/src/sampbuf.h
@@ -0,0 +1,111 @@
+/* Sample Buffer with producer / consummer model */
+
+/* (C) 2012 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/>.
+ */
+
+#ifndef __SAMPBUF_H__
+#define __SAMPBUF_H__
+
+
+#include <complex.h>
+#include <stdint.h>
+
+#include <osmocom/core/linuxlist.h>
+
+#include "ringbuf.h"
+
+
+/* Sample Actor */
+/* ------------ */
+
+struct sample_actor;
+
+struct sample_actor_desc {
+ int (*init)(struct sample_actor *sc, void *params);
+ void (*fini)(struct sample_actor *sc);
+ int (*work)(struct sample_actor *sc,
+ float complex *data, unsigned int len);
+ int priv_size;
+};
+
+struct sample_actor {
+ /* List */
+ struct llist_head list;
+
+ /* Desc */
+ const struct sample_actor_desc *desc;
+
+ /* State tracking */
+ uint64_t time;
+
+ /* Private data */
+ void *priv;
+};
+
+
+struct sample_actor *
+sact_alloc(const struct sample_actor_desc *desc, void *params);
+
+void
+sact_free(struct sample_actor *sact);
+
+
+/* Sample Buffer */
+/* ------------- */
+
+#define RB_LEN (1 << 24) /* 16 Mb */
+#define MAX_CHANS 10
+
+struct sample_buf {
+ /* Channels */
+ int n_chans;
+
+ /* Underlying storage */
+ struct osmo_ringbuf *rb[MAX_CHANS];
+
+ /* Time/Sample tracking */
+ uint64_t chan_wtime[MAX_CHANS];
+ uint64_t chan_rtime[MAX_CHANS];
+
+ /* Producer */
+ struct sample_actor *producer[MAX_CHANS];
+
+ /* Consumers */
+ struct llist_head consumers[MAX_CHANS];
+};
+
+
+struct sample_buf *
+sbuf_alloc(int n_chans);
+
+void
+sbuf_free(struct sample_buf *sbuf);
+
+
+struct sample_actor *
+sbuf_set_producer(struct sample_buf *sbuf, int chan_id,
+ const struct sample_actor_desc *desc, void *params);
+
+struct sample_actor *
+sbuf_add_consumer(struct sample_buf *sbuf, int chan_id,
+ const struct sample_actor_desc *desc, void *params);
+
+
+void sbuf_work(struct sample_buf *sbuf);
+
+
+#endif /* __SAMPBUF_H__ */
generated by cgit v1.2.3 (git 2.25.1) at 2024-04-23 19:17:06 +0000