fw/layer1: Adding TAO loop to correct TPU window offset

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

8 files changed, 111 insertions, 2 deletions

diff --git a/src/target/firmware/include/layer1/sync.h b/src/target/firmware/include/layer1/sync.h
index 06ce6a00..879e4cfb 100644
--- a/src/target/firmware/include/layer1/sync.h
+++ b/src/target/firmware/include/layer1/sync.h
@@ -71,6 +71,7 @@ struct l1s_state {
 
 	/* The current TPU offset register */
 	uint32_t	tpu_offset;
+	int32_t		tpu_offset_correction;
 
 	/* TX parameters */
 	int8_t		ta;
diff --git a/src/target/firmware/include/layer1/toa.h b/src/target/firmware/include/layer1/toa.h
new file mode 100644
index 00000000..dea9dd96
--- /dev/null
+++ b/src/target/firmware/include/layer1/toa.h
@@ -0,0 +1,10 @@
+#ifndef _L1_TOA_H
+#define _L1_TOA_H
+
+/* Input a qbits error sample into the TOA averaging */
+void toa_input(int32_t offset, uint32_t snr);
+
+/* Reset the TOA counters */
+void toa_reset(void);
+
+#endif
diff --git a/src/target/firmware/layer1/Makefile b/src/target/firmware/layer1/Makefile
index 4f834ea4..455a444a 100644
--- a/src/target/firmware/layer1/Makefile
+++ b/src/target/firmware/layer1/Makefile
@@ -1,8 +1,8 @@
 
 LIBRARIES+=layer1
 layer1_DIR=layer1
-layer1_SRCS=avg.c agc.c afc.c sync.c tdma_sched.c tpu_window.c init.c l23_api.c \
-		mframe_sched.c sched_gsmtime.c async.c rfch.c apc.c
+layer1_SRCS=avg.c agc.c afc.c toa.c sync.c tdma_sched.c tpu_window.c init.c \
+		l23_api.c mframe_sched.c sched_gsmtime.c async.c rfch.c apc.c
 
 layer1_SRCS += prim_pm.c prim_rach.c prim_tx_nb.c prim_rx_nb.c prim_fbsb.c \
 		prim_freq.c prim_utils.c prim_tch.c
diff --git a/src/target/firmware/layer1/prim_fbsb.c b/src/target/firmware/layer1/prim_fbsb.c
index 5177651e..d46e4a42 100644
--- a/src/target/firmware/layer1/prim_fbsb.c
+++ b/src/target/firmware/layer1/prim_fbsb.c
@@ -42,6 +42,7 @@
 
 #include <layer1/sync.h>
 #include <layer1/afc.h>
+#include <layer1/toa.h>
 #include <layer1/tdma_sched.h>
 #include <layer1/mframe_sched.h>
 #include <layer1/tpu_window.h>
@@ -554,6 +555,9 @@ void l1s_fbsb_req(uint8_t base_fn, struct l1ctl_fbsb_req *req)
 	/* Make sure we start at a 'center' AFCDAC output value */
 	afc_reset();
 
+	/* Reset the TOA loop counters */
+	toa_reset();
+
 	if (fbs.req.flags & L1CTL_FBSB_F_FB0)
 		tdma_schedule_set(base_fn, fb_sched_set, 0);
 	else if (fbs.req.flags & L1CTL_FBSB_F_FB1)
diff --git a/src/target/firmware/layer1/prim_rx_nb.c b/src/target/firmware/layer1/prim_rx_nb.c
index 9edb081c..80002884 100644
--- a/src/target/firmware/layer1/prim_rx_nb.c
+++ b/src/target/firmware/layer1/prim_rx_nb.c
@@ -41,6 +41,7 @@
 
 #include <layer1/sync.h>
 #include <layer1/afc.h>
+#include <layer1/toa.h>
 #include <layer1/tdma_sched.h>
 #include <layer1/mframe_sched.h>
 #include <layer1/tpu_window.h>
@@ -100,6 +101,9 @@ static int l1s_nb_resp(__unused uint8_t p1, uint8_t burst_id, uint16_t p3)
 	else
 		afc_input(rxnb.meas[burst_id].freq_err, rf_arfcn, 0);
 
+	/* feed computed TOA into TA loop */
+	toa_input(rxnb.meas[burst_id].toa_qbit << 2, rxnb.meas[burst_id].snr);
+
 	/* Tell the RF frontend to set the gain appropriately */
 	rffe_set_gain(rxnb.meas[burst_id].pm_dbm8/8, CAL_DSP_TGT_BB_LVL);
 
diff --git a/src/target/firmware/layer1/prim_tch.c b/src/target/firmware/layer1/prim_tch.c
index 4c08e3e8..d525ce15 100644
--- a/src/target/firmware/layer1/prim_tch.c
+++ b/src/target/firmware/layer1/prim_tch.c
@@ -45,6 +45,7 @@
 #include <layer1/sync.h>
 #include <layer1/afc.h>
 #include <layer1/agc.h>
+#include <layer1/toa.h>
 #include <layer1/tdma_sched.h>
 #include <layer1/mframe_sched.h>
 #include <layer1/tpu_window.h>
@@ -173,6 +174,9 @@ static int l1s_tch_resp(__unused uint8_t p1, __unused uint8_t p2, uint16_t p3)
 	else
 		afc_input(rx_tch.meas[meas_id].freq_err, arfcn, 0);
 
+	/* feed computed TOA into TA loop */
+	toa_input(rx_tch.meas[meas_id].toa_qbit << 2, rx_tch.meas[meas_id].snr);
+
 	/* Tell the RF frontend to set the gain appropriately */
 	rffe_set_gain(rx_tch.meas[meas_id].pm_dbm8 / 8, CAL_DSP_TGT_BB_LVL);
 
@@ -502,6 +506,9 @@ static int l1s_tch_a_resp(__unused uint8_t p1, __unused uint8_t p2, uint16_t p3)
 	else
 		afc_input(rx_tch_a.meas[burst_id].freq_err, arfcn, 0);
 
+	/* feed computed TOA into TA loop */
+	toa_input(rx_tch_a.meas[burst_id].toa_qbit << 2, rx_tch_a.meas[burst_id].snr);
+
 	/* Tell the RF frontend to set the gain appropriately */
 	rffe_set_gain(rx_tch_a.meas[burst_id].pm_dbm8 / 8, CAL_DSP_TGT_BB_LVL);
 
diff --git a/src/target/firmware/layer1/toa.c b/src/target/firmware/layer1/toa.c
new file mode 100644
index 00000000..adb32f2d
--- /dev/null
+++ b/src/target/firmware/layer1/toa.c
@@ -0,0 +1,80 @@
+/* AFC (Automatic Frequency Correction) Implementation */
+
+/* (C) 2010 by Harald Welte <laforge@gnumonks.org>
+ * (C) 2011 by Andreas Eversberg <jolly@eversberg.eu>
+ *
+ * All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+
+#include <stdint.h>
+#include <stdio.h>
+
+#include <debug.h>
+#include <osmocore/gsm_utils.h>
+
+#include <layer1/toa.h>
+#include <layer1/avg.h>
+#include <layer1/sync.h>
+
+/* Over how many TDMA frames do we want to average? */
+#define TOA_PERIOD		250
+/* How many of our measurements have to be valid? */
+#define TOA_MIN_MUN_VALID	125
+
+// FIXME:
+#define TOA_SNR_THRESHOLD       2560    /* 2.5 dB in fx6.10 */
+
+struct toa_state {
+	struct running_avg ravg;		/* running average */
+};
+
+
+static void toa_ravg_output(struct running_avg *ravg, int32_t avg);
+
+static struct toa_state toa_state = {
+	.ravg = {
+		.outfn = &toa_ravg_output,
+		.period = TOA_PERIOD,
+		.min_valid = TOA_MIN_MUN_VALID,
+	},
+};
+
+void toa_input(int32_t offset, uint32_t snr)
+{
+	int valid = 1;
+
+	if (snr < TOA_SNR_THRESHOLD || offset < 0 || offset >31)
+		valid = 0;
+	runavg_input(&toa_state.ravg, offset, valid);
+	runavg_check_output(&toa_state.ravg);
+}
+
+void toa_reset(void)
+{
+	toa_state.ravg.num_samples = toa_state.ravg.num_samples_valid = 0;
+	toa_state.ravg.acc_val = 0;
+}
+
+/* callback function for runavg */
+static void toa_ravg_output(struct running_avg *ravg, int32_t avg)
+{
+	if (avg != 16) {
+		printf("TOA AVG is not 16 qbits, correcting (got %ld)\n", avg);
+		l1s.tpu_offset_correction = avg - 16;
+	}
+}
diff --git a/src/target/firmware/layer1/tpu_window.c b/src/target/firmware/layer1/tpu_window.c
index 09437da5..8d9c8994 100644
--- a/src/target/firmware/layer1/tpu_window.c
+++ b/src/target/firmware/layer1/tpu_window.c
@@ -73,6 +73,9 @@ static int _win_setup(__unused uint8_t p1, __unused uint8_t p2, __unused uint16_
 
 	rfch_get_params(&l1s.next_time, NULL, NULL, &tn);
 
+	l1s.tpu_offset = (5000 + l1s.tpu_offset + l1s.tpu_offset_correction) % 5000;
+	l1s.tpu_offset_correction = 0;
+
 	tpu_enq_at(4740);
 	tpu_enq_sync((5000 + l1s.tpu_offset + (L1_BURST_LENGTH_Q * tn)) % 5000);