aboutsummaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorSylvain Munaut <tnt@246tNt.com>2015-04-05 15:51:37 +0200
committerSylvain Munaut <tnt@246tNt.com>2015-04-05 15:51:37 +0200
commit14e67cdaa90ef4de0876cad1b6cfb6ccf6c684a8 (patch)
tree490eae71c27df23356824f59c8d5012d4a4f5de4
parent42c03de385e4eeb94fe852a1ab2fc00ac6f38366 (diff)
rtfwk: Split into separate files for each task
Signed-off-by: Sylvain Munaut <tnt@246tNt.com>
-rw-r--r--src/Makefile.am4
-rw-r--r--src/gmr1_rx_live.c1356
-rw-r--r--src/rtfwk/Makefile.am3
-rw-r--r--src/rtfwk/common.c75
-rw-r--r--src/rtfwk/common.h81
-rw-r--r--src/rtfwk/sa_bcch_ccch.c392
-rw-r--r--src/rtfwk/sa_bcch_ccch.h36
-rw-r--r--src/rtfwk/sa_fcch.c253
-rw-r--r--src/rtfwk/sa_fcch.h32
-rw-r--r--src/rtfwk/sa_file.h8
-rw-r--r--src/rtfwk/sa_tch3.c471
-rw-r--r--src/rtfwk/sa_tch3.h40
-rw-r--r--src/rtfwk/sa_tch9.c258
-rw-r--r--src/rtfwk/sa_tch9.h40
-rw-r--r--src/rtfwk/sampbuf.h8
15 files changed, 1694 insertions, 1363 deletions
diff --git a/src/Makefile.am b/src/Makefile.am
index 844bc46..9fb870f 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -11,9 +11,9 @@ gmr1_rx_LDADD = $(top_builddir)/src/l1/libgmr1-l1.a \
$(LIBOSMOCORE_LIBS) $(LIBOSMODSP_LIBS) $(FFTW3F_LIBS)
gmr1_rx_live_SOURCES = gmr1_rx_live.c gsmtap.c
-gmr1_rx_live_LDADD = $(top_builddir)/src/l1/libgmr1-l1.a \
+gmr1_rx_live_LDADD = $(top_builddir)/src/rtfwk/libgmr1-rtfwk.a \
+ $(top_builddir)/src/l1/libgmr1-l1.a \
$(top_builddir)/src/sdr/libgmr1-sdr.a \
- $(top_builddir)/src/rtfwk/libgmr1-rtfwk.a \
$(LIBOSMOCORE_LIBS) $(LIBOSMODSP_LIBS) $(FFTW3F_LIBS)
gmr1_rach_gen_SOURCES = gmr1_rach_gen.c
diff --git a/src/gmr1_rx_live.c b/src/gmr1_rx_live.c
index 1c66288..45dfc12 100644
--- a/src/gmr1_rx_live.c
+++ b/src/gmr1_rx_live.c
@@ -17,1368 +17,20 @@
* 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 <stdlib.h>
+#include <string.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 "rtfwk/common.h"
#include "rtfwk/sampbuf.h"
+#include "rtfwk/sa_fcch.h"
#include "rtfwk/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[])
diff --git a/src/rtfwk/Makefile.am b/src/rtfwk/Makefile.am
index cba617c..6c174bc 100644
--- a/src/rtfwk/Makefile.am
+++ b/src/rtfwk/Makefile.am
@@ -5,4 +5,5 @@ AM_LDFLAGS = $(LIBOSMOCORE_LIBS) $(LIBOSMODSP_LIBS) $(FFTW3F_LIBS)
noinst_LIBRARIES = libgmr1-rtfwk.a
libgmr1_rtfwk_a_SOURCES = \
- ringbuf.c sa_file.c sampbuf.c
+ common.c ringbuf.c sampbuf.c sa_file.c \
+ sa_bcch_ccch.c sa_fcch.c sa_tch3.c sa_tch9.c
diff --git a/src/rtfwk/common.c b/src/rtfwk/common.c
new file mode 100644
index 0000000..ba182f6
--- /dev/null
+++ b/src/rtfwk/common.c
@@ -0,0 +1,75 @@
+/* GMR-1 RT framework common functions */
+
+/* (C) 2015 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 <osmocom/dsp/cxvec.h>
+#include <osmocom/dsp/cxvec_math.h>
+
+#include <osmocom/gmr1/sdr/defs.h>
+#include <osmocom/gmr1/sdr/pi4cxpsk.h>
+
+#include "common.h"
+
+
+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;
+}
+
+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;
+}
+
+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;
+}
diff --git a/src/rtfwk/common.h b/src/rtfwk/common.h
new file mode 100644
index 0000000..f04cb6e
--- /dev/null
+++ b/src/rtfwk/common.h
@@ -0,0 +1,81 @@
+/* GMR-1 RT framework: Common definitions */
+
+/* (C) 2015 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 __RTFWK_COMMON_H__
+#define __RTFWK_COMMON_H__
+
+#include <math.h>
+
+#include <osmocom/dsp/cxvec.h>
+#include <osmocom/dsp/cxvec_math.h>
+
+#include <osmocom/gmr1/sdr/defs.h>
+
+#include "sampbuf.h"
+
+
+struct gsmtap_inst;
+struct gmr1_pi4cxpsk_burst;
+
+
+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;
+};
+
+
+int
+win_map(struct osmo_cxvec *win,
+ float complex *data, int data_len,
+ int begin, int win_len);
+
+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);
+
+float
+burst_energy(struct osmo_cxvec *burst);
+
+
+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);
+}
+
+
+#endif /* __RTFWK_COMMON_H__ */
diff --git a/src/rtfwk/sa_bcch_ccch.c b/src/rtfwk/sa_bcch_ccch.c
new file mode 100644
index 0000000..d05a76a
--- /dev/null
+++ b/src/rtfwk/sa_bcch_ccch.c
@@ -0,0 +1,392 @@
+/* GMR-1 RT framework: BCCH/CCCH task */
+
+/* (C) 2015 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 <stdio.h>
+
+#include <osmocom/core/gsmtap.h>
+#include <osmocom/core/gsmtap_util.h>
+
+#include <osmocom/dsp/cxvec.h>
+
+#include <osmocom/gmr1/gsmtap.h>
+#include <osmocom/gmr1/l1/bcch.h>
+#include <osmocom/gmr1/l1/ccch.h>
+#include <osmocom/gmr1/sdr/defs.h>
+#include <osmocom/gmr1/sdr/pi4cxpsk.h>
+#include <osmocom/gmr1/sdr/nb.h>
+
+#include "common.h"
+#include "sampbuf.h"
+#include "sa_bcch_ccch.h"
+#include "sa_tch3.h"
+
+
+#define BCCH_MARGIN 100
+
+
+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),
+};
diff --git a/src/rtfwk/sa_bcch_ccch.h b/src/rtfwk/sa_bcch_ccch.h
new file mode 100644
index 0000000..60b5ec7
--- /dev/null
+++ b/src/rtfwk/sa_bcch_ccch.h
@@ -0,0 +1,36 @@
+/* GMR-1 RT framework: BCCH/CCCH task */
+
+/* (C) 2015 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 __RTFWK_SA_BCCH_CCCH_H__
+#define __RTFWK_SA_BCCH_CCCH_H__
+
+#include <stdint.h>
+
+struct app_state;
+
+struct bcch_sink_params {
+ struct app_state *as;
+ int chan_id;
+ uint64_t align;
+ float freq_err;
+};
+
+extern const struct sample_actor_desc bcch_sink;
+
+#endif /* __RTFWK_SA_BCCH_CCCH_H__ */
diff --git a/src/rtfwk/sa_fcch.c b/src/rtfwk/sa_fcch.c
new file mode 100644
index 0000000..292601f
--- /dev/null
+++ b/src/rtfwk/sa_fcch.c
@@ -0,0 +1,253 @@
+/* GMR-1 RT framework: FCCH task */
+
+/* (C) 2015 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 <stdio.h>
+
+#include <osmocom/dsp/cxvec.h>
+
+#include <osmocom/gmr1/sdr/defs.h>
+#include <osmocom/gmr1/sdr/fcch.h>
+
+#include "common.h"
+#include "sampbuf.h"
+#include "sa_fcch.h"
+#include "sa_bcch_ccch.h"
+
+
+#define START_DISCARD 8000
+
+
+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),
+};
diff --git a/src/rtfwk/sa_fcch.h b/src/rtfwk/sa_fcch.h
new file mode 100644
index 0000000..d2e60b8
--- /dev/null
+++ b/src/rtfwk/sa_fcch.h
@@ -0,0 +1,32 @@
+/* GMR-1 RT framework: FCCH task */
+
+/* (C) 2015 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 __RTFWK_SA_FCCH_H__
+#define __RTFWK_SA_FCCH_H__
+
+struct app_state;
+
+struct fcch_sink_params {
+ struct app_state *as;
+ int chan_id;
+};
+
+extern const struct sample_actor_desc fcch_sink;
+
+#endif /* __RTFWK_SA_FCCH_H__ */
diff --git a/src/rtfwk/sa_file.h b/src/rtfwk/sa_file.h
index e65301a..d9882d4 100644
--- a/src/rtfwk/sa_file.h
+++ b/src/rtfwk/sa_file.h
@@ -1,4 +1,4 @@
-/* File source/sink for use with Sample Buffer */
+/* GMR-1 RT framework: File source/sink */
/* (C) 2012 by Sylvain Munaut <tnt@246tNt.com>
* All Rights Reserved
@@ -17,8 +17,8 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
-#ifndef __SA_FILE_H__
-#define __SA_FILE_H__
+#ifndef __RTFWK_SA_FILE_H__
+#define __RTFWK_SA_FILE_H__
#include "sampbuf.h"
@@ -27,4 +27,4 @@ extern const struct sample_actor_desc sa_file_src;
extern const struct sample_actor_desc sa_file_sink;
-#endif /* __SA_FILE_H__ */
+#endif /* __RTFWK_SA_FILE_H__ */
diff --git a/src/rtfwk/sa_tch3.c b/src/rtfwk/sa_tch3.c
new file mode 100644
index 0000000..8251b29
--- /dev/null
+++ b/src/rtfwk/sa_tch3.c
@@ -0,0 +1,471 @@
+/* GMR-1 RT framework: TCH3 task */
+
+/* (C) 2015 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 <stdio.h>
+#include <string.h>
+
+#include <osmocom/core/gsmtap.h>
+#include <osmocom/core/gsmtap_util.h>
+
+#include <osmocom/dsp/cxvec.h>
+
+#include <osmocom/gmr1/gsmtap.h>
+#include <osmocom/gmr1/l1/a5.h>
+#include <osmocom/gmr1/l1/facch3.h>
+#include <osmocom/gmr1/l1/tch3.h>
+#include <osmocom/gmr1/sdr/defs.h>
+#include <osmocom/gmr1/sdr/dkab.h>
+#include <osmocom/gmr1/sdr/pi4cxpsk.h>
+#include <osmocom/gmr1/sdr/nb.h>
+
+#include "common.h"
+#include "sampbuf.h"
+#include "sa_tch3.h"
+#include "sa_tch9.h"
+
+
+#define TCH3_MARGIN 10
+
+
+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),
+};
diff --git a/src/rtfwk/sa_tch3.h b/src/rtfwk/sa_tch3.h
new file mode 100644
index 0000000..2f52061
--- /dev/null
+++ b/src/rtfwk/sa_tch3.h
@@ -0,0 +1,40 @@
+/* GMR-1 RT framework: TCH3 task */
+
+/* (C) 2015 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 __RTFWK_SA_TCH3_H__
+#define __RTFWK_SA_TCH3_H__
+
+#include <stdint.h>
+
+struct app_state;
+
+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;
+};
+
+extern const struct sample_actor_desc tch3_sink;
+
+#endif /* __RTFWK_SA_TCH3_H__ */
diff --git a/src/rtfwk/sa_tch9.c b/src/rtfwk/sa_tch9.c
new file mode 100644
index 0000000..34ebdb5
--- /dev/null
+++ b/src/rtfwk/sa_tch9.c
@@ -0,0 +1,258 @@
+/* GMR-1 RT framework: TCH9 task */
+
+/* (C) 2015 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 <stdio.h>
+#include <string.h>
+
+#include <osmocom/core/gsmtap.h>
+#include <osmocom/core/gsmtap_util.h>
+
+#include <osmocom/dsp/cxvec.h>
+
+#include <osmocom/gmr1/gsmtap.h>
+#include <osmocom/gmr1/l1/a5.h>
+#include <osmocom/gmr1/l1/facch9.h>
+#include <osmocom/gmr1/l1/tch9.h>
+#include <osmocom/gmr1/l1/interleave.h>
+#include <osmocom/gmr1/sdr/defs.h>
+#include <osmocom/gmr1/sdr/pi4cxpsk.h>
+#include <osmocom/gmr1/sdr/nb.h>
+
+#include "common.h"
+#include "sampbuf.h"
+#include "sa_tch9.h"
+
+
+#define TCH9_MARGIN 50
+
+
+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),
+};
diff --git a/src/rtfwk/sa_tch9.h b/src/rtfwk/sa_tch9.h
new file mode 100644
index 0000000..4f5b330
--- /dev/null
+++ b/src/rtfwk/sa_tch9.h
@@ -0,0 +1,40 @@
+/* GMR-1 RT framework: TCH9 task */
+
+/* (C) 2015 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 __RTFWK_SA_TCH9_H__
+#define __RTFWK_SA_TCH9_H__
+
+#include <stdint.h>
+
+struct app_state;
+
+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];
+};
+
+extern const struct sample_actor_desc tch9_sink;
+
+#endif /* __RTFWK_SA_TCH9_H__ */
diff --git a/src/rtfwk/sampbuf.h b/src/rtfwk/sampbuf.h
index d7fe23b..183005a 100644
--- a/src/rtfwk/sampbuf.h
+++ b/src/rtfwk/sampbuf.h
@@ -1,4 +1,4 @@
-/* Sample Buffer with producer / consummer model */
+/* GMR-1 RT framework: Sample Buffer with producer/consumer model */
/* (C) 2012 by Sylvain Munaut <tnt@246tNt.com>
* All Rights Reserved
@@ -17,8 +17,8 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
-#ifndef __SAMPBUF_H__
-#define __SAMPBUF_H__
+#ifndef __RTFWK_SAMPBUF_H__
+#define __RTFWK_SAMPBUF_H__
#include <complex.h>
@@ -108,4 +108,4 @@ sbuf_add_consumer(struct sample_buf *sbuf, int chan_id,
void sbuf_work(struct sample_buf *sbuf);
-#endif /* __SAMPBUF_H__ */
+#endif /* __RTFWK_SAMPBUF_H__ */