fw/layer1: Add hopping support in rfch utils.

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

1 files changed, 94 insertions, 1 deletions

diff --git a/src/target/firmware/layer1/rfch.c b/src/target/firmware/layer1/rfch.c
index 995cea4a..50331bf5 100644
--- a/src/target/firmware/layer1/rfch.c
+++ b/src/target/firmware/layer1/rfch.c
@@ -22,9 +22,98 @@
 
 #include <stdint.h>
 
+#include <osmocore/gsm_utils.h>
+
 #include <layer1/sync.h>
 
 
+/*
+ * Hopping sequence generation
+ *
+ * The algorithm is explained in GSM 05.02 Section 6.2.3
+ *
+ *	if HSN = 0 (cyclic hopping) then:
+ *		MAI, integer (0 .. N-1) :
+ *			MAI = (FN + MAIO) modulo N
+ *
+ *	else:
+ *		M, integer (0 .. 152) :
+ *			M = T2 + RNTABLE((HSN xor T1R) + T3)
+ *
+ *		S, integer (0 .. N-1) :
+ *			M' = M modulo (2 ^ NBIN)
+ *			T' = T3 modulo (2 ^ NBIN)
+ *
+ *			if M' < N then:
+ *				S = M'
+ *			else:
+ *				S = (M'+T') modulo N
+ *
+ *		MAI, integer (0 .. N-1) :
+ *			MAI = (S + MAIO) modulo N
+ */
+
+static uint8_t rn_table[114] = {
+	 48,  98,  63,   1,  36,  95,  78, 102,  94,  73,
+	  0,  64,  25,  81,  76,  59, 124,  23, 104, 100,
+	101,  47, 118,  85,  18,  56,  96,  86,  54,   2,
+	 80,  34, 127,  13,   6,  89,  57, 103,  12,  74,
+	 55, 111,  75,  38, 109,  71, 112,  29,  11,  88,
+	 87,  19,   3,  68, 110,  26,  33,  31,   8,  45,
+	 82,  58,  40, 107,  32,   5, 106,  92,  62,  67,
+	 77, 108, 122,  37,  60,  66, 121,  42,  51, 126,
+	117, 114,   4,  90,  43,  52,  53, 113, 120,  72,
+	 16,  49,   7,  79, 119,  61,  22,  84,   9,  97,
+	 91,  15,  21,  24,  46,  39,  93, 105,  65,  70,
+	125,  99,  17, 123,
+};
+
+
+static int pow_nbin_mask(int n)
+{
+	int x;
+	x =	(n     ) |
+		(n >> 1) |
+		(n >> 2) |
+		(n >> 3) |
+		(n >> 4) |
+		(n >> 5) |
+		(n >> 6);
+	return x;
+}
+
+static int16_t rfch_hop_seq_gen(struct gsm_time *t,
+                                uint8_t hsn, uint8_t maio,
+                                uint8_t n, uint16_t *arfcn_tbl)
+{
+	int mai;
+
+	if (!hsn) {
+		/* cyclic hopping */
+		mai = (t->fn + maio) % n;
+	} else {
+		/* pseudo random hopping */
+		int m, mp, tp, s, pnm;
+
+		pnm = pow_nbin_mask(n);
+
+		m = t->t2 + rn_table[(hsn ^ (t->t1 & 63)) + t->t3];
+		mp = m & pnm;
+
+		if (mp < n)
+			s = mp;
+		else {
+			tp = t->t3 & pnm;
+			s = (mp + tp) % n;
+		}
+
+		mai = (s + maio) % n;
+	}
+
+	return arfcn_tbl ? arfcn_tbl[mai] : mai;
+}
+
+
 /* RF Channel parameters */
 void rfch_get_params(struct gsm_time *t,
                      uint16_t *arfcn_p, uint8_t *tsc_p, uint8_t *tn_p)
@@ -37,7 +126,11 @@ void rfch_get_params(struct gsm_time *t,
 	} else {
 		/* Dedicated channel */
 		if (l1s.dedicated.h) {
-			/* Not supported yet */
+			*arfcn_p = rfch_hop_seq_gen(t,
+					l1s.dedicated.h1.hsn,
+					l1s.dedicated.h1.maio,
+					l1s.dedicated.h1.n,
+					l1s.dedicated.h1.ma);
 		} else {
 			*arfcn_p = l1s.dedicated.h0.arfcn;
 		}