From 98e43839b9b04f02864bb33d8755678c48d60f4e Mon Sep 17 00:00:00 2001
From: Max <ikj1234i@yahoo.com>
Date: Wed, 22 Nov 2017 19:04:25 -0500
Subject: [PATCH] ysf frame decode

---
 op25/gr-op25_repeater/lib/ysf_const.h       | 71 ++++++++++++++++++++-
 op25/gr-op25_repeater/lib/ysf_tx_sb_impl.cc | 14 +---
 2 files changed, 72 insertions(+), 13 deletions(-)

diff --git a/op25/gr-op25_repeater/lib/ysf_const.h b/op25/gr-op25_repeater/lib/ysf_const.h
index 3538c50..a9883ab 100644
--- a/op25/gr-op25_repeater/lib/ysf_const.h
+++ b/op25/gr-op25_repeater/lib/ysf_const.h
@@ -1,5 +1,6 @@
 //
 // YSF Encoder (C) Copyright 2017 Max H. Parke KA1RBI
+// thx gr-ysf fr_vch_decoder_bb_impl.cc * Copyright 2015 Mathias Weyland *
 // 
 // This file is part of OP25
 // 
@@ -21,6 +22,62 @@
 #ifndef INCLUDED_YSF_CONST_H
 #define INCLUDED_YSF_CONST_H
 
+#include <stdint.h>
+
+static void decode_49bit(int b[9], const uint8_t src[49]) {
+	for (int i=0; i<9; i++)
+		b[i] = 0;
+	b[0] |= src[0] << 6;
+	b[0] |= src[1] << 5;
+	b[0] |= src[2] << 4;
+	b[0] |= src[3] << 3;
+	b[1] |= src[4] << 4;
+	b[1] |= src[5] << 3;
+	b[1] |= src[6] << 2;
+	b[1] |= src[7] << 1;
+	b[2] |= src[8] << 4;
+	b[2] |= src[9] << 3;
+	b[2] |= src[10] << 2;
+	b[2] |= src[11] << 1;
+	b[3] |= src[12] << 8;
+	b[3] |= src[13] << 7;
+	b[3] |= src[14] << 6;
+	b[3] |= src[15] << 5;
+	b[3] |= src[16] << 4;
+	b[3] |= src[17] << 3;
+	b[3] |= src[18] << 2;
+	b[3] |= src[19] << 1;
+	b[4] |= src[20] << 6;
+	b[4] |= src[21] << 5;
+	b[4] |= src[22] << 4;
+	b[4] |= src[23] << 3;
+	b[5] |= src[24] << 4;
+	b[5] |= src[25] << 3;
+	b[5] |= src[26] << 2;
+	b[5] |= src[27] << 1;
+	b[6] |= src[28] << 3;
+	b[6] |= src[29] << 2;
+	b[6] |= src[30] << 1;
+	b[7] |= src[31] << 3;
+	b[7] |= src[32] << 2;
+	b[7] |= src[33] << 1;
+	b[8] |= src[34] << 2;
+	b[1] |= src[35];
+	b[2] |= src[36];
+	b[0] |= src[37] << 2;
+	b[0] |= src[38] << 1;
+	b[0] |= src[39];
+	b[3] |= src[40];
+	b[4] |= src[41] << 2;
+	b[4] |= src[42] << 1;
+	b[4] |= src[43];
+	b[5] |= src[44];
+	b[6] |= src[45];
+	b[7] |= src[46];
+	b[8] |= src[47] << 1;
+	b[8] |= src[48];
+}
+
 static const int gly_24_12[] = {
 	0, 6379, 10558, 12757, 19095, 21116, 25513, 31554, 36294, 38189, 42232, 48147, 51025, 57274, 61039, 63108, 
 	66407, 72588, 76377, 78514, 84464, 86299, 90318, 96293, 102049, 104010, 108447, 114548, 115766, 122077, 126216, 128483, 
@@ -279,7 +336,9 @@ static const int gly_24_12[] = {
 	16648732, 16650999, 16655138, 16661449, 16662667, 16668768, 16673205, 16675166, 16680922, 16686897, 16690916, 16692751, 16698701, 16700838, 16704627, 16710808, 
 	16714107, 16716176, 16719941, 16726190, 16729068, 16734983, 16739026, 16740921, 16745661, 16751702, 16756099, 16758120, 16764458, 16766657, 16770836, 16777215 };
 
-static const uint8_t scramble_code[180] = {
+static inline void ysf_scramble(uint8_t buf[], const int len)
+{	// buffer is (de)scrambled in place
+    static const uint8_t scramble_code[180] = {
 	1, 0, 0, 1, 0, 0, 1, 1, 1, 1, 0, 1, 0, 1, 1, 1, 
 	0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1, 
 	1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 1, 0, 1, 1, 1, 1, 
@@ -292,11 +351,19 @@ static const uint8_t scramble_code[180] = {
 	0, 1, 1, 1, 1, 1, 0, 0, 1, 1, 0, 1, 1, 0, 0, 0, 
 	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
 	0, 0, 0, 0
-};
+    };
+
+	assert(len <= (int)sizeof(scramble_code));
+	for (int i=0; i<len; i++) {
+		buf[i] = buf[i] ^ scramble_code[i];
+	}
+}
 
 // frame sync dibits
 static const uint8_t ysf_fs[20] = { 3, 1, 1, 0, 1, 3, 0, 1, 3, 0, 2, 1, 1, 2, 0, 3, 1, 0, 3, 1 };
 
+static const uint64_t YSF_FRAME_SYNC_MAGIC = 0xd471c9634dLL;
+
 /* thx gr-ysf fr_vch_decoder_bb_impl.cc * Copyright 2015 Mathias Weyland */
 // I hold Sylvain Munaut in high esteem for figuring this out.
 static const uint8_t ysf_permutation[144] = {
diff --git a/op25/gr-op25_repeater/lib/ysf_tx_sb_impl.cc b/op25/gr-op25_repeater/lib/ysf_tx_sb_impl.cc
index 82ae1aa..7b7274e 100644
--- a/op25/gr-op25_repeater/lib/ysf_tx_sb_impl.cc
+++ b/op25/gr-op25_repeater/lib/ysf_tx_sb_impl.cc
@@ -165,14 +165,6 @@ static inline void generate_fich(uint8_t result[100], int fi, int cs, int cm, in
 	trellis_interleave(result, pre_trellis, 20, 5);
 }
 
-static inline void scramble(uint8_t buf[], int len)
-{
-	assert(len <= sizeof(scramble_code));
-	for (int i=0; i<len; i++) {
-		buf[i] = buf[i] ^ scramble_code[i];
-	}
-}
-
 // encode DCH - input is bits, result is dibits
 static inline void generate_dch(uint8_t result[180], const uint8_t input[160])
 {
@@ -180,7 +172,7 @@ static inline void generate_dch(uint8_t result[180], const uint8_t input[160])
 	memset(pre_trellis, 0, sizeof(pre_trellis));
 
 	memcpy(pre_trellis, input, 160);
-	scramble(pre_trellis, 160);
+	ysf_scramble(pre_trellis, 160);
 
 	uint16_t crc = crc16(pre_trellis, 176);
 	store_i(crc, pre_trellis+160, 16);
@@ -199,7 +191,7 @@ static inline void generate_dch_vd2(uint8_t result[100], const uint8_t input[80]
 	memset(pre_trellis, 0, sizeof(pre_trellis));
 
 	memcpy(pre_trellis, input, 80);
-	scramble(pre_trellis, 80);
+	ysf_scramble(pre_trellis, 80);
 
 	uint16_t crc = crc16(pre_trellis, 96);
 	store_i(crc, pre_trellis+80, 16);
@@ -223,7 +215,7 @@ static inline void generate_vch_vd2(uint8_t result[52], const uint8_t input[49])
 	}
 	memcpy(buf+81, input+27, 22);
 	buf[103] = 0;
-	scramble(buf, 104);
+	ysf_scramble(buf, 104);
 
 	uint8_t bit_result[104];
 	int x=4;