1 files changed, 122 insertions, 0 deletions
diff --git a/src/target/firmware/layer1/afc.c b/src/target/firmware/layer1/afc.c
new file mode 100644
index 00000000..846d208f
--- /dev/null
+++ b/src/target/firmware/layer1/afc.c
@@ -0,0 +1,122 @@
+/* AFC (Automatic Frequency Correction) Implementation */
+
+/* (C) 2010 by Harald Welte <laforge@gnumonks.org>
+ *
+ * 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 <gsm.h>
+
+#include <layer1/afc.h>
+#include <layer1/avg.h>
+#include <calypso/dsp.h>
+
+/* Over how many TDMA frames do we want to average? (this may change in dedicated mode) */
+#define AFC_PERIOD		40
+/* How many of our measurements have to be valid? */
+#define AFC_MIN_MUN_VALID	8
+
+/* The actual AFC code */
+
+struct afc_state {
+	struct running_avg ravg;		/* running average */
+	int16_t		dac_value;		/* current DAC output value */
+	uint16_t	arfcn;
+};
+
+static void afc_ravg_output(struct running_avg *ravg, int32_t avg);
+
+static struct afc_state afc_state = {
+	.ravg = {
+		.outfn = &afc_ravg_output,
+		.period = AFC_PERIOD,
+		.min_valid = AFC_MIN_MUN_VALID,
+	},
+};
+
+/* The AFC DAC in the ABB has to be configured as follows:
+ * DAC = 1MHz / 947MHz * FreqErr(Hz) / AFCslop(ppm/LSB)
+ * where:
+ *	947 MHz is the center of EGSM
+ *	AFCslope is coded F1.15, thus a normalization factor of 2^15 aplpies
+ */
+
+#define AFC_NORM_FACTOR_GSM	((1<<15) / 947)
+#define AFC_NORM_FACTOR_DCS	((1<<15) / 1894)
+
+/* we assume 4.9ppb per LSB, equals 0.0049 * 32768 == 160 */
+#define AFC_SLOPE		160
+//#define AFC_SLOPE		141
+
+/* The DSP can measure the frequency error in the following ranges:
+ * 	FB_MODE0:	+/- 20 kHz
+ *	FB_MODE1:	+/-  4 kHz
+ *	Sync Burst:	+/-  1 kHz
+ *	Normal Burst:	+/- 400 Hz
+ */
+
+/* Update the AFC with a frequency error, bypassing averaging */
+void afc_correct(int16_t freq_error, uint16_t arfcn)
+{
+	int32_t afc_norm_factor;
+	int16_t delta;
+
+	switch (gsm_arfcn2band(arfcn)) {
+	case GSM_900:
+	case GSM_850:
+		afc_norm_factor = AFC_NORM_FACTOR_GSM;
+		break;
+	default:
+		afc_norm_factor = AFC_NORM_FACTOR_DCS;
+	}
+
+	delta = (int16_t) ((afc_norm_factor * (int32_t)freq_error) / AFC_SLOPE);
+	printd("afc_correct(error=%dHz, arfcn=%u): delta=%d, afc_dac(old=%d,new=%d)\n",
+		freq_error, arfcn, delta, afc_state.dac_value, afc_state.dac_value+delta);
+	afc_state.dac_value += delta;
+
+	/* The AFC DAC has only 13 bits */
+	if (afc_state.dac_value > 4095)
+		afc_state.dac_value = 4095;
+	else if (afc_state.dac_value < -4096)
+		afc_state.dac_value = -4096;
+}
+
+void afc_input(int32_t freq_error, uint16_t arfcn, int valid)
+{
+	afc_state.arfcn = arfcn;
+	runavg_input(&afc_state.ravg, freq_error, valid);
+	runavg_check_output(&afc_state.ravg);
+}
+
+/* callback function for runavg */
+static void afc_ravg_output(struct running_avg *ravg, int32_t avg)
+{
+	afc_correct(avg, afc_state.arfcn);
+}
+
+/* Update DSP with new AFC DAC value to be used for next TDMA frame */
+void afc_load_dsp(void)
+{
+	dsp_api.db_w->d_afc = afc_state.dac_value;
+	dsp_api.db_w->d_ctrl_abb |= (1 << B_AFC);
+}