diff options
Diffstat (limited to 'op25/gr-op25_repeater/lib')
69 files changed, 10507 insertions, 850 deletions
diff --git a/op25/gr-op25_repeater/lib/CCITTChecksumReverse.cpp b/op25/gr-op25_repeater/lib/CCITTChecksumReverse.cpp new file mode 100644 index 0000000..0b5b0fb --- /dev/null +++ b/op25/gr-op25_repeater/lib/CCITTChecksumReverse.cpp @@ -0,0 +1,93 @@ +/*
+ * Copyright (C) 2009,2011,2014 by Jonathan Naylor, G4KLX
+ *
+ * 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; version 2 of the License.
+ *
+ * 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.
+ */
+
+#include <stdio.h>
+#include <assert.h>
+
+#include "CCITTChecksumReverse.h"
+
+static const unsigned short ccittTab[] = {
+ 0x0000,0x1189,0x2312,0x329b,0x4624,0x57ad,0x6536,0x74bf,
+ 0x8c48,0x9dc1,0xaf5a,0xbed3,0xca6c,0xdbe5,0xe97e,0xf8f7,
+ 0x1081,0x0108,0x3393,0x221a,0x56a5,0x472c,0x75b7,0x643e,
+ 0x9cc9,0x8d40,0xbfdb,0xae52,0xdaed,0xcb64,0xf9ff,0xe876,
+ 0x2102,0x308b,0x0210,0x1399,0x6726,0x76af,0x4434,0x55bd,
+ 0xad4a,0xbcc3,0x8e58,0x9fd1,0xeb6e,0xfae7,0xc87c,0xd9f5,
+ 0x3183,0x200a,0x1291,0x0318,0x77a7,0x662e,0x54b5,0x453c,
+ 0xbdcb,0xac42,0x9ed9,0x8f50,0xfbef,0xea66,0xd8fd,0xc974,
+ 0x4204,0x538d,0x6116,0x709f,0x0420,0x15a9,0x2732,0x36bb,
+ 0xce4c,0xdfc5,0xed5e,0xfcd7,0x8868,0x99e1,0xab7a,0xbaf3,
+ 0x5285,0x430c,0x7197,0x601e,0x14a1,0x0528,0x37b3,0x263a,
+ 0xdecd,0xcf44,0xfddf,0xec56,0x98e9,0x8960,0xbbfb,0xaa72,
+ 0x6306,0x728f,0x4014,0x519d,0x2522,0x34ab,0x0630,0x17b9,
+ 0xef4e,0xfec7,0xcc5c,0xddd5,0xa96a,0xb8e3,0x8a78,0x9bf1,
+ 0x7387,0x620e,0x5095,0x411c,0x35a3,0x242a,0x16b1,0x0738,
+ 0xffcf,0xee46,0xdcdd,0xcd54,0xb9eb,0xa862,0x9af9,0x8b70,
+ 0x8408,0x9581,0xa71a,0xb693,0xc22c,0xd3a5,0xe13e,0xf0b7,
+ 0x0840,0x19c9,0x2b52,0x3adb,0x4e64,0x5fed,0x6d76,0x7cff,
+ 0x9489,0x8500,0xb79b,0xa612,0xd2ad,0xc324,0xf1bf,0xe036,
+ 0x18c1,0x0948,0x3bd3,0x2a5a,0x5ee5,0x4f6c,0x7df7,0x6c7e,
+ 0xa50a,0xb483,0x8618,0x9791,0xe32e,0xf2a7,0xc03c,0xd1b5,
+ 0x2942,0x38cb,0x0a50,0x1bd9,0x6f66,0x7eef,0x4c74,0x5dfd,
+ 0xb58b,0xa402,0x9699,0x8710,0xf3af,0xe226,0xd0bd,0xc134,
+ 0x39c3,0x284a,0x1ad1,0x0b58,0x7fe7,0x6e6e,0x5cf5,0x4d7c,
+ 0xc60c,0xd785,0xe51e,0xf497,0x8028,0x91a1,0xa33a,0xb2b3,
+ 0x4a44,0x5bcd,0x6956,0x78df,0x0c60,0x1de9,0x2f72,0x3efb,
+ 0xd68d,0xc704,0xf59f,0xe416,0x90a9,0x8120,0xb3bb,0xa232,
+ 0x5ac5,0x4b4c,0x79d7,0x685e,0x1ce1,0x0d68,0x3ff3,0x2e7a,
+ 0xe70e,0xf687,0xc41c,0xd595,0xa12a,0xb0a3,0x8238,0x93b1,
+ 0x6b46,0x7acf,0x4854,0x59dd,0x2d62,0x3ceb,0x0e70,0x1ff9,
+ 0xf78f,0xe606,0xd49d,0xc514,0xb1ab,0xa022,0x92b9,0x8330,
+ 0x7bc7,0x6a4e,0x58d5,0x495c,0x3de3,0x2c6a,0x1ef1,0x0f78};
+
+CCCITTChecksumReverse::CCCITTChecksumReverse() :
+m_crc16(0xFFFFU)
+{
+}
+
+CCCITTChecksumReverse::~CCCITTChecksumReverse()
+{
+}
+
+void CCCITTChecksumReverse::update(const unsigned char* data, unsigned int length)
+{
+ assert(data != NULL);
+
+ for (unsigned int i = 0U; i < length; i++)
+ m_crc16 = ((uint16_t)(m_crc8[1U])) ^ ccittTab[m_crc8[0U] ^ data[i]];
+}
+
+void CCCITTChecksumReverse::result(unsigned char* data)
+{
+ assert(data != NULL);
+
+ m_crc16 = ~m_crc16;
+
+ data[0U] = m_crc8[0U];
+ data[1U] = m_crc8[1U];
+}
+
+bool CCCITTChecksumReverse::check(const unsigned char* data)
+{
+ assert(data != NULL);
+
+ unsigned char sum[2U];
+ result(sum);
+
+ return sum[0U] == data[0U] && sum[1U] == data[1U];
+}
+
+void CCCITTChecksumReverse::reset()
+{
+ m_crc16 = 0xFFFFU;
+}
diff --git a/op25/gr-op25_repeater/lib/CCITTChecksumReverse.h b/op25/gr-op25_repeater/lib/CCITTChecksumReverse.h new file mode 100644 index 0000000..e60f93c --- /dev/null +++ b/op25/gr-op25_repeater/lib/CCITTChecksumReverse.h @@ -0,0 +1,39 @@ +/*
+ * Copyright (C) 2009,2011,2014 by Jonathan Naylor, G4KLX
+ *
+ * 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; version 2 of the License.
+ *
+ * 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.
+ */
+
+#ifndef CCITTChecksumReverse_H
+#define CCITTChecksumReverse_H
+
+#include <stdint.h>
+
+class CCCITTChecksumReverse {
+public:
+ CCCITTChecksumReverse();
+ ~CCCITTChecksumReverse();
+
+ void update(const unsigned char* data, unsigned int length);
+
+ void result(unsigned char* data);
+
+ bool check(const unsigned char* data);
+
+ void reset();
+
+private:
+ union {
+ uint16_t m_crc16;
+ uint8_t m_crc8[2U];
+ };
+};
+
+#endif
diff --git a/op25/gr-op25_repeater/lib/CMakeLists.txt b/op25/gr-op25_repeater/lib/CMakeLists.txt index d35a5b4..1464230 100644 --- a/op25/gr-op25_repeater/lib/CMakeLists.txt +++ b/op25/gr-op25_repeater/lib/CMakeLists.txt @@ -28,10 +28,12 @@ list(APPEND op25_repeater_sources ambe_encoder_sb_impl.cc dmr_bs_tx_bb_impl.cc ysf_tx_sb_impl.cc + nxdn_tx_sb_impl.cc dstar_tx_sb_impl.cc vocoder_impl.cc gardner_costas_cc_impl.cc p25_frame_assembler_impl.cc + frame_assembler_impl.cc fsk4_slicer_fb_impl.cc ) list(APPEND op25_repeater_sources @@ -52,6 +54,11 @@ list(APPEND op25_repeater_sources ambe.c mbelib.c ambe_encoder.cc + rx_sync.cc + op25_audio.cc + CCITTChecksumReverse.cpp + value_string.cc + nxdn.cc ) add_library(gnuradio-op25_repeater SHARED ${op25_repeater_sources}) @@ -61,7 +68,7 @@ else(CMAKE_SYSTEM_NAME STREQUAL "Darwin") find_library(GR_FILTER_LIBRARY libgnuradio-filter.so ) endif(CMAKE_SYSTEM_NAME STREQUAL "Darwin") set(GR_FILTER_LIBRARIES ${GR_FILTER_LIBRARY}) -target_link_libraries(gnuradio-op25_repeater ${Boost_LIBRARIES} ${GNURADIO_RUNTIME_LIBRARIES} ${GR_FILTER_LIBRARIES} imbe_vocoder) +target_link_libraries(gnuradio-op25_repeater ${Boost_LIBRARIES} gnuradio::gnuradio-runtime ${GR_FILTER_LIBRARIES} imbe_vocoder) set_target_properties(gnuradio-op25_repeater PROPERTIES DEFINE_SYMBOL "gnuradio_op25_repeater_EXPORTS") ######################################################################## @@ -97,4 +104,21 @@ target_link_libraries( GR_ADD_TEST(test_op25_repeater test-op25_repeater) +######################################################################## +# d2460 +######################################################################## +list(APPEND d2460_sources + ${CMAKE_CURRENT_SOURCE_DIR}/d2460.cc + ${CMAKE_CURRENT_SOURCE_DIR}/ambe.c + ${CMAKE_CURRENT_SOURCE_DIR}/mbelib.c + ${CMAKE_CURRENT_SOURCE_DIR}/ambe_encoder.cc + ${CMAKE_CURRENT_SOURCE_DIR}/software_imbe_decoder.cc + ${CMAKE_CURRENT_SOURCE_DIR}/imbe_decoder.cc + ${CMAKE_CURRENT_SOURCE_DIR}/p25p2_vf.cc + ${CMAKE_CURRENT_SOURCE_DIR}/rs.cc +) + +add_executable(op25-d2460 ${d2460_sources}) +target_link_libraries(op25-d2460 imbe_vocoder) + add_subdirectory(imbe_vocoder) diff --git a/op25/gr-op25_repeater/lib/ambe.c b/op25/gr-op25_repeater/lib/ambe.c index aa43fb2..4ee5fd4 100644 --- a/op25/gr-op25_repeater/lib/ambe.c +++ b/op25/gr-op25_repeater/lib/ambe.c @@ -20,6 +20,18 @@ #include "ambe3600x2250_const.h" #include "ambe3600x2400_const.h" +#ifndef M_PI +# define M_PI 3.14159265358979323846 /* pi */ +#endif + +#ifndef M_E +# define M_E 2.7182818284590452354 /* e */ +#endif + +#ifndef M_SQRT2 +# define M_SQRT2 1.41421356237309504880 /* sqrt(2) */ +#endif + static int mbe_dequantizeAmbeParms (mbe_parms * cur_mp, mbe_parms * prev_mp, const int *b, int dstar) { @@ -140,7 +152,7 @@ mbe_dequantizeAmbeParms (mbe_parms * cur_mp, mbe_parms * prev_mp, const int *b, #endif if (dstar) { deltaGamma = AmbePlusDg[b2]; - cur_mp->gamma = deltaGamma + ((float) 1.0 * prev_mp->gamma); + cur_mp->gamma = deltaGamma + ((float) 0.5 * prev_mp->gamma); } else { deltaGamma = AmbeDg[b2]; cur_mp->gamma = deltaGamma + ((float) 0.5 * prev_mp->gamma); diff --git a/op25/gr-op25_repeater/lib/ambe_encoder.cc b/op25/gr-op25_repeater/lib/ambe_encoder.cc index b134294..60465d1 100644 --- a/op25/gr-op25_repeater/lib/ambe_encoder.cc +++ b/op25/gr-op25_repeater/lib/ambe_encoder.cc @@ -545,6 +545,7 @@ static void encode_49bit(uint8_t outp[49], const int b[9]) { ambe_encoder::ambe_encoder(void) : d_49bit_mode(false), d_dstar_mode(false), + d_alt_dstar_interleave(false), d_gain_adjust(0) { mbe_parms enh_mp; @@ -579,7 +580,7 @@ void ambe_encoder::encode(int16_t samples[], uint8_t codeword[]) encode_ambe(vocoder.param(), b, &cur_mp, &prev_mp, d_dstar_mode, d_gain_adjust); if (d_dstar_mode) { - interleaver.encode_dstar(codeword, b); + interleaver.encode_dstar(codeword, b, d_alt_dstar_interleave); } else if (d_49bit_mode) { encode_49bit(codeword, b); } else { diff --git a/op25/gr-op25_repeater/lib/ambe_encoder.h b/op25/gr-op25_repeater/lib/ambe_encoder.h index 0d5f1a1..0b2706f 100644 --- a/op25/gr-op25_repeater/lib/ambe_encoder.h +++ b/op25/gr-op25_repeater/lib/ambe_encoder.h @@ -28,7 +28,8 @@ public: ambe_encoder(void); void set_49bit_mode(void); void set_dstar_mode(void); - void set_gain_adjust(float gain_adjust) {d_gain_adjust = gain_adjust;} + void set_gain_adjust(const float gain_adjust) {d_gain_adjust = gain_adjust;} + void set_alt_dstar_interleave(const bool v) { d_alt_dstar_interleave = v; } private: imbe_vocoder vocoder; p25p2_vf interleaver; @@ -37,6 +38,7 @@ private: bool d_49bit_mode; bool d_dstar_mode; float d_gain_adjust; + bool d_alt_dstar_interleave; }; #endif /* INCLUDED_AMBE_ENCODER_H */ diff --git a/op25/gr-op25_repeater/lib/bch.cc b/op25/gr-op25_repeater/lib/bch.cc index 7ff4faf..e2e58a7 100644 --- a/op25/gr-op25_repeater/lib/bch.cc +++ b/op25/gr-op25_repeater/lib/bch.cc @@ -1,7 +1,7 @@ #include <stdio.h>
#include <vector>
-#include <bch.h>
+#include "bch.h"
/*
* Copyright 2010, KA1RBI
*/
diff --git a/op25/gr-op25_repeater/lib/bit_utils.h b/op25/gr-op25_repeater/lib/bit_utils.h new file mode 100644 index 0000000..e8c9daa --- /dev/null +++ b/op25/gr-op25_repeater/lib/bit_utils.h @@ -0,0 +1,59 @@ +// +// This file is part of OP25 +// +// OP25 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 3, or (at your option) +// any later version. +// +// OP25 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 OP25; see the file COPYING. If not, write to the Free +// Software Foundation, Inc., 51 Franklin Street, Boston, MA +// 02110-1301, USA. + +#ifndef INCLUDED_OP25_REPEATER_BIT_UTILS_H +#define INCLUDED_OP25_REPEATER_BIT_UTILS_H + +#include <stdint.h> + +static inline void store_i(int reg, uint8_t val[], int len) { + for (int i=0; i<len; i++){ + val[i] = (reg >> (len-1-i)) & 1; + } +} + +static inline int load_i(const uint8_t val[], int len) { + int acc = 0; + for (int i=0; i<len; i++){ + acc = (acc << 1) + (val[i] & 1); + } + return acc; +} + +static inline uint64_t load_reg64(const uint8_t val[], int len) { + uint64_t acc = 0; + for (int i=0; i<len; i++){ + acc = (acc << 1) + (val[i] & 1); + } + return acc; +} + +static inline void dibits_to_bits(uint8_t * bits, const uint8_t * dibits, const int n_dibits) { + for (int i=0; i<n_dibits; i++) { + bits[i*2] = (dibits[i] >> 1) & 1; + bits[i*2+1] = dibits[i] & 1; + } +} + +static inline void bits_to_dibits(uint8_t* dest, const uint8_t* src, int n_dibits) { + for (int i=0; i<n_dibits; i++) { + dest[i] = src[i*2] * 2 + src[i*2+1]; + } +} + +#endif /* INCLUDED_OP25_REPEATER_BIT_UTILS_H */ diff --git a/op25/gr-op25_repeater/lib/check_frame_sync.h b/op25/gr-op25_repeater/lib/check_frame_sync.h index d361f19..bf2498c 100644 --- a/op25/gr-op25_repeater/lib/check_frame_sync.h +++ b/op25/gr-op25_repeater/lib/check_frame_sync.h @@ -6,7 +6,7 @@ static inline bool check_frame_sync(uint64_t x, int err_threshold, int len) { int errs=0; - static const uint64_t mask = (1LL<<len)-1; + const uint64_t mask = (1LL<<len)-1LL; x = x & mask; // source: https://en.wikipedia.org/wiki/Hamming_weight static const uint64_t m1 = 0x5555555555555555; //binary: 0101... diff --git a/op25/gr-op25_repeater/lib/crc16.h b/op25/gr-op25_repeater/lib/crc16.h new file mode 100644 index 0000000..77f9dd6 --- /dev/null +++ b/op25/gr-op25_repeater/lib/crc16.h @@ -0,0 +1,35 @@ +/* + * This file is part of OP25 + * + * This 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 3, or (at your option) + * any later version. + * + * This software 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 software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + +#ifndef INCLUDED_CRC16_H +#define INCLUDED_CRC16_H + +static inline uint16_t crc16(const uint8_t buf[], int len) { + uint32_t poly = (1<<12) + (1<<5) + (1<<0); + uint32_t crc = 0; + for(int i=0; i<len; i++) { + uint8_t bit = buf[i] & 1; + crc = ((crc << 1) | bit) & 0x1ffff; + if (crc & 0x10000) + crc = (crc & 0xffff) ^ poly; + } + crc = crc ^ 0xffff; + return crc & 0xffff; +} +#endif /* INCLUDED_CRC16_H */ diff --git a/op25/gr-op25_repeater/lib/d2460.cc b/op25/gr-op25_repeater/lib/d2460.cc new file mode 100644 index 0000000..16b87f9 --- /dev/null +++ b/op25/gr-op25_repeater/lib/d2460.cc @@ -0,0 +1,362 @@ +#include <stdio.h> +#include <time.h> +#include <stdlib.h> +#include <unistd.h> +#include <ctype.h> +#include <errno.h> +#include <termios.h> +#include <string.h> +#include <fcntl.h> + +#include <sys/stat.h> +#include <sys/types.h> +#include <sys/select.h> +#include <sys/socket.h> +#include <sys/param.h> + +#include <netinet/in.h> + +#include <stdint.h> + +#include "mbelib.h" +#include "ambe.h" +#include "p25p2_vf.h" +#include "imbe_decoder.h" +#include "software_imbe_decoder.h" +#include "imbe_vocoder/imbe_vocoder.h" +#include "ambe_encoder.h" + +static const float GAIN_ADJUST=7.0; /* attenuation (dB) */ + +typedef uint16_t Uns; +static const Uns RC_OK=0; + +static const char prodid[] = "OP25 "; +static const char verstring[] = "1.0"; + +static ambe_encoder encoder; +static software_imbe_decoder software_decoder; +static p25p2_vf interleaver; +static mbe_parms cur_mp; +static mbe_parms prev_mp; +static mbe_parms enh_mp; + +static const Uns DV3K_START_BYTE = 0x61; +enum +{ + DV3K_CONTROL_RATEP = 0x0A, + DV3K_CONTROL_CHANFMT = 0x15, + DV3K_CONTROL_PRODID = 0x30, + DV3K_CONTROL_VERSTRING = 0x31, + DV3K_CONTROL_RESET = 0x33, + DV3K_CONTROL_READY = 0x39 +}; +static const Uns DV3K_AMBE_FIELD_CHAND = 0x01; +static const Uns DV3K_AMBE_FIELD_CMODE = 0x02; +static const Uns DV3K_AMBE_FIELD_TONE = 0x08; +static const Uns DV3K_AUDIO_FIELD_SPEECHD = 0x00; +static const Uns DV3K_AUDIO_FIELD_CMODE = 0x02; + +#pragma DATA_ALIGN(dstar_state, 2) +static Uns bitstream[72]; + +static Uns get_byte(Uns offset, Uns *p) +{ + Uns word = p[offset >> 1]; + return (offset & 1) ? (word >> 8) : (word & 0xff); +} + +static void set_byte(Uns offset, Uns *p, Uns byte) +{ + p[offset >> 1] = + (offset & 1) ? (byte << 8) | (p[offset >> 1] & 0xff) + : (p[offset >> 1] & 0xff00) | (byte & 0xff); +} + +static Uns get_word(Uns offset, Uns *p) +{ + return get_byte(offset + 1, p) | (get_byte(offset, p) << 8); +} + +static void set_word(Uns offset, Uns *p, Uns word) +{ + set_byte(offset, p, word >> 8); + set_byte(offset + 1, p, word & 0xff); +} + +static void set_cstring(Uns offset, Uns *p, const char *str) +{ + do + set_byte(offset++, p, *str); + while (*str++ != 0); +} + +static Uns pkt_check_ratep(Uns offset, Uns *p) +{ + static const Uns ratep[] = { + 0x01, 0x30, 0x07, 0x63, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x48 }; + Uns i; + for (i = 0; i < sizeof(ratep); ++i) + if (get_byte(offset + i, p) != ratep[i]) + return 0; + return 1; +} + +static void pack(Uns bits, Uns offset, Uns *p, Uns *bitstream) +{ + Uns i; + Uns byte = 0; + for (i = 0; i < bits; ++i) + { + byte |= bitstream[i] << (7 - (i & 7)); + if ((i & 7) == 7) + { + set_byte(offset++, p, byte); + byte = 0; + } + } + if (i & 7) + set_byte(offset, p, byte); +} + +static void unpack(Uns bits, Uns offset, Uns *bitstream, Uns *p) +{ + Uns i; + Uns byte; + for (i = 0; i < bits; ++i) + { + if ((i & 7) == 0) + byte = get_byte(offset++, p); + bitstream[i] = (byte >> (7 - (i & 7))) & 1; + } +} + +static int response_len = -1; + +static void bksnd(void*task, Uns bid, Uns len) +{ + response_len = len; +} + +static void vocoder_setup(void) { + encoder.set_dstar_mode(); + encoder.set_gain_adjust(GAIN_ADJUST); + encoder.set_alt_dstar_interleave(true); + mbe_initMbeParms (&cur_mp, &prev_mp, &enh_mp); +} + +static void dump(unsigned char *p, ssize_t n) +{ + int i; + for (i = 0; i < n; ++i) + printf("%02x%c", p[i], i % 16 == 15 ? '\n' : ' '); + if (i % 16) + printf("\n"); +} + +static Uns pkt_process(Uns*pkt, Uns cnt) +{ + Uns bid=0; + Uns len = cnt << 1; + Uns payload_length; + Uns i; + Uns cmode = 0; + Uns tone = 0; + uint8_t codeword[72]; + int b[9]; + int K; + int rc = -1; + + if (len < 4 || cnt > 256) + goto fail; + + if (get_byte(0, pkt) != DV3K_START_BYTE) + goto fail; + + payload_length = get_word(1, pkt); + if (payload_length == 0) + goto fail; + if (4 + payload_length > len) + goto fail; + + switch (get_byte(3, pkt)) + { + case 0: + switch (get_byte(4, pkt)) + { + case DV3K_CONTROL_RATEP: + if (payload_length != 13) + goto fail; + if (!pkt_check_ratep(5, pkt)) + goto fail; + set_word(1, pkt, 1); + bksnd(NULL, bid, 3); + return RC_OK; + case DV3K_CONTROL_CHANFMT: + if (payload_length != 3) + goto fail; + if (get_word(5, pkt) != 0x0001) + goto fail; + set_word(1, pkt, 2); + set_byte(5, pkt, 0); + bksnd(NULL, bid, 3); + return RC_OK; + case DV3K_CONTROL_PRODID: + set_word(1, pkt, 8); + set_cstring(5, pkt, prodid); + bksnd(NULL, bid, 6); + return RC_OK; + case DV3K_CONTROL_VERSTRING: + set_word(1, pkt, 5); + set_cstring(5, pkt, verstring); + bksnd(NULL, bid, 5); + return RC_OK; + case DV3K_CONTROL_RESET: + if (payload_length != 1) + goto fail; + vocoder_setup(); + set_byte(4, pkt, DV3K_CONTROL_READY); + bksnd(NULL, bid, 3); + return RC_OK; + default: + goto fail; + } + case 1: + switch (payload_length) + { + case 17: + if (get_byte(18, pkt) != DV3K_AMBE_FIELD_TONE) + goto fail; + tone = get_word(19, pkt); + /* FALLTHROUGH */ + case 14: + if (get_byte(15, pkt) != DV3K_AMBE_FIELD_CMODE) + goto fail; + cmode = get_word(16, pkt); + /* FALLTHROUGH */ + case 11: + if (get_byte(4, pkt) != DV3K_AMBE_FIELD_CHAND) + goto fail; + if (get_byte(5, pkt) != 72) + goto fail; + unpack(72, 6, bitstream, pkt); + break; + default: + goto fail; + } + + for (i = 0; i < 72; i++) { + codeword[i] = bitstream[i]; + } + interleaver.decode_dstar(codeword, b, true); + if (b[0] >= 120) { + memset(6+(char*)pkt, 0, 320); // silence + // FIXME: add handling for tone case (b0=126) + } else { + rc = mbe_dequantizeAmbe2400Parms(&cur_mp, &prev_mp, b); + printf("B\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n", b[0], b[1], b[2], b[3], b[4], b[5], b[6], b[7], b[8]); + K = 12; + if (cur_mp.L <= 36) + K = int(float(cur_mp.L + 2.0) / 3.0); + software_decoder.decode_tap(cur_mp.L, K, cur_mp.w0, &cur_mp.Vl[1], &cur_mp.Ml[1]); + audio_samples *samples = software_decoder.audio(); + int16_t snd; + for (i=0; i < 160; i++) { + if (samples->size() > 0) { + snd = (int16_t)(samples->front()); + samples->pop_front(); + } else { + snd = 0; + } + set_word(6 + (i << 1), pkt, snd); + } + mbe_moveMbeParms (&cur_mp, &prev_mp); + } + + set_word(1, pkt, 322); + set_byte(3, pkt, 2); + set_byte(4, pkt, DV3K_AUDIO_FIELD_SPEECHD); + set_byte(5, pkt, 160); + bksnd(NULL, bid, 165); + return RC_OK; + case 2: + if (payload_length != 322 && payload_length != 325) + goto fail; + if (get_byte(4, pkt) != DV3K_AUDIO_FIELD_SPEECHD) + goto fail; + if (get_byte(5, pkt) != 160) + goto fail; + if (payload_length == 325) + { + if (get_byte(326, pkt) != DV3K_AUDIO_FIELD_CMODE) + goto fail; + cmode = get_word(323, pkt); + } + int16_t samples[160]; + for (i=0; i < 160; i++) { + samples[i] = (int16_t) get_word(6 + (i << 1), pkt); + } + encoder.encode(samples, codeword); + for (i = 0; i < 72; i++) { + bitstream[i] = codeword[i]; + } + set_word(1, pkt, 11); + set_byte(3, pkt, 1); + set_byte(4, pkt, DV3K_AMBE_FIELD_CHAND); + set_byte(5, pkt, 72); + pack(72, 6, pkt, bitstream); + bksnd(NULL, bid, 8); + return RC_OK; + default: + goto fail; + } + +fail: + bksnd(NULL, bid, 0); + return RC_OK; +} + +int main() +{ + int sockfd; + const ssize_t size_max = 1024; + ssize_t size_in, size_out; + char buf_in[size_max], buf_out[size_max]; + socklen_t length = sizeof(struct sockaddr_in); + struct sockaddr_in sa = { 0 }; + Uns rc; + + vocoder_setup(); + + if ((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) + exit(2); + + sa.sin_family = AF_INET; + sa.sin_port = htons(2460); + sa.sin_addr.s_addr = htonl(INADDR_ANY); + + if (bind(sockfd, (struct sockaddr *)&sa, sizeof(sa)) < 0) + exit(3); + + while (1) + { + if ((size_in = recvfrom(sockfd, buf_in, size_max, + 0, (struct sockaddr *)&sa, &length)) < 0) + exit(4); + + if (size_in & 1) + buf_in[size_in++] = 0; + + rc = pkt_process((Uns*)buf_in, size_in >> 1); + if (response_len <= 0) + exit(9); + + size_out = 4 + ntohs(*(short *)&buf_in[1]); + if (sendto(sockfd, buf_in, size_out, 0, (struct sockaddr *)&sa, + sizeof(struct sockaddr_in)) != size_out) + exit(7); + } + + return 0; +} diff --git a/op25/gr-op25_repeater/lib/dmr_bs_tx_bb_impl.cc b/op25/gr-op25_repeater/lib/dmr_bs_tx_bb_impl.cc index 3609c58..52972a4 100644 --- a/op25/gr-op25_repeater/lib/dmr_bs_tx_bb_impl.cc +++ b/op25/gr-op25_repeater/lib/dmr_bs_tx_bb_impl.cc @@ -80,7 +80,7 @@ static bool crc8_ok(const uint8_t bits[], unsigned int len) { return (crc == crc8(bits,len)); } -static inline int store_i(int reg, uint8_t val[], int len) { +static inline void store_i(int reg, uint8_t val[], int len) { for (int i=0; i<len; i++){ val[i] = (reg >> (len-1-i)) & 1; } @@ -186,8 +186,6 @@ static void generate_cach(uint8_t at, uint8_t tc, uint8_t lcss, const uint8_t ca int tact = hamming_7_4[ (at << 3) | (tc << 2) | lcss ]; //printf ("tact %d %x\n", tact, tact); //print_result("cach_payload_bits", cach_bits, 17); - static const uint8_t cach_tact_bits[] = {0, 4, 8, 12, 14, 18, 22}; - static const uint8_t cach_payload_bits[] = {1,2,3,5,6,7,9,10,11,13,15,16,17,19,20,21,23}; for (int i=0; i<7; i++) { result[cach_tact_bits[i]] = (tact >> (6-i)) & 1; } diff --git a/op25/gr-op25_repeater/lib/dmr_const.h b/op25/gr-op25_repeater/lib/dmr_const.h index 0ea6ca0..31196a0 100644 --- a/op25/gr-op25_repeater/lib/dmr_const.h +++ b/op25/gr-op25_repeater/lib/dmr_const.h @@ -21,11 +21,24 @@ #ifndef INCLUDED_OP25_REPEATER_DMR_CONST_H #define INCLUDED_OP25_REPEATER_DMR_CONST_H +#include <stdint.h> + static const int hamming_7_4[] = { 0, 11, 22, 29, 39, 44, 49, 58, 69, 78, 83, 88, 98, 105, 116, 127, }; - + +static const int hamming_7_4_decode[] = { + 0, 0, 0, 1, 0, 8, 2, 4, 0, 1, 1, 1, 5, 3, 9, 1, + 0, 6, 2, 10, 2, 3, 2, 2, 11, 3, 7, 1, 3, 3, 2, 3, + 0, 6, 12, 4, 5, 4, 4, 4, 5, 13, 7, 1, 5, 5, 5, 4, + 6, 6, 7, 6, 14, 6, 2, 4, 7, 6, 7, 7, 5, 3, 7, 15, + 0, 8, 12, 10, 8, 8, 9, 8, 11, 13, 9, 1, 9, 8, 9, 9, + 11, 10, 10, 10, 14, 8, 2, 10, 11, 11, 11, 10, 11, 3, 9, 15, + 12, 13, 12, 12, 14, 8, 12, 4, 13, 13, 12, 13, 5, 13, 9, 15, + 14, 6, 12, 10, 14, 14, 14, 15, 11, 13, 7, 15, 14, 15, 15, 15 +}; + static const int hamming_17_12[] = { 0, 37, 74, 111, 148, 177, 222, 251, 269, 296, 327, 354, 409, 444, 467, 502, @@ -822,9 +835,62 @@ static const int hamming_16_11[] = { static const uint8_t dmr_bs_voice_sync[24] = { 1,3,1,1,1,1,3,3,3,1,1,3,3,1,3,3,1,3,1,1,3,3,1,3 }; +static const uint64_t DMR_VOICE_SYNC_MAGIC = 0x755fd7df75f7LL; +static const uint64_t DMR_IDLE_SYNC_MAGIC = 0xdff57d75df5dLL; static const uint8_t dmr_bs_idle_sync[24] = { 3,1,3,3,3,3,1,1,1,3,3,1,1,3,1,1,3,1,3,3,1,1,3,1 }; +static const uint8_t cach_tact_bits[] = {0, 4, 8, 12, 14, 18, 22}; +static const uint8_t cach_payload_bits[] = {1,2,3,5,6,7,9,10,11,13,15,16,17,19,20,21,23}; + +static const uint16_t _pc[] = {0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0}; + +static const uint32_t slookup_16_11[] = {0, 1, 2, 0, 4, 0, 0, 32, 8, 0, 0, 512, 0, 64, 2048, 0, 16, 0, 0, 32768, 0, 1024, 256, 0, 0, 128, 16384, 0, 4096, 0, 0, 8192}; +static const uint32_t masks_16_11[] = {62864, 31432, 15716, 60194, 42721}; +static const uint32_t nmasks_16_11 = 5; + +static const uint32_t slookup_16_7[] = {0, 1, 2, 3, 4, 5, 6, 0, 8, 9, 10, 0, 12, 0, 0, 32832, 16, 17, 18, 0, 20, 0, 0, 0, 24, 0, 0, 0, 0, 0, 16640, 0, 32, 33, 34, 0, 36, 0, 0, 0, 40, 0, 0, 6144, 0, 0, 0, 0, 48, 0, 0, 576, 0, 0, 0, 0, 0, 0, 0, 0, 33280, 0, 0, 0, 64, 65, 66, 0, 68, 0, 0, 32776, 72, 0, 0, 32772, 0, 32770, 32769, 32768, 80, 0, 0, 544, 0, 12288, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32784, 96, 0, 0, 528, 0, 1152, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32800, 0, 514, 513, 512, 0, 0, 0, 516, 0, 0, 0, 520, 0, 0, 10240, 0, 128, 129, 130, 0, 132, 0, 0, 0, 136, 0, 0, 0, 0, 0, 0, 0, 144, 0, 0, 0, 0, 0, 1536, 0, 0, 0, 0, 0, 0, 0, 0, 0, 160, 0, 0, 0, 0, 1088, 0, 0, 0, 24576, 33792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8448, 0, 0, 0, 0, 0, 0, 0, 0, 192, 0, 0, 0, 0, 1056, 0, 0, 0, 2304, 0, 0, 0, 0, 0, 32896, 0, 0, 0, 0, 0, 0, 0, 18432, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1028, 4352, 0, 1025, 1024, 0, 1026, 0, 0, 0, 0, 0, 1032, 0, 0, 0, 0, 0, 640, 0, 1040, 0, 0, 0, 0, 0, 0, 20480, 0, 0, 0, 256, 257, 258, 0, 260, 0, 0, 5120, 264, 0, 0, 0, 0, 0, 16400, 0, 272, 0, 0, 0, 0, 0, 16392, 0, 0, 0, 16388, 0, 16386, 0, 16384, 16385, 288, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3072, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8320, 0, 0, 0, 0, 0, 0, 16416, 0, 320, 0, 0, 0, 0, 0, 0, 0, 0, 2176, 0, 0, 0, 0, 0, 33024, 0, 49152, 9216, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16448, 0, 0, 0, 4224, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16896, 0, 0, 0, 0, 0, 768, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 384, 0, 0, 0, 0, 0, 34816, 0, 0, 2112, 0, 0, 0, 0, 0, 0, 0, 4608, 0, 0, 0, 0, 0, 8224, 0, 0, 0, 0, 0, 0, 16512, 0, 0, 0, 4160, 0, 0, 0, 0, 8208, 0, 0, 0, 0, 0, 36864, 0, 0, 0, 0, 0, 8196, 0, 8194, 8193, 8192, 0, 0, 2560, 0, 0, 0, 0, 8200, 0, 2056, 4128, 0, 8704, 0, 0, 0, 2049, 2048, 0, 2050, 0, 2052, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2064, 0, 0, 0, 0, 0, 0, 4098, 0, 4096, 4097, 0, 1280, 4100, 0, 0, 2080, 4104, 0, 0, 0, 0, 0, 0, 0, 4112, 0, 0, 0, 0, 8256, 40960, 0, 0, 17408, 0, 0, 0, 0}; +static const uint32_t masks_16_7[] = {30976, 15488, 40512, 13856, 25104, 51208, 58372, 61954, 44545}; +static const uint32_t nmasks_16_7 = 9; + +static const uint32_t slookup_17_12[] = {0, 1, 2, 0, 4, 32, 0, 2048, 8, 0, 64, 0, 0, 256, 4096, 0, 16, 1024, 0, 0, 128, 0, 0, 0, 0, 0, 512, 65536, 8192, 16384, 0, 32768}; +static const uint32_t masks_17_12[] = {124560, 127816, 63908, 105026, 118049}; +static const uint32_t nmasks_17_12 = 5; + +static inline int64_t _hamming_decode(const uint32_t r_codeword, const int n, const int k, const uint32_t *slookup, const uint32_t *masks, const uint32_t nmasks, const int *encode_tab) { + uint32_t syn = 0; + for (uint32_t i=0; i<nmasks; i++) { + uint32_t acc = masks[i] & r_codeword; + uint32_t sum = 0; + for (; acc > 0; acc >>= 8) { + sum += _pc[acc & 0xff]; + } + syn = (syn << 1) + (sum & 1); + } + if (syn == 0) + return (r_codeword >> (n - k)); // decode, no error case + uint32_t sl = slookup[syn]; + if (sl == 0) + return -1; // decode failed + uint32_t decoded = (r_codeword ^ sl) >> (n - k); // corrected but not yet trusted result + uint32_t encoded = encode_tab[decoded]; // re-encode and verify codewords match + if ((r_codeword ^ sl) == encoded) + return decoded; // decode OK with 1 error corrected + return -2; // decode verification failed +} + +static inline int64_t hamming_17_12_decode(const uint32_t r_codeword) { + return _hamming_decode(r_codeword, 17, 12, slookup_17_12, masks_17_12, nmasks_17_12, hamming_17_12); +} + +static inline int64_t hamming_16_11_decode(const uint32_t r_codeword) { + return _hamming_decode(r_codeword, 16, 11, slookup_16_11, masks_16_11, nmasks_16_11, hamming_16_11); +} + +static inline int64_t hamming_16_7_decode(const uint32_t r_codeword) { + return _hamming_decode(r_codeword, 16, 7, slookup_16_7, masks_16_7, nmasks_16_7, hamming_16_7); +} + #endif /* INCLUDED_OP25_REPEATER_DMR_CONST_H */ diff --git a/op25/gr-op25_repeater/lib/dstar_header.h b/op25/gr-op25_repeater/lib/dstar_header.h new file mode 100644 index 0000000..034bb26 --- /dev/null +++ b/op25/gr-op25_repeater/lib/dstar_header.h @@ -0,0 +1,130 @@ +/* + * Copyright (C) 2010,2011,2015 by Jonathan Naylor, G4KLX + * + * 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; version 2 of the License. + * + * 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. + */ + +/* from OpenDV/DStarRepeater */ + +#ifndef INCLUDED_DSTAR_HEADER_H +#define INCLUDED_DSTAR_HEADER_H + +#include <stdio.h> +#include <stdint.h> +#include <string.h> + +#include "CCITTChecksumReverse.h" +#include "bit_utils.h" + +static inline uint8_t rev_byte(const uint8_t b) { + uint8_t rc = 0; + for (int i=0; i<8; i++) { + rc |= ((b >> i) & 1) << (7-i); + } + return(rc); +} + +static inline void make_dstar_header( + uint8_t header_data_buf[480], // result (bits) + const uint8_t flag1, const uint8_t flag2, const uint8_t flag3, + const char RptCall2[], + const char RptCall1[], + const char YourCall[], + const char MyCall1[], + const char MyCall2[]) +{ + uint8_t m_headerData[60]; + static const unsigned int SLOW_DATA_BLOCK_SIZE = 6U; + static const unsigned int SLOW_DATA_FULL_BLOCK_SIZE = SLOW_DATA_BLOCK_SIZE * 10U; + static const unsigned char SLOW_DATA_TYPE_HEADER = 0x50U; + + ::memset(m_headerData, 'f', SLOW_DATA_FULL_BLOCK_SIZE); + + m_headerData[0U] = SLOW_DATA_TYPE_HEADER | 5U; + m_headerData[1U] = flag1; + m_headerData[2U] = flag2; + m_headerData[3U] = flag3; + m_headerData[4U] = RptCall2[0]; + m_headerData[5U] = RptCall2[1]; + + m_headerData[6U] = SLOW_DATA_TYPE_HEADER | 5U; + m_headerData[7U] = RptCall2[2]; + m_headerData[8U] = RptCall2[3]; + m_headerData[9U] = RptCall2[4]; + m_headerData[10U] = RptCall2[5]; + m_headerData[11U] = RptCall2[6]; + + m_headerData[12U] = SLOW_DATA_TYPE_HEADER | 5U; + m_headerData[13U] = RptCall2[7]; + m_headerData[14U] = RptCall1[0]; + m_headerData[15U] = RptCall1[1]; + m_headerData[16U] = RptCall1[2]; + m_headerData[17U] = RptCall1[3]; + + m_headerData[18U] = SLOW_DATA_TYPE_HEADER | 5U; + m_headerData[19U] = RptCall1[4]; + m_headerData[20U] = RptCall1[5]; + m_headerData[21U] = RptCall1[6]; + m_headerData[22U] = RptCall1[7]; + m_headerData[23U] = YourCall[0]; + + m_headerData[24U] = SLOW_DATA_TYPE_HEADER | 5U; + m_headerData[25U] = YourCall[1]; + m_headerData[26U] = YourCall[2]; + m_headerData[27U] = YourCall[3]; + m_headerData[28U] = YourCall[4]; + m_headerData[29U] = YourCall[5]; + + m_headerData[30U] = SLOW_DATA_TYPE_HEADER | 5U; + m_headerData[31U] = YourCall[6]; + m_headerData[32U] = YourCall[7]; + m_headerData[33U] = MyCall1[0]; + m_headerData[34U] = MyCall1[1]; + m_headerData[35U] = MyCall1[2]; + + m_headerData[36U] = SLOW_DATA_TYPE_HEADER | 5U; + m_headerData[37U] = MyCall1[3]; + m_headerData[38U] = MyCall1[4]; + m_headerData[39U] = MyCall1[5]; + m_headerData[40U] = MyCall1[6]; + m_headerData[41U] = MyCall1[7]; + + m_headerData[42U] = SLOW_DATA_TYPE_HEADER | 5U; + m_headerData[43U] = MyCall2[0]; + m_headerData[44U] = MyCall2[1]; + m_headerData[45U] = MyCall2[2]; + m_headerData[46U] = MyCall2[3]; + + CCCITTChecksumReverse cksum; + cksum.update(m_headerData + 1U, 5U); + cksum.update(m_headerData + 7U, 5U); + cksum.update(m_headerData + 13U, 5U); + cksum.update(m_headerData + 19U, 5U); + cksum.update(m_headerData + 25U, 5U); + cksum.update(m_headerData + 31U, 5U); + cksum.update(m_headerData + 37U, 5U); + cksum.update(m_headerData + 43U, 4U); + + unsigned char checkSum[2U]; + cksum.result(checkSum); + + m_headerData[47U] = checkSum[0]; + + m_headerData[48U] = SLOW_DATA_TYPE_HEADER | 1U; + m_headerData[49U] = checkSum[1]; + + for (int i=0; i<SLOW_DATA_FULL_BLOCK_SIZE; i+=3) { + store_i(rev_byte(m_headerData[i] ^ 0x70), header_data_buf+i*8, 8); + store_i(rev_byte(m_headerData[i+1] ^ 0x4f), header_data_buf+((i+1)*8), 8); + store_i(rev_byte(m_headerData[i+2] ^ 0x93), header_data_buf+((i+2)*8), 8); + } +} + +#endif /* INCLUDED_DSTAR_HEADER_H */ diff --git a/op25/gr-op25_repeater/lib/dstar_tx_sb_impl.cc b/op25/gr-op25_repeater/lib/dstar_tx_sb_impl.cc index c89ce12..c0d2cce 100644 --- a/op25/gr-op25_repeater/lib/dstar_tx_sb_impl.cc +++ b/op25/gr-op25_repeater/lib/dstar_tx_sb_impl.cc @@ -29,7 +29,8 @@ #include "p25p2_vf.h" #include "dstar_tx_sb_impl.h" // #include "dstar_const.h" -#include <op25_imbe_frame.h> +#include "op25_imbe_frame.h" +#include "dstar_header.h" #include <vector> #include <stdint.h> @@ -52,6 +53,15 @@ static inline void print_result(char title[], const uint8_t r[], int len) { } #endif +static inline void sstring(const char s[], char dest[8]) { + memset(dest, ' ', 8); + memcpy(dest, s, std::min(strlen(s), (size_t)8)); + for (int i=0; i<8; i++) { + if (dest[i] < ' ') + dest [i] = ' '; + } +} + static const uint8_t FS[24] = { 1,0,1,0,1,0,1,0,1,0,1,1,0,1,0,0,0,1,1,0,1,0,0,0 }; static const uint8_t FS_DUMMY[24] = { 0,0,0,0,1,1,1,1,0,0,0,0,1,1,1,1,0,0,0,0,1,1,1,1 }; @@ -102,8 +112,13 @@ dstar_tx_sb_impl::config() FILE * fp1 = fopen(d_config_file, "r"); char line[256]; char * cp; - // TODO: add code to generate slow speed datastream - return; + int flag1, flag2, flag3; + char rptcall1[8]; + char rptcall2[8]; + char urcall[8]; + char mycall1[8]; + char mycall2[8]; + if (!fp1) { fprintf(stderr, "dstar_tx_sb_impl:config: failed to open %s\n", d_config_file); return; @@ -112,12 +127,25 @@ dstar_tx_sb_impl::config() cp = fgets(line, sizeof(line) - 2, fp1); if (!cp) break; if (line[0] == '#') continue; -#if 0 - if (memcmp(line, "ft=", 3) == 0) - sscanf(&line[3], "%d", &d_ft); -#endif + if (memcmp(line, "flag1=", 6) == 0) + sscanf(&line[6], "%x", &flag1); + else if (memcmp(line, "flag2=", 6) == 0) + sscanf(&line[6], "%x", &flag2); + else if (memcmp(line, "flag3=", 6) == 0) + sscanf(&line[6], "%x", &flag3); + else if (memcmp(line, "rptcall2=", 9) == 0) + sstring(&line[9], rptcall2); + else if (memcmp(line, "rptcall1=", 9) == 0) + sstring(&line[9], rptcall1); + else if (memcmp(line, "urcall=", 7) == 0) + sstring(&line[7], urcall); + else if (memcmp(line, "mycall1=", 8) == 0) + sstring(&line[8], mycall1); + else if (memcmp(line, "mycall2=", 8) == 0) + sstring(&line[8], mycall2); } fclose(fp1); + make_dstar_header(d_dstar_header_data, flag1 & 0xff, flag2 & 0xff, flag3 & 0xff, rptcall2, rptcall1, urcall, mycall1, mycall2); } void @@ -151,7 +179,7 @@ dstar_tx_sb_impl::general_work (int noutput_items, if (d_frame_counter == 0) memcpy(out+72, FS, 24); else - memcpy(out+72, FS_DUMMY, 24); + memcpy(out+72, d_dstar_header_data+((d_frame_counter-1) * 24), 24); d_frame_counter = (d_frame_counter + 1) % 21; in += 160; nconsumed += 160; diff --git a/op25/gr-op25_repeater/lib/dstar_tx_sb_impl.h b/op25/gr-op25_repeater/lib/dstar_tx_sb_impl.h index 4451702..6570161 100644 --- a/op25/gr-op25_repeater/lib/dstar_tx_sb_impl.h +++ b/op25/gr-op25_repeater/lib/dstar_tx_sb_impl.h @@ -60,6 +60,7 @@ namespace gr { const char * d_config_file; ambe_encoder d_encoder; int d_frame_counter; + uint8_t d_dstar_header_data[480]; }; } // namespace op25_repeater diff --git a/op25/gr-op25_repeater/lib/ezpwd/asserter b/op25/gr-op25_repeater/lib/ezpwd/asserter new file mode 100644 index 0000000..5e0a19e --- /dev/null +++ b/op25/gr-op25_repeater/lib/ezpwd/asserter @@ -0,0 +1,128 @@ +#ifndef _EZPWD_ASSERTER +#define _EZPWD_ASSERTER + +#include <algorithm> +#include <cstring> +#include <iostream> +#include <iomanip> +#include <sstream> + +namespace ezpwd { + +#define ISEQUAL( ... ) isequal(__FILE__, __LINE__, __VA_ARGS__ ) +#define ISTRUE( ... ) istrue( __FILE__, __LINE__, __VA_ARGS__ ) +#define ISFALSE( ... ) isfalse(__FILE__, __LINE__, __VA_ARGS__ ) +#define ISNEAR( ... ) isnear( __FILE__, __LINE__, __VA_ARGS__ ) +#define FAILURE( ... ) failure(__FILE__, __LINE__, __VA_ARGS__ ) + + struct asserter { + bool failed; // The last test failed + int failures; // Total number of failures + std::string out; // Last failure + + asserter() + : failed( false ) + , failures( 0 ) + , out() + { + ; + } + + // + // output( <std::ostream> ) -- Output description of last failed test (or nothing if successful) + // <std::ostream> << <asserter> + // + std::ostream &output( + std::ostream &lhs ) + const + { + return lhs << out; + } + + // + // (bool) <asserter> -- Return status of last test + // + operator bool() + { + return failed; + } + + template < typename T > + asserter &istrue( const char *file, int line, const T &a, const std::string &comment = std::string() ) + { + return isequal( file, line, !!a, true, comment ); + } + + template < typename T > + asserter &isfalse( const char *file, int line, const T &a, const std::string &comment = std::string() ) + { + return isequal( file, line, !!a, false, comment ); + } + + template < typename T > + asserter &isequal( const char *file, int line, const T &a, const T &b, const std::string &comment = std::string() ) + { + if ( ! ( a == b )) { + std::ostringstream oss; + oss << a << " != " << b; + return failure( file, line, oss.str(), comment ); + } + return success(); + } + + template < typename T > + asserter &isnear( const char *file, int line, const T &a, const T &b, const T &delta, const std::string &comment = std::string() ) + { + T difference; + difference = ( a < b + ? T( b - a ) + : T( a - b )); + if ( ! ( difference < ( delta < T( 0 ) ? T( -delta ) : T( delta )))) { + std::ostringstream oss; + oss << std::setprecision( 13 ) << a << " != " << b << " +/- " << delta; + return failure( file, line, oss.str(), comment ); + } + return success(); + } + + asserter &failure( const char *file, int line, const std::string &comparison, + const std::string &comment = std::string() ) + { + ++failures; + const char *needle = "/"; + const char *slash = std::find_end( file, file + strlen( file ), + needle, needle + strlen( needle )); + if ( slash == file + strlen( file )) + slash = file; + else + slash += 1; + + std::ostringstream oss; + oss + << std::setw( 24 ) << slash << ", " + << std::setw( -5 ) << line + << "; FAILURE: " << comparison + << ( comment.size() ? ": " : "" ) << comment + << std::endl; + out = oss.str(); + failed = true; + return *this; + } + + asserter &success() + { + out.clear(); + failed = false; + return *this; + } + }; // class asserter +} // namespace ezpwd + +std::ostream &operator<<( + std::ostream &lhs, + ezpwd::asserter &rhs ) +{ + return rhs.output( lhs ); +} + +#endif // _EZPWD_ARRAY diff --git a/op25/gr-op25_repeater/lib/ezpwd/bch b/op25/gr-op25_repeater/lib/ezpwd/bch new file mode 100644 index 0000000..eb45db6 --- /dev/null +++ b/op25/gr-op25_repeater/lib/ezpwd/bch @@ -0,0 +1,485 @@ +/* + * Ezpwd Reed-Solomon -- Reed-Solomon encoder / decoder library + * + * Copyright (c) 2017, Hard Consulting Corporation. + * + * Ezpwd Reed-Solomon 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 3 of + * the License, or (at your option) any later version. See the LICENSE file at the top of the + * source tree. Ezpwd Reed-Solomon is also available under Commercial license. The Djelic BCH code + * under djelic/ and the c++/ezpwd/bch_base wrapper is redistributed under the terms of the GPLv2+, + * regardless of the overall licensing terms. + * + * Ezpwd Reed-Solomon 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. + */ +#ifndef _EZPWD_BCH +#define _EZPWD_BCH + +#include <sstream> +#include "rs_base" // Basic DEBUG, EZPWD_... preprocessor stuff, ezpwd::log_, etc. +#include "bch_base" + +namespace ezpwd { + // + // ezpwd::bch_base -- Interface to underlying Djelic Linux Kernel API + // ezpwd::bch<SYMBOLS, CORRECTION> -- General BCH codec types; min. CORRECTION capacity, undefined PAYLOAD + // + // These implementations retain the original Djelic Linux Kernel API; specifically, they find + // a BCH codec of the given Galois order M (ie. has codewords of size 2**M-1), and at least the + // target bit-error correction capacity T. They may correct more than T errors, and the number + // of parity ECC bits will be selected by the algorithm. You need to compute the maximum + // non-parity payload by computing _bch->n - _bch->ecc_bits. + // + // The data and parity bits must always be on separate blocks of int8_t/uint8_t-sized data + // in the container. This is required because the underlying API must break the data and parity + // out as separate arrays for processing. So, if the computed ecc_bits is not evenly divisible + // by 8, some care must be taken to ensure that it is packed into exactly ecc_bytes of data at + // the end of the supplied container. Alternatively, it can be kept in a separate container. + // This is probably safest, as the bch_base/bch/BCH classes will never attempt to resize the + // data/parity containers when supplied separately. + // + // Like the Reed-Solomon APIs, the bch_base/bch/BCH APIs will alter the size of a variable + // container in encode(...), to add the BCH ECC "parity" data (eg. std::vector, std::string). + // Fixed containers (eg. std::array) are never resized, and it is assumed that ecc_bytes of + // parity data exist at the end of the container. + // + class bch_base { + public: + ezpwd::bch_control *_bch; + + bch_base( const bch_base & ) = delete; // no copy-constructor + + bch_base( + size_t m, + size_t t, + unsigned int prim_poly = 0 ) + : _bch( ezpwd::init_bch( int( m ), int( t ), prim_poly )) + { + ; + } + + virtual ~bch_base() + { + ezpwd::free_bch( this->_bch ); + } + + size_t ecc_bytes() + const + { + return _bch->ecc_bytes; + } + size_t ecc_bits() + const + { + return _bch->ecc_bits; + } + + size_t t() + const + { + return _bch->t; + } + + // + // <ostream> << bch_base -- output codec in standard BCH( N, N-ECC, T ) form + // + virtual std::ostream &output( + std::ostream &lhs ) + const + { + return lhs << *this->_bch; + } + + // + // encode -- container interfaces + // + // Returns number of ECC *bits* initialized (to be consistent w/ decode, which returns + // number of bit errors corrected). + // + int encode( + std::string &data ) + const + { + typedef uint8_t uT; + typedef std::pair<uT *, uT *> + uTpair; + data.resize( data.size() + ecc_bytes() ); + return encode( uTpair( (uT *)&data.front(), (uT *)&data.front() + data.size() )); + } + + int encode( + const std::string &data, + std::string &parity ) + const + { + typedef uint8_t uT; + typedef std::pair<const uT *, const uT *> + cuTpair; + typedef std::pair<uT *, uT *> + uTpair; + parity.resize( ecc_bytes() ); + return encode( cuTpair( (const uT *)&data.front(), (const uT *)&data.front() + data.size() ), + uTpair( (uT *)&parity.front(), (uT *)&parity.front() + parity.size() )); + } + + template < typename T > + int encode( + std::vector<T> &data ) + const + { + typedef typename std::make_unsigned<T>::type + uT; + typedef std::pair<uT *, uT *> + uTpair; + data.resize( data.size() + ecc_bytes() ); + return encode( uTpair( (uT *)&data.front(), (uT *)&data.front() + data.size() )); + } + + template < typename T > + int encode( + const std::vector<T>&data, + std::vector<T> &parity ) + const + { + typedef typename std::make_unsigned<T>::type + uT; + typedef std::pair<const uT *, const uT *> + cuTpair; + typedef std::pair<uT *, uT *> + uTpair; + parity.resize( ecc_bytes() ); + return encode( cuTpair( (uT *)&data.front(), (uT *)&data.front() + data.size() ), + uTpair( (uT *)&parity.front(), (uT *)&parity.front() + parity.size() )); + } + + template < typename T, size_t N > + int encode( + std::array<T,N> &data, + int pad = 0 ) // ignore 'pad' symbols at start of array + const + { + typedef typename std::make_unsigned<T>::type + uT; + typedef std::pair<uT *, uT *> + uTpair; + return encode( uTpair( (uT *)&data.front() + pad, (uT *)&data.front() + data.size() )); + } + + + // + // encode -- denote data+parity or data, parity using pairs of uint8_t iterators + // encode -- base implementation, in terms of uint8_t pointers + // + virtual int encode( + const std::pair<uint8_t *, uint8_t *> + &data ) + const + { + return encode( data.first, data.second - data.first - ecc_bytes(), data.second - ecc_bytes() ); + } + + virtual int encode( + const std::pair<const uint8_t *, const uint8_t *> + &data, + const std::pair<uint8_t *, uint8_t *> + &parity ) + const + { + if ( size_t( parity.second - parity.first ) != ecc_bytes() ) { + EZPWD_RAISE_OR_RETURN( std::runtime_error, "BCH: parity length incompatible with number of ECC bytes", -1 ); + } + return encode( data.first, data.second - data.first, parity.first ); + } + + virtual int encode( + const uint8_t *data, + size_t len, + uint8_t *parity ) + const + { + memset( parity, 0, ecc_bytes() ); // Djelic encode_bch requires ECC to be initialized to 0 + ezpwd::encode_bch( this->_bch, data, len, parity ); + return int( ecc_bits() ); + } + + // + // decode -- container interface, w/ optional corrected bit-error positions reported + // + // Does not correct errors in parity! + // + int decode( + std::string &data, + std::vector<int> *position= 0 ) + const + { + typedef uint8_t uT; + typedef std::pair<uT *, uT *> + uTpair; + return decode( uTpair( (uT *)&data.front(), (uT *)&data.front() + data.size() ), + position ); + } + + int decode( + std::string &data, + std::string &parity, + std::vector<int> *position= 0 ) + const + { + typedef uint8_t uT; + typedef std::pair<uT *, uT *> + uTpair; + return decode( uTpair( (uT *)&data.front(), (uT *)&data.front() + data.size() ), + uTpair( (uT *)&parity.front(), (uT *)&parity.front() + parity.size() ), + position ); + } + + template < typename T > + int decode( + std::vector<T> &data, + std::vector<int> *position= 0 ) + const + { + typedef typename std::make_unsigned<T>::type + uT; + typedef std::pair<uT *, uT *> + uTpair; + return decode( uTpair( (uT *)&data.front(), (uT *)&data.front() + data.size() ), + position ); + } + + template < typename T > + int decode( + std::vector<T> &data, + std::vector<T> &parity, + std::vector<int> *position= 0 ) + const + { + typedef typename std::make_unsigned<T>::type + uT; + typedef std::pair<uT *, uT *> + uTpair; + return decode( uTpair( (uT *)&data.front(), (uT *)&data.front() + data.size() ), + uTpair( (uT *)&parity.front(), (uT *)&parity.front() + parity.size() ), + position ); + } + + template < typename T, size_t N > + int decode( + std::array<T,N> &data, + int pad = 0, // ignore 'pad' symbols at start of array + std::vector<int> *position= 0 ) + const + { + typedef typename std::make_unsigned<T>::type + uT; + typedef std::pair<uT *, uT *> + uTpair; + return decode( uTpair( (uT *)&data.front() + pad, (uT *)&data.front() + data.size() ), + position ); + } + + // + // decode -- denote data+parity or data, parity using pairs of uint8_t iterators + // decode -- decode and correct BCH codeword, returning number of corrections, or -1 if failed + // + // Corrects data in-place (unlike the basic Djelic Linux Kernel API, which only returns + // error positions. For consistency with ezpwd::rs..., we report all error positions as + // std::vector<int>, even though the underlying Djelic API reports them as arrays of + // unsigned int. + // + virtual int decode( + const std::pair<uint8_t *, uint8_t *> + &data, + std::vector<int> *position= 0 ) + const + { + return decode( data.first, data.second - data.first - ecc_bytes(), data.second - ecc_bytes(), + position ); + } + + virtual int decode( + const std::pair<uint8_t *, uint8_t *> + &data, + const std::pair<uint8_t *, uint8_t *> + &parity, + std::vector<int> *position= 0 ) + const + { + if ( size_t( parity.second - parity.first ) != ecc_bytes() ) { + EZPWD_RAISE_OR_RETURN( std::runtime_error, "BCH: parity length incompatible with number ECC bytes", -1 ); + } + return decode( data.first, data.second - data.first, parity.first, + position ); + } + virtual int decode( + uint8_t *data, + size_t len, + uint8_t *parity, + std::vector<int> *position= 0 ) + const + { + if ( position ) + position->resize( t() * 2 ); // may be able to correct beyond stated capacity! + int corrects = ezpwd::correct_bch( + this->_bch, data, len, parity, 0, 0, + position ? (unsigned int *)&(*position)[0] : 0 ); + if ( position && corrects >= 0 ) + position->resize( corrects ); + return corrects; + } + + // + // {en,de}coded -- returns an encoded/corrected copy of the provided container + // + // NOTE: + // + // Must return exceptions on failure; If exceptions inhibited, returns a + // default-constructed instance of the supplied data container. This may be sufficient to + // reliably deduce failure; if not, this interface should not be used. + // + // Overloads decoded to also allow recovery of corrected error positions and count. + // + template <typename C> + C encoded( + C data ) + const + { + if ( encode( data ) < 0 ) + EZPWD_RAISE_OR_RETURN( std::runtime_error, "BCH: Could not encode data", C() ); + return data; + } + + template <typename C> + C decoded( + C data ) + const + { + if ( decode( data ) < 0 ) + EZPWD_RAISE_OR_RETURN( std::runtime_error, "BCH: Could not decode data", C() ); + return data; + } + + template <typename C> + C decoded( + C data, + std::vector<int> &position ) + const + { + if ( decode( data, &position ) < 0 ) + EZPWD_RAISE_OR_RETURN( std::runtime_error, "BCH: Could not decode data", C() ); + return data; + } + + }; // class bch_base + + template < size_t SYMBOLS, size_t CORRECTION > + class bch + : public bch_base + { + public: + bch() + : bch_base( ezpwd::log_<SYMBOLS + 1>::value, CORRECTION ) + { + ; + } + + virtual ~bch() + { + ; + } + }; // class bch + + // + // std::ostream << ezpwd::bch_base + // + // Output a BCH codec description in standard form eg. BCH( 255, 239, 2 ) + // + inline + std::ostream &operator<<( + std::ostream &lhs, + const ezpwd::bch_base + &rhs ) + { + return rhs.output( lhs ); + } + + // + // ezpwd::BCH<SYMBOLS, PAYLOAD, CAPACITY> -- Standard BCH codec types + // + // Specify and create a standard BCH codec with exactly the specified capacities. We create + // the undering BCH codec using SYMBOLS and CORRECTION capacity; the actual correction capacity + // T, the number of PARITY bits and hence PAYLOAD (CAPACITY - PARITY) is selected automatically + // by the underlying Djelic Linux Kernel BCH codec API. For this interface, we demand that the + // caller *knows* all of these values at compile time, however, mostly for future optimization + // purposes. We validate them, and fail the constructor if they don't match. See bch_test for + // an enumeration of all possible BCH codecs. + // + // In the future, this API may be re-implemented to not use the generic BCH API, but a more + // optimized locally-defined implementation that leverages the fixed SYMBOLS, PAYLOAD and + // CORRECTION capacities to produce more optimal code. + // + template < size_t SYMBOLS, size_t PAYLOAD, size_t CORRECTION > + class BCH + : public bch<SYMBOLS, CORRECTION> + { + public: + static const size_t M = ezpwd::log_<SYMBOLS + 1>::value; // Galois field order; eg. 255 --> 8 + static const size_t N = SYMBOLS; + static const size_t T = CORRECTION; + static const size_t LOAD = PAYLOAD; + + BCH() + : bch<SYMBOLS, CORRECTION>() + { + if ( this->_bch->t != T || this->_bch->n != N + || this->_bch->n - this->_bch->ecc_bits != LOAD ) { + std::ostringstream err; + this->output( err ) + << " specified doesn't match underlying " << *this->_bch << " produced."; + EZPWD_RAISE_OR_ABORT( std::runtime_error, err.str().c_str() ); + } + } + + virtual ~BCH() + { + ; + } + + // + // <ostream> << BCH<...> -- output codec in standard BCH( N, N-ECC, T ) form + // + virtual std::ostream &output( + std::ostream &lhs ) + const + { + return lhs + << "BCH( " << std::setw( 3 ) << N + << ", " << std::setw( 3 ) << LOAD + << ", " << std::setw( 3 ) << T + << " )"; + } + }; // class BCH + + + // + // std::ostream << ezpwd::BCH<...> + // + // Output a BCH codec description in standard form eg. BCH( 255, 239, 2 ) + // + // NOTE: clang/gcc disagree on the scoping of operator<< template/non-template functions... + // + template <size_t SYMBOLS, size_t PAYLOAD, size_t CORRECTION> + inline + std::ostream &operator<<( + std::ostream &lhs, + const ezpwd::BCH<SYMBOLS, PAYLOAD, CORRECTION> + &rhs ) + { + return rhs.output( lhs ); + } + +} // namespace ezpwd + +#endif // _EZPWD_BCH diff --git a/op25/gr-op25_repeater/lib/ezpwd/bch_base b/op25/gr-op25_repeater/lib/ezpwd/bch_base new file mode 100644 index 0000000..0aa0592 --- /dev/null +++ b/op25/gr-op25_repeater/lib/ezpwd/bch_base @@ -0,0 +1,219 @@ +/* + * Ezpwd Reed-Solomon -- Reed-Solomon encoder / decoder library + * + * Copyright (c) 2017, Hard Consulting Corporation. + * + * Ezpwd Reed-Solomon 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 3 of + * the License, or (at your option) any later version. See the LICENSE file at the top of the + * source tree. Ezpwd Reed-Solomon is also available under Commercial license. The Djelic BCH code + * under djelic/ and the c++/ezpwd/bch_base wrapper is redistributed under the terms of the GPLv2+, + * regardless of the overall licensing terms. + * + * Ezpwd Reed-Solomon 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. + */ +#ifndef _EZPWD_BCH_BASE +#define _EZPWD_BCH_BASE + +#include <iostream> +#include <iomanip> +#include <vector> + +// +// Presently, we simply import the Linux Kernel's "C" BCH API directly into the ezpwd:: namespace In +// order to compile, you must (at least) run "make djelic" in the base directory of the +// https://github.com/pjkundert/bch.git repo. +// +namespace ezpwd { + /** + * struct bch_control - BCH control structure + * @m: Galois field order + * @n: maximum codeword size in bits (= 2^m-1) + * @t: error correction capability in bits + * @ecc_bits: ecc exact size in bits, i.e. generator polynomial degree (<=m*t) + * @ecc_bytes: ecc max size (m*t bits) in bytes + * @a_pow_tab: Galois field GF(2^m) exponentiation lookup table + * @a_log_tab: Galois field GF(2^m) log lookup table + * @mod8_tab: remainder generator polynomial lookup tables + * @ecc_buf: ecc parity words buffer + * @ecc_buf2: ecc parity words buffer + * @xi_tab: GF(2^m) base for solving degree 2 polynomial roots + * @syn: syndrome buffer + * @cache: log-based polynomial representation buffer + * @elp: error locator polynomial + * @poly_2t: temporary polynomials of degree 2t + */ + + /** + * init_bch - initialize a BCH encoder/decoder + * @m: Galois field order, should be in the range 5-15 + * @t: maximum error correction capability, in bits + * @prim_poly: user-provided primitive polynomial (or 0 to use default) + * + * Returns: + * a newly allocated BCH control structure if successful, NULL otherwise + * + * This initialization can take some time, as lookup tables are built for fast + * encoding/decoding; make sure not to call this function from a time critical + * path. Usually, init_bch() should be called on module/driver init and + * free_bch() should be called to release memory on exit. + * + * You may provide your own primitive polynomial of degree @m in argument + * @prim_poly, or let init_bch() use its default polynomial. + * + * Once init_bch() has successfully returned a pointer to a newly allocated + * BCH control structure, ecc length in bytes is given by member @ecc_bytes of + * the structure. + */ + + /** + * encode_bch - calculate BCH ecc parity of data + * @bch: BCH control structure + * @data: data to encode + * @len: data length in bytes + * @ecc: ecc parity data, must be initialized by caller + * + * The @ecc parity array is used both as input and output parameter, in order to + * allow incremental computations. It should be of the size indicated by member + * @ecc_bytes of @bch, and should be initialized to 0 before the first call. + * + * The exact number of computed ecc parity bits is given by member @ecc_bits of + * @bch; it may be less than m*t for large values of t. + */ + + /** + * decode_bch - decode received codeword and find bit error locations + * @bch: BCH control structure + * @data: received data, ignored if @calc_ecc is provided + * @len: data length in bytes, must always be provided + * @recv_ecc: received ecc, if NULL then assume it was XORed in @calc_ecc + * @calc_ecc: calculated ecc, if NULL then calc_ecc is computed from @data + * @syn: hw computed syndrome data (if NULL, syndrome is calculated) + * @errloc: output array of error locations + * + * Returns: + * The number of errors found, or -EBADMSG if decoding failed, or -EINVAL if + * invalid parameters were provided + * + * Depending on the available hw BCH support and the need to compute @calc_ecc + * separately (using encode_bch()), this function should be called with one of + * the following parameter configurations - + * + * by providing @data and @recv_ecc only: + * decode_bch(@bch, @data, @len, @recv_ecc, NULL, NULL, @errloc) + * + * by providing @recv_ecc and @calc_ecc: + * decode_bch(@bch, NULL, @len, @recv_ecc, @calc_ecc, NULL, @errloc) + * + * by providing ecc = recv_ecc XOR calc_ecc: + * decode_bch(@bch, NULL, @len, NULL, ecc, NULL, @errloc) + * + * by providing syndrome results @syn: + * decode_bch(@bch, NULL, @len, NULL, NULL, @syn, @errloc) + * + * Once decode_bch() has successfully returned with a positive value, error + * locations returned in array @errloc should be interpreted as follows - + * + * if (errloc[n] >= 8*len), then n-th error is located in ecc (no need for + * data correction) + * + * if (errloc[n] < 8*len), then n-th error is located in data and can be + * corrected with statement data[errloc[n]/8] ^= 1 << (errloc[n] % 8); + * + * Note that this function does not perform any data correction by itself, it + * merely indicates error locations. + */ + + /** + * init_bch - initialize a BCH encoder/decoder + * @m: Galois field order, should be in the range 5-15 + * @t: maximum error correction capability, in bits + * @prim_poly: user-provided primitive polynomial (or 0 to use default) + * + * Returns: + * a newly allocated BCH control structure if successful, NULL otherwise + * + * This initialization can take some time, as lookup tables are built for fast + * encoding/decoding; make sure not to call this function from a time critical + * path. Usually, init_bch() should be called on module/driver init and + * free_bch() should be called to release memory on exit. + * + * You may provide your own primitive polynomial of degree @m in argument + * @prim_poly, or let init_bch() use its default polynomial. + * + * Once init_bch() has successfully returned a pointer to a newly allocated + * BCH control structure, ecc length in bytes is given by member @ecc_bytes of + * the structure. + */ + + extern "C" { + #include "../../djelic_bch.h" + } + + // + // correct_bch -- corrects data (but not parity!), as suggested by decode_bch, above + // + // A convenience interface that defaults all of the not strictly required parameters, and + // automatically corrects bit-errors in data *and* the supplied parity. Does not attempt to + // correct bit errors found in the parity data. If not supplied, 'errloc' is allocated + // internally; otherwise, it is assumed to be of at least size bch->t (the minimum error + // correction capacity of the BCH codec). + // + // However, beware -- at larger values of T, the actual correction capacity of the BCH codec + // could be greater than the requested T. Therefore, it is recommended that you always supply a + // larger than required errloc array; recommend T*2? + // + inline + int correct_bch( + struct bch_control *bch, + uint8_t *data, + unsigned int len, + uint8_t *recv_ecc, + const uint8_t *calc_ecc= 0, + const unsigned int *syn = 0, + unsigned int *errloc = 0 ) // must be sized at least bch->t; often, greater! + { + unsigned int _errloc[511]; // much larger than the correction capacity of largest supported BCH codec + if ( ! errloc ) + errloc = _errloc; + int err = decode_bch( bch, data, len, recv_ecc, calc_ecc, syn, errloc ); + if ( err > 0 ) { + // A +'ve number of bit-error correction locations were found + for ( int n = 0; n < err; ++n ) { + /** + * if (errloc[n] < 8*len), then n-th error is located in data and can be corrected + * with statement data[errloc[n]/8] ^= 1 << (errloc[n] % 8). If in the parity, it + * is assumed to be located at the end of the data, so offset by 'len' bytes. + */ + if ( errloc[n] < 8*len ) { + data[errloc[n] / 8] ^= 1 << ( errloc[n] % 8 ); + } else if ( recv_ecc && errloc[n] < 8 * len + 8 * bch->ecc_bytes ) { + recv_ecc[errloc[n] / 8 - len] + ^= 1 << ( errloc[n] % 8 ); + } + } + } + return err; + } + + // + // <ostream> << <ezpwd::bch_control> -- output codec in standard BCH( N, N-ECC, T ) form + // + inline + std::ostream &operator<<( + std::ostream &lhs, + const ezpwd::bch_control + &bch ) + { + return lhs + << "BCH( " << std::setw( 3 ) << bch.n + << ", " << std::setw( 3 ) << bch.n - bch.ecc_bits + << ", " << std::setw( 3 ) << bch.t + << " )"; + } + +} // namespace ezpwd + +#endif // _EZPWD_BCH_BASE diff --git a/op25/gr-op25_repeater/lib/ezpwd/corrector b/op25/gr-op25_repeater/lib/ezpwd/corrector new file mode 100644 index 0000000..23a36a0 --- /dev/null +++ b/op25/gr-op25_repeater/lib/ezpwd/corrector @@ -0,0 +1,506 @@ +/* + * Ezpwd Reed-Solomon -- Reed-Solomon encoder / decoder library + * + * Copyright (c) 2014, Hard Consulting Corporation. + * + * Ezpwd Reed-Solomon 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 3 of + * the License, or (at your option) any later version. See the LICENSE file at the top of the + * source tree. Ezpwd Reed-Solomon is also available under Commercial license. c++/ezpwd/rs_base + * is redistributed under the terms of the LGPL, regardless of the overall licensing terms. + * + * Ezpwd Reed-Solomon 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. + */ +#ifndef _EZPWD_CORRECTOR +#define _EZPWD_CORRECTOR + +#include "rs" +#include "serialize" + +namespace ezpwd { + + // + // best_avg -- collect <password>,<confidence> guesses, and return the unambiguous best one + // + typedef std::map<std::string, std::pair<int, int>> // (<password>, (<count>, <avgsum>)) + best_avg_base_t; + class best_avg + : public best_avg_base_t + { + public: + using best_avg_base_t::begin; + using best_avg_base_t::end; + using best_avg_base_t::insert; + using best_avg_base_t::find; + using best_avg_base_t::iterator; + using best_avg_base_t::const_iterator; + using best_avg_base_t::value_type; + using best_avg_base_t::mapped_type; + // + // add -- add the given pct to the current average for <string> str + // + iterator add( + const std::string &str, + int pct ) + { + iterator i = find( str ); + if ( i == end() ) + i = insert( i, value_type( str, mapped_type() )); + i->second.first += 1; + i->second.second += pct; + return i; + } + + // + // best -- return the unambiguously best value (average is >, or == but longer), or end() + // + const_iterator best() + const + { + const_iterator top = end(); + bool uni = false; + for ( const_iterator i = begin(); i != end(); ++i ) { + if ( top == end() + or i->second.second/i->second.first > top->second.second/top->second.first + or ( i->second.second/i->second.first == top->second.second/top->second.first + and i->first.size() > top->first.size())) { + top = i; + uni = true; + } else if ( i->second.second/i->second.first == top->second.second/top->second.first + and i->first.size() == top->first.size()) { + uni = false; + } + } + return uni ? top : end(); + } + + // + // evaluation -- process a (<password>,(<count>,<avgsum>)) into (<average>,<password>) + // sort -- return a multimap indexed by <average> --> <string> + // output -- output the <string>: <average>, sorted by average + // + static std::pair<const int,const std::string &> + evaluation( const value_type &val ) + { + return std::pair<const int,const std::string &>( val.second.second/val.second.first, val.first ); + } + typedef std::multimap<const int,const std::string &> + sorted_t; + sorted_t sort() + const + { + sorted_t dst; + std::transform( begin(), end(), std::inserter( dst, dst.begin() ), evaluation ); + return dst; + } + std::ostream &output( + std::ostream &lhs ) + const + { + for ( auto i : sort() ) + lhs << std::setw( 16 ) << i.second + << ": " << std::setw( 3 ) << i.first + << std::endl; + return lhs; + } + }; +} // namespace ezpwd + +std::ostream &operator<<( + std::ostream &lhs, + const ezpwd::best_avg &rhs ) +{ + return rhs.output( lhs ); +} + +namespace ezpwd { + // + // ezpwd::corrector -- Apply statistical corrections to a string, returning the confidence + // + // All methods are static; no instance is required, as this is primarily used to create + // external language APIs. + // + template < + size_t PARITY, + size_t N = 64, + typename SERIAL = serialize::base< N, serialize::ezpwd< N >>> + class corrector { + public: + static + std::ostream &output( + std::ostream &lhs ) + { + lhs << "corrector<PARITY=" << PARITY << ",N=" << N << ",SERIAL=" << SERIAL() << ">"; + return lhs; + } + + // + // parity(<string>) -- Returns 'PARITY' base-N symbols of R-S parity to the supplied password + // + static std::string parity( + const std::string &password ) + { + std::string parity; + rscodec.encode( password, parity ); + SERIAL::encode( parity ); + return parity; + } + + // + // encode(<string>) -- append PARITY base-N parity symbols to password + // + // The supplied password buffer size must be sufficient to contain PARITY additional + // symbols, plus the terminating NUL. Returns the resultant encoded password size + // (excluding the NUL). + // + static size_t encode( + std::string &password ) + { + password += parity( password ); + return password.size(); + } + + static size_t encode( + char *password, + size_t size ) // maximum available size + { + size_t len = ::strlen( password ); // length w/o terminating NUL + if ( len + PARITY + 1 > size ) + throw std::runtime_error( "ezpwd::rspwd::encode password buffer has insufficient capacity" ); + std::string par = parity( std::string( password, password + len )); + if ( par.size() != PARITY ) + throw std::runtime_error( "ezpwd::rspwd::encode computed parity with incorrect size" ); + std::copy( par.begin(), par.end(), password + len ); + len += PARITY; + password[len] = 0; + return len; + } + + // + // decode(<string>[,...]) -- Applies R-S error correction on the encoded string, removing parity + // + // Up to 'PARITY' Reed-Solomon parity symbols are examined, to determine if the supplied + // string is a valid R-S codeword and hence very likely to be correct. Optionally supply a + // vector of erasure positions. + // + // An optional 'minimum' final password length may be provided; no R-S parity is assumed + // to exist in the first 'minimum' password characters (default: PARITY). This prevents + // accidentally finding valid R-S codewords in passwords of known minimum length; validation + // codes, for example. Likewise, the optional 'maximum' allows us to limit the number of + // parity symbols that may be assumed to be missing from the end of the codeword. + // + // Returns a confidence strength rating, which is the ratio: + // + // 100 - ( errors * 2 + erasures ) * 100 / parity + // + // if an R-S codeword was solved, and 0 otherwise. If a codeword is solved, but the number + // of errors and erasures corrected indicates that all parity was consumed, the caller may + // opt to not use the corrected string, because there is a chance that our R-S polynomial + // was overwhelmed with errors and actually returned an incorrect codeword. Therefore, + // solving a codeword using all available parity results in 100 - PARITY * 100 / PARITY == + // 0, which indicates that there is no certainty of correctness; all R-S parity resources + // were used in error/erasure recover, with none left to confirm that the result is actually + // correct. If only zero-strength results are achieved, the longest will be returned (the + // full, original string). + // + // Supports the following forms of error/erasure: + // + // 0) Full parity. All data and parity supplied, and an R-S codeword is solved. + // + // 1) Partial parity. All data and some parity supplied; remainder are deemed erasures. + // + // If PARITY > 2, then up to PARITY/2-1 trailing parity terms are marked as erasures. + // If the R-S codeword is solved and a safe number of errors are found, then we can have + // reasonable confidence that the string is correct. + // + // 1a) Erase errors. Permute the combinations of up to PARITY-1 erasures. + // + // o) Raw password. No parity terms supplied; not an R-S codeword + // + // If none of the error/erasure forms succeed, the password is returned unmodified. + // + // If a non-zero 'minimum' or 'maximum' are provided, they constrain the possible + // resultant password sizes that will be attempted. + // + static + int decode( + std::string &password, + const std::vector<int> + &erasures, + size_t minimum = PARITY,//always deemed at least 1 + size_t maximum = 0 ) // if 0, no limit + { + int confidence; + best_avg best; + + // Full/Partial parity. Apply some parity erasure if we have some erasure/correction + // capability while maintaining at least one excess parity symbol for verification. + // This can potentially result in longer password being returned, if the R-S decoder + // accidentally solves a codeword. + // + // For example, if PARITY=3 (or 4) then (PARITY+1)/2 == 2, and we would only attempt up + // to 1 parity erasure. This would leave 1 parity symbol to replace the 1 erasure, and + // 1 remaining to validate the integrity of the password. + // + // The password must be long enough to contain at least 1 non-parity symbol, and the + // designated number of non-erased parity symbols! However, by convention we'll demand + // that the password contain at least PARITY symbols -- any less, and we can + // accidentally correct the few remaining password symbols. + // + // Also, if any parity symbols won't decode (eg. were entered in error), we must deem + // them to be erasures, too, and if the number of erasures exceeds the capacity of the + // R-S codec, it'll fail (throw an exception, or at best solve with 0 confidence). + for ( size_t era = 0 // how many parity symbols to deem erased + ; era < (PARITY+1)/2 + ; ++era ) { + if ( password.size() < ( minimum ? minimum : 1 ) + PARITY - era ) { +#if defined( DEBUG ) && DEBUG >= 1 + output( std::cout ) + << " Rejected too short password \"" + << password << std::string( era, '_' ) + << "\"" << " (" << era << " parity skipped)" + << std::endl; +#endif + continue; // too few password symbols to start checking parity + } + + if ( maximum and password.size() > maximum + PARITY - era ) { +#if defined( DEBUG ) && DEBUG >= 1 + output( std::cout ) + << " Rejected too long password \"" + << password << std::string( era, '_' ) + << "\"" << " (" << era << " parity skipped)" + << std::endl; +#endif + continue; // too few parity symbols erased to start checking parity + } + + // Copy password, adding 'era' additional NULs + std::string fixed( password.size() + era, 0 ); + std::copy( password.begin(), password.end(), fixed.begin() ); + + // Decode the base-N parity, denoting any invalid (mistyped or trailing NUL) symbols + // as erasures (adjust erasure offsets to be from start of password, not start of + // parity). All newly added 'era' symbols will be NUL, and will be invalid. After + // decoding parity, if we've slipped below our minimum R-S capacity threshold + // (ie. because of mistyped parity symbols), don't attempt. + std::vector<int> all_era; + SERIAL::decode( fixed.begin() + fixed.size() - PARITY, + fixed.begin() + fixed.size(), &all_era, 0, + serialize::ws_invalid, serialize::pd_invalid ); + if ( all_era.size() >= (PARITY+1)/2 ) { +#if defined( DEBUG ) && DEBUG >= 1 + output( std::cout ) + << " Rejected low parity password \"" + << password << std::string( era, '_' ) + << "\"" << " (" << all_era.size() << " parity erasures + " + << era << " skipped)" + << std::endl; +#endif + continue; // Too many missing parity symbols + } + if ( all_era.size() + erasures.size() > PARITY ) { +#if defined( DEBUG ) && DEBUG >= 1 + output( std::cout ) + << " Rejected hi erasure password \"" + << password << std::string( era, '_' ) + << "\"" << " (" << all_era.size() + erasures.size() << " total erasures + " + << era << " skipped)" + << std::endl; +#endif + continue; // Total erasures beyond capacity + } + for ( auto &o : all_era ) + o += fixed.size() - PARITY; + std::copy( erasures.begin(), erasures.end(), std::back_inserter( all_era )); + + // Enough parity to try to decode. A successful R-S decode with 0 (remaining) + // confidence indicates a successfully validated R-S codeword! Use it (ex. parity). + try { + std::vector<int> position; + int corrects= rscodec.decode( fixed, all_era, &position ); + confidence = strength<PARITY>( corrects, all_era, position ); + fixed.resize( fixed.size() - PARITY ); + if ( confidence >= 0 ) + best.add( fixed, confidence ); +#if defined( DEBUG ) && DEBUG >= 1 + output( std::cout ) + << " Reed-Solomon w/ " << era << " of " << PARITY + << " parity erasures " << std::setw( 3 ) << confidence + << "% confidence: \"" << password + << "\" ==> \"" << fixed + << "\" (corrects: " << corrects + << ", erasures at " << all_era + << ", fixed at " << position << "): " + << std::endl + << best; +#endif + } catch ( std::exception &exc ) { +#if defined( DEBUG ) && DEBUG >= 2 // should see only when ezpwd::reed_solomon<...>::decode fails + output( std::cout ) << " invalid part parity password: " << exc.what() << std::endl; +#endif + } + } + + // Partial parity, but below threshold for usable error detection. For the first 1 to + // (PARITY+1)/2 parity symbols (eg. for PARITY == 3, (PARITY+1)/2 == 1 ), we cannot + // perform meaningful error or erasure detection. However, if we see that the terminal + // symbols match the R-S symbols we expect from a correct password, we'll ascribe a + // partial confidence due to the matching parity symbols. + // + // password: sock1t + // w/ 3 parity: sock1tkeB + // password ----^^^^^^ + // ^^^--- parity + // + for ( size_t era = (PARITY+1)/2 // how many parity symbols are not present + ; era < PARITY + ; ++era ) { + if ( password.size() < ( minimum ? minimum : 1 ) + PARITY - era ) { +#if defined( DEBUG ) && DEBUG >= 1 + output( std::cout ) + << " Rejected too short password \"" + << password << std::string( era, '_' ) + << "\"" + << std::endl; +#endif + continue; // too few password symbols to start checking parity + } + if ( maximum and password.size() > maximum + PARITY - era ) { +#if defined( DEBUG ) && DEBUG >= 1 + output( std::cout ) + << " Rejected too long password \"" + << password << std::string( era, '_' ) + << "\"" << " (" << era << " parity skipped)" + << std::endl; +#endif + continue; // too few parity symbols erased to start checking parity + } + std::string fixed = password; + size_t len = password.size() - ( PARITY - era ); + fixed.resize( len ); + encode( fixed ); + auto differs = std::mismatch( fixed.begin(), fixed.end(), password.begin() ); + size_t par_equ = differs.second - password.begin(); + if ( par_equ < len || par_equ > len + PARITY ) + throw std::runtime_error( "miscomputed R-S parity matching length" ); + par_equ -= len; + + // At least one parity symbol is requires to give any confidence + if ( par_equ > 0 ) { + std::string basic( fixed.begin(), fixed.begin() + len ); + confidence = par_equ * 100 / PARITY; // each worth a normal parity symbol + best.add( basic, confidence ); +#if defined( DEBUG ) && DEBUG >= 1 + output( std::cout ) + << " Check Chars. w/ " << era << " of " << PARITY + << " parity missing " << std::setw( 3 ) << confidence + << "% confidence: \"" << password + << "\" ==> \"" << basic + << " (from computed: \"" << fixed << "\")" + << ": " + << std::endl + << best; +#endif + } + } + + // Select the best guess and return its confidence. Otherwise, use raw password? If no + // error/erasure attempts succeeded (if no 'best' w/ confidence >= 0), then we'll use + // the raw password w/ 0 confidence, if it meets the minimum/maximum length + // requirements. + confidence = -1; + if ( password.size() >= ( minimum ? minimum : 1 ) + and ( maximum == 0 or password.size() <= maximum )) + confidence = 0; + + typename best_avg::const_iterator + bi = best.best(); +#if defined( DEBUG ) + output( std::cout ) + << " Selected " << ( bi != best.end() ? "corrected" : "unmodified" ) + << " password \"" << ( bi != best.end() ? bi->first : password ) + << "\" of length " << ( minimum ? minimum : 1) << "-" << maximum + << " (vs. \"" << password + << "\") w/ confidence " << (bi != best.end() ? bi->second.second : confidence ) + << "%, from: " + << std::endl + << best; +#endif + if ( bi != best.end() ) { + auto better = best.evaluation( *bi ); // --> (<average>,<password>) + password = better.second; + confidence = better.first; + } + return confidence; + } + + static + int decode( + std::string &password, + size_t minimum = PARITY, + size_t maximum = 0 ) + { + return decode( password, std::vector<int>(), minimum, maximum ); + } + + // + // decode(<char*>,<size_t>,<size_t>,<size_t>) -- C interface to decode(<string>) + // + // Traditional C interface. The provided NUL-terminated password+parity is decoded + // (parity removed), and the confidence % is returned. + // + // If any failure occurs, a -'ve value will be returned, and the supplied password + // buffer will be used to contain an error description. + // + static int decode( + char *password, // NUL terminated + size_t siz, // available size + size_t minimum = PARITY,//minimum resultant password length + size_t maximum = 0 ) // maximum '' + { + std::string corrected( password ); + int confidence; + try { + confidence = decode( corrected, minimum, maximum ); + if ( corrected.size() + 1 > siz ) + throw std::runtime_error( "password buffer has insufficient capacity" ); + std::copy( corrected.begin(), corrected.end(), password ); + password[corrected.size()] = 0; + } catch ( std::exception &exc ) { + confidence = -1; + ezpwd::streambuf_to_buffer sbf( password, siz ); + std::ostream( &sbf ) << "corrector<" << PARITY << "> failed: " << exc.what(); + } + return confidence; + } + + // + // rscodec -- A ?-bit RS(N-1,N-1-PARITY) Reed-Solomon codec + // + // Encodes and decodes R-S symbols over the lower 6 bits of the supplied data. Requires + // that the last N (parity) symbols of the data are in the range [0,63]. The excess bits on + // the data symbols are masked and restored during decoding. + // + static const ezpwd::RS<N-1,N-1-PARITY> + rscodec; + }; + + template < size_t PARITY, size_t N, typename SERIAL > + const ezpwd::RS<N-1,N-1-PARITY> + corrector<PARITY,N,SERIAL>::rscodec; + +} // namespace ezpwd + +template < size_t PARITY, size_t N, typename SERIAL > +std::ostream &operator<<( + std::ostream &lhs, + const ezpwd::corrector<PARITY,N,SERIAL> + &rhs ) +{ + return rhs.output( lhs ); +} + +#endif // _EZPWD_CORRECTOR diff --git a/op25/gr-op25_repeater/lib/ezpwd/definitions b/op25/gr-op25_repeater/lib/ezpwd/definitions new file mode 100644 index 0000000..1ecf9a4 --- /dev/null +++ b/op25/gr-op25_repeater/lib/ezpwd/definitions @@ -0,0 +1,9 @@ +// +// C++ Definitions -- include once in a single C++ compilation unit +// +#ifndef _EZPWD_DEFINITIONS +#define _EZPWD_DEFINITIONS + +#include "serialize_definitions" + +#endif // _EZPWD_DEFINITIONS
\ No newline at end of file diff --git a/op25/gr-op25_repeater/lib/ezpwd/ezcod b/op25/gr-op25_repeater/lib/ezpwd/ezcod new file mode 100644 index 0000000..8fe0a90 --- /dev/null +++ b/op25/gr-op25_repeater/lib/ezpwd/ezcod @@ -0,0 +1,725 @@ +/* + * Ezpwd Reed-Solomon -- Reed-Solomon encoder / decoder library + * + * Copyright (c) 2014, Hard Consulting Corporation. + * + * Ezpwd Reed-Solomon 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 3 of + * the License, or (at your option) any later version. See the LICENSE file at the top of the + * source tree. Ezpwd Reed-Solomon is also available under Commercial license. c++/ezpwd/rs_base + * is redistributed under the terms of the LGPL, regardless of the overall licensing terms. + * + * Ezpwd Reed-Solomon 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. + */ +#ifndef _EZPWD_EZCOD +#define _EZPWD_EZCOD + +#include <math.h> // M_PI +#include <cmath> +#include <cctype> + +#include <cstdint> +#include <ezpwd/rs> +#include <ezpwd/output> +#include <ezpwd/serialize> + +// +// EZCOD 3:10 location code w/ Reed-Solomon Error Correction, and average 3m accuracy +// +// - each successive symbol provides greater precision +// - codes nearby each-other are identical in leading characters +// - average 3m precision achieved in 9 symbols +// - more than 4 base-10 digits of precision in both lat and lon after the decimal +// - from 1 to 3 symbols of Reed-Solomon parity +// - 1 parity symbol supplies validation w/ strength equivalent to a check character +// - 2 parity symbols provides correction of 1 lost symbol (no errors) +// - 3 parity symbols provides correction of any 1 error, with verification, +// or recovery of up to any 3 lost symbols (with no other errors) +// + +// +// To achieve at least 4 decimal digits of precision after the decimal point, we must have +// defined lat to within 1 part in 1,800,000, and lon to within 1 part in 3,600,000. As each symbol +// supplies bits, we'll refine the computed lat/lon further, reducing the outstanding fraction of +// "parts" yet to be defined. +// +// bits +// symbols latitude longitude +// bits mul parts bits mul parts +// 1 2 4 4 3 8 8 +// 2 2 4 16 3 8 64 +// 3 3 8 128 2 4 256 // not quite integer lat/lon accuracy +// +// 4 2 4 512 3 8 2,048 +// 5 3 8 4,096 2 4 8,192 +// 6 2 4 16,384 3 8 65,536 +// +// 7 3 8 131,072 2 4 262,144 +// 8 2 4 524,288 3 8 2,097,152 +// 9 3 8 4,194,304 2 4 8,388,608 parts resolution in 3:10 code +// over [-90, 90] over [-180,180] yields ~3m resolution +// +// vs. 1,800,000 3,600,000 parts resolution in 10:10 code +// over [-90, 90] over [-180,180] yields ~10m resolution +// +// Therefore, within 9 symbols we define lat and lon with better than double the precision of +// 10:10 code's 4 decimal digits after the decimal point. This yields an approximate lineal +// precision of 40,075,000m / 8,388,608 == ~5m in both dimensions at the equator, vs. 40,075,000m / +// 3,600,000 == ~11m for 10:10 codes. +// +// The 10:10 code provides a single check character, which provides about P(1-1/32) certainty +// that the provided code is correct. With EZCOD 3:10/11/12 codes, we provide varying levels of +// detection/correction strength. +// +// - 1 parity symbol: act as a check character (like 10:10 codes), or provide 1 symbol of erasure +// (lost symbol) recovery with no excess parity for validation. +// +// - 2 parity symbols: provide 1 symbol of erasure correction (w/ no other errors) with 1 excess parity +// symbol for validation, or 1 symbol of error detection with no excess parity for validation. +// +// - 3 parity symbols: correct 1 error anywhere w/ 1 excess parity symbol for validation, or up +// to 3 erasures with no excess parity for validation. +// +// Therefore, we'll provide Reed-Solomon RS(31,28-30) error correction (5 bit symbols, +// indicating 31 symbols in the field, and from 1 to 3 roots, therefore up to 28 data symbols in the +// field) over the 9 lat/lon data symbols. +// +// +// MINIMIZING ERROR +// +// Each input lat/lon coordinate will be effectively truncated by the encoding procedure to the +// level of precision (parts) encoded by each symbol. Subsequent symbols then add their (smaller) +// parts to increase precision. +// +// After the last symbol, we know that the actual input coordinates where somewhere +// within the rectangle: +// +// [0,0] -> [0,lon_precision] -> [lat_precision,lon_precision] -> [lat_precision,0] +// +// At first glance, the best way is to perform rounding instead of truncation on ecoding, by +// simply adding 1/2 of the precision. Then, the unmodified output lat/lon decoded represents the +// point nearest actual input coordinate. However, this is NOT ideal. Remember -- the decoding may +// not have access to all the symbols! We want to minimize the error even if only some of the +// symbols are available. Thus, we must apply a correction on decoding. +// +// One way gain rounding instead of truncation on decoding is, after adding the last symbol's +// precision, to add 50% of the value represented by the first bit of the next (missing) symbol's +// precision parts. This would be analogous to receiving the first 2 digits of a 3 digit number: +// +// original: 123 +// received: 12_ +// range: [120,130) +// guessed: 125 (add 1/2 of the parts represented by the missing digit) +// +// If this is done, then the resulting coordinate would be in the middle of the rectangle of +// possible input lat/lon values that could have resulted in the encoded value. This also works if +// we don't receive and decode all of the symbols; We'll end up with a lat/lon in the middle of the +// (large) rectangle of possible input coordinates. +// + + +namespace ezpwd { + + class ezcod_base { + public: + double latitude; // [-90,+90] angle, degrees + double latitude_error; // total error bar, in meters + double longitude; // [-180,180] + double longitude_error; // total error bar, in meters + double accuracy; // linear accuracy radius, in meters + int confidence; // % parity in excess of last decode + double certainty; // and the associated probability + + explicit ezcod_base( + double _lat = 0, + double _lon = 0 ) + : latitude( _lat ) + , latitude_error( 0 ) + , longitude( _lon ) + , longitude_error( 0 ) + , accuracy( 0 ) + , confidence( 100 ) + , certainty( 1 ) + { + ; + } + virtual ~ezcod_base() + { + ; + } + + typedef std::pair<unsigned char, unsigned char> + symbols_t; + virtual symbols_t symbols() + const + = 0; + virtual std::ostream &output( + std::ostream &lhs ) + const + = 0; + virtual std::string encode( + unsigned _preci = 0 ) // override precision + const + = 0; + virtual int decode( + const std::string &_ezcod ) + = 0; + }; +} // namespace ezpwd + +inline std::ostream &operator<<( + std::ostream &lhs, + const ezpwd::ezcod_base + &rhs ) +{ + return rhs.output( lhs ); +} + +namespace ezpwd { + + // + // ezcod<P,L> -- defaults to 1 PARITY and 9 location symbols (3m) of PRECISION + // + template < unsigned P=1, unsigned L=9 > + class ezcod + : public ezcod_base { + + private: + typedef std::array<symbols_t, 12> + bits_t; + static const bits_t bits; + typedef std::array<std::pair<uint32_t, uint32_t>, 12> + parts_t; + static const parts_t parts; + +#if defined( DEBUG ) + public: +#endif + static const ezpwd::RS<31,31-P> + rscodec; + + public: + static constexpr const unsigned PARITY = P; // specified symbols of R-S parity + static constexpr const unsigned PRECISION = L; // default symbols of location precision + static constexpr const unsigned CHUNK = 3; // default chunk size + + static constexpr const char SEP_NONE = -1; + static constexpr const char SEP_DEFAULT = 0; + static constexpr const char SEP_DOT = '.'; + static constexpr const char SEP_BANG = '!'; + static constexpr const char SEP_SPACE = ' '; + + static constexpr const char CHK_NONE = -1; + static constexpr const char CHK_DEFAULT = 0; + static constexpr const char CHK_DASH = '-'; + static constexpr const char CHK_SPACE = ' '; + + unsigned precision; + unsigned chunk; // Location symbol chunk sizes + char separator; // Separator between location and parity symbols + char space; // Fill space between location symbol chunks + + // + // ezcod<P,L>() -- supply non-defaults for location precision, chunk size, etc. + // + explicit ezcod( + double _lat = 0, + double _lon = 0, + unsigned _preci = 0, + unsigned _chunk = 0, + char _seper = 0, + char _space = 0 ) + : ezcod_base( _lat, _lon ) + , precision( _preci ? _preci : PRECISION ) + , chunk( _chunk ? _chunk : CHUNK ) + , separator( _seper ) + , space( _space ) + { + if ( P < 1 ) + throw std::runtime_error( "ezpwd::ezcod:: At least one parity symbol must be specified" ); + if ( precision < 1 || precision > bits.max_size() ) + throw std::runtime_error( std::string( "ezpwd::ezcod:: Only 1-" ) + + std::to_string( bits.max_size() ) + + " location symbol may be specified" ); + } + explicit ezcod( + const std::string &_ezcod, + unsigned _preci = 0, + unsigned _chunk = 0, + char _seper = 0, + char _space = 0 ) + : ezcod( 0, 0, _preci, _chunk, _seper, _space ) + { + decode( _ezcod ); + } + virtual ~ezcod() + { + ; + } + + // + // symbols -- return working parity and location precision + // + virtual ezcod_base::symbols_t + symbols() + const + { + return ezcod_base::symbols_t( P, precision ); + } + + virtual std::ostream &output( + std::ostream &lhs ) + const + { + std::streamsize prec = lhs.precision(); + std::ios_base::fmtflags + flg = lhs.flags(); + lhs.precision( 10 ); + std::string uni = "m "; + double acc = accuracy; + double dec = 2; + if ( acc > 1000 ) { + uni = "km"; + acc /= 1000; + } else if ( acc < 1 ) { + uni = "mm"; + acc *= 1000; + } + if ( acc >= 100 ) + dec = 0; + else if ( acc >= 10 ) + dec = 1; + + lhs << encode( precision ) + << " (" << std::setw( 3 ) << confidence + << "%) == " << std::showpos << std::fixed << std::setprecision( 10 ) << std::setw( 15 ) << latitude + << ", " << std::showpos << std::fixed << std::setprecision( 10 ) << std::setw( 15 ) << longitude + << " +/- " << std::noshowpos << std::fixed << std::setprecision( dec ) << std::setw( 6 ) << acc << uni; + lhs.precision( prec ); + lhs.flags( flg ); + return lhs; + } + + // + // encode() -- encode the lat/lon to 'precision' symbols EZCOD representation + // + virtual std::string encode( + unsigned _preci = 0 ) // override precision + const + { + // Convert lat/lon into a fraction of number of parts assigned to each + double lat_frac= ( latitude + 90 ) / 180; + if ( lat_frac < 0 || lat_frac > 1 ) + throw std::runtime_error( "ezpwd::ezcod::encode: Latitude not in range [-90,90]" ); + double lon_frac= ( longitude + 180 ) / 360; + if ( lon_frac < 0 || lon_frac > 1 ) + throw std::runtime_error( "ezpwd::ezcod::encode: Longitude not in range [-180,180]" ); + if ( _preci == 0 ) + _preci = precision; + if ( _preci < 1 || _preci > bits.max_size() ) + throw std::runtime_error( std::string( "ezpwd::ezcod:: Only 1-" ) + + std::to_string( bits.max_size() ) + + " location symbol may be specified" ); + + // Compute the integer number of lat/lon parts represented by each coordinate, for the + // specified level of precision, and then truncate to the range [0,..._parts), + // eg. Latitude 90 --> 89.999... + uint32_t lat_parts = parts[_preci-1].first; // [ -90,90 ] / 4,194,304 parts in 9 symbols + uint32_t lon_parts = parts[_preci-1].second; // [-180,180] / 8,388,608 parts '' + + uint32_t lat_rem = std::min( lat_parts-1, uint32_t( lat_parts * lat_frac )); + uint32_t lon_rem = std::min( lon_parts-1, uint32_t( lon_parts * lon_frac )); + + // Initial loop condition; lat/lon multiplier is left at the base multiplier of the + // previous loop. Then, loop computing the units multiplier, and hten removing the most + // significant bits (multiples of the units multiplier). They will both reach 1 + unsigned int lat_mult= lat_parts; + unsigned int lon_mult= lon_parts; + + std::string res; + res.reserve( _preci // approximate result length + + ( chunk && chunk < _preci + ? _preci / chunk - 1 + : 0 ) + + 1 + P ); + for ( auto &b : bits ) { + unsigned char lat_bits= b.first; + unsigned char lon_bits= b.second; + lat_mult >>= lat_bits; + lon_mult >>= lon_bits; + if ( ! lat_mult || ! lon_mult ) + break; + + // Each set of bits represents the number of times the current multiplier (after + // division by the number of bits we're outputting) would go into the remainder. + // Eg. If _mult was 1024, and _rem is 123 and _bits is 3, we're going to put out + // the next 3 bits of the value 199. The last value ended removing all multiples of + // 1024. So, we first get the new multiplier: 1024 >> 3 == 128. So, we're + // indicating, as a 3-bit value, how many multiples of 128 there are in the value + // 199: 199 / 128 == 1, so the 3-bit value we output is 001 + uint32_t lat_val = lat_rem / lat_mult; + lat_rem -= lat_val * lat_mult; + + uint32_t lon_val = lon_rem / lon_mult; + lon_rem -= lon_val * lon_mult; + + res += char( ( lat_val << lon_bits ) | lon_val ); + } + + // Add the R-S parity symbols and base-32 encode, add parity separator and chunk + rscodec.encode( res ); + serialize::base32::encode( res ); + switch( separator ) { + case SEP_NONE: + break; + case SEP_DOT: default: + res.insert( _preci, 1, SEP_DOT ); + break; + case SEP_BANG: + case SEP_SPACE: + res.insert( _preci, 1, separator ); + break; + } + if ( space != CHK_NONE && chunk && chunk < _preci ) { + for ( unsigned c = _preci / chunk - 1; c > 0; --c ) { + switch ( space ) { + case CHK_NONE: + break; + case CHK_SPACE: default: + res.insert( c * chunk, 1, CHK_SPACE ); + break; + case CHK_DASH: + res.insert( c * chunk, 1, space ); + break; + } + } + } + + return res; + } + + // + // deserialize -- Extract base-32, skip whitespace, mark invalid symbols as erasures + // validate -- Remove base-32 encoding, validate and remove parity, returning confidence + // decode -- Attempt to decode a lat/lon, returning the confidence percentage + // + // If data but no parity symbols are supplied, no error checking is performed, and the + // confidence returned will be 0%. No erasures within the supplied data are allowed (as + // there is no capacity to correct them), and an exception will be thrown. + // + // If parity is supplied, then erasures are allowed. So long as the total number of + // erasures is <= the supplied parity symbols, then the decode will proceed (using the + // parity symbols to fill in the erasures), and the returned confidence will reflect the + // amount of unused parity capacity. Each erasure consumes one parity symbol to repair. + // + // We'll allow question-mark or any of the slash characters: "_/\?" to indicate an + // erasure. Either of the "!." symbol may be used to indicates the split between location + // symbols and parity symbols, and must be in a position that corresponds to the indicated + // number of location (this->precision) and parity 'P' symbols. Whitespace symbols and dash + // are ignored: " -". + // + // Thus, an EZCOD like "R3U 1JU QUY!0" may only be decoded by an ezcod<P=1>. Without + // the "!" or ".", it could be an ezcod<P=2> w/precision == 8 -- there's no way to know for + // sure. If no explicit position-parity separator is given, then we assume the default: + // this->precision location symbols, then up to P parity symbols. If additional parity + // symbols are supplied after the separator, then However, an ezcod...<P=3> + // + // If an explicit "!" or "." separator IS provided, then we will attempt to decode the + // position with the given number of position symbols, and up to P parity symbols. + // + // NOTE + // + // Due to a perhaps unexpected feature of R-S codewords, a codeword with MORE parity + // can be successfully decoded by an R-S codec specifying LESS parity symbols. It so + // happens that the data plus (excess) parity + (remaining) parity is STILL a valid codeword + // (so long as the R-S Galois parameters are identical). + // + // Therefore, EZCODs with more parity are accepted by EZCOD parsers configured for less + // parity. Of course, they will have less error/erasure correction strength -- using the + // correctly configured EZCOD codec expecting more R-S parity will maximize the value of all + // the supplied parity. + // + // The full amount of parity (ie. everything after the location/parity separator) is + // discarded in all cases, before the EZCOD location is decoded. + // + private: + + unsigned deserialize( + std::string &dec, + std::vector<int> &erasure, + std::vector<char> &invalid ) + const + { + serialize::base32::decode( dec, &erasure, &invalid ); + + // The special symbol '!' or '.' indicates the end of the EZCOD location symbols and the + // beginning of parity: ensure the symbol counts are consistent with the encoding. By + // default the parity symbols begin at offset precision. If we see more than precision + // symbols, we assume that the Lth and subsequent symbols are parity. If a + // location/parity separator is provided, it must be at position this->precision! + // Return offset of start of parity in codeword. + unsigned parbeg = this->PRECISION; // Parity begins after Location, by default + for ( unsigned i = 0; i < invalid.size(); ++i ) { + switch ( invalid[i] ) { + case '!': case '.': + // Remember the offset of the first parity symbol (it'll be in the position of + // the last '!' or '.' symbol we're about to erase), and adjust the indices of + // any erasures following. + parbeg = erasure[i]; + dec.erase( parbeg, 1 ); + invalid.erase( invalid.begin() + i ); + erasure.erase( erasure.begin() + i ); + for ( unsigned j = i; j < erasure.size(); ++j ) + erasure[j] -= 1; + break; + case '_': case '/': case '\\': case '?': + break; + default: + throw std::runtime_error( std::string( "ezpwd::ezcod::decode: invalid symbol presented: '" ) + + invalid[i] + "'" ); + } + } +#if defined( DEBUG ) && DEBUG >= 1 + std::cout << " --> 0x" << std::vector<uint8_t>( dec.begin(), dec.begin() + std::min( size_t( parbeg ), dec.length()) ) + << " + 0x" << std::vector<uint8_t>( dec.begin() + std::min( size_t( parbeg ), dec.length()), + dec.begin() + dec.length() ) + << " parity" << std::endl; +#endif + return parbeg; + } + + int validate( + std::string &dec ) + const + { + // Compute and return validity (which may later be assigned to this->confidence) + int validity = 0; // if no R-S parity provided + +#if defined( DEBUG ) && DEBUG >= 1 + std::cout << *this << " validate( " << dec << " ) "; +#endif + std::vector<int> erasure; + std::vector<char> invalid; + unsigned parbeg = deserialize( dec, erasure, invalid ); + + if ( dec.size() > parbeg || erasure.size() > 0 ) { + // Some R-S parity symbol(s) were provided (or erasures were marked). See if we can + // successfully decode/correct, or (at least) use one parity symbol as a check + // character. If we identify more erasures than R-S parity, we must fail; we can't + // recover the data. This will of course be the case if we have *any* erasures in + // the data, and no parity. + unsigned parity = 0; + if ( dec.size() > parbeg ) + parity = dec.size() - parbeg; + while ( dec.size() < parbeg + P ) { + erasure.push_back( dec.size() ); + dec.resize( dec.size() + 1 ); + } +#if defined( DEBUG ) && DEBUG >= 2 + std::cout << " --> erasures: " << erasure.size() << " vs. parity: " << parity + << ": " << std::vector<uint8_t>( dec.begin(), dec.end() ) << std::endl; +#endif + if ( erasure.size() > parity ) { + // We cannot do R-S decoding; not enough parity symbols to even cover erasures. + // If parity symbol(s) were provided ('parity' > 0), and all erasures were due the + // missing remaining parity symbols, we can use the existing parity symbol(s) as + // "check character(s)", by simply re-encoding the supplied non-parity data, and + // see if the generated parity symbol(s) match the supplied parity. This has + // basically the same strength as the 10:10 code's check character. + if ( parity + erasure.size() == P ) { + // All erasures must be at end, in remaining parity symbols! + std::string chk( dec.begin(), dec.begin() + parbeg ); + rscodec.encode( chk ); + // each parity symbol provided must match the corresponding encoded chk symbol + for ( unsigned i = 0; i < parity; ++i ) + if ( dec[parbeg+i] != chk[parbeg+i] ) + throw std::runtime_error( "ezpwd::ezcod::decode: Error correction failed; check character mismatch" ); + // Check character(s) matched; erasure.size()/P of confidence gone + validity = ezpwd::strength<P>( erasure.size(), erasure, erasure ); + } else + throw std::runtime_error( "ezpwd::ezcod::decode: Error correction failed; too many erasures" ); + } else { + // We can try R-S decoding; we have (at least) enough parity to try to recover + // any missing symbol(s). + std::vector<int>position; + int corrects= rscodec.decode( dec, erasure, &position ); + if ( corrects < 0 ) + throw std::runtime_error( "ezpwd::ezcod::decode: Error correction failed; R-S decode failed" ); + // Compute confidence, from spare parity capacity. Since R-S decode will not + // return the position of erasures that turn out (by accident) to be correct, + // but they have consumed parity capacity, we re-add them into the correction + // position vector. If the R-S correction reports more corrections than the + // parity can possibly have handled correctly, (eg. 2 reported erasures and an + // unexpected error), then the decode is almost certainly incorrect; fail. + validity = ezpwd::strength<P>( corrects, erasure, position ); + if ( validity < 0 ) + throw std::runtime_error( "ezpwd::ezcod::decode: Error correction failed; R-S decode overwhelmed" ); + } + if ( dec.size() > parbeg ) + dec.resize( parbeg ); // Discard any parity symbols + } + return validity; + } + + public: + virtual int decode( const std::string &str ) + { + // Decode the R-S encoding, computing the confidence. Will raise an exception on any + // error. Don't change this->confidence, this->latitude, ... until there is no longer a + // chance of exception. + std::string decoded( str ); + int validity= validate( decoded ); + + // Unpack the supplied location data; we'll take as much as we are given, up to the + // maximum possible 12 symbols supported (9 symbols yielding ~3m resolution). + uint32_t lat_tot = 0; + uint32_t lon_tot = 0; + + uint32_t lat_mult= 1; + uint32_t lon_mult= 1; + + auto di = decoded.begin(); + for ( auto &b : bits ) { + if ( di == decoded.end() ) + break; + unsigned char c = *di++; + + unsigned char lat_bits= b.first; + unsigned char lon_bits= b.second; + + uint32_t lat_val = c >> lon_bits; + uint32_t lon_val = c & (( 1 << lon_bits ) - 1 ); + + lat_mult <<= lat_bits; + lat_tot <<= lat_bits; + lat_tot += lat_val; + + lon_mult <<= lon_bits; + lon_tot <<= lon_bits; + lon_tot += lon_val; + } + + // Convert the sum of lat/lon parts back into degrees, and round the (truncated) value + // to the middle of the error rectangle. This allows us to minimize error even if we + // didn't have access to all of the origin symbols to decode. The absolute error bar as + // a proportional factor [0,1) for lat/lon is at most the scale of the last parts + // multiplier used. We'll use this later to compute the error in meters; for example, + // if the last value we added worked out to be worth units of 25m of the circumference, + // then we must now be within [0,25m) of the original point. + double lat_err = 1.0 / lat_mult; + double lon_err = 1.0 / lon_mult; + latitude = 180 * ( double( lat_tot ) / lat_mult + lat_err / 2 ) - 90; + longitude = 360 * ( double( lon_tot ) / lon_mult + lon_err / 2 ) - 180; + + // Remember the decoded location precision for future encoding (overrides the default). + // Compute the certainty probability (0.0,1.0] given the number of parity symbols in + // excess: Given a base-32 symbol: 1 - 1 / ( 32 ^ P ) where P is the number of + // unconsumed parity. + precision = decoded.size(); + confidence = validity; + certainty = 0.0; + if ( PARITY * confidence / 100 ) + certainty = 1.0 - 1.0 / std::pow( double( 32.0 ), + double( PARITY * confidence / 100 )); + + // Compute the resolution error (in m.) of the decoded lat/lon and compute the minimum + // accuracy -- the radius of the circle around the computed latitude/longitude, inside + // which the original latitude/longitude must have been. + // + // original latitude error bar + // \ / + // o - + // | longitude error bar + // | / + // |--x--| + // /| + // / | + // computed - + // + // The maximum distance is the length of the diagonal of the error rectangle defined by + // 1/2 the latitude/longitude error bars. + // + double lon_circ= 1 * M_PI * 6371000; + double lat_circ= 2 * M_PI * 6371000 * std::cos( latitude * M_PI / 180 ); + latitude_error = lat_err * lon_circ; + longitude_error = lon_err * lat_circ; + + accuracy = sqrt( latitude_error / 2 * latitude_error / 2 + + longitude_error / 2 * longitude_error / 2 ); + return confidence; + } + }; // class ezcod + + + // + // ezcod::rscodec -- Reed-Solomon parity codec + // ezcod::bits -- distribution of lat/lon precision in each code symbol + // + // Quickly establishes an extra bit of precision for Longitude, and then evenly distributes + // future precision between lat/lon, always maintaining extra precision for Longitude. + // + template < unsigned P, unsigned L > + const ezpwd::RS<31,31-P> ezcod<P,L>::rscodec; + + // Number of lat/lon bits represented for each location symbol + template < unsigned P, unsigned L > + const typename ezcod<P,L>::bits_t + ezcod<P,L>::bits = { + { + // bits per symbol lat lon + ezcod<P,L>::bits_t::value_type( 2, 3 ), + ezcod<P,L>::bits_t::value_type( 2, 3 ), + ezcod<P,L>::bits_t::value_type( 3, 2 ), + // -- -- + // 7 8 + ezcod<P,L>::bits_t::value_type( 2, 3 ), + ezcod<P,L>::bits_t::value_type( 3, 2 ), + ezcod<P,L>::bits_t::value_type( 2, 3 ), + // -- -- + // 14 16 + ezcod<P,L>::bits_t::value_type( 3, 2 ), + ezcod<P,L>::bits_t::value_type( 2, 3 ), + ezcod<P,L>::bits_t::value_type( 3, 2 ), + // -- -- + // 22 23 + ezcod<P,L>::bits_t::value_type( 2, 3 ), + ezcod<P,L>::bits_t::value_type( 3, 2 ), + ezcod<P,L>::bits_t::value_type( 2, 3 ), + // -- -- + // 29 31 + } + }; + + // Total number of parts that lat/lon is subdivided into, for that number of location symbols. + template < unsigned P, unsigned L > + const typename ezcod<P,L>::parts_t + ezcod<P,L>::parts = { + { + // parts per symbol lat parts lon parts lat lon bits + ezcod<P,L>::parts_t::value_type( 1UL << 2, 1UL << 3 ), // 2, 3 + ezcod<P,L>::parts_t::value_type( 1UL << 4, 1UL << 6 ), // 2, 3 + ezcod<P,L>::parts_t::value_type( 1UL << 7, 1UL << 8 ), // 3, 2 + // -- -- + // 7 8 + ezcod<P,L>::parts_t::value_type( 1UL << 9, 1UL << 11 ), // 2, 3 + ezcod<P,L>::parts_t::value_type( 1UL << 12, 1UL << 13 ), // 3, 2 + ezcod<P,L>::parts_t::value_type( 1UL << 14, 1UL << 16 ), // 2, 3 + // -- -- + // 14 16 + ezcod<P,L>::parts_t::value_type( 1UL << 17, 1UL << 18 ), // 3, 2 + ezcod<P,L>::parts_t::value_type( 1UL << 19, 1UL << 21 ), // 2, 3 + ezcod<P,L>::parts_t::value_type( 1UL << 22, 1UL << 23 ), // 3, 2 + // -- -- + // 22 23 + ezcod<P,L>::parts_t::value_type( 1UL << 24, 1UL << 26 ), // 2, 3 + ezcod<P,L>::parts_t::value_type( 1UL << 27, 1UL << 28 ), // 3, 2 + ezcod<P,L>::parts_t::value_type( 1UL << 29, 1UL << 31 ), // 2, 3 + // -- -- + // 29 31 + } + }; +} // namespace ezpwd + +#endif // _EZPWD_EZCOD diff --git a/op25/gr-op25_repeater/lib/ezpwd/output b/op25/gr-op25_repeater/lib/ezpwd/output new file mode 100644 index 0000000..230be80 --- /dev/null +++ b/op25/gr-op25_repeater/lib/ezpwd/output @@ -0,0 +1,344 @@ +#ifndef _EZPWD_OUTPUT +#define _EZPWD_OUTPUT + +#include <cstdio> +#include <streambuf> +#include <iostream> +#include <cctype> +#include <iomanip> +#include <sstream> +#include <set> +#include <map> +#include <vector> + +// +// ezpwd::hexchr -- escape/hexify char c, output using func/meth f, in width w >= 2 +// ezpwd::hexify -- hexify something that can be converted to an unsigned char +// ezpwd::hexout -- hexify each element in the range (beg,end], limited by stream's width +// +// std::ostream << ezpwd::hexify( c ) // output any char escaped/hex +// std::ostream << ezpwd::hexout( beg, end ) // output any char iterator to ostream +// std::ostream << std::vector<unsigend char> +// std::ostream << std::array<unsigend char, N> +// ezpwd::hexchr( c, [](unsigned char c){...;} )// output escaped/hex char via functor +// ezpwd::hexout( beg, end, FILE* ) // output any char iterator to FILE* +// +// Output unprintable unsigned char data in hex, escape printable/whitespace data. +// +namespace ezpwd { + + struct hexify { + unsigned char c; + std::streamsize w; + explicit hexify( + unsigned char _c, + std::streamsize _w = 2 ) + : c( _c ) + , w( _w ) + { ; } + explicit hexify( + char _c, + std::streamsize _w = 2 ) + : c( (unsigned char)_c ) + , w( _w ) + { ; } + }; + struct hexstr { + const std::string &s; + explicit hexstr( + const std::string &_s ) + : s( _s ) + { ; } + }; + + template <typename F> // a functor taking a char + void + hexchr( unsigned char c, F f = []( unsigned char c ) { std::cout.put( c );}, size_t w = 2 ) + { + for ( ; w > 2; --w ) + f( ' ' ); + if ( std::isprint( c ) || std::isspace( c ) + || c == '\0' || c == '\a' || c == '\b' || c == 0x1B ) { // '\e' is not standard + switch ( c ) { + case 0x00: f( '\\' ); f( '0' ); break; // NUL + case 0x07: f( '\\' ); f( 'a' ); break; // BEL + case 0x08: f( '\\' ); f( 'b' ); break; // BS + case 0x09: f( '\\' ); f( 't' ); break; // HT + case 0x0A: f( '\\' ); f( 'n' ); break; // LF + case 0x0B: f( '\\' ); f( 'v' ); break; // VT + case 0x0C: f( '\\' ); f( 'f' ); break; // FF + case 0x0D: f( '\\' ); f( 'r' ); break; // CR + case 0x1B: f( '\\' ); f( 'e' ); break; // ESC + case '\"': f( '\\' ); f( '"' ); break; // " + case '\'': f( '\\' ); f( '\''); break; // ' + case '\\': f( '\\' ); f( '\\'); break; // '\' + default: f( ' ' ); f( c ); // space, any other printable character + } + } else { + f( "0123456789ABCDEF"[( c >> 4 ) & 0x0f ] ); + f( "0123456789ABCDEF"[( c >> 0 ) & 0x0f ] ); + } + } + + + inline + std::ostream &operator<<( + std::ostream &lhs, + const ezpwd::hexify&rhs ) + { + ezpwd::hexchr( rhs.c, [ &lhs ]( unsigned char c ) { lhs.put( c ); }, rhs.w ); + return lhs; + } + + template < typename iter_t > + inline + std::ostream &hexout( + std::ostream &lhs, + const iter_t &beg, + const iter_t &end ) + { + std::streamsize wid = lhs.width( 0 ); + int col = 0; + for ( auto i = beg; i != end; ++i ) { + if ( wid && col == wid ) { + lhs << std::endl; + col = 0; + } + lhs << hexify( *i ); + ++col; + } + return lhs; + } + + template < typename iter_t > + inline + std::FILE *hexout( + const iter_t &beg, + const iter_t &end, + std::FILE *lhs ) + { + for ( auto i = beg; i != end; ++i ) { + ezpwd::hexchr( *i, [ lhs ]( unsigned char c ) { std::fputc( c, lhs ); } ); + } + return lhs; + } + + inline + std::ostream &operator<<( + std::ostream &lhs, + const ezpwd::hexstr&rhs ) + { + return ezpwd::hexout( lhs, rhs.s.begin(), rhs.s.end() ); + } +} // namespace ezpwd + +namespace std { + template < size_t S > + inline + std::ostream &operator<<( + std::ostream &lhs, + const std::array<unsigned char,S> + &rhs ) + { + return ezpwd::hexout( lhs, rhs.begin(), rhs.end() ); + } + + inline + std::ostream &operator<<( + std::ostream &lhs, + const std::vector<unsigned char> + &rhs ) + { + return ezpwd::hexout( lhs, rhs.begin(), rhs.end() ); + } + + // + // <ostream&> << pair<T,U> + // <ostream&> << set<T> -- sorted by T + // <ostream&> << map<T,U> -- sorted by T (key) + // <ostream&> << vector<T> + // + // Handle output of various container types. + // + // Output pairs and sets of pairs, respecting specified widths (as appropriate). For example + // a set of pairs of integeters 's', if output as "... << std::setw( 13 ) << s;", would yield: + // + // ( 1, 2) ( 3, 4) ... + // + + template <class T, class U> + std::ostream &operator<<( + std::ostream &lhs, + const std::pair<T,U> &rhs ) + { + std::streamsize w = std::max( std::streamsize( 0 ), + std::streamsize( lhs.width() - 3 )); + lhs << std::setw( 0 ) + << '(' << std::setw( w / 2 ) << rhs.first + << ',' << std::setw( w - w / 2 ) << rhs.second + << ')'; + return lhs; + } + + template <class T> + std::ostream &operator<<( + std::ostream &lhs, + const std::set<T> &rhs ) + { + std::streamsize w = lhs.width(); // If width is set, use if for each item + for ( typename std::set<T>::const_iterator + si = rhs.begin() + ; si != rhs.end() + ; ++si ) { + if ( si != rhs.begin()) + lhs << ' '; + lhs << std::setw( w ) << *si; + } + lhs << std::setw( 0 ); // If container empty, must clear + return lhs; + } + +template <class T, class U> +std::ostream &operator<<( + std::ostream &lhs, + const std::map<T,U>&rhs ) +{ + std::streamsize w = lhs.width(); // If width is set, use if for each item + std::vector<T> key; + for ( typename std::map<T,U>::const_iterator + mi = rhs.begin() + ; mi != rhs.end() + ; ++mi ) + key.push_back( mi->first ); + std::sort( key.begin(), key.end() ); + for ( typename std::vector<T>::const_iterator + ki = key.begin() + ; ki != key.end() + ; ++ki ) { + if ( ki != key.begin()) + lhs << ' '; + lhs << std::setw( w ) << *rhs.find( *ki ); + } + lhs << std::setw( 0 ); // If container empty, must clear + return lhs; +} + +template <class T> +std::ostream &operator<<( + std::ostream &lhs, + const std::vector<T> &rhs ) +{ + for( size_t i = 0; i < rhs.size(); ++i ) { + if ( i ) + lhs << ", "; + lhs << rhs[i]; + } + + return lhs; +} +} // namespace std + +// +// ezpwd::buf_t -- describe a C string buffer, to allow C++ output operations +// ezpwd::streambuf_to_buf_t -- output charcters, always NUL terminated +// +// <buf_t> << ... -- Copy the <string> into the C <char*,size_t> buffer, always NUL terminating +// +// Copies <string> contents into buffer, and always NUL-terminates. Returns advanced buf_t (NOT +// including the terminating NUL, suitable for repeating ... << <string> operations. +// +// std::ostream( &<streambuf_to_buf_t> ) << ... +// +// Use standard ostream operations to send output to a C buffer, always NUL +// terminating, and never exceeding capacity. +// +namespace ezpwd { + + typedef std::pair<char *,size_t> + buf_t; + + class streambuf_to_buffer + : public std::streambuf { + private: + char *_buf; + size_t _siz; + public: + // + // streambuf_to_buf_t -- remember buf_t details + // ~streambuf_to_buf_t -- no virtual destructor required; nothing to clean up + // + streambuf_to_buffer( + char *buf, + size_t siz ) + : _buf( buf ) + , _siz( siz ) + { + if ( _siz > 0 ) + *_buf = 0; + } + explicit streambuf_to_buffer( + const buf_t &buf ) + : streambuf_to_buffer( buf.first, buf.second ) + { + ; + } + + // + // overflow -- Append c, always NUL terminating + // + virtual int overflow( + int c ) + { + if ( _siz <= 1 ) + return EOF; // No room for c and NUL; EOF + if ( EOF == c ) + return 0; // EOF provided; do nothing + --_siz; + *_buf++ = char( c ); + *_buf = 0; + return c; + } + }; // class streambuf_to_buffer + +} // namespace ezpwd + +namespace std { + + inline + ezpwd::buf_t operator<<( + const ezpwd::buf_t &buf, + const std::string &str ) + { + if ( buf.first && str.size() + 1 <= buf.second ) { + std::copy( str.begin(), str.end(), buf.first ); + buf.first[str.size()] = 0; + return ezpwd::buf_t( buf.first + str.size(), buf.second - str.size() ); + } else if ( buf.first && buf.second ) { + std::copy( str.begin(), str.begin() + buf.second - 1, buf.first ); + buf.first[buf.second-1] = 0; + return ezpwd::buf_t( buf.first + buf.second - 1, 1 ); + } + return buf; // NULL pointer or 0 size. + } + + + // + // <std::string> << ... + // + // Useful (but inefficient) standard output formatting directly to a std::string. Use only for + // testing code, for efficiency reasons... + // + template < typename T > + inline + std::string operator<<( + const std::string &lhs, + const T &rhs ) + { + std::ostringstream oss; + oss << rhs; + return std::string( lhs ).append( oss.str() ); + } + +} // namespace std + +#endif // _EZPWD_OUTPUT diff --git a/op25/gr-op25_repeater/lib/ezpwd/rs b/op25/gr-op25_repeater/lib/ezpwd/rs new file mode 100644 index 0000000..3479a49 --- /dev/null +++ b/op25/gr-op25_repeater/lib/ezpwd/rs @@ -0,0 +1,168 @@ +/* + * Ezpwd Reed-Solomon -- Reed-Solomon encoder / decoder library + * + * Copyright (c) 2014, Hard Consulting Corporation. + * + * Ezpwd Reed-Solomon 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 3 of + * the License, or (at your option) any later version. See the LICENSE file at the top of the + * source tree. Ezpwd Reed-Solomon is also available under Commercial license. c++/ezpwd/rs_base + * is redistributed under the terms of the LGPL, regardless of the overall licensing terms. + * + * Ezpwd Reed-Solomon 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. + */ +#ifndef _EZPWD_RS +#define _EZPWD_RS + +#include "rs_base" + +// +// ezpwd::RS<SYMBOLS,PAYLOAD> -- Implements an RS(SYMBOLS,PAYLOAD) codec +// ezpwd::RS_CCSDS<...> -- CCSDS standard 8-bit R-S codec +// +// Support for Reed-Solomon codecs for symbols of 2 to 16 bits is supported. The R-S "codeword" +// for an N-bit symbol is defined to be 2^N-1 symbols in size. For example, for 5-bit symbols, +// 2^5-1 == 31, so the notation for defining an Reed-Solomon codec for 5-bit symbols is always: +// RS(31,PAYLOAD), where PAYLOAD is always some value less than 31. The difference is the number of +// "parity" symbols. +// +// For example, to define an RS codeword of 31 symbols w/ 4 symbols of parity and up to 27 +// symbols of data, you would say: RS(31,27). Of course, you can supply smaller amounts of data; +// the balance is assumed to be NUL (zero) symbols. +// +namespace ezpwd { + + // + // __RS( ... ) -- Define a reed-solomon codec + // + // @SYMBOLS: Total number of symbols; must be a power of 2 minus 1, eg 2^8-1 == 255 + // @PAYLOAD: The maximum number of non-parity symbols, eg 253 ==> 2 parity symbols + // @POLY: A primitive polynomial appropriate to the SYMBOLS size + // @FCR: The first consecutive root of the Reed-Solomon generator polynomial + // @PRIM: The primitive root of the generator polynomial + // +# define __RS_TYP( TYPE, SYMBOLS, PAYLOAD, POLY, FCR, PRIM ) \ + ezpwd::reed_solomon< \ + TYPE, \ + ezpwd::log_< (SYMBOLS) + 1 >::value, \ + (SYMBOLS) - (PAYLOAD), FCR, PRIM, \ + ezpwd::gfpoly< \ + ezpwd::log_< (SYMBOLS) + 1 >::value, \ + POLY >> + +# define __RS( NAME, TYPE, SYMBOLS, PAYLOAD, POLY, FCR, PRIM ) \ + __RS_TYP( TYPE, SYMBOLS, PAYLOAD, POLY, FCR, PRIM ) { \ + NAME() \ + : __RS_TYP( TYPE, SYMBOLS, PAYLOAD, POLY, FCR, PRIM )() \ + {;} \ + } + + // + // RS<SYMBOLS, PAYLOAD> -- Standard partial specializations for Reed-Solomon codec type access + // + // Normally, Reed-Solomon codecs are described with terms like RS(255,252). Obtain various + // standard Reed-Solomon codecs using macros of a similar form, eg. RS<255, 252>. Standard PLY, + // FCR and PRM values are provided for various SYMBOL sizes, along with appropriate basic types + // capable of holding all internal Reed-Solomon tabular data. + // + // In order to provide "default initialization" of const RS<...> types, a user-provided + // default constructor must be provided. + // + template < size_t SYMBOLS, size_t PAYLOAD > struct RS; + template < size_t PAYLOAD > struct RS< 3, PAYLOAD> : public __RS( RS, uint8_t, 3, PAYLOAD, 0x7, 1, 1 ); + template < size_t PAYLOAD > struct RS< 7, PAYLOAD> : public __RS( RS, uint8_t, 7, PAYLOAD, 0xb, 1, 1 ); + template < size_t PAYLOAD > struct RS< 15, PAYLOAD> : public __RS( RS, uint8_t, 15, PAYLOAD, 0x13, 1, 1 ); + template < size_t PAYLOAD > struct RS< 31, PAYLOAD> : public __RS( RS, uint8_t, 31, PAYLOAD, 0x25, 1, 1 ); + template < size_t PAYLOAD > struct RS< 63, PAYLOAD> : public __RS( RS, uint8_t, 63, PAYLOAD, 0x43, 1, 1 ); + template < size_t PAYLOAD > struct RS< 127, PAYLOAD> : public __RS( RS, uint8_t, 127, PAYLOAD, 0x89, 1, 1 ); + template < size_t PAYLOAD > struct RS< 255, PAYLOAD> : public __RS( RS, uint8_t, 255, PAYLOAD, 0x11d, 1, 1 ); + template < size_t PAYLOAD > struct RS< 511, PAYLOAD> : public __RS( RS, uint16_t, 511, PAYLOAD, 0x211, 1, 1 ); + template < size_t PAYLOAD > struct RS< 1023, PAYLOAD> : public __RS( RS, uint16_t, 1023, PAYLOAD, 0x409, 1, 1 ); + template < size_t PAYLOAD > struct RS< 2047, PAYLOAD> : public __RS( RS, uint16_t, 2047, PAYLOAD, 0x805, 1, 1 ); + template < size_t PAYLOAD > struct RS< 4095, PAYLOAD> : public __RS( RS, uint16_t, 4095, PAYLOAD, 0x1053, 1, 1 ); + template < size_t PAYLOAD > struct RS< 8191, PAYLOAD> : public __RS( RS, uint16_t, 8191, PAYLOAD, 0x201b, 1, 1 ); + template < size_t PAYLOAD > struct RS<16383, PAYLOAD> : public __RS( RS, uint16_t, 16383, PAYLOAD, 0x4443, 1, 1 ); + template < size_t PAYLOAD > struct RS<32767, PAYLOAD> : public __RS( RS, uint16_t, 32767, PAYLOAD, 0x8003, 1, 1 ); + template < size_t PAYLOAD > struct RS<65535, PAYLOAD> : public __RS( RS, uint16_t, 65535, PAYLOAD, 0x1100b, 1, 1 ); + + template < size_t SYMBOLS, size_t PAYLOAD > struct RS_CCSDS; + template < size_t PAYLOAD > struct RS_CCSDS<255, PAYLOAD> : public __RS( RS_CCSDS, uint8_t, 255, PAYLOAD, 0x187, 112, 11 ); + + + // + // strength<PARITY> -- compute strength (given N parity symbols) of R-S correction + // + // Returns a confidence strength rating, which is the ratio: + // + // 100 - ( errors * 2 + erasures ) * 100 / parity + // + // which is proportional to the number of parity symbols unused by the reported number of + // corrected symbols. If 0, then all parity resources were consumed to recover the R-S + // codeword, and we can have no confidence in the result. If -'ve, indicates more parity + // resources were consumed than available, indicating that the result is likely incorrect. + // + // Accounts for the fact that a signalled erasure may not be reported in the corrected + // position vector, if the symbol happens to match the computed value. Note that even if the + // error or erasure occurs within the "parity" portion of the codeword, this doesn't reduce the + // effective strength -- all symbols in the R-S complete codeword are equally effective in + // recovering any other symbol in error/erasure. + // + template < size_t PARITY > + int strength( + int corrected, + const std::vector<int>&erasures, // original erasures positions + const std::vector<int>&positions ) // all reported correction positions + { + // -'ve indicates R-S failure; all parity consumed, but insufficient to correct the R-S + // codeword. Missing an unknown number of additional required parity symbols, so just + // return -1 as the strength. + if ( corrected < 0 ) { +#if defined( DEBUG ) && DEBUG >= 2 + std::cout + << corrected << " corrections (R-S decode failure) == -1 confidence" + << std::endl; +#endif + return -1; + } + if ( corrected != int( positions.size() )) + EZPWD_RAISE_OR_RETURN( std::runtime_error, "inconsistent R-S decode results", -1 ); + + // Any erasures that don't turn out to contain errors are not returned as fixed positions. + // However, they have consumed parity resources. Search for each erasure location in + // positions, and if not reflected, add to the corrected/missed counters. + int missed = 0; + for ( auto e : erasures ) { + if ( std::find( positions.begin(), positions.end(), e ) == positions.end() ) { + ++corrected; + ++missed; +#if defined( DEBUG ) && DEBUG >= 2 + std::cout + << corrected << " corrections (R-S erasure missed): " << e + << std::endl; +#endif + } + } + int errors = corrected - erasures.size(); + int consumed= errors * 2 + erasures.size(); + int confidence= 100 - consumed * 100 / PARITY; +#if defined( DEBUG ) && DEBUG >= 2 + std::cout + << corrected << " corrections (R-S decode success)" + << " at: " << positions + << ", " << erasures.size() + missed + << " erasures (" << missed + << " unreported) at: " << erasures + << ") ==> " << errors + << " errors, and " << consumed << " / " << PARITY + << " parity used == " << confidence + << "% confidence" + << std::endl; +#endif + return confidence; + } + +} // namespace ezpwd + +#endif // _EZPWD_RS diff --git a/op25/gr-op25_repeater/lib/ezpwd/rs_base b/op25/gr-op25_repeater/lib/ezpwd/rs_base new file mode 100644 index 0000000..5dad4e3 --- /dev/null +++ b/op25/gr-op25_repeater/lib/ezpwd/rs_base @@ -0,0 +1,1344 @@ +/* + * Ezpwd Reed-Solomon -- Reed-Solomon encoder / decoder library + * + * Copyright (c) 2014, Hard Consulting Corporation. + * + * Ezpwd Reed-Solomon 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 3 of + * the License, or (at your option) any later version. See the LICENSE file at the top of the + * source tree. Ezpwd Reed-Solomon is also available under Commercial license. The + * c++/ezpwd/rs_base file is redistributed under the terms of the LGPL, regardless of the overall + * licensing terms. + * + * Ezpwd Reed-Solomon 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. + * + * The core Reed-Solomon codec implementation in c++/ezpwd/rs_base is by Phil Karn, converted to C++ + * by Perry Kundert (perry@hardconsulting.com), and may be used under the terms of the LGPL. Here + * is the terms from Phil's README file (see phil-karn/fec-3.0.1/README): + * + * COPYRIGHT + * + * This package is copyright 2006 by Phil Karn, KA9Q. It may be used + * under the terms of the GNU Lesser General Public License (LGPL). See + * the file "lesser.txt" in this package for license details. + * + * The c++/ezpwd/rs_base file is, therefore, redistributed under the terms of the LGPL, while the + * rest of Ezpwd Reed-Solomon is distributed under either the GPL or Commercial licenses. + * Therefore, even if you have obtained Ezpwd Reed-Solomon under a Commercial license, you must make + * available the source code of the c++/ezpwd/rs_base file with your product. One simple way to + * accomplish this is to include the following URL in your code or documentation: + * + * https://github.com/pjkundert/ezpwd-reed-solomon/blob/master/c++/ezpwd/rs_base + * + * + * The Linux 3.15.1 version of lib/reed_solomon was also consulted as a cross-reference, which (in + * turn) is basically verbatim copied from Phil Karn's LGPL implementation, to ensure that no new + * defects had been found and fixed; there were no meaningful changes made to Phil's implementation. + * I've personally been using Phil's implementation for years in a heavy industrial use, and it is + * rock-solid. + * + * However, both Phil's and the Linux kernel's (copy of Phil's) implementation will return a + * "corrected" decoding with impossible error positions, in some cases where the error load + * completely overwhelms the R-S encoding. These cases, when detected, are rejected in this + * implementation. This could be considered a defect in Phil's (and hence the Linux kernel's) + * implementations, which results in them accepting clearly incorrect R-S decoded values as valid + * (corrected) R-S codewords. We chose the report failure on these attempts. + * + */ +#ifndef _EZPWD_RS_BASE +#define _EZPWD_RS_BASE + +#include <algorithm> +#include <array> +#include <cstdint> +#include <cstring> +#include <iostream> +#include <type_traits> +#include <vector> + +// +// Preprocessor defines available: +// +// EZPWD_NO_EXCEPTS -- define to use no exceptions; return -1, or abort on catastrophic failures +// EZPWD_NO_MOD_TAB -- define to force no "modnn" Galois modulo table acceleration +// EZPWD_ARRAY_SAFE -- define to force usage of bounds-checked arrays for most tabular data +// EZPWD_ARRAY_TEST -- define to force erroneous sizing of some arrays for non-production testing +// + +#if defined( DEBUG ) && DEBUG >= 2 +# include "output" // ezpwd::hex... std::ostream shims for outputting containers of uint8_t data +#endif + +#if defined( EZPWD_NO_EXCEPTS ) +# include <cstdio> // No exceptions; don't use C++ ostream +# define EZPWD_RAISE_OR_ABORT( typ, str ) do { \ + std::fputs(( str ), stderr ); std::fputc( '\n', stderr ); \ + abort(); \ + } while ( false ) +# define EZPWD_RAISE_OR_RETURN( typ, str, ret ) return ( ret ) +#else +# define EZPWD_RAISE_OR_ABORT( typ, str ) throw ( typ )( str ) +# define EZPWD_RAISE_OR_RETURN( typ, str, ret ) throw ( typ )( str ) +#endif + +namespace ezpwd { + + // ezpwd::log_<N,B> -- compute the log base B of N at compile-time + template <size_t N, size_t B=2> struct log_{ enum { value = 1 + log_<N/B, B>::value }; }; + template <size_t B> struct log_<1, B>{ enum { value = 0 }; }; + template <size_t B> struct log_<0, B>{ enum { value = 0 }; }; + + // + // reed_solomon_base - Reed-Solomon codec generic base class + // + class reed_solomon_base { + public: + virtual size_t datum() const = 0; // a data element's bits + virtual size_t symbol() const = 0; // a symbol's bits + virtual int size() const = 0; // R-S block size (maximum total symbols) + virtual int nroots() const = 0; // R-S roots (parity symbols) + virtual int load() const = 0; // R-S net payload (data symbols) + + virtual ~reed_solomon_base() + { + ; + } + reed_solomon_base() + { + ; + } + + virtual std::ostream &output( + std::ostream &lhs ) + const + { + return lhs << "RS(" << this->size() << "," << this->load() << ")"; + } + + // + // {en,de}code -- Compute/Correct errors/erasures in a Reed-Solomon encoded container + // + /// The encoded parity symbols may be included in 'data' (len includes nroots() parity + /// symbols), or may (optionally) supplied separately in (at least nroots()-sized) + /// 'parity'. + /// + /// For decode, optionally specify some known erasure positions (up to nroots()). If + /// non-empty 'erasures' is provided, it contains the positions of each erasure. If a + /// non-zero pointer to a 'position' vector is provided, its capacity will be increased to + /// be capable of storing up to 'nroots()' ints; the actual deduced error locations will be + /// returned. + /// + /// RETURN VALUE + /// + /// Return -1 on error. The encode returns the number of parity symbols produced; + /// decode returns the number of symbols corrected. Both errors and erasures are included, + /// so long as they are actually different than the deduced value. In other words, if a + /// symbol is marked as an erasure but it actually turns out to be correct, it's index will + /// NOT be included in the returned count, nor the modified erasure vector! + /// + + // + // encode(<string>) -- extend string to contain parity, or place in supplied parity string + // encode(<vector>) -- extend vector to contain parity, or place in supplied parity vector + // encode(<array>) -- ignore 'pad' elements of array, puts nroots() parity symbols at end + // encode(pair<iter,iter>) -- encode all except the last nroots() of the data, put parity at end + // encode(pair<iter,iter>, pair<iter,iter>) -- encode data between first <iter> pair, put parity in second pair + // + int encode( + std::string &data ) + const + { + typedef uint8_t uT; + typedef std::pair<uT *, uT *> + uTpair; + data.resize( data.size() + nroots() ); + return encode( uTpair( (uT *)&data.front(), (uT *)&data.front() + data.size() )); + } + + int encode( + const std::string &data, + std::string &parity ) + const + { + typedef uint8_t uT; + typedef std::pair<const uT *, const uT *> + cuTpair; + typedef std::pair<uT *, uT *> + uTpair; + parity.resize( nroots() ); + return encode( cuTpair( (const uT *)&data.front(), (const uT *)&data.front() + data.size() ), + uTpair( (uT *)&parity.front(), (uT *)&parity.front() + parity.size() )); + } + + template < typename T > + int encode( + std::vector<T> &data ) + const + { + typedef typename std::make_unsigned<T>::type + uT; + typedef std::pair<uT *, uT *> + uTpair; + data.resize( data.size() + nroots() ); + return encode( uTpair( (uT *)&data.front(), (uT *)&data.front() + data.size() )); + } + template < typename T > + int encode( + const std::vector<T>&data, + std::vector<T> &parity ) + const + { + typedef typename std::make_unsigned<T>::type + uT; + typedef std::pair<const uT *, const uT *> + cuTpair; + typedef std::pair<uT *, uT *> + uTpair; + parity.resize( nroots() ); + return encode( cuTpair( (uT *)&data.front(), (uT *)&data.front() + data.size() ), + uTpair( (uT *)&parity.front(), (uT *)&parity.front() + parity.size() )); + } + + template < typename T, size_t N > + int encode( + std::array<T,N> &data, + int pad = 0 ) // ignore 'pad' symbols at start of array + const + { + typedef typename std::make_unsigned<T>::type + uT; + typedef std::pair<uT *, uT *> + uTpair; + return encode( uTpair( (uT *)&data.front() + pad, (uT *)&data.front() + data.size() )); + } + + virtual int encode( + const std::pair<uint8_t *, uint8_t *> + &data ) + const + = 0; + virtual int encode( + const std::pair<const uint8_t *, const uint8_t *> + &data, + const std::pair<uint8_t *, uint8_t *> + &parity ) + const + = 0; + virtual int encode( + const std::pair<uint16_t *, uint16_t *> + &data ) + const + = 0; + virtual int encode( + const std::pair<const uint16_t *, const uint16_t *> + &data, + const std::pair<uint16_t *, uint16_t *> + &parity ) + const + = 0; + virtual int encode( + const std::pair<uint32_t *, uint32_t *> + &data ) + const + = 0; + virtual int encode( + const std::pair<const uint32_t *, const uint32_t *> + &data, + const std::pair<uint32_t *, uint32_t *> + &parity ) + const + = 0; + + int decode( + std::string &data, + const std::vector<int> + &erasure = std::vector<int>(), + std::vector<int> *position= 0 ) + const + { + typedef uint8_t uT; + typedef std::pair<uT *, uT *> + uTpair; + return decode( uTpair( (uT *)&data.front(), (uT *)&data.front() + data.size() ), + erasure, position ); + } + + int decode( + std::string &data, + std::string &parity, + const std::vector<int> + &erasure = std::vector<int>(), + std::vector<int> *position= 0 ) + const + { + typedef uint8_t uT; + typedef std::pair<uT *, uT *> + uTpair; + return decode( uTpair( (uT *)&data.front(), (uT *)&data.front() + data.size() ), + uTpair( (uT *)&parity.front(), (uT *)&parity.front() + parity.size() ), + erasure, position ); + } + + template < typename T > + int decode( + std::vector<T> &data, + const std::vector<int> + &erasure = std::vector<int>(), + std::vector<int> *position= 0 ) + const + { + typedef typename std::make_unsigned<T>::type + uT; + typedef std::pair<uT *, uT *> + uTpair; + return decode( uTpair( (uT *)&data.front(), (uT *)&data.front() + data.size() ), + erasure, position ); + } + + template < typename T > + int decode( + std::vector<T> &data, + std::vector<T> &parity, + const std::vector<int> + &erasure = std::vector<int>(), + std::vector<int> *position= 0 ) + const + { + typedef typename std::make_unsigned<T>::type + uT; + typedef std::pair<uT *, uT *> + uTpair; + return decode( uTpair( (uT *)&data.front(), (uT *)&data.front() + data.size() ), + uTpair( (uT *)&parity.front(), (uT *)&parity.front() + parity.size() ), + erasure, position ); + } + + template < typename T, size_t N > + int decode( + std::array<T,N> &data, + int pad = 0, // ignore 'pad' symbols at start of array + const std::vector<int> + &erasure = std::vector<int>(), + std::vector<int> *position= 0 ) + const + { + typedef typename std::make_unsigned<T>::type + uT; + typedef std::pair<uT *, uT *> + uTpair; + return decode( uTpair( (uT *)&data.front() + pad, (uT *)&data.front() + data.size() ), + erasure, position ); + } + + virtual int decode( + const std::pair<uint8_t *, uint8_t *> + &data, + const std::vector<int> + &erasure = std::vector<int>(), + std::vector<int> *position= 0 ) + const + = 0; + virtual int decode( + const std::pair<uint8_t *, uint8_t *> + &data, + const std::pair<uint8_t *, uint8_t *> + &parity, + const std::vector<int> + &erasure = std::vector<int>(), + std::vector<int> *position= 0 ) + const + = 0; + virtual int decode( + const std::pair<uint16_t *, uint16_t *> + &data, + const std::vector<int> + &erasure = std::vector<int>(), + std::vector<int> *position= 0 ) + const + = 0; + virtual int decode( + const std::pair<uint16_t *, uint16_t *> + &data, + const std::pair<uint16_t *, uint16_t *> + &parity, + const std::vector<int> + &erasure = std::vector<int>(), + std::vector<int> *position= 0 ) + const + = 0; + virtual int decode( + const std::pair<uint32_t *, uint32_t *> + &data, + const std::vector<int> + &erasure = std::vector<int>(), + std::vector<int> *position= 0 ) + const + = 0; + virtual int decode( + const std::pair<uint32_t *, uint32_t *> + &data, + const std::pair<uint32_t *, uint32_t *> + &parity, + const std::vector<int> + &erasure = std::vector<int>(), + std::vector<int> *position= 0 ) + const + = 0; + }; // class reed_solomon_base + + // + // std::ostream << ezpwd::reed_solomon<...> + // + // Output a R-S codec description in standard form eg. RS(255,253) + // + inline + std::ostream &operator<<( + std::ostream &lhs, + const ezpwd::reed_solomon_base + &rhs ) + { + return rhs.output( lhs ); + } + + // + // gfpoly - default field polynomial generator functor. + // + template < int SYM, int PLY > + struct gfpoly { + int operator() ( int sr ) + const + { + if ( sr == 0 ) + sr = 1; + else { + sr <<= 1; + if ( sr & ( 1 << SYM )) + sr ^= PLY; + sr &= (( 1 << SYM ) - 1); + } + return sr; + } + }; + + // + // class reed_solomon_tabs -- R-S tables common to all RS(NN,*) with same SYM, PRM and PLY + // + template < typename TYP, int SYM, int PRM, class PLY > + class reed_solomon_tabs + : public reed_solomon_base { + + public: + typedef TYP symbol_t; + static const size_t DATUM = 8 * sizeof TYP(); // bits / TYP + static const size_t SYMBOL = SYM; // bits / symbol + static const int MM = SYM; + static const int SIZE = ( 1 << SYM ) - 1; // maximum symbols in field + static const int NN = SIZE; + static const int A0 = SIZE; + static const int MODS // modulo table: 1/2 the symbol size squared, up to 4k +#if defined( EZPWD_NO_MOD_TAB ) + = 0; +#else + = SYM > 8 ? ( 1 << 12 ) : ( 1 << SYM << SYM/2 ); +#endif + + static int iprim; // initialized to -1, below + + protected: + static std::array<TYP, +#if not defined( EZPWD_ARRAY_TEST ) + NN + 1> +#else +# warning "EZPWD_ARRAY_TEST: Erroneously declaring alpha_to size!" + NN > +#endif + alpha_to; + static std::array<TYP,NN + 1> + index_of; + static std::array<TYP,MODS> + mod_of; + virtual ~reed_solomon_tabs() + { + ; + } + reed_solomon_tabs() + : reed_solomon_base() + { + // Do init if not already done. We check one value which is initialized to -1; this is + // safe, 'cause the value will not be set 'til the initializing thread has completely + // initialized the structure. Worst case scenario: multiple threads will initialize + // identically. No mutex necessary. + if ( iprim >= 0 ) + return; + +#if defined( DEBUG ) && DEBUG >= 1 + std::cout << "RS(" << SIZE << ",*): Initialize for " << NN << " symbols size, " << MODS << " modulo table." << std::endl; +#endif + // Generate Galois field lookup tables + index_of[0] = A0; // log(zero) = -inf + alpha_to[A0] = 0; // alpha**-inf = 0 + PLY poly; + int sr = poly( 0 ); + for ( int i = 0; i < NN; i++ ) { + index_of[sr] = i; + alpha_to[i] = sr; + sr = poly( sr ); + } + // If it's not primitive, raise exception or abort + if ( sr != alpha_to[0] ) { + EZPWD_RAISE_OR_ABORT( std::runtime_error, "reed-solomon: Galois field polynomial not primitive" ); + } + + // Generate modulo table for some commonly used (non-trivial) values + for ( int x = NN; x < NN + MODS; ++x ) + mod_of[x-NN] = _modnn( x ); + // Find prim-th root of 1, index form, used in decoding. + int iptmp = 1; + while ( iptmp % PRM != 0 ) + iptmp += NN; + iprim = iptmp / PRM; + } + + // + // modnn -- modulo replacement for galois field arithmetics, optionally w/ table acceleration + // + // @x: the value to reduce (will never be -'ve) + // + // where + // MM = number of bits per symbol + // NN = (2^MM) - 1 + // + // Simple arithmetic modulo would return a wrong result for values >= 3 * NN + // + TYP _modnn( + int x ) + const + { + while ( x >= NN ) { + x -= NN; + x = ( x >> MM ) + ( x & NN ); + } + return x; + } + TYP modnn( + int x ) + const + { + while ( x >= NN + MODS ) { + x -= NN; + x = ( x >> MM ) + ( x & NN ); + } + if ( MODS && x >= NN ) + x = mod_of[x-NN]; + return x; + } + }; + + // + // class reed_solomon - Reed-Solomon codec + // + // @TYP: A symbol datum; {en,de}code operates on arrays of these + // @DATUM: Bits per datum (a TYP()) + // @SYM{BOL}, MM: Bits per symbol + // @NN: Symbols per block (== (1<<MM)-1) + // @alpha_to: log lookup table + // @index_of: Antilog lookup table + // @genpoly: Generator polynomial + // @NROOTS: Number of generator roots = number of parity symbols + // @FCR: First consecutive root, index form + // @PRM: Primitive element, index form + // @iprim: prim-th root of 1, index form + // @PLY: The primitive generator polynominal functor + // + // All reed_solomon<T, ...> instances with the same template type parameters share a common + // (static) set of alpha_to, index_of and genpoly tables. The first instance to be constructed + // initializes the tables. + // + // Each specialized type of reed_solomon implements a specific encode/decode method + // appropriate to its datum 'TYP'. When accessed via a generic reed_solomon_base pointer, only + // access via "safe" (size specifying) containers or iterators is available. + // + template < typename TYP, int SYM, int RTS, int FCR, int PRM, class PLY > + class reed_solomon + : public reed_solomon_tabs<TYP, SYM, PRM, PLY> { + + public: + typedef reed_solomon_tabs<TYP, SYM, PRM, PLY> + tabs_t; + using tabs_t::DATUM; + using tabs_t::SYMBOL; + using tabs_t::MM; + using tabs_t::SIZE; + using tabs_t::NN; + using tabs_t::A0; + + using tabs_t::iprim; + + using tabs_t::alpha_to; + using tabs_t::index_of; + + using tabs_t::modnn; + + static const int NROOTS = RTS; + static const int LOAD = SIZE - NROOTS; // maximum non-parity symbol payload + + protected: + static std::array<TYP, NROOTS + 1> + genpoly; + + public: + virtual size_t datum() const + { + return DATUM; + } + + virtual size_t symbol() const + { + return SYMBOL; + } + + virtual int size() const + { + return SIZE; + } + + virtual int nroots() const + { + return NROOTS; + } + + virtual int load() const + { + return LOAD; + } + + using reed_solomon_base::encode; + virtual int encode( + const std::pair<uint8_t *, uint8_t *> + &data ) + const + { + return encode_mask( data.first, data.second - data.first - NROOTS, data.second - NROOTS ); + } + + virtual int encode( + const std::pair<const uint8_t *, const uint8_t *> + &data, + const std::pair<uint8_t *, uint8_t *> + &parity ) + const + { + if ( parity.second - parity.first != NROOTS ) { + EZPWD_RAISE_OR_RETURN( std::runtime_error, "reed-solomon: parity length incompatible with number of roots", -1 ); + } + return encode_mask( data.first, data.second - data.first, parity.first ); + } + + virtual int encode( + const std::pair<uint16_t *, uint16_t *> + &data ) + const + { + return encode_mask( data.first, data.second - data.first - NROOTS, data.second - NROOTS ); + } + + virtual int encode( + const std::pair<const uint16_t *, const uint16_t *> + &data, + const std::pair<uint16_t *, uint16_t *> + &parity ) + const + { + if ( parity.second - parity.first != NROOTS ) { + EZPWD_RAISE_OR_RETURN( std::runtime_error, "reed-solomon: parity length incompatible with number of roots", -1 ); + } + return encode_mask( data.first, data.second - data.first, parity.first ); + } + + virtual int encode( + const std::pair<uint32_t *, uint32_t *> + &data ) + const + { + return encode_mask( data.first, data.second - data.first - NROOTS, data.second - NROOTS ); + } + + virtual int encode( + const std::pair<const uint32_t *, const uint32_t *> + &data, + const std::pair<uint32_t *, uint32_t *> + &parity ) + const + { + if ( parity.second - parity.first != NROOTS ) { + EZPWD_RAISE_OR_RETURN( std::runtime_error, "reed-solomon: parity length incompatible with number of roots", -1 ); + } + return encode_mask( data.first, data.second - data.first, parity.first ); + } + + template < typename INP > + int encode_mask( + const INP *data, + int len, + INP *parity ) // pointer to all NROOTS parity symbols + + const + { + if ( len < 1 ) { + EZPWD_RAISE_OR_RETURN( std::runtime_error, "reed-solomon: must provide space for all parity and at least one non-parity symbol", -1 ); + } + + const TYP *dataptr; + TYP *pariptr; + const size_t INPUT = 8 * sizeof ( INP ); + + if ( DATUM != SYMBOL || DATUM != INPUT ) { + // Our DATUM (TYP) size (eg. uint8_t ==> 8, uint16_t ==> 16, uint32_t ==> 32) + // doesn't exactly match our R-S SYMBOL size (eg. 6), or our INP size; Must mask and + // copy. The INP data must fit at least the SYMBOL size! + if ( SYMBOL > INPUT ) { + EZPWD_RAISE_OR_RETURN( std::runtime_error, "reed-solomon: output data type too small to contain symbols", -1 ); + } + std::array<TYP,SIZE> tmp; + TYP msk = static_cast<TYP>( ~0UL << SYMBOL ); + for ( int i = 0; i < len; ++i ) + tmp[LOAD - len + i] = data[i] & ~msk; + dataptr = &tmp[LOAD - len]; + pariptr = &tmp[LOAD]; + + encode( dataptr, len, pariptr ); + + // we copied/masked data; copy the parity symbols back (may be different sizes) + for ( int i = 0; i < NROOTS; ++i ) + parity[i] = pariptr[i]; + } else { + // Our R-S SYMBOL size, DATUM size and INP type size exactly matches; use in-place. + dataptr = reinterpret_cast<const TYP *>( data ); + pariptr = reinterpret_cast<TYP *>( parity ); + + encode( dataptr, len, pariptr ); + } + return NROOTS; + } + + using reed_solomon_base::decode; + virtual int decode( + const std::pair<uint8_t *, uint8_t *> + &data, + const std::vector<int> + &erasure = std::vector<int>(), + std::vector<int> *position= 0 ) + const + { + return decode_mask( data.first, data.second - data.first, (uint8_t *)0, + erasure, position ); + } + + virtual int decode( + const std::pair<uint8_t *, uint8_t *> + &data, + const std::pair<uint8_t *, uint8_t *> + &parity, + const std::vector<int> + &erasure = std::vector<int>(), + std::vector<int> *position= 0 ) + const + { + if ( parity.second - parity.first != NROOTS ) { + EZPWD_RAISE_OR_RETURN( std::runtime_error, "reed-solomon: parity length incompatible with number of roots", -1 ); + } + return decode_mask( data.first, data.second - data.first, parity.first, + erasure, position ); + } + + virtual int decode( + const std::pair<uint16_t *, uint16_t *> + &data, + const std::vector<int> + &erasure = std::vector<int>(), + std::vector<int> *position= 0 ) + const + { + return decode_mask( data.first, data.second - data.first, (uint16_t *)0, + erasure, position ); + } + + virtual int decode( + const std::pair<uint16_t *, uint16_t *> + &data, + const std::pair<uint16_t *, uint16_t *> + &parity, + const std::vector<int> + &erasure = std::vector<int>(), + std::vector<int> *position= 0 ) + const + { + if ( parity.second - parity.first != NROOTS ) { + EZPWD_RAISE_OR_RETURN( std::runtime_error, "reed-solomon: parity length incompatible with number of roots", -1 ); + } + return decode_mask( data.first, data.second - data.first, parity.first, + erasure, position ); + } + + virtual int decode( + const std::pair<uint32_t *, uint32_t *> + &data, + const std::vector<int> + &erasure = std::vector<int>(), + std::vector<int> *position= 0 ) + const + { + return decode_mask( data.first, data.second - data.first, (uint32_t *)0, + erasure, position ); + } + + virtual int decode( + const std::pair<uint32_t *, uint32_t *> + &data, + const std::pair<uint32_t *, uint32_t *> + &parity, + const std::vector<int> + &erasure = std::vector<int>(), + std::vector<int> *position= 0 ) + const + { + if ( parity.second - parity.first != NROOTS ) { + EZPWD_RAISE_OR_RETURN( std::runtime_error, "reed-solomon: parity length incompatible with number of roots", -1 ); + } + return decode_mask( data.first, data.second - data.first, parity.first, + erasure, position ); + } + + // + // decode_mask -- mask INP data into valid SYMBOL data + // + /// Incoming data may be in a variety of sizes, and may contain information beyond the + /// R-S symbol capacity. For example, we might use a 6-bit R-S symbol to correct the lower + /// 6 bits of an 8-bit data character. This would allow us to correct common substitution + /// errors (such as '2' for '3', 'R' for 'T', 'n' for 'm'). + /// + template < typename INP > + int decode_mask( + INP *data, + int len, + INP *parity = 0, // either 0, or pointer to all parity symbols + const std::vector<int> + &erasure = std::vector<int>(), + std::vector<int> *position= 0 ) + const + { + if ( len < ( parity ? 0 : NROOTS ) + 1 ) { + EZPWD_RAISE_OR_RETURN( std::runtime_error, "reed-solomon: must provide all parity and at least one non-parity symbol", -1 ); + } + if ( ! parity ) { + len -= NROOTS; + parity = data + len; + } + + TYP *dataptr; + TYP *pariptr; + const size_t INPUT = 8 * sizeof ( INP ); + + std::array<TYP,SIZE> tmp; + TYP msk = static_cast<TYP>( ~0UL << SYMBOL ); + const bool cpy = DATUM != SYMBOL || DATUM != INPUT; + if ( cpy ) { + // Our DATUM (TYP) size (eg. uint8_t ==> 8, uint16_t ==> 16, uint32_t ==> 32) + // doesn't exactly match our R-S SYMBOL size (eg. 6), or our INP size; Must copy. + // The INP data must fit at least the SYMBOL size! + if ( SYMBOL > INPUT ) { + EZPWD_RAISE_OR_RETURN( std::runtime_error, "reed-solomon: input data type too small to contain symbols", -1 ); + } + for ( int i = 0; i < len; ++i ) { + tmp[LOAD - len + i] = data[i] & ~msk; + } + dataptr = &tmp[LOAD - len]; + for ( int i = 0; i < NROOTS; ++i ) { + if ( TYP( parity[i] ) & msk ) { + EZPWD_RAISE_OR_RETURN( std::runtime_error, "reed-solomon: parity data contains information beyond R-S symbol size", -1 ); + } + tmp[LOAD + i] = parity[i]; + } + pariptr = &tmp[LOAD]; + } else { + // Our R-S SYMBOL size, DATUM size and INPUT type sizes exactly matches + dataptr = reinterpret_cast<TYP *>( data ); + pariptr = reinterpret_cast<TYP *>( parity ); + } + + int corrects; + if ( ! erasure.size() && ! position ) { + // No erasures, and error position info not wanted. + corrects = decode( dataptr, len, pariptr ); + } else { + // Either erasure location info specified, or resultant error position info wanted; + // Prepare pos (a temporary, if no position vector provided), and copy any provided + // erasure positions. After number of corrections is known, resize the position + // vector. Thus, we use any supplied erasure info, and optionally return any + // correction position info separately. + std::vector<int> _pos; + std::vector<int> &pos = position ? *position : _pos; + pos.resize( std::max( size_t( NROOTS ), erasure.size() )); + std::copy( erasure.begin(), erasure.end(), pos.begin() ); + corrects = decode( dataptr, len, pariptr, + &pos.front(), erasure.size() ); + if ( corrects > int( pos.size() )) { + EZPWD_RAISE_OR_RETURN( std::runtime_error, "reed-solomon: FATAL: produced too many corrections; possible corruption!", -1 ); + } + pos.resize( std::max( 0, corrects )); + } + + if ( cpy && corrects > 0 ) { + for ( int i = 0; i < len; ++i ) { + data[i] &= msk; + data[i] |= tmp[LOAD - len + i]; + } + for ( int i = 0; i < NROOTS; ++i ) { + parity[i] = tmp[LOAD + i]; + } + } + return corrects; + } + + virtual ~reed_solomon() + { + ; + } + reed_solomon() + : reed_solomon_tabs<TYP, SYM, PRM, PLY>() + { + // We check one element of the array; this is safe, 'cause the value will not be + // initialized 'til the initializing thread has completely initialized the array. Worst + // case scenario: multiple threads will initialize identically. No mutex necessary. + if ( genpoly[0] ) + return; + +#if defined( DEBUG ) && DEBUG >= 2 + std::cout << "RS(" << SIZE << "," << LOAD << "): Initialize for " << NROOTS << " roots." << std::endl; +#endif + std::array<TYP, NROOTS + 1> + tmppoly; // uninitialized + // Form RS code generator polynomial from its roots. Only lower-index entries are + // consulted, when computing subsequent entries; only index 0 needs initialization. + tmppoly[0] = 1; + for ( int i = 0, root = FCR * PRM; i < NROOTS; i++, root += PRM ) { + tmppoly[i + 1] = 1; + // Multiply tmppoly[] by @**(root + x) + for ( int j = i; j > 0; j-- ) { + if ( tmppoly[j] != 0 ) + tmppoly[j] = tmppoly[j - 1] + ^ alpha_to[modnn(index_of[tmppoly[j]] + root)]; + else + tmppoly[j] = tmppoly[j - 1]; + } + // tmppoly[0] can never be zero + tmppoly[0] = alpha_to[modnn(index_of[tmppoly[0]] + root)]; + } + // convert NROOTS entries of tmppoly[] to genpoly[] in index form for quicker encoding, + // in reverse order so genpoly[0] is last element initialized. + for ( int i = NROOTS; i >= 0; --i ) + genpoly[i] = index_of[tmppoly[i]]; + } + + int encode( + const TYP *data, + int len, + TYP *parity ) // at least nroots + const + { + // Check length parameter for validity + int pad = NN - NROOTS - len; + if ( pad < 0 || pad >= NN ) { + EZPWD_RAISE_OR_RETURN( std::runtime_error, "reed-solomon: data length incompatible with block size and error correction symbols", -1 ); + } + for ( int i = 0; i < NROOTS; i++ ) + parity[i] = 0; + for ( int i = 0; i < len; i++ ) { + TYP feedback= index_of[data[i] ^ parity[0]]; + if ( feedback != A0 ) + for ( int j = 1; j < NROOTS; j++ ) + parity[j] ^= alpha_to[modnn(feedback + genpoly[NROOTS - j])]; + + std::rotate( parity, parity + 1, parity + NROOTS ); + if ( feedback != A0 ) + parity[NROOTS - 1] = alpha_to[modnn(feedback + genpoly[0])]; + else + parity[NROOTS - 1] = 0; + } +#if defined( DEBUG ) && DEBUG >= 2 + std::cout << *this << " encode " << std::vector<TYP>( data, data + len ) + << " --> " << std::vector<TYP>( parity, parity + NROOTS ) << std::endl; +#endif + return NROOTS; + } + + int decode( + TYP *data, + int len, + TYP *parity, // Requires: at least NROOTS + int *eras_pos= 0, // Capacity: at least NROOTS + int no_eras = 0, // Maximum: at most NROOTS + TYP *corr = 0 ) // Capacity: at least NROOTS + const + { + typedef std::array< TYP, NROOTS > + typ_nroots; + typedef std::array< TYP, NROOTS+1 > + typ_nroots_1; + typedef std::array< int, NROOTS > + int_nroots; + + typ_nroots_1 lambda { { 0 } }; + typ_nroots syn; + typ_nroots_1 b; + typ_nroots_1 t; + typ_nroots_1 omega; + int_nroots root; + typ_nroots_1 reg; + int_nroots loc; + int count = 0; + + // Check length parameter and erasures for validity + int pad = NN - NROOTS - len; + if ( pad < 0 || pad >= NN ) { + EZPWD_RAISE_OR_RETURN( std::runtime_error, "reed-solomon: data length incompatible with block size and error correction symbols", -1 ); + } + if ( no_eras ) { + if ( no_eras > NROOTS ) { + EZPWD_RAISE_OR_RETURN( std::runtime_error, "reed-solomon: number of erasures exceeds capacity (number of roots)", -1 ); + } + for ( int i = 0; i < no_eras; ++i ) { + if ( eras_pos[i] < 0 || eras_pos[i] >= len + NROOTS ) { + EZPWD_RAISE_OR_RETURN( std::runtime_error, "reed-solomon: erasure positions outside data+parity", -1 ); + } + } + } + + // form the syndromes; i.e., evaluate data(x) at roots of g(x) + for ( int i = 0; i < NROOTS; i++ ) + syn[i] = data[0]; + + for ( int j = 1; j < len; j++ ) { + for ( int i = 0; i < NROOTS; i++ ) { + if ( syn[i] == 0 ) { + syn[i] = data[j]; + } else { + syn[i] = data[j] + ^ alpha_to[modnn(index_of[syn[i]] + ( FCR + i ) * PRM)]; + } + } + } + + for ( int j = 0; j < NROOTS; j++ ) { + for ( int i = 0; i < NROOTS; i++ ) { + if ( syn[i] == 0 ) { + syn[i] = parity[j]; + } else { + syn[i] = parity[j] + ^ alpha_to[modnn(index_of[syn[i]] + ( FCR + i ) * PRM)]; + } + } + } + + // Convert syndromes to index form, checking for nonzero condition + TYP syn_error = 0; + for ( int i = 0; i < NROOTS; i++ ) { + syn_error |= syn[i]; + syn[i] = index_of[syn[i]]; + } + + int deg_lambda = 0; + int deg_omega = 0; + int r = no_eras; + int el = no_eras; + if ( ! syn_error ) { + // if syndrome is zero, data[] is a codeword and there are no errors to correct. + count = 0; + goto finish; + } + + lambda[0] = 1; + if ( no_eras > 0 ) { + // Init lambda to be the erasure locator polynomial. Convert erasure positions + // from index into data, to index into Reed-Solomon block. + lambda[1] = alpha_to[modnn(PRM * (NN - 1 - ( eras_pos[0] + pad )))]; + for ( int i = 1; i < no_eras; i++ ) { + TYP u = modnn(PRM * (NN - 1 - ( eras_pos[i] + pad ))); + for ( int j = i + 1; j > 0; j-- ) { + TYP tmp = index_of[lambda[j - 1]]; + if ( tmp != A0 ) { + lambda[j] ^= alpha_to[modnn(u + tmp)]; + } + } + } + } + +#if DEBUG >= 1 + // Test code that verifies the erasure locator polynomial just constructed + // Needed only for decoder debugging. + + // find roots of the erasure location polynomial + for( int i = 1; i<= no_eras; i++ ) + reg[i] = index_of[lambda[i]]; + + count = 0; + for ( int i = 1, k = iprim - 1; i <= NN; i++, k = modnn( k + iprim )) { + TYP q = 1; + for ( int j = 1; j <= no_eras; j++ ) { + if ( reg[j] != A0 ) { + reg[j] = modnn( reg[j] + j ); + q ^= alpha_to[reg[j]]; + } + } + if ( q != 0 ) + continue; + // store root and error location number indices + root[count] = i; + loc[count] = k; + count++; + } + if ( count != no_eras ) { + std::cout << "ERROR: count = " << count << ", no_eras = " << no_eras + << "lambda(x) is WRONG" + << std::endl; + count = -1; + goto finish; + } +#if DEBUG >= 2 + if ( count ) { + std::cout + << "Erasure positions as determined by roots of Eras Loc Poly: "; + for ( int i = 0; i < count; i++ ) + std::cout << loc[i] << ' '; + std::cout << std::endl; + std::cout + << "Erasure positions as determined by roots of eras_pos array: "; + for ( int i = 0; i < no_eras; i++ ) + std::cout << eras_pos[i] << ' '; + std::cout << std::endl; + } +#endif +#endif + + for ( int i = 0; i < NROOTS + 1; i++ ) + b[i] = index_of[lambda[i]]; + + // + // Begin Berlekamp-Massey algorithm to determine error+erasure locator polynomial + // + while ( ++r <= NROOTS ) { // r is the step number + // Compute discrepancy at the r-th step in poly-form + TYP discr_r = 0; + for ( int i = 0; i < r; i++ ) { + if (( lambda[i] != 0 ) && ( syn[r - i - 1] != A0 )) { + discr_r ^= alpha_to[modnn(index_of[lambda[i]] + syn[r - i - 1])]; + } + } + discr_r = index_of[discr_r]; // Index form + if ( discr_r == A0 ) { + // 2 lines below: B(x) <-- x*B(x) + // Rotate the last element of b[NROOTS+1] to b[0] + std::rotate( b.begin(), b.begin()+NROOTS, b.end() ); + b[0] = A0; + } else { + // 7 lines below: T(x) <-- lambda(x)-discr_r*x*b(x) + t[0] = lambda[0]; + for ( int i = 0; i < NROOTS; i++ ) { + if ( b[i] != A0 ) { + t[i + 1] = lambda[i + 1] + ^ alpha_to[modnn(discr_r + b[i])]; + } else + t[i + 1] = lambda[i + 1]; + } + if ( 2 * el <= r + no_eras - 1 ) { + el = r + no_eras - el; + // 2 lines below: B(x) <-- inv(discr_r) * lambda(x) + for ( int i = 0; i <= NROOTS; i++ ) { + b[i] = ((lambda[i] == 0) + ? A0 + : modnn(index_of[lambda[i]] - discr_r + NN)); + } + } else { + // 2 lines below: B(x) <-- x*B(x) + std::rotate( b.begin(), b.begin()+NROOTS, b.end() ); + b[0] = A0; + } + lambda = t; + } + } + + // Convert lambda to index form and compute deg(lambda(x)) + for ( int i = 0; i < NROOTS + 1; i++ ) { + lambda[i] = index_of[lambda[i]]; + if ( lambda[i] != NN ) + deg_lambda = i; + } + // Find roots of error+erasure locator polynomial by Chien search + reg = lambda; + count = 0; // Number of roots of lambda(x) + for ( int i = 1, k = iprim - 1; i <= NN; i++, k = modnn( k + iprim )) { + TYP q = 1; // lambda[0] is always 0 + for ( int j = deg_lambda; j > 0; j-- ) { + if ( reg[j] != A0 ) { + reg[j] = modnn( reg[j] + j ); + q ^= alpha_to[reg[j]]; + } + } + if ( q != 0 ) + continue; // Not a root + // store root (index-form) and error location number +#if DEBUG >= 2 + std::cout << "count " << count << " root " << i << " loc " << k << std::endl; +#endif + root[count] = i; + loc[count] = k; + // If we've already found max possible roots, abort the search to save time + if ( ++count == deg_lambda ) + break; + } + if ( deg_lambda != count ) { + // deg(lambda) unequal to number of roots => uncorrectable error detected + count = -1; + goto finish; + } + // + // Compute err+eras evaluator poly omega(x) = s(x)*lambda(x) (modulo x**NROOTS). in + // index form. Also find deg(omega). + // + deg_omega = deg_lambda - 1; + for ( int i = 0; i <= deg_omega; i++ ) { + TYP tmp = 0; + for ( int j = i; j >= 0; j-- ) { + if (( syn[i - j] != A0 ) && ( lambda[j] != A0 )) + tmp ^= alpha_to[modnn(syn[i - j] + lambda[j])]; + } + omega[i] = index_of[tmp]; + } + + // + // Compute error values in poly-form. num1 = omega(inv(X(l))), num2 = inv(X(l))**(fcr-1) + // and den = lambda_pr(inv(X(l))) all in poly-form + // + for ( int j = count - 1; j >= 0; j-- ) { + TYP num1 = 0; + for ( int i = deg_omega; i >= 0; i-- ) { + if ( omega[i] != A0 ) + num1 ^= alpha_to[modnn(omega[i] + i * root[j])]; + } + TYP num2 = alpha_to[modnn(root[j] * ( FCR - 1 ) + NN)]; + TYP den = 0; + + // lambda[i+1] for i even is the formal derivative lambda_pr of lambda[i] + for ( int i = std::min(deg_lambda, NROOTS - 1) & ~1; i >= 0; i -= 2 ) { + if ( lambda[i + 1] != A0 ) { + den ^= alpha_to[modnn(lambda[i + 1] + i * root[j])]; + } + } +#if defined( DEBUG ) && DEBUG >= 1 + if ( den == 0 ) { + std::cout << "ERROR: denominator = 0" << std::endl; + count = -1; + goto finish; + } +#endif + // Apply error to data. Padding ('pad' unused symbols) begin at index 0. + if ( num1 != 0 ) { + if ( loc[j] < pad ) { + // If the computed error position is in the 'pad' (the unused portion of the + // R-S data capacity), then our solution has failed -- we've computed a + // correction location outside of the data and parity we've been provided! +#if DEBUG >= 2 + std::cout + << "ERROR: RS(" << SIZE <<"," << LOAD + << ") computed error location: " << loc[j] + << " within " << pad << " pad symbols, not within " + << LOAD - pad << " data or " << NROOTS << " parity" + << std::endl; +#endif + count = -1; + goto finish; + } + + TYP cor = alpha_to[modnn(index_of[num1] + + index_of[num2] + + NN - index_of[den])]; + // Store the error correction pattern, if a correction buffer is available + if ( corr ) + corr[j] = cor; + // If a data/parity buffer is given and the error is inside the message or + // parity data, correct it + if ( loc[j] < ( NN - NROOTS )) { + if ( data ) { + data[loc[j] - pad] ^= cor; + } + } else if ( loc[j] < NN ) { + if ( parity ) + parity[loc[j] - ( NN - NROOTS )] ^= cor; + } + } + } + + finish: +#if defined( DEBUG ) && DEBUG > 0 + if ( count > NROOTS ) + std::cout << "ERROR: Number of corrections: " << count << " exceeds NROOTS: " << NROOTS << std::endl; +#endif +#if defined( DEBUG ) && DEBUG > 1 + std::cout << "data x" << std::setw( 3 ) << len << ": " << std::vector<uint8_t>( data, data + len ) << std::endl; + std::cout << "parity x" << std::setw( 3 ) << NROOTS << ": " << std::string( len * 2, ' ' ) << std::vector<uint8_t>( parity, parity + NROOTS ) << std::endl; + if ( count > 0 ) { + std::string errors( 2 * ( len + NROOTS ), ' ' ); + for ( int i = 0; i < count; ++i ) { + errors[2*(loc[i]-pad)+0] = 'E'; + errors[2*(loc[i]-pad)+1] = 'E'; + } + for ( int i = 0; i < no_eras; ++i ) { + errors[2*(eras_pos[i])+0] = 'e'; + errors[2*(eras_pos[i])+1] = 'e'; + } + std::cout << "e)ra,E)rr x" << std::setw( 3 ) << count << ": " << errors << std::endl; + } +#endif + if ( eras_pos != NULL ) { + for ( int i = 0; i < count; i++) + eras_pos[i] = loc[i] - pad; + } + return count; + } + }; // class reed_solomon + + // + // Define the static reed_solomon...<...> members; allowed in header for template types. + // + // The reed_solomon_tags<...>::iprim < 0 is used to indicate to the first instance that the + // static tables require initialization. + // + template < typename TYP, int SYM, int PRM, class PLY > + int reed_solomon_tabs< TYP, SYM, PRM, PLY >::iprim = -1; + + template < typename TYP, int SYM, int PRM, class PLY > + std::array< TYP, reed_solomon_tabs< TYP, SYM, PRM, PLY > +#if not defined( EZPWD_ARRAY_TEST ) + ::NN + 1 > +#else +# warning "EZPWD_ARRAY_TEST: Erroneously defining alpha_to size!" + ::NN > +#endif + reed_solomon_tabs< TYP, SYM, PRM, PLY >::alpha_to; + + template < typename TYP, int SYM, int PRM, class PLY > + std::array< TYP, reed_solomon_tabs< TYP, SYM, PRM, PLY >::NN + 1 > + reed_solomon_tabs< TYP, SYM, PRM, PLY >::index_of; + template < typename TYP, int SYM, int PRM, class PLY > + std::array< TYP, reed_solomon_tabs< TYP, SYM, PRM, PLY >::MODS > + reed_solomon_tabs< TYP, SYM, PRM, PLY >::mod_of; + + template < typename TYP, int SYM, int RTS, int FCR, int PRM, class PLY > + std::array< TYP, reed_solomon< TYP, SYM, RTS, FCR, PRM, PLY >::NROOTS + 1 > + reed_solomon< TYP, SYM, RTS, FCR, PRM, PLY >::genpoly; + +} // namespace ezpwd + +#endif // _EZPWD_RS_BASE diff --git a/op25/gr-op25_repeater/lib/ezpwd/serialize b/op25/gr-op25_repeater/lib/ezpwd/serialize new file mode 100644 index 0000000..ae4a62c --- /dev/null +++ b/op25/gr-op25_repeater/lib/ezpwd/serialize @@ -0,0 +1,1188 @@ +/* + * Ezpwd Reed-Solomon -- Reed-Solomon encoder / decoder library + * + * Copyright (c) 2014, Hard Consulting Corporation. + * + * Ezpwd Reed-Solomon 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 3 of + * the License, or (at your option) any later version. See the LICENSE file at the top of the + * source tree. Ezpwd Reed-Solomon is also available under Commercial license. c++/ezpwd/rs_base + * is redistributed under the terms of the LGPL, regardless of the overall licensing terms. + * + * Ezpwd Reed-Solomon 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. + */ +#ifndef _EZPWD_SERIALIZE +#define _EZPWD_SERIALIZE + +#include <iterator> +#include <array> + +#include "output" + +// +// EZPWD (no padding) and RFC4648 (enforce padding) base-N codecs +// +// scatter -- 8-bit binary data into 5-bit (base32) and 6-bit (base64) chunks (optionally w/pad) +// gather -- collect up 5/6-bit binary data back into 8-bit characters (optionally w/pad) +// ..._standard -- enforce RFC4648-standard padding +// +// Scatters or gathers 8-bit binary character data to 5/6-bit symbols, suitable for base32/64 +// encoding. +// +// encode -- convert binary data to ASCII base32/64, in-place +// decode -- convert ASCII base32/64 to binary data, in-place +// ..._standard -- enforce RFC4648-standard padding +// +// Transforms iterable containers of char between ASCII symbols and binary data, always in-place. +// The decode may alter the size of the result (by ignoring whitespace). +// +// In general the ezpwd::base32/64 en/decoders are designed to produce easily human-usable +// encodings, and can ignore common whitespace characters and '-' to allow human-readable +// formatting. The RFC4648 Standard base 32/64 and Crockford base32 encodings are also supported. +// +// Adding new symbol encodings (and even new bases, up to base-128) is trivial. +// +namespace ezpwd { + + namespace serialize { + + enum chr_t { + pd = -1, // padding + nv = -2, // invalid + ws = -3, // whitespace + }; + + enum ws_use_t { + ws_invalid = 0, // Whitespace is invalid + ws_ignored = 1, // Whitespace ignored (the default) + }; + + enum pd_use_t { + pd_invalid = 0, // Padding is not expected (invalid) + pd_ignored = 1, // Padding is ignored, and automatically supplied if required (the default) + pd_enforce = 2, // Padding is expected and enforced + }; + + // + // serialize::tables -- base class for TABLES in all base<N,TABLES> + // + // Every serialize::table<BASE> specialization must have an encoder table of size BASE, + // and a decoder table of size 127. + // + + // + // hex<16/32> -- RFC4648 4-bit (standard 16 symbol) and 5-bit (32 symbol) + // standard<16/32/64> -- RFC4648 standard tables + // standard_url<64> -- RFC4648 standard tables (base-64 URL-safe) + // ezpwd<16/32/64> -- EZPWD tables + // crockford<32> -- Crockford (base32 only) + // + // These types are passed as the TABLE template parameter to base<32/64,TABLE> class + // template instantiations. They must specify an encoder table of size BASE, and a decoder + // table of size 127. + // + template < size_t N > struct hex { }; + template < size_t N > struct uuencode { }; + template < size_t N > struct standard { }; + template < size_t N > struct standard_url { }; + template < size_t N > struct ezpwd { }; + template < size_t N > struct crockford { }; + + // + // base<16> tables -- basic hexadecimal supported (hex, standard, ezpwd identical) + // + template <> struct hex<16> { + static const constexpr std::array<char,16> + encoder { { + '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', 'A', 'B', 'C', 'D', 'E', 'F', + } }; + static const constexpr std::array<char,127> + decoder { { + nv, nv, nv, nv, nv, nv, nv, nv, nv, ws, ws, ws, ws, ws, nv, nv, // 9-13: <TAB>,<NL>,<VT>,<FF>,<CR> + nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, // + ws, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, // !"#$%&`()*+,-./ + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, nv, nv, nv, nv, nv, nv, // 0123456789:;<=>? '=' is pad + nv, 10, 11, 12, 13, 14, 15, nv, nv, nv, nv, nv, nv, nv, nv, nv, // @ABCDEFGHIJKLMNO + nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, // PQRSTUVWXYZ[\]^_ + nv, 10, 11, 12, 13, 14, 15, nv, nv, nv, nv, nv, nv, nv, nv, nv, // `abcdefghijklmno + nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, // pqrstuvwxyz{|}~ + } }; + }; // struct serialize::hex<16> + + template <> struct standard<16> { + static const constexpr std::array<char,16> + encoder = hex<16>::encoder; + static const constexpr std::array<char,127> + decoder = hex<16>::decoder; + }; + + template <> struct ezpwd<16> { + static const constexpr std::array<char,16> + encoder = hex<16>::encoder; + static const constexpr std::array<char,127> + decoder = hex<16>::decoder; + }; + + // + // base<32> tables -- ezpwd, and RFC4648 hex32, crockford and standard + // + template <> struct hex<32> { + static const constexpr std::array<char,32> + encoder { { + '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', 'A', 'B', 'C', 'D', 'E', 'F', + 'G', 'H', 'I', 'J', 'K', 'L', 'M', 'N', + 'O', 'P', 'Q', 'R', 'S', 'T', 'U', 'V', + } }; + static const constexpr std::array<char,127> + decoder { { + nv, nv, nv, nv, nv, nv, nv, nv, nv, ws, ws, ws, ws, ws, nv, nv, // 9-13: <TAB>,<NL>,<VT>,<FF>,<CR> + nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, // + ws, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, // !"#$%&`()*+,-./ + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, nv, nv, nv, nv, nv, nv, // 0123456789:;<=>? '=' is pad + nv, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, // @ABCDEFGHIJKLMNO + 25, 26, 27, 28, 29, 30, 31, nv, nv, nv, nv, nv, nv, nv, nv, nv, // PQRSTUVWXYZ[\]^_ + nv, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, // `abcdefghijklmno + 25, 26, 27, 28, 29, 30, 31, nv, nv, nv, nv, nv, nv, nv, nv, // pqrstuvwxyz{|}~ + } }; + }; // struct serialize::hex<32> + + template <> struct standard<32> { // RFC4648 Standard + static const constexpr std::array<char,32> + encoder { { + 'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', + 'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P', + 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', + 'Y', 'Z', '2', '3', '4', '5', '6', '7', + } }; + static const constexpr std::array<char,127> + decoder { { + nv, nv, nv, nv, nv, nv, nv, nv, nv, ws, ws, ws, ws, ws, nv, nv, // 9-13: <TAB>,<NL>,<VT>,<FF>,<CR> + nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, // + ws, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, // !"#$%&`()*+,-./ + nv, nv, 26, 27, 28, 29, 30, 31, nv, nv, nv, nv, nv, pd, nv, nv, // 0123456789:;<=>? + nv, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, // @ABCDEFGHIJKLMNO '=' is pad + 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, nv, nv, nv, nv, nv, // PQRSTUVWXYZ[\]^_ + nv, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, // `abcdefghijklmno + 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, nv, nv, nv, nv, // pqrstuvwxyz{|}~ + } }; + }; // struct serialize::standard<32> + + template <> struct ezpwd<32> { + static const constexpr std::array<char,32> + encoder { { + '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', 'A', 'B', 'C', 'D', 'E', 'F', + 'G', 'H', 'J', 'K', 'L', 'M', 'N', 'P', + 'Q', 'R', 'T', 'U', 'V', 'W', 'X', 'Y', + } }; + + static const constexpr std::array<char,127> + decoder { { + nv, nv, nv, nv, nv, nv, nv, nv, nv, ws, ws, ws, ws, ws, nv, nv, // 9-13: <TAB>,<NL>,<VT>,<FF>,<CR> + nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, // + ws, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, ws, nv, nv, // !"#$%&`()*+,-./ '-' is whitespace + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, nv, nv, nv, pd, nv, nv, // 0123456789:;<=>? '=' is pad + nv, 10, 11, 12, 13, 14, 15, 16, 17, 1, 18, 19, 20, 21, 22, 0, // @ABCDEFGHIJKLMNO + 23, 24, 25, 5, 26, 27, 28, 29, 30, 31, 2, nv, nv, nv, nv, nv, // PQRSTUVWXYZ[\]^_ + nv, 10, 11, 12, 13, 14, 15, 16, 17, 1, 18, 19, 20, 21, 22, 0, // `abcdefghijklmno + 23, 24, 25, 5, 26, 27, 28, 29, 30, 31, 2, nv, nv, nv, nv, // pqrstuvwxyz{|}~ + } }; + }; // struct serialize::ezpwd<32> + + template <> struct crockford<32> { + static const constexpr std::array<char,32> + encoder { { + '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', 'A', 'B', 'C', 'D', 'E', 'F', + 'G', 'H', 'J', 'K', 'M', 'N', 'P', 'Q', + 'R', 'S', 'T', 'V', 'W', 'X', 'Y', 'Z', + } }; + + static const constexpr std::array<char,127> + decoder { { + nv, nv, nv, nv, nv, nv, nv, nv, nv, ws, ws, ws, ws, ws, nv, nv, // 9-13: <TAB>,<NL>,<VT>,<FF>,<CR> + nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, // + ws, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, ws, nv, nv, // !"#$%&`()*+,-./ '-' is whitespace + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, nv, nv, nv, pd, nv, nv, // 0123456789:;<=>? '=' is pad + nv, 10, 11, 12, 13, 14, 15, 16, 17, 1, 18, 19, 1, 20, 21, 0, // @ABCDEFGHIJKLMNO + 22, 23, 24, 25, 26, nv, 27, 28, 29, 30, 31, nv, nv, nv, nv, nv, // PQRSTUVWXYZ[\]^_ + nv, 10, 11, 12, 13, 14, 15, 16, 17, 1, 18, 19, 1, 20, 21, 0, // `abcdefghijklmno + 22, 23, 24, 25, 26, nv, 27, 28, 29, 30, 31, nv, nv, nv, nv, // pqrstuvwxyz{|}~ + } }; + }; // struct serialize::crockford<32> + + // + // base<64> tables + // + template <> struct uuencode<64> { + static const constexpr std::array<char,64> + encoder { { + ' ', '!', '"', '#', '$', '%', '&','\'', + '(', ')', '*', '+', ',', '-', '.', '/', + '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', ':', ';', '<', '=', '>', '?', + '@', 'A', 'B', 'C', 'D', 'E', 'F', 'G', + 'H', 'I', 'J', 'K', 'L', 'M', 'N', 'O', + 'P', 'Q', 'R', 'S', 'T', 'U', 'V', 'W', + 'X', 'Y', 'Z', '[','\\', ']', '^', '_', + } }; + static const constexpr std::array<char,127> + decoder { { + nv, nv, nv, nv, nv, nv, nv, nv, nv, ws, ws, ws, ws, ws, nv, nv, // 9-13: <TAB>,<NL>,<VT>,<FF>,<CR> + nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, // + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, // !"#$%&`()*+,-./ + 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, // 0123456789:;<=>? '=' is pad + 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, // @ABCDEFGHIJKLMNO + 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, // PQRSTUVWXYZ[\]^_ + nv, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, // `abcdefghijklmno + 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, nv, nv, nv, nv, nv, // pqrstuvwxyz{|}~ + } }; + }; // struct serialize::standard<64> + + template <> struct standard<64> { + static const constexpr std::array<char,64> // RFC4648 Standard + encoder { { + 'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', + 'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P', + 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', + 'Y', 'Z', 'a', 'b', 'c', 'd', 'e', 'f', + 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', + 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', + 'w', 'x', 'y', 'z', '0', '1', '2', '3', + '4', '5', '6', '7', '8', '9', '+', '/', + } }; + static const constexpr std::array<char,127> + decoder { { + nv, nv, nv, nv, nv, nv, nv, nv, nv, ws, ws, ws, ws, ws, nv, nv, // 9-13: <TAB>,<NL>,<VT>,<FF>,<CR> + nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, // + ws, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, 62, nv, nv, nv, 63, // !"#$%&`()*+,-./ + 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, nv, nv, nv, pd, nv, nv, // 0123456789:;<=>? '=' is pad + nv, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, // @ABCDEFGHIJKLMNO + 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, nv, nv, nv, nv, nv, // PQRSTUVWXYZ[\]^_ + nv, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, // `abcdefghijklmno + 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, nv, nv, nv, nv, // pqrstuvwxyz{|}~ + } }; + }; // struct serialize::standard<64> + + template <> struct standard_url<64> { + static const constexpr std::array<char,64> + encoder { { + 'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', + 'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P', + 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', + 'Y', 'Z', 'a', 'b', 'c', 'd', 'e', 'f', + 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', + 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', + 'w', 'x', 'y', 'z', '0', '1', '2', '3', + '4', '5', '6', '7', '8', '9', '-', '_', + } }; + static const constexpr std::array<char,127> + decoder { { + nv, nv, nv, nv, nv, nv, nv, nv, nv, ws, ws, ws, ws, ws, nv, nv, // 9-13: <TAB>,<NL>,<VT>,<FF>,<CR> + nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, // + ws, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, 62, nv, nv, // !"#$%&`()*+,-./ + 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, nv, nv, nv, pd, nv, nv, // 0123456789:;<=>? '=' is pad + nv, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, // @ABCDEFGHIJKLMNO + 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, nv, nv, nv, nv, 63, // PQRSTUVWXYZ[\]^_ + nv, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, // `abcdefghijklmno + 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, nv, nv, nv, nv, // pqrstuvwxyz{|}~ + } }; + }; // struct serialize::standar_url<64> + + template <> struct ezpwd<64> { + static const constexpr std::array<char,64> // '+' and '.' are URL safe, and we treat '-' as whitespace + encoder { { + 'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', + 'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P', + 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', + 'Y', 'Z', 'a', 'b', 'c', 'd', 'e', 'f', + 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', + 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', + 'w', 'x', 'y', 'z', '0', '1', '2', '3', + '4', '5', '6', '7', '8', '9', '+', '.', + } }; + static const constexpr std::array<char,127> + decoder { { + nv, nv, nv, nv, nv, nv, nv, nv, nv, ws, ws, ws, ws, ws, nv, nv, // 9-13: <TAB>,<NL>,<VT>,<FF>,<CR> + nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, // + ws, nv, nv, nv, nv, nv, nv, nv, nv, nv, nv, 62, nv, ws, 63, nv, // !"#$%&`()*+,-./ '-' is whitespace + 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, nv, nv, nv, pd, nv, nv, // 0123456789:;<=>? '=' is pad + nv, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, // @ABCDEFGHIJKLMNO + 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, nv, nv, nv, nv, 63, // PQRSTUVWXYZ[\]^_ + nv, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, // `abcdefghijklmno + 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, nv, nv, nv, nv, // pqrstuvwxyz{|}~ + } }; + }; // struct serialize::ezpwd<64> + + // + // base_generic<N, TABLES> -- generic base-N conversion using TABLES<N>. Doesn't have scatter/gather. + // + // Every base-N converter requires a TABLE class of matching N, and can encode/decode. + // in-place using the table::encoder/decoder arrays. Only specific values of N have class + // template specialization which includes scatter/gather methods -- the generic base-N + // template does not.. + // + template < size_t N, typename TABLES = ezpwd< N >> + class base_generic + : public TABLES { + public: + static + std::ostream &output( + std::ostream &lhs ) + { + lhs << "base<" << N; +#if defined( DEBUG ) && DEBUG >= 3 + lhs << "," << TABLES::encoder; +#endif + lhs << ">"; + return lhs; + } + + // + // base<N>::encode(<string>) -- encode the supplied sequence of data in the domain (0,N] to base-N + // base<N>::encode(<iter>,<iter>) -- encode the supplied std::string of (0,N] symbols in-place to base-N + // + // Items from iterator must be convertible to a table index (ie. not -'ve, and within + // the table size N). If the table contains 'ws' entries, they may optionally be ignored. + // If the input symbol indexes outside the provided encoder table (or an 'nv' entry), then an + // exception will be thrown. + // + // If a non-zero 'pad' character is supplied, then the -1 (pd) character value will be + // allowed (normally occurring only at the end of the input range), and a 'pad' will be + // emitted for each one. + // + template < typename I > + static + I encode( + I begin, + I end, + char pad = '=' ) // '=' for standards-compliance + { + for ( I i = begin; i != end; ++i ) { + if ( *i >= 0 and size_t( *i ) < TABLES::encoder.size() ) + *i = TABLES::encoder[*i]; + else if ( pd == *i and pad ) + *i = pad; + else + throw std::runtime_error( + std::string( "ezpwd::serialize::encode: invalid base-" ) << N + << " binary symbol presented: " << *i ); + } + return end; + } + + static + std::string &encode( + std::string &symbols, + char pad = '=' ) // '=' for standards-compliance + { + encode( symbols.begin(), symbols.end(), pad ); + return symbols; + } + + // + // base<N>::decode(<iter>,<iter>) -- decode base-N symbols in-place, collapsing spaces. + // base<N>::decode(<string>) -- decode base-N symbols in supplied std::string, collapsing spaces, in-place. + // + // Items from iterator must be convertible to a table index (ie. not -'ve, and within + // the table size, which is always 127). + // + // ws_ignored -- skip whitepace/- symbols (the default) + // ws_invalid -- consider whitespace/- symbols as invalid symbols + // + // pd_invalid -- consider padding symbols as invalid + // pd_ignored -- skip padding symbols + // pd_enforce -- leave any padding symbols in place (the default) + // + // If erasure vector supplied, marks invalid symbols as erasures; otherwise, throws + // exception. Ignores whitespace. Will return an iterator to just after the last output + // symbol used in the provided range (eg. to shorten the container, if desired), leaving any + // remaining symbols unchanged. The <string> version returns the same string reference + // passed in (shortened, if spaces/padding ignored). + // + // If an invalid vector is supplied, we'll also return the offending input symbol(s); if + // an exception is raised (no erasure vector supplied), only one symbol will be in invalid. + // + // NOTE: will quite likely return an iterator before the supplied 'end', indicating that the + // output has been truncated (shortened), due to collapsing spaces! + // + template < typename I > + static + I decode( + I begin, + I end, + std::vector<int> *erasure, // Deem invalid symbols as erasures + std::vector<char> *invalid = 0, // and return the symbols + ws_use_t ws_use = ws_ignored, // skip any whitespace + pd_use_t pd_use = pd_invalid ) // no padding expected; invalid + { + if ( erasure ) + erasure->clear(); + if ( invalid ) + invalid->clear(); + I i, o; + for ( i = o = begin; i != end; ++i ) { + size_t ti( *i ); + char c = ti < TABLES::decoder.size() ? TABLES::decoder[ti] : char( nv ); + if ( ws == c ) + switch ( ws_use ) { + case ws_invalid: + c = nv; + break; + case ws_ignored: default: + continue; + } + if ( pd == c ) + switch ( pd_use ) { + case pd_invalid: + c = nv; + break; + case pd_enforce: + break; + case pd_ignored: default: + continue; + } + if ( nv == c ) { + // Invalid symbol; optionally remember them. Mark as erasure? Or throw. + if ( invalid ) + invalid->push_back( *i ); + if ( erasure ) { + erasure->push_back( o - begin ); // index of offending symbol in output + c = 0; // will be output w/ 0 value + } else { + throw std::runtime_error( + std::string( "ezpwd::serialize::decode: invalid base-" ) << N + << " ASCII symbol presented: " << int( *i ) << " '" << *i << "'" ); + } + } + *o++ = c; + } + return o; + } + + template < typename I > + static + I decode( + I begin, + I end, + ws_use_t ws_use = ws_ignored, + pd_use_t pd_use = pd_invalid ) + { + return decode( begin, end, 0, 0, ws_use, pd_use ); + } + + template < typename I > + static + I decode_standard( + I begin, + I end ) + { + return decode( begin, end, 0, 0, ws_ignored, pd_enforce ); // RFC4648 padding + } + + static + std::string &decode( + std::string &symbols, + std::vector<int> *erasure = 0, + std::vector<char> *invalid = 0, + ws_use_t ws_use = ws_ignored, + pd_use_t pd_use = pd_invalid ) + { + auto last = decode( symbols.begin(), symbols.end(), erasure, invalid, + ws_use, pd_use ); + if ( last != symbols.end() ) + symbols.resize( last - symbols.begin() ); // eliminated some whitespace + return symbols; + } + static + std::string &decode_standard( + std::string &symbols, + std::vector<int> *erasure = 0, + std::vector<char> *invalid = 0 ) + { + return decode( symbols, erasure, invalid, ws_ignored, pd_enforce ); // RFC4648 padding + } + + // + // gather_next -- return next symbol to gather into 8-bit output + // + // Fails if padding mixed in with data, or a data symbol exceeds encoding bit depth + // (eg. 5 bits for base-32). + // + // If auto-padding, allow the caller to differentiate + // + template < typename I > + static + int gather_next( I &beg, I end, pd_use_t pd_use, int previous ) + { + if ( beg == end ) { + if ( pd_enforce == pd_use ) + throw std::logic_error( std::string( "base-" ) << N << " gather error; insufficient data"); + // automatically pad; return nv on underflow while un-emitted data remains, finally pd + return previous >= 0 ? nv : pd; + } + int c = *beg++; + if ( previous < 0 and c >= 0 ) + throw std::logic_error( + std::string( "base-" ) << N << " gather error; data following padding" ); + if ( c >= char( N ) or c < pd ) + throw std::logic_error( + std::string( "base-" ) << N << " gather error; symbol value " << int( c ) << " beyond capacity" ); + return c; + } + }; // class serialize::base_generic + } // namespace serialize + + template < size_t N, typename TABLES > + std::ostream &operator<<( + std::ostream &lhs, + const ezpwd::serialize::base_generic<N,TABLES> + &rhs ) + { + return rhs.output( lhs ); + } + + namespace serialize { + // + // ezpwd::serialize::base<N> -- Arbitrary bases (other than those with specializations below) + // + // Can en/decode, but no scatter/gather implementation, nor encode_size. + // + template < size_t N, typename TABLES > + class base + : public base_generic< N, TABLES > { + public: + using base_generic< N, TABLES >::encode; + using base_generic< N, TABLES >::decode; + }; + + + // + // ezpwd::serialize::base<16> -- transform individual characters between 4-bit binary and a base-16 + // + template < typename TABLES > + class base< 16, TABLES > + : public base_generic< 16, TABLES > { + public: + using base_generic< 16, TABLES >::encode; + using base_generic< 16, TABLES >::decode; + using base_generic< 16, TABLES >::gather_next; + + static constexpr size_t encode_size( + size_t len, + pd_use_t /* pd_use */ = pd_invalid ) + { + return len * 2; // encoding base-16 always exactly doubles size + } + + // + // base<16>::scatter -- distribute a range of input bytes to an output iterator in 4-bit chunks + // + // The same implementation is used for both random-access iterators, and for all + // other forward iterators. + // + template < typename I, typename O, typename iterator_tag > + static + O scatter( + I beg, + I end, + O out, + pd_use_t, // ignored; never needs to pad + iterator_tag ) // ignored + { + while ( beg != end ) { + int c0 = *beg++; + *out++ = char((c0 & 0xff) >> 4); + *out++ = char((c0 & 0x0f)); + } + return out; + } + + template < typename I, typename O > + static + O scatter( + I beg, + I end, + O out, + pd_use_t pd_use = pd_invalid ) + { + return scatter( beg, end, out, pd_use, typename std::iterator_traits<I>::iterator_category() ); + } + + template < typename I, typename O > + static + O scatter_standard( + I beg, + I end, + O out ) + { + return scatter( beg, end, out, pd_enforce, typename std::iterator_traits<I>::iterator_category() ); + } + + // + // base<16>::gather -- collect 4-bit chunks into 8-bit characters + // + // If underflow occurs (not enough data collected to output complete last char), then an + // exception will be raised. However, if 'pad' is set, then output will automatically be + // padded, flushing out any un-emitted data remaining in previous 4-bit base-16 symbol. + // + // For correct base-16 data produced by 'scatter', this will allow 'gather' to always + // produce the identical output as was originally provided to 'scatter'. However, if simply + // truncated base-16 input is provided (eg. only 1 symbols of an 2-symbol 8-bit + // base-16 group), then the final 8-bit symbol from the original data will be missing. + // + template < typename I, typename O > + static + O gather( + I beg, + I end, + O out, + pd_use_t pd_use = pd_invalid ) + { + while ( beg != end ) { + int c0 = gather_next( beg, end, pd_use, 0 ); + int c1 = gather_next( beg, end, pd_use, c0 ); + if ( c0 >= 0 and c1 >= 0 ) + *out++ = ( ((c0 < 0 ? 0 : c0 ) << 4) + | (c1 < 0 ? 0 : c1 )); + } + return out; + } + template < typename I, typename O > + static + O gather_standard( + I beg, + I end, + O out ) + { + return gather( beg, end, out, pd_enforce ); + } + }; // ezpwd::base<16> + + // + // ezpwd::serialize::base16... + // + // Shortcut typedefs for the available base16 codec. + // + typedef base< 16, hex< 16 >> + base16; + typedef base< 16, hex< 16 >> + base16_standard; + + + // + // ezpwd::serialize::base<32> -- transform individual characters between 5-bit binary and a base-32 + // + // The char values [0,32) are mapped by base<32>::encode onto something like + // + // 0123456789ABCDEFGHJKLMNPQRTUVWXY TABLES=ezpwd<32> (default) + // 0123456789ABCDEFGHJKMNPQRSTVWXYZ TABLES=crockford<32> + // ABCDEFGHIJKLMNOPQRSTUVWXYZ234567 TABLES=standard<32> + // + // and base<32>::decode performs the inverse. In addition to folding lower-case to + // upper-case, the following mappings occur in the ezpwd<32>::decoder table: + // + // O -> 0 + // Z -> 2 + // S -> 5 + // I -> 1 + // + // B -> 8 could not be included due to the limited size of the capitalized alpha-numeric + // symbol pool, unfortunately, but the other substitutions were prioritized as they are + // more likely to occur in standard printed text. + // + // Any characters encountered outside [0,32) by encode and outside the above set by + // decode raise an exception, unless an erasure vector is provided, in which case we supply + // a 0 value and store the index of the invalid symbol in the vector. + // + template < typename TABLES > + class base< 32, TABLES > + : public base_generic< 32, TABLES > { + public: + using base_generic< 32, TABLES >::encode; + using base_generic< 32, TABLES >::decode; + using base_generic< 32, TABLES >::gather_next; + + static constexpr size_t encode_size( + size_t len, + pd_use_t pd_use = pd_invalid ) + { + return ( pd_enforce == pd_use + ? ( len + 4 ) / 5 * 8 + : ( len * 8 + 4 ) / 5 ); + } + + // + // base<32>::scatter -- distribute a range of input bytes to an output iterator in 5-bit chunks + // + // Separate implementation are provided for random-access iterators (with fewer + // comparisons necessary) and for all other forward iterators. + // + template < typename I, typename O, typename iterator_tag > + static + O scatter( + I beg, + I end, + O out, + pd_use_t pd_use, + iterator_tag ) + { + while ( beg != end ) { + int c0 = *beg++; + *out++ = char((c0 & 0xff) >> 3); + + if ( beg == end ) { + *out++ = char((c0 & 0x07) << 2); + if ( pd_enforce == pd_use ) { + *out++ = EOF; + *out++ = EOF; + *out++ = EOF; + *out++ = EOF; + *out++ = EOF; + *out++ = EOF; + } + return out; + } + + int c1 = *beg++; + *out++ = char((c0 & 0x07) << 2 | (c1 & 0xff) >> 6); + *out++ = char((c1 & 0x3f) >> 1); + + if ( beg == end ) { + *out++ = char((c1 & 0x01) << 4); + if ( pd_enforce == pd_use ) { + *out++ = EOF; + *out++ = EOF; + *out++ = EOF; + *out++ = EOF; + } + return out; + } + + int c2 = *beg++; + *out++ = char((c1 & 0x01) << 4 | (c2 & 0xff) >> 4); + + if ( beg == end ) { + *out++ = char((c2 & 0x0f) << 1); + if ( pd_enforce == pd_use ) { + *out++ = EOF; + *out++ = EOF; + *out++ = EOF; + } + return out; + } + + int c3 = *beg++; + *out++ = char((c2 & 0x0f) << 1 | (c3 & 0xff) >> 7); + *out++ = char((c3 & 0x7f) >> 2); + + if ( beg == end ) { + *out++ = char((c3 & 0x03) << 3); + if ( pd_enforce == pd_use ) { + *out++ = EOF; + } + return out; + } + + int c4 = *beg++; + *out++ = char((c3 & 0x03) << 3 | (c4 & 0xff) >> 5); + *out++ = char((c4 & 0x1f)); + } + + return out; + } + + template < typename I, typename O > + static + O scatter( + I beg, + I end, + O out, + pd_use_t pd_use, + std::random_access_iterator_tag ) + { + while ( end - beg >= 5 ) { + int c0 = *beg++; + *out++ = char((c0 & 0xff) >> 3); + int c1 = *beg++; + *out++ = char((c0 & 0x07) << 2 | (c1 & 0xff) >> 6); + *out++ = char((c1 & 0x3f) >> 1); + int c2 = *beg++; + *out++ = char((c1 & 0x01) << 4 | (c2 & 0xff) >> 4); + int c3 = *beg++; + *out++ = char((c2 & 0x0f) << 1 | (c3 & 0xff) >> 7); + *out++ = char((c3 & 0x7f) >> 2); + int c4 = *beg++; + *out++ = char((c3 & 0x03) << 3 | (c4 & 0xff) >> 5); + *out++ = char((c4 & 0x1f)); + } + + switch ( end - beg ) { + case 4: { + int c0 = *beg++; + *out++ = char((c0 & 0xff) >> 3); + int c1 = *beg++; + *out++ = char((c0 & 0x07) << 2 | (c1 & 0xff) >> 6); + *out++ = char((c1 & 0x3f) >> 1); + int c2 = *beg++; + *out++ = char((c1 & 0x01) << 4 | (c2 & 0xff) >> 4); + int c3 = *beg++; + *out++ = char((c2 & 0x0f) << 1 | (c3 & 0xff) >> 7); + *out++ = char((c3 & 0x7f) >> 2); + *out++ = char((c3 & 0x03) << 3); + if ( pd_enforce == pd_use ) { + *out++ = EOF; + } + return out; + } + case 3: { + int c0 = *beg++; + *out++ = char((c0 & 0xff) >> 3); + int c1 = *beg++; + *out++ = char((c0 & 0x07) << 2 | (c1 & 0xff) >> 6); + *out++ = char((c1 & 0x3f) >> 1); + int c2 = *beg++; + *out++ = char((c1 & 0x01) << 4 | (c2 & 0xff) >> 4); + *out++ = char((c2 & 0x0f) << 1); + if ( pd_enforce == pd_use ) { + *out++ = EOF; + *out++ = EOF; + *out++ = EOF; + } + return out; + } + case 2: { + int c0 = *beg++; + *out++ = char((c0 & 0xff) >> 3); + int c1 = *beg++; + *out++ = char((c0 & 0x07) << 2 | (c1 & 0xff) >> 6); + *out++ = char((c1 & 0x3f) >> 1); + *out++ = char((c1 & 0x01) << 4); + if ( pd_enforce == pd_use ) { + *out++ = EOF; + *out++ = EOF; + *out++ = EOF; + *out++ = EOF; + } + return out; + } + case 1: { + int c0 = *beg++; + *out++ = char((c0 & 0xff) >> 3); + *out++ = char((c0 & 0x07) << 2); + if ( pd_enforce == pd_use ) { + *out++ = EOF; + *out++ = EOF; + *out++ = EOF; + *out++ = EOF; + *out++ = EOF; + *out++ = EOF; + } + return out; + } + default: + return out; + } + } + + template < typename I, typename O > + static + O scatter( + I beg, + I end, + O out, + pd_use_t pd_use = pd_invalid ) + { + return scatter( beg, end, out, pd_use, typename std::iterator_traits<I>::iterator_category() ); + } + + template < typename I, typename O > + static + O scatter_standard( + I beg, + I end, + O out ) + { + return scatter( beg, end, out, pd_enforce, typename std::iterator_traits<I>::iterator_category() ); + } + + // + // base<32>::gather -- collect 5-bit chunks into 8-bit characters + // + // If underflow occurs (not enough data collected to output complete last char), then an + // exception will be raised. However, if 'pad' is set, then output will automatically be + // padded, discarding any un-emitted data remaining in previous 5-bit base-32 symbols. + // + // For correct base-32 data produced by 'scatter', this will allow 'gather' to always + // produce the identical output as was originally provided to 'scatter'. However, if simply + // truncated base-32 input is provided (eg. only 1, 3 or 5 symbols of an 8-symbol 40-bit + // base-32 group), then the final 8-bit symbol from the original data will be missing. + // + template < typename I, typename O > + static + O gather( + I beg, + I end, + O out, + pd_use_t pd_use = pd_invalid ) + { + while ( beg != end ) { + int c0 = gather_next( beg, end, pd_use, 0 ); + int c1 = gather_next( beg, end, pd_use, c0 ); + if ( c0 >= 0 and c1 >= 0 ) + *out++ = ( ((c0 < 0 ? 0 : c0 ) << 3) + | (c1 < 0 ? 0 : c1 ) >> 2 ); + int c2 = gather_next( beg, end, pd_use, c1 ); + int c3 = gather_next( beg, end, pd_use, c2 ); + if ( c1 >= 0 and c2 >= 0 and c3 >= 0 ) + *out++ = ( ((c1 < 0 ? 0 : c1) & 0x03) << 6 + | (c2 < 0 ? 0 : c2) << 1 + | (c3 < 0 ? 0 : c3) >> 4 ); + int c4 = gather_next( beg, end, pd_use, c3 ); + if ( c3 >= 0 and c4 >= 0 ) + *out++ = ( ((c3 < 0 ? 0 : c3) & 0x0f) << 4 + | (c4 < 0 ? 0 : c4) >> 1 ); + int c5 = gather_next( beg, end, pd_use, c4 ); + int c6 = gather_next( beg, end, pd_use, c5 ); + if ( c4 >=0 and c5 >= 0 and c6 >= 0 ) + *out++ = ( ((c4 < 0 ? 0 : c4) & 0x01) << 7 + | (c5 < 0 ? 0 : c5) << 2 + | (c6 < 0 ? 0 : c6) >> 3 ); + int c7 = gather_next( beg, end, pd_use, c6 ); + if ( c6 >= 0 and c7 >= 0 ) + *out++ = ( ((c6 < 0 ? 0 : c6 ) & 0x07) << 5 + | (c7 < 0 ? 0 : c7 )); + } + return out; + } + template < typename I, typename O > + static + O gather_standard( + I beg, + I end, + O out ) + { + return gather( beg, end, out, pd_enforce ); + } + }; // ezpwd::base<32> + + // + // ezpwd::serialize::base32... + // + // Shortcut typedefs for the available base32 codecs. + // + typedef base< 32, ezpwd< 32 >> + base32; + typedef base< 32, hex< 32 >> + base32_hex; + typedef base< 32, serialize::standard< 32 >> + base32_standard; + typedef base< 32, crockford< 32 >> + base32_crockford; + + + // + // ezpwd::serialize::base<64> -- transform individual characters between 6-bit binary and base-64 + // + // The char values [0,64) are mapped by base64::encode onto either the + // standard or ezpwd (url-safe) tables: + // + // ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/ -- RFC4648 standard + // ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789-_ -- RFC4648 standard_url + // ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+. -- ezpwd + // + // and base<64>::decode performs the inverse (handles both standard and url-safe encodings). + // + // Any characters encountered outside [0,64) by encode and outside the above set + // by decode raise an exception. + // + template < typename TABLES > + class base< 64, TABLES > + : public base_generic< 64, TABLES > { + public: + using base_generic< 64, TABLES >::encode; + using base_generic< 64, TABLES >::decode; + using base_generic< 64, TABLES >::gather_next; + + static constexpr size_t encode_size( + size_t len, + pd_use_t pd_use = pd_invalid ) + { + return ( pd_enforce == pd_use + ? ( len + 2 ) / 3 * 4 + : ( len * 4 + 2 ) / 3 ); + } + + // + // base<64>::scatter -- distribute a range of input bytes to an output iterator in 6-bit chunks + // + template < typename I, typename O, typename iterator_tag > + static + O scatter( + I beg, + I end, + O out, + pd_use_t pd_use, + iterator_tag ) + { + while ( beg != end ) { + int c0 = *beg++; + *out++ = char((c0 & 0xff) >> 2); + + if ( beg == end ) { + *out++ = char((c0 & 0x03) << 4); + if ( pd_enforce == pd_use ) { + *out++ = EOF; + *out++ = EOF; + } + return out; + } + + int c1 = *beg++; + *out++ = char((c0 & 0x03) << 4 | (c1 & 0xff) >> 4); + + if ( beg == end ) { + *out++ = char((c1 & 0x0f) << 2); + if ( pd_enforce == pd_use ) { + *out++ = EOF; + } + return out; + } + + int c2 = *beg++; + *out++ = char((c1 & 0x0f) << 2 | (c2 & 0xff) >> 6); + *out++ = char((c2 & 0x3f)); + } + + return out; + } + + template < typename I, typename O > + static + O scatter( + I beg, + I end, + O out, + pd_use_t pd_use, + std::random_access_iterator_tag ) + { + while ( end - beg >= 3 ) { + int c0 = *beg++; + *out++ = char((c0 & 0xff) >> 2); + int c1 = *beg++; + *out++ = char((c0 & 0x03) << 4 | (c1 & 0xff) >> 4); + int c2 = *beg++; + *out++ = char((c1 & 0x0f) << 2 | (c2 & 0xff) >> 6); + *out++ = char((c2 & 0x3f)); + } + + switch ( end - beg ) { + case 2: { + int c0 = *beg++; + *out++ = char((c0 & 0xff) >> 2); + int c1 = *beg++; + *out++ = char((c0 & 0x03) << 4 | (c1 & 0xff) >> 4); + *out++ = char((c1 & 0x0f) << 2); + if ( pd_enforce == pd_use ) { + *out++ = EOF; + } + return out; + } + case 1: { + int c0 = *beg++; + *out++ = char((c0 & 0xff) >> 2); + *out++ = char((c0 & 0x03) << 4); + if ( pd_enforce == pd_use ) { + *out++ = EOF; + *out++ = EOF; + } + return out; + } + default: + return out; + } + } + + template < typename I, typename O > + static + O scatter( + I beg, + I end, + O out, + pd_use_t pd_use = pd_invalid ) + { + return scatter( beg, end, out, pd_use, typename std::iterator_traits<I>::iterator_category() ); + } + + + template < typename I, typename O > + static + O scatter_standard( + I beg, + I end, + O out ) + { + return scatter( beg, end, out, pd_enforce, typename std::iterator_traits<I>::iterator_category() ); + } + + // + // base<64>::gather -- collect 6-bit chunks into 8-bit characters + // + template < typename I, typename O > + static + O gather( + I beg, + I end, + O out, + pd_use_t pd_use = pd_invalid ) + { + while ( beg != end ) { + int c0 = gather_next( beg, end, pd_use, 0 ); + int c1 = gather_next( beg, end, pd_use, c0 ); + if ( c0 >= 0 and c1 >= 0 ) + *out++ = ( ((c0 < 0 ? 0 : c0 ) << 2 ) + | (c1 < 0 ? 0 : c1 ) >> 4 ); + int c2 = gather_next( beg, end, pd_use, c1 ); + if ( c1 >= 0 and c2 >= 0 ) + *out++ = ( ((c1 < 0 ? 0 : c1) & 0x0f) << 4 + | (c2 < 0 ? 0 : c2) >> 2 ); + int c3 = gather_next( beg, end, pd_use, c2 ); + if ( c2 >= 0 and c3 >= 0 ) + *out++ = ( ((c2 < 0 ? 0 : c2) & 0x03) << 6 + | (c3 < 0 ? 0 : c3)); + } + return out; + } + template < typename I, typename O > + static + O gather_standard( + I beg, + I end, + O out ) + { + return gather( beg, end, out, pd_enforce ); + } + }; // ezpwd::serialize::base<64> + + // + // ezpwd::serialize::base64... + // + // Shortcut typedefs for the standard base-64 codecs. + // + typedef base< 64, ezpwd< 64 >> + base64; + typedef base< 64, standard< 64 >> + base64_standard; + typedef base< 64, standard_url< 64 >> + base64_standard_url; + typedef base< 64, uuencode< 64 >> + base64_uuencode; + } // namespace ezpwd::serialize + +} // namespace ezpwd + +#endif // _EZPWD_SERIALIZE diff --git a/op25/gr-op25_repeater/lib/ezpwd/serialize_definitions b/op25/gr-op25_repeater/lib/ezpwd/serialize_definitions new file mode 100644 index 0000000..00b3fb7 --- /dev/null +++ b/op25/gr-op25_repeater/lib/ezpwd/serialize_definitions @@ -0,0 +1,57 @@ + +// +// The encoder/decoder tables for all ezpwd::serialize::... base<N> codecs +// +// Must be included in exactly one C++ compilation unit. +// + +#ifndef _EZPWD_SERIALIZE_DEFINITIONS +#define _EZPWD_SERIALIZE_DEFINITIONS + +#include "serialize" + +// +// base<16> tables for RFC4864 standard +// +const constexpr std::array<char,16> + ezpwd::serialize::hex<16>::encoder; +const constexpr std::array<char,127> + ezpwd::serialize::hex<16>::decoder; + +// +// base<32> tables for RFC4864 standard, and the Hex32, EZPWD and Crockford codecs +// +const constexpr std::array<char,32> + ezpwd::serialize::hex<32>::encoder; +const constexpr std::array<char,127> + ezpwd::serialize::hex<32>::decoder; +const constexpr std::array<char,32> + ezpwd::serialize::standard<32>::encoder; +const constexpr std::array<char,127> + ezpwd::serialize::standard<32>::decoder; +const constexpr std::array<char,32> + ezpwd::serialize::ezpwd<32>::encoder; +const constexpr std::array<char,127> + ezpwd::serialize::ezpwd<32>::decoder; +const constexpr std::array<char,32> + ezpwd::serialize::crockford<32>::encoder; +const constexpr std::array<char,127> + ezpwd::serialize::crockford<32>::decoder; + +// +// base<64> tables for RFC4864 standard (regular and url), and the EZPWD codecs +// +const constexpr std::array<char,64> + ezpwd::serialize::standard<64>::encoder; +const constexpr std::array<char,127> + ezpwd::serialize::standard<64>::decoder; +const constexpr std::array<char,64> + ezpwd::serialize::standard_url<64>::encoder; +const constexpr std::array<char,127> + ezpwd::serialize::standard_url<64>::decoder; +const constexpr std::array<char,64> + ezpwd::serialize::ezpwd<64>::encoder; +const constexpr std::array<char,127> + ezpwd::serialize::ezpwd<64>::decoder; + +#endif // _EZPWD_SERIALIZE_DEFINITIONS diff --git a/op25/gr-op25_repeater/lib/ezpwd/timeofday b/op25/gr-op25_repeater/lib/ezpwd/timeofday new file mode 100644 index 0000000..3016138 --- /dev/null +++ b/op25/gr-op25_repeater/lib/ezpwd/timeofday @@ -0,0 +1,73 @@ +#ifndef _EZPWD_TIMEOFDAY +#define _EZPWD_TIMEOFDAY + +#include <sys/time.h> + +// +// ezpwd::timeofday -- Return current time. +// ezpwd::epoch -- The UNIX epoch. +// ezpwd::seconds -- convert timeval to a real-valued seconds +// <timeval> < <timeval>-- less-than comparison on timevals +// <timeval> - <timeval>-- difference on timevals +// +namespace ezpwd { + inline + timeval timeofday() + { + timeval tv; + ::gettimeofday( &tv, NULL ); + return tv; + } + + timeval epoch() + { + timeval tv; + tv.tv_sec = 0; + tv.tv_usec = 0; + return tv; + } + + inline + double seconds( const timeval &rhs ) + { + return rhs.tv_usec / 1000000.0 + rhs.tv_sec; + } +} // namespace ezpwd + +inline +bool operator<( + const timeval &lhs, + const timeval &rhs ) +{ + return ( lhs.tv_sec < rhs.tv_sec + || (( lhs.tv_sec == rhs.tv_sec ) + && ( lhs.tv_usec < rhs.tv_usec ))); +} + +inline +timeval operator-( + const timeval &lhs, + timeval rhs ) // copy; adjusted... +{ + timeval result; + if ( lhs < rhs ) { + result = ezpwd::epoch(); + } else { + // See http://www.gnu.org/software/libc/manual/html_node/Elapsed-Time.html + if ( lhs.tv_usec < rhs.tv_usec ) { + int sec = ( rhs.tv_usec - lhs.tv_usec ) / 1000000 + 1; + rhs.tv_usec -= sec * 1000000; + rhs.tv_sec += sec; + } + if ( lhs.tv_usec - rhs.tv_usec > 1000000 ) { + int sec = ( lhs.tv_usec - rhs.tv_usec ) / 1000000; + rhs.tv_usec += sec * 1000000; + rhs.tv_sec -= sec; + } + result.tv_sec = lhs.tv_sec - rhs.tv_sec; + result.tv_usec = lhs.tv_usec - rhs.tv_usec; + } + return result; +} + +#endif // _EZPWD_TIMEOFDAY diff --git a/op25/gr-op25_repeater/lib/frame_assembler_impl.cc b/op25/gr-op25_repeater/lib/frame_assembler_impl.cc new file mode 100644 index 0000000..8f0f3ca --- /dev/null +++ b/op25/gr-op25_repeater/lib/frame_assembler_impl.cc @@ -0,0 +1,107 @@ +/* -*- c++ -*- */ +/* + * Copyright 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017 Max H. Parke KA1RBI + * + * This 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 3, or (at your option) + * any later version. + * + * This software 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 software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include <gnuradio/io_signature.h> +#include "frame_assembler_impl.h" + +#include <errno.h> +#include <stdio.h> +#include <stdlib.h> +#include <stdint.h> +#include <string.h> +#include <errno.h> +#include <vector> +#include <sys/time.h> + +namespace gr { + namespace op25_repeater { + + void frame_assembler_impl::set_xormask(const char*p) { + } + + void frame_assembler_impl::set_nac(int nac) { + } + + void frame_assembler_impl::set_slotid(int slotid) { + } + + void frame_assembler_impl::insert_whitelist(int grpaddr) { + d_sync.insert_whitelist(grpaddr); + } + void frame_assembler_impl::insert_blacklist(int grpaddr) { + d_sync.insert_blacklist(grpaddr); + } + + frame_assembler::sptr + frame_assembler::make(const char* options, int debug, gr::msg_queue::sptr queue, int msgq_id) + { + return gnuradio::get_initial_sptr + (new frame_assembler_impl(options, debug, queue, msgq_id)); + } + + /* + * The private constructor + */ + + /* + * Our virtual destructor. + */ + frame_assembler_impl::~frame_assembler_impl() + { + } + +static const int MIN_IN = 1; // mininum number of input streams +static const int MAX_IN = 1; // maximum number of input streams + +/* + * The private constructor + */ + frame_assembler_impl::frame_assembler_impl(const char* options, int debug, gr::msg_queue::sptr queue, int msgq_id) + : gr::block("frame_assembler", + gr::io_signature::make (MIN_IN, MAX_IN, sizeof (char)), + gr::io_signature::make (0, 0, 0)), + d_msg_queue(queue), + d_sync(options, debug, queue, msgq_id), + d_msgq_id(msgq_id) +{ +} + +int +frame_assembler_impl::general_work (int noutput_items, + gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items) +{ + + const uint8_t *in = (const uint8_t *) input_items[0]; + + for (int i=0; i<ninput_items[0]; i++) + d_sync.rx_sym(in[i]); + consume_each(ninput_items[0]); + // Tell runtime system how many output items we produced. + return 0; +} + + } /* namespace op25_repeater */ +} /* namespace gr */ diff --git a/op25/gr-op25_repeater/lib/frame_assembler_impl.h b/op25/gr-op25_repeater/lib/frame_assembler_impl.h new file mode 100644 index 0000000..6080f60 --- /dev/null +++ b/op25/gr-op25_repeater/lib/frame_assembler_impl.h @@ -0,0 +1,74 @@ +/* -*- c++ -*- */ +/* + * Copyright 2009, 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017 Max H. Parke KA1RBI + * + * This 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 3, or (at your option) + * any later version. + * + * This software 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 software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + +#ifndef INCLUDED_OP25_REPEATER_FRAME_ASSEMBLER_IMPL_H +#define INCLUDED_OP25_REPEATER_FRAME_ASSEMBLER_IMPL_H + +#include <op25_repeater/frame_assembler.h> + +#include <gnuradio/msg_queue.h> +#include <sys/socket.h> +#include <sys/time.h> +#include <netinet/in.h> +#include <arpa/inet.h> +#include <deque> + +#include "rx_sync.h" + +typedef std::deque<uint8_t> dibit_queue; + +namespace gr { + namespace op25_repeater { + + class frame_assembler_impl : public frame_assembler + { + private: + int d_debug; + gr::msg_queue::sptr d_msg_queue; + rx_sync d_sync; + int d_msgq_id; + + // internal functions + + void queue_msg(int duid); + void set_xormask(const char*p) ; + void set_nac(int nac) ; + void set_slotid(int slotid) ; + void insert_whitelist(int grpaddr); + void insert_blacklist(int grpaddr); + + public: + + public: + frame_assembler_impl(const char* options, int debug, gr::msg_queue::sptr queue, int msgq_id); + ~frame_assembler_impl(); + + // Where all the action really happens + + int general_work(int noutput_items, + gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items); + }; + + } // namespace op25_repeater +} // namespace gr + +#endif /* INCLUDED_OP25_REPEATER_FRAME_ASSEMBLER_IMPL_H */ diff --git a/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.cc b/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.cc index 766652d..dc0fe4a 100644 --- a/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.cc +++ b/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.cc @@ -35,8 +35,10 @@ #include <stdexcept> #include <cstdio> #include <string.h> +#include <sys/time.h> #include "p25_frame.h" +#include "p25p2_framer.h" #include "check_frame_sync.h" #define ENABLE_COSTAS_CQPSK_HACK 0 @@ -47,6 +49,10 @@ static const float M_TWOPI = 2 * M_PI; static const gr_complex PT_45 = gr_expj( M_PI / 4.0 ); static const int NUM_COMPLEX=100; +static const int FM_COUNT=500; // number of samples per measurement frame + +static const int N_FILES = 4; // number of filenames to cycle through + namespace gr { namespace op25_repeater { @@ -56,10 +62,103 @@ static inline std::complex<float> sgn(std::complex<float>c) { return c/abs(c); } +#define UPDATE_COUNT(c) if (d_event_type == c) { \ + d_event_count ++; \ + } else { \ + d_event_count = 1; \ + d_event_type = c; \ + } + +static int tuning_score(gr_complex buf[], int bufp, int bufl, int sync_l, int sps) { + int n_scan_samples = sps * 6; + float atan_prev = 0; + float score = 0; + int p = bufp - (n_scan_samples + 1); + if (p < 0) + p += bufl; + int n_tests = 0; + int n_plus = 0; + // a series of N consecutive samples treated as N-1 connected line segments + // is evaluated to find the angle, in radians, at the junction of each pair + // of lines. + // when a frequency tuning error is present the algebraic sum of the angles + // will contain a significant positive or negative bias. + for (int i = 0; i < n_scan_samples; i++) { + gr_complex sample1 = buf[ (p+i) % bufl ]; + gr_complex sample2 = buf[ (p+i+1) % bufl ]; + gr_complex diff = sample2 - sample1; + float atan = gr::fast_atan2f(diff.real(), diff.imag()); + if (i == 0) { + atan_prev = atan; + continue; + } + float atan_diff = atan - atan_prev; + if (atan_diff > M_PI) + atan_diff -= M_TWOPI; + if (atan_diff < -M_PI) + atan_diff += M_TWOPI; + atan_prev = atan; + score += atan_diff; + n_tests += 1; + if (atan_diff > 0) + n_plus += 1; + } + float f1 = (score > 0) ? n_plus : n_tests - n_plus; + float f2 = n_tests; + int pct = (int) (100*f1/f2 + 0.5); + if (score < 0) + pct *= -1; + return pct; +} + +static inline bool is_future(struct timeval*t) { + struct timeval current_t; + gettimeofday(¤t_t,0); + if (t->tv_sec > current_t.tv_sec) + return true; + else if (t->tv_sec < current_t.tv_sec) + return false; + else if (t->tv_usec > current_t.tv_usec) + return true; + else + return false; +} + +void gardner_costas_cc_impl::dump_samples(int error_amt) { + // TODO = disk I/O from inside a gr flow graph block work function (tsk tsk) + char tmp_filename[256]; + char filename[256]; + char line[64]; + FILE *fp1; + if (!d_enable_sync_plot) + return; + if (d_prev_sample == NULL) + return; + if (is_future(&d_next_sample_time)) + return; + gettimeofday(&d_next_sample_time,0); + d_next_sample_time.tv_sec += 1; + sprintf(filename, "sample-%ld-%d.dat", unique_id(), d_sample_file_id); + d_sample_file_id ++; + d_sample_file_id = d_sample_file_id % N_FILES; + sprintf(line, "%u %d %d %d\n", d_prev_sample_p, (d_is_tdma) ? 2 : 1, (int) (d_omega + 0.5), error_amt); + strcpy(tmp_filename, filename); + strcat(tmp_filename, ".tmp"); + fp1 = fopen(tmp_filename, "wb"); + if (!fp1) + return; + fwrite (line, 1, strlen(line), fp1); + fwrite (d_prev_sample, 1, d_n_prev_sample * sizeof(gr_complex), fp1); + fclose(fp1); + + rename(tmp_filename, filename); +} + uint8_t gardner_costas_cc_impl::slicer(float sym) { uint8_t dibit = 0; + int sps = (int) (d_omega + 0.5); static const float PI_4 = M_PI / 4.0; - static const float d_slice_levels[4] = {-2.0*PI_4, 0.0*PI_4, 2.0*PI_4, 4.0*PI_4}; + static const float d_slice_levels[4] = {(float)-2.0*PI_4, (float)0.0*PI_4, (float)2.0*PI_4, (float)4.0*PI_4}; if (d_slice_levels[3] < 0) { dibit = 1; if (d_slice_levels[3] <= sym && sym < d_slice_levels[0]) @@ -78,16 +177,90 @@ uint8_t gardner_costas_cc_impl::slicer(float sym) { if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ P25_FRAME_SYNC_MAGIC, 0, 48)) { // fprintf(stderr, "P25P1 Framing detect\n"); + UPDATE_COUNT(' ') + d_sync_valid_until = d_sample_count + 2400 * sps; + dump_samples(0); + } + else if(check_frame_sync((nid_accum & P25P2_FRAME_SYNC_MASK) ^ P25P2_FRAME_SYNC_MAGIC, 0, 40)) { +// fprintf(stderr, "P25P2 Framing detect\n"); + UPDATE_COUNT(' ') + d_sync_valid_until = d_sample_count + 2400 * sps; + dump_samples(0); + } + if (d_is_tdma) { + if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0x000104015155LL, 0, 40)) { + fprintf(stderr, "TDMA: channel %d tuning error -1200\n", -1); + UPDATE_COUNT('-') + d_sync_valid_until = d_sample_count + 2400 * sps; + dump_samples(-1200); } + else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xfefbfeaeaaLL, 0, 40)) { + fprintf(stderr, "TDMA: channel %d tuning error +1200\n", -1); + UPDATE_COUNT('+') + d_sync_valid_until = d_sample_count + 2400 * sps; + dump_samples(1200); + } + else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xa8a2a80800LL, 0, 40)) { + int score = tuning_score(d_prev_sample, d_prev_sample_p, d_n_prev_sample, 20, sps); + int error = 24; + if (score == -100) { + error = 2400; + UPDATE_COUNT('>') + fprintf(stderr, "TDMA: channel %d tuning error %d\n", -1, error); + } else if (score == 100) { + error = -2400; + UPDATE_COUNT('<') + fprintf(stderr, "TDMA: channel %d tuning error %d\n", -1, error); + } else { + fprintf(stderr, "TDMA: channel %d tuning error +/-2400, confidence %d\n", -1, score); + } + d_sync_valid_until = d_sample_count + 2400 * sps; + dump_samples(error); + } + } else { if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0x001050551155LL, 0, 48)) { - fprintf(stderr, "tuning error -1200\n"); +// fprintf(stderr, "tuning error -1200\n"); + UPDATE_COUNT('-') + d_sync_valid_until = d_sample_count + 2400 * sps; + dump_samples(-1200); } - if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xFFEFAFAAEEAALL, 0, 48)) { - fprintf(stderr, "tuning error +1200\n"); + else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xFFEFAFAAEEAALL, 0, 48)) { +// fprintf(stderr, "tuning error +1200\n"); + UPDATE_COUNT('+') + d_sync_valid_until = d_sample_count + 2400 * sps; + dump_samples(1200); } - if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xAA8A0A008800LL, 0, 48)) { - fprintf(stderr, "tuning error +/- 2400\n"); + else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xAA8A0A008800LL, 0, 48)) { + int score = tuning_score(d_prev_sample, d_prev_sample_p, d_n_prev_sample, 24, sps); + int error = 24; + if (score == -100) { + error = 2400; + UPDATE_COUNT('>') + fprintf(stderr, "channel %d block id %ld tuning error %d\n", -1, (long)unique_id(), error); + } else if (score == 100) { + error = -2400; + UPDATE_COUNT('<') + fprintf(stderr, "channel %d block id %ld tuning error %d\n", -1, (long)unique_id(), error); + } else { + fprintf(stderr, "channel %d block id %ld tuning error +/-2400, confidence %d\n", -1, (long)unique_id(), score); + } + d_sync_valid_until = d_sample_count + 2400 * sps; + dump_samples(error); } + } + if (d_event_type == ' ' || d_event_count < 5) { + d_update_request = 0; + } else { + if (d_event_type == '+' && d_fm > 0) + d_update_request = -1; + else if (d_event_type == '-' && d_fm < 0) + d_update_request = 1; + else if (d_event_type == '<') + d_update_request = -2; + else if (d_event_type == '>') + d_update_request = 2; + else d_update_request = 0; + } return dibit; } @@ -117,11 +290,20 @@ uint8_t gardner_costas_cc_impl::slicer(float sym) { d_alpha(alpha), d_beta(beta), d_interp_counter(0), d_theta(M_PI / 4.0), d_phase(0), d_freq(0), d_max_freq(max_freq), - nid_accum(0) + nid_accum(0), d_prev(0), + d_event_count(0), d_event_type(' '), + d_symbol_seq(samples_per_symbol * 4800), + d_update_request(0), + d_fm(0), d_fm_accum(0), d_fm_count(0), d_muted(false), d_is_tdma(false), + d_enable_sync_plot(false), + d_prev_sample(NULL), d_n_prev_sample(0), d_prev_sample_p(0), + d_sample_file_id(0), + d_sample_count(0), d_sync_valid_until(0) { set_omega(samples_per_symbol); set_relative_rate (1.0 / d_omega); set_history(d_twice_sps); // ensure extra input is available + gettimeofday(&d_next_sample_time, 0); } /* @@ -129,6 +311,10 @@ uint8_t gardner_costas_cc_impl::slicer(float sym) { */ gardner_costas_cc_impl::~gardner_costas_cc_impl() { + if (d_prev_sample != NULL) { + free(d_prev_sample); + d_prev_sample = NULL; + } delete [] d_dl; delete d_interp; } @@ -146,6 +332,17 @@ void gardner_costas_cc_impl::set_omega (float omega) { memset(d_dl, 0, NUM_COMPLEX * sizeof(gr_complex)); } +float gardner_costas_cc_impl::get_freq_error (void) { + if (!recent_sync()) + return 0.0; + return (d_freq); +} + +int gardner_costas_cc_impl::get_error_band (void) { + if (!recent_sync()) + return 0; + return (d_update_request); +} void gardner_costas_cc_impl::forecast(int noutput_items, gr_vector_int &ninput_items_required) @@ -190,21 +387,20 @@ gardner_costas_cc_impl::phase_error_tracking(gr_complex sample) phase_error = phase_error_detector_qpsk(sample); d_freq += d_beta*phase_error*abs(sample); // adjust frequency based on error - d_phase += d_freq + d_alpha*phase_error*abs(sample); // adjust phase based on error + d_phase += d_alpha*phase_error*abs(sample); // adjust phase based on error - // Make sure we stay within +-2pi - while(d_phase > M_TWOPI) + // Make sure we stay within +-pi + while(d_phase > M_PI) d_phase -= M_TWOPI; - while(d_phase < -M_TWOPI) + while(d_phase < -M_PI) d_phase += M_TWOPI; // Limit the frequency range d_freq = gr::branchless_clip(d_freq, d_max_freq); #if VERBOSE_COSTAS - printf("cl: phase_error: %f phase: %f freq: %f sample: %f+j%f constellation: %f+j%f\n", - phase_error, d_phase, d_freq, sample.real(), sample.imag(), - d_constellation[d_current_const_point].real(), d_constellation[d_current_const_point].imag()); + fprintf(stderr, "COSTAS\t%f\t%f\t%f\t%f+j%f\n", + phase_error, d_phase, d_freq, sample.real(), sample.imag()); #endif } @@ -219,6 +415,14 @@ gardner_costas_cc_impl::general_work (int noutput_items, int i=0, o=0; gr_complex symbol, sample, nco; + gr_complex interp_samp, interp_samp_mid, diffdec; + float error_real, error_imag, symbol_error; + + if (d_prev_sample == NULL) { + d_n_prev_sample = (int) (d_omega + 0.5); // sps + d_n_prev_sample *= (d_is_tdma) ? 32 : 25; // enough for p25p1 or p25p2 sync + d_prev_sample = (gr_complex *) calloc(d_n_prev_sample, sizeof(gr_complex)); + } while((o < noutput_items) && (i < ninput_items[0])) { while((d_mu > 1.0) && (i < ninput_items[0])) { @@ -226,24 +430,43 @@ gardner_costas_cc_impl::general_work (int noutput_items, d_phase += d_freq; // Keep phase clamped and not walk to infinity - while(d_phase > M_TWOPI) + while(d_phase > M_PI) d_phase -= M_TWOPI; - while(d_phase < -M_TWOPI) + while(d_phase < -M_PI) d_phase += M_TWOPI; nco = gr_expj(d_phase+d_theta); // get the NCO value for derotating the curr symbol = in[i]; sample = nco*symbol; // get the downconverted symbol + if (d_enable_sync_plot && d_prev_sample != NULL) { + d_prev_sample[d_prev_sample_p] = sample; + d_prev_sample_p ++; + d_prev_sample_p %= d_n_prev_sample; + } + d_dl[d_dl_index] = sample; d_dl[d_dl_index + d_twice_sps] = sample; d_dl_index ++; d_dl_index = d_dl_index % d_twice_sps; i++; + d_sample_count++; + gr_complex df = symbol * conj(d_prev); + float fmd = atan2f(df.imag(), df.real()); + d_fm_accum += fmd; + d_fm_count ++; + if (d_fm_count % FM_COUNT == 0) { + d_fm = d_fm_accum / FM_COUNT; + d_fm_accum = 0; + } + d_prev = symbol; } if(i < ninput_items[0]) { + // to mitigate tracking drift in the event of no input signal + // skip tracking on muted channel + if (!d_muted) { float half_omega = d_omega / 2.0; int half_sps = (int) floorf(half_omega); float half_mu = d_mu + half_omega - (float) half_sps; @@ -255,18 +478,19 @@ gardner_costas_cc_impl::general_work (int noutput_items, // half_mu the fractional part, of the halfway mark. // locate two points, separated by half of one symbol time // interp_samp is (we hope) at the optimum sampling point - gr_complex interp_samp_mid = d_interp->interpolate(&d_dl[ d_dl_index ], d_mu); - gr_complex interp_samp = d_interp->interpolate(&d_dl[ d_dl_index + half_sps], half_mu); - - float error_real = (d_last_sample.real() - interp_samp.real()) * interp_samp_mid.real(); - float error_imag = (d_last_sample.imag() - interp_samp.imag()) * interp_samp_mid.imag(); - gr_complex diffdec = interp_samp * conj(d_last_sample); - (void)slicer(std::arg(diffdec)); + interp_samp_mid = d_interp->interpolate(&d_dl[ d_dl_index ], d_mu); + interp_samp = d_interp->interpolate(&d_dl[ d_dl_index + half_sps], half_mu); + + error_real = (d_last_sample.real() - interp_samp.real()) * interp_samp_mid.real(); + error_imag = (d_last_sample.imag() - interp_samp.imag()) * interp_samp_mid.imag(); + diffdec = interp_samp * conj(d_last_sample); + if (!d_muted) // if muted, assume channel idle (suspend tuning error checks) + (void)slicer(std::arg(diffdec)); d_last_sample = interp_samp; // save for next time #if 1 - float symbol_error = error_real + error_imag; // Gardner loop error + symbol_error = error_real + error_imag; // Gardner loop error #else - float symbol_error = ((sgn(interp_samp) - sgn(d_last_sample)) * conj(interp_samp_mid)).real(); + symbol_error = ((sgn(interp_samp) - sgn(d_last_sample)) * conj(interp_samp_mid)).real(); #endif if (std::isnan(symbol_error)) symbol_error = 0.0; if (symbol_error < -1.0) symbol_error = -1.0; @@ -275,13 +499,21 @@ gardner_costas_cc_impl::general_work (int noutput_items, d_omega = d_omega + d_gain_omega * symbol_error * abs(interp_samp); // update omega based on loop error d_omega = d_omega_mid + gr::branchless_clip(d_omega-d_omega_mid, d_omega_rel); // make sure we don't walk away #if VERBOSE_GARDNER - printf("%f\t%f\t%f\t%f\t%f\n", symbol_error, d_mu, d_omega, error_real, error_imag); + fprintf(stderr, "%f\t%f\t%f\t%f\t%f\n", symbol_error, d_mu, d_omega, error_real, error_imag); #endif + } else { + symbol_error = 0; + } /* end of if (!d_muted) */ d_mu += d_omega + d_gain_mu * symbol_error; // update mu based on loop error + if (!d_muted) { phase_error_tracking(diffdec * PT_45); - - out[o++] = interp_samp; + } /* end of if (!d_muted) */ + + if (d_muted) + out[o++] = 0.0; + else + out[o++] = interp_samp; } } diff --git a/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.h b/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.h index 7fc8eb7..aa9e8b7 100644 --- a/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.h +++ b/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.h @@ -66,7 +66,19 @@ namespace gr { void set_verbose (bool verbose) { d_verbose = verbose; } //! Sets value of omega and its min and max values - void set_omega (float omega); + inline void set_omega (float omega); + inline float get_freq_error(void); + inline int get_error_band(void); + inline void set_muted(bool v) { + if (v == false && d_muted == true) { + d_event_count = 0; // mute state change from muted to unmuted + } + d_muted = v; + } + inline bool is_muted(void) { return d_muted; } + inline void set_tdma(bool v) { d_is_tdma = v; } + inline bool is_tdma(void) { return d_is_tdma; } + inline void enable_sync_plot(bool v) { d_enable_sync_plot = v; } protected: bool input_sample0(gr_complex, gr_complex& outp); @@ -100,8 +112,32 @@ protected: uint64_t nid_accum; + gr_complex d_prev; + int d_event_count; + char d_event_type; + int d_symbol_seq; + int d_update_request; + float d_fm; + float d_fm_accum; + int d_fm_count; + bool d_muted; + bool d_is_tdma; + bool d_enable_sync_plot; + + gr_complex *d_prev_sample; + unsigned int d_n_prev_sample; + unsigned int d_prev_sample_p; + + struct timeval d_next_sample_time; + int d_sample_file_id; + + unsigned int d_sample_count; + unsigned int d_sync_valid_until; + float phase_error_detector_qpsk(gr_complex sample); void phase_error_tracking(gr_complex sample); + void dump_samples(int); + bool recent_sync(void) { return d_sample_count <= d_sync_valid_until; } }; } // namespace op25_repeater diff --git a/op25/gr-op25_repeater/lib/log_ts.h b/op25/gr-op25_repeater/lib/log_ts.h new file mode 100644 index 0000000..fbd6bb3 --- /dev/null +++ b/op25/gr-op25_repeater/lib/log_ts.h @@ -0,0 +1,44 @@ +/* -*- c++ -*- */ +/* + * Copyright 2018 Graham J. Norbury + * + * This 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 3, or (at your option) + * any later version. + * + * This software 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 software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + +#ifndef INCLUDED_LOG_TS_H +#define INCLUDED_LOG_TS_H + +#include <sys/time.h> + +class log_ts +{ +private: + struct timeval curr_time; + char log_ts[20]; + +public: + inline const char* get() + { + if (gettimeofday(&curr_time, 0) == 0) + sprintf(log_ts, "%010lu.%06lu", curr_time.tv_sec, curr_time.tv_usec); + else + log_ts[0] = 0; + + return log_ts; + } +}; + +#endif // INCLUDED_LOG_TS_H diff --git a/op25/gr-op25_repeater/lib/mbelib.c b/op25/gr-op25_repeater/lib/mbelib.c index a9ed3bf..9581d5b 100644 --- a/op25/gr-op25_repeater/lib/mbelib.c +++ b/op25/gr-op25_repeater/lib/mbelib.c @@ -19,6 +19,18 @@ #include "mbelib.h" #include "mbelib_const.h" +#ifndef M_PI +# define M_PI 3.14159265358979323846 /* pi */ +#endif + +#ifndef M_E +# define M_E 2.7182818284590452354 /* e */ +#endif + +#ifndef M_SQRT2 +# define M_SQRT2 1.41421356237309504880 /* sqrt(2) */ +#endif + void mbe_printVersion (char *str) { diff --git a/op25/gr-op25_repeater/lib/nxdn.cc b/op25/gr-op25_repeater/lib/nxdn.cc new file mode 100644 index 0000000..2e4a85d --- /dev/null +++ b/op25/gr-op25_repeater/lib/nxdn.cc @@ -0,0 +1,354 @@ +/* -*- c++ -*- */ +/* + * NXDN Encoder/Decoder (C) Copyright 2019 Max H. Parke KA1RBI + * + * This file is part of OP25 + * + * This 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 3, or (at your option) + * any later version. + * + * This software 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 software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + +#include <stdio.h> +#include <stdint.h> +#include <string.h> +#include <assert.h> +#include "bit_utils.h" + +#include "nxdn.h" +#include "nxdn_const.h" + +static const uint8_t scramble_t[] = { + 2, 5, 6, 7, 10, 12, 14, 16, 17, 22, 23, 25, 26, 27, 28, 30, 33, 34, 36, 37, 38, 41, 45, 47, + 52, 54, 56, 57, 59, 62, 63, 64, 65, 66, 67, 69, 70, 73, 76, 79, 81, 82, 84, 85, 86, 87, 88, + 89, 92, 95, 96, 98, 100, 103, 104, 107, 108, 116, 117, 121, 122, 125, 127, 131, 132, 134, + 137, 139, 140, 141, 142, 143, 144, 145, 147, 151, 153, 154, 158, 159, 160, 162, 164, 165, + 168, 170, 171, 174, 175, 176, 177, 181}; + +static const int PARITY[] = {0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1}; + +static inline uint16_t crc16(const uint8_t buf[], int len, uint32_t crc) { + uint32_t poly = (1<<12) + (1<<5) + (1<<0); + for(int i=0; i<len; i++) { + uint8_t bit = buf[i] & 1; + crc = ((crc << 1) | bit) & 0x1ffff; + if (crc & 0x10000) + crc = (crc & 0xffff) ^ poly; + } + crc = crc ^ 0xffff; + return crc & 0xffff; +} + +static uint16_t crc15(const uint8_t buf[], int len) { + uint8_t s[15]; + uint8_t a; + for (int i=0;i<15;i++) + s[i] = 1; + for (int i=0;i<len;i++) { + a = buf[i] ^ s[0]; + s[0] = a ^ s[1]; + s[1] = s[2]; + s[2] = s[3]; + s[3] = a ^ s[4]; + s[4] = a ^ s[5]; + s[5] = s[6]; + s[6] = s[7]; + s[7] = a ^ s[8]; + s[8] = a ^ s[9]; + s[9] = s[10]; + s[10] = s[11]; + s[11] = s[12]; + s[12] = a ^ s[13]; + s[13] = s[14]; + s[14] = a; + } + return load_i(s, 15); +} + +static uint16_t crc16(const uint8_t buf[], int len) { + int crc = 0xc3ee; + int poly = (1<<12) + (1<<5) + 1; + for (int i=0;i<len;i++) { + crc = ((crc << 1) | buf[i]) & 0x1ffff; + if(crc & 0x10000) + crc = (crc & 0xffff) ^ poly; + } + crc = crc ^ 0xffff; + return crc & 0xffff; +} + +static uint8_t crc6(const uint8_t buf[], int len) { + uint8_t s[6]; + uint8_t a; + for (int i=0;i<6;i++) + s[i] = 1; + for (int i=0;i<len;i++) { + a = buf[i] ^ s[0]; + s[0] = a ^ s[1]; + s[1] = s[2]; + s[2] = s[3]; + s[3] = a ^ s[4]; + s[4] = a ^ s[5]; + s[5] = a; + } + return load_i(s, 6); +} + +static uint16_t crc12(const uint8_t buf[], int len) { + uint8_t s[12]; + uint8_t a; + for (int i=0;i<12;i++) + s[i] = 1; + for (int i=0;i<len;i++) { + a = buf[i] ^ s[0]; + s[0] = a ^ s[1]; + s[1] = s[2]; + s[2] = s[3]; + s[3] = s[4]; + s[4] = s[5]; + s[5] = s[6]; + s[6] = s[7]; + s[7] = s[8]; + s[8] = a ^ s[9]; + s[9] = a ^ s[10]; + s[10] = a ^ s[11]; + s[11] = a; + } + return load_i(s, 12); +} + +// trellis_1_2 encode: source is in bits, result in bits +static inline void trellis_encode(uint8_t result[], const uint8_t source[], int result_len, int reg) +{ + for (int i=0; i<result_len; i+=2) { + reg = (reg << 1) | source[i>>1]; + result[i] = PARITY[reg & 0x19]; + result[i+1] = PARITY[reg & 0x17]; + } +} + +// simplified trellis 2:1 decode; source and result in bits +// assumes that encoding was done with NTEST trailing zero bits +// result_len should be set to the actual number of data bits +// in the original unencoded message (excl. these trailing bits) +static inline void trellis_decode(uint8_t result[], const uint8_t source[], int result_len) +{ + int reg = 0; + int min_d; + int min_bt; + static const int NTEST = 4; + static const int NTESTC = 1 << NTEST; + uint8_t bt[NTEST]; + uint8_t tt[NTEST*2]; + int dstats[4]; + int sum; + for (int p=0; p < 4; p++) + dstats[p] = 0; + for (int p=0; p < result_len; p++) { + for (int i=0; i<NTESTC; i++) { + bt[0] = (i&8)>>3; + bt[1] = (i&4)>>2; + bt[2] = (i&2)>>1; + bt[3] = (i&1); + trellis_encode(tt, bt, NTEST*2, reg); + sum=0; + for (int j=0; j<NTEST*2; j++) { + sum += tt[j] ^ source[p*2+j]; + } + if (i == 0 || sum < min_d) { + min_d = sum; + min_bt = bt[0]; + } + } + result[p] = min_bt; + reg = (reg << 1) | min_bt; + dstats[(min_d > 3) ? 3 : min_d] += 1; + } + // fprintf (stderr, "stats\t%d %d %d %d\n", dstats[0], dstats[1], dstats[2], dstats[3]); +} + +void nxdn_descramble(uint8_t dibits[], int len) { + for (int i=0; i<sizeof(scramble_t); i++) { + if (scramble_t[i] >= len) + break; + dibits[scramble_t[i]] ^= 0x2; // invert sign of scrambled dibits + } +} + +static inline void cfill(uint8_t result[], const uint8_t src[], int len) { + for (int i=0; i<len; i++) + result[i] = load_i(src+i*8, 8); +} + +void nxdn_decode_cac(const uint8_t dibits[], int len, uint8_t answer[], int& answer_len) { + uint8_t cacbits[300]; + uint8_t deperm[300]; + uint8_t depunc[350]; + uint8_t decode[171]; + int id=0; + uint16_t crc; + + assert (len == 150); + if (answer_len < 19) { + answer_len = -1; + return; + } + dibits_to_bits(cacbits, dibits, 150); + for (int i=0; i<300; i++) { + deperm[PERM_12_25[i]] = cacbits[i]; + } + for (int i=0; i<25; i++) { + depunc[id++] = deperm[i*12]; + depunc[id++] = deperm[i*12+1]; + depunc[id++] = deperm[i*12+2]; + depunc[id++] = 0; + depunc[id++] = deperm[i*12+3]; + depunc[id++] = deperm[i*12+4]; + depunc[id++] = deperm[i*12+5]; + depunc[id++] = deperm[i*12+6]; + depunc[id++] = deperm[i*12+7]; + depunc[id++] = deperm[i*12+8]; + depunc[id++] = deperm[i*12+9]; + depunc[id++] = 0; + depunc[id++] = deperm[i*12+10]; + depunc[id++] = deperm[i*12+11]; + } + trellis_decode(decode, depunc, 171); + crc = crc16(decode, 171, 0xc3ee); + if (crc != 0) { + answer_len = -1; + return; // ignore msg if crc failed + } + // result length after crc and 3 zero bits removed = 152 = 19 bytes + cfill(answer, decode, 19); + answer_len = 19; /* return 19 bytes */ +} + +void nxdn_decode_facch(const uint8_t dibits[], int len, uint8_t answer[], int& answer_len) { + uint8_t bits[144]; + uint8_t deperm[144]; + uint8_t depunc[192]; + uint8_t trellis_buf[92]; + uint16_t crc; + uint16_t crc2; + int out; + char buf[128]; + assert (len == 72); + if (answer_len < 10) { + answer_len = -1; + return; + } + dibits_to_bits(bits, dibits, 72); + for (int i=0; i<144; i++) + deperm[PERM_16_9[i]] = bits[i]; + out = 0; + for (int i=0; i<144; i+=3) { + depunc[out++] = deperm[i+0]; + depunc[out++] = 0; + depunc[out++] = deperm[i+1]; + depunc[out++] = deperm[i+2]; + } + trellis_decode(trellis_buf, depunc, 92); + crc = crc12(trellis_buf, 92); + if (crc) { + answer_len = -1; + return; + } + cfill(answer, trellis_buf, 10); + answer_len = 10; +} + +void nxdn_decode_facch2_udch(const uint8_t dibits[], int len, uint8_t answer[], int& answer_len) { + uint8_t bits[348]; + uint8_t deperm[348]; + uint8_t depunc[406]; + uint8_t trellis_buf[199]; + int id=0; + uint16_t crc; + assert (len == 174); + if (answer_len < 23) { + answer_len = -1; + return; + } + dibits_to_bits(bits, dibits, 174); + for (int i=0; i<348; i++) { + deperm[PERM_12_29[i]] = bits[i]; + } + for (int i=0; i<29; i++) { + depunc[id++] = deperm[i*12]; + depunc[id++] = deperm[i*12+1]; + depunc[id++] = deperm[i*12+2]; + depunc[id++] = 0; + depunc[id++] = deperm[i*12+3]; + depunc[id++] = deperm[i*12+4]; + depunc[id++] = deperm[i*12+5]; + depunc[id++] = deperm[i*12+6]; + depunc[id++] = deperm[i*12+7]; + depunc[id++] = deperm[i*12+8]; + depunc[id++] = deperm[i*12+9]; + depunc[id++] = 0; + depunc[id++] = deperm[i*12+10]; + depunc[id++] = deperm[i*12+11]; + } + trellis_decode(trellis_buf, depunc, 199); + crc = crc15(trellis_buf, 199); + if (crc != 0) { + answer_len = -1; + return; // ignore msg if crc failed + } + // pack 184 bits in 23 bytes + cfill(answer, trellis_buf, 23); + answer_len = 23; +} + +void nxdn_decode_sacch(const uint8_t dibits[], int len, uint8_t answer[], int& answer_len) { + // global NEXT_S, SACCH + uint8_t bits[60]; + uint8_t deperm[60]; + uint8_t depunc[72]; + uint8_t trellis_buf[32]; + int o=0; + uint8_t crc; + + assert (len == 30); + if (answer_len < 26) { + answer_len = -1; + return; + } + dibits_to_bits(bits, dibits, 30); + for (int i=0; i<60; i++) + deperm[PERM_12_5[i]] = bits[i]; + for (int p=0; p<60; p+= 10) { + depunc[o++] = deperm[p+0]; + depunc[o++] = deperm[p+1]; + depunc[o++] = deperm[p+2]; + depunc[o++] = deperm[p+3]; + depunc[o++] = deperm[p+4]; + depunc[o++] = 0; + depunc[o++] = deperm[p+5]; + depunc[o++] = deperm[p+6]; + depunc[o++] = deperm[p+7]; + depunc[o++] = deperm[p+8]; + depunc[o++] = deperm[p+9]; + depunc[o++] = 0; + } + trellis_decode(trellis_buf, depunc, 32); + crc = crc6(trellis_buf, 32); + if (crc) { + answer_len = -1; + return; + } + memcpy(answer, trellis_buf, 26); + answer_len = 26; // answer is 26 bits, not packed +} diff --git a/op25/gr-op25_repeater/lib/nxdn.h b/op25/gr-op25_repeater/lib/nxdn.h new file mode 100644 index 0000000..47bdecd --- /dev/null +++ b/op25/gr-op25_repeater/lib/nxdn.h @@ -0,0 +1,31 @@ +// +// NXDN Encoder (C) Copyright 2019 Max H. Parke KA1RBI +// thx gr-ysf fr_vch_decoder_bb_impl.cc * Copyright 2015 Mathias Weyland * +// +// This file is part of OP25 +// +// OP25 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 3, or (at your option) +// any later version. +// +// OP25 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 OP25; see the file COPYING. If not, write to the Free +// Software Foundation, Inc., 51 Franklin Street, Boston, MA +// 02110-1301, USA. + +#ifndef INCLUDED_NXDN_H +#define INCLUDED_NXDN_H + +void nxdn_descramble(uint8_t dibits[], int len); +void nxdn_decode_cac(const uint8_t dibits[], int len, uint8_t answer[], int& answer_len); +void nxdn_decode_facch(const uint8_t dibits[], int len, uint8_t answer[], int& answer_len); +void nxdn_decode_facch2_udch(const uint8_t dibits[], int len, uint8_t answer[], int& answer_len); +void nxdn_decode_sacch(const uint8_t dibits[], int len, uint8_t answer[], int& answer_len); + +#endif /* INCLUDED_NXDN_H */ diff --git a/op25/gr-op25_repeater/lib/nxdn_const.h b/op25/gr-op25_repeater/lib/nxdn_const.h new file mode 100644 index 0000000..3452410 --- /dev/null +++ b/op25/gr-op25_repeater/lib/nxdn_const.h @@ -0,0 +1,98 @@ +// +// NXDN Encoder (C) Copyright 2019 Max H. Parke KA1RBI +// thx gr-ysf fr_vch_decoder_bb_impl.cc * Copyright 2015 Mathias Weyland * +// +// This file is part of OP25 +// +// OP25 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 3, or (at your option) +// any later version. +// +// OP25 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 OP25; see the file COPYING. If not, write to the Free +// Software Foundation, Inc., 51 Franklin Street, Boston, MA +// 02110-1301, USA. + +#ifndef INCLUDED_NXDN_CONST_H +#define INCLUDED_NXDN_CONST_H + +#include <stdint.h> + +/* postamble + frame sync (FS) */ +static const uint64_t NXDN_POSTFS_SYNC_MAGIC = 0x5775fdcdf59LL; +/* frame sync + scrambled rendition of LICH=0x6e (a halfrate voice 4V) */ +static const uint64_t NXDN_CONV_SYNC_MAGIC = 0xcdf59d5dfLL; +static const uint64_t NXDN_FS6E_SYNC_MAGIC = 0xcdf5975d7LL; +static const uint64_t NXDN_SYNC_MAGIC = 0xcdf59LL; + +static const uint8_t PERM_12_5[] = { + 0,12,24,36,48, + 1,13,25,37,49, + 2,14,26,38,50, + 3,15,27,39,51, + 4,16,28,40,52, + 5,17,29,41,53, + 6,18,30,42,54, + 7,19,31,43,55, + 8,20,32,44,56, + 9,21,33,45,57, + 10,22,34,46,58, + 11,23,35,47,59 +}; + +static const uint8_t PERM_16_9[] = { + 0, 16, 32, 48, 64, 80, 96, 112, 128, + 1, 17, 33, 49, 65, 81, 97, 113, 129, + 2, 18, 34, 50, 66, 82, 98, 114, 130, + 3, 19, 35, 51, 67, 83, 99, 115, 131, + 4, 20, 36, 52, 68, 84, 100, 116, 132, + 5, 21, 37, 53, 69, 85, 101, 117, 133, + 6, 22, 38, 54, 70, 86, 102, 118, 134, + 7, 23, 39, 55, 71, 87, 103, 119, 135, + 8, 24, 40, 56, 72, 88, 104, 120, 136, + 9, 25, 41, 57, 73, 89, 105, 121, 137, + 10, 26, 42, 58, 74, 90, 106, 122, 138, + 11, 27, 43, 59, 75, 91, 107, 123, 139, + 12, 28, 44, 60, 76, 92, 108, 124, 140, + 13, 29, 45, 61, 77, 93, 109, 125, 141, + 14, 30, 46, 62, 78, 94, 110, 126, 142, + 15, 31, 47, 63, 79, 95, 111, 127, 143 +}; + +static const uint16_t PERM_12_25[] = { + 0,12,24,36,48,60,72,84,96,108,120,132,144,156,168,180,192,204,216,228,240,252,264,276,288, + 1,13,25,37,49,61,73,85,97,109,121,133,145,157,169,181,193,205,217,229,241,253,265,277,289, + 2,14,26,38,50,62,74,86,98,110,122,134,146,158,170,182,194,206,218,230,242,254,266,278,290, + 3,15,27,39,51,63,75,87,99,111,123,135,147,159,171,183,195,207,219,231,243,255,267,279,291, + 4,16,28,40,52,64,76,88,100,112,124,136,148,160,172,184,196,208,220,232,244,256,268,280,292, + 5,17,29,41,53,65,77,89,101,113,125,137,149,161,173,185,197,209,221,233,245,257,269,281,293, + 6,18,30,42,54,66,78,90,102,114,126,138,150,162,174,186,198,210,222,234,246,258,270,282,294, + 7,19,31,43,55,67,79,91,103,115,127,139,151,163,175,187,199,211,223,235,247,259,271,283,295, + 8,20,32,44,56,68,80,92,104,116,128,140,152,164,176,188,200,212,224,236,248,260,272,284,296, + 9,21,33,45,57,69,81,93,105,117,129,141,153,165,177,189,201,213,225,237,249,261,273,285,297, + 10,22,34,46,58,70,82,94,106,118,130,142,154,166,178,190,202,214,226,238,250,262,274,286,298, + 11,23,35,47,59,71,83,95,107,119,131,143,155,167,179,191,203,215,227,239,251,263,275,287,299 +}; + +static const uint16_t PERM_12_29[] = { + 0,12,24,36,48,60,72,84,96,108,120,132,144,156,168,180,192,204,216,228,240,252,264,276,288,300,312,324,336, + 1,13,25,37,49,61,73,85,97,109,121,133,145,157,169,181,193,205,217,229,241,253,265,277,289,301,313,325,337, + 2,14,26,38,50,62,74,86,98,110,122,134,146,158,170,182,194,206,218,230,242,254,266,278,290,302,314,326,338, + 3,15,27,39,51,63,75,87,99,111,123,135,147,159,171,183,195,207,219,231,243,255,267,279,291,303,315,327,339, + 4,16,28,40,52,64,76,88,100,112,124,136,148,160,172,184,196,208,220,232,244,256,268,280,292,304,316,328,340, + 5,17,29,41,53,65,77,89,101,113,125,137,149,161,173,185,197,209,221,233,245,257,269,281,293,305,317,329,341, + 6,18,30,42,54,66,78,90,102,114,126,138,150,162,174,186,198,210,222,234,246,258,270,282,294,306,318,330,342, + 7,19,31,43,55,67,79,91,103,115,127,139,151,163,175,187,199,211,223,235,247,259,271,283,295,307,319,331,343, + 8,20,32,44,56,68,80,92,104,116,128,140,152,164,176,188,200,212,224,236,248,260,272,284,296,308,320,332,344, + 9,21,33,45,57,69,81,93,105,117,129,141,153,165,177,189,201,213,225,237,249,261,273,285,297,309,321,333,345, + 10,22,34,46,58,70,82,94,106,118,130,142,154,166,178,190,202,214,226,238,250,262,274,286,298,310,322,334,346, + 11,23,35,47,59,71,83,95,107,119,131,143,155,167,179,191,203,215,227,239,251,263,275,287,299,311,323,335,347 +}; + +#endif /* INCLUDED_NXDN_CONST_H */ diff --git a/op25/gr-op25_repeater/lib/nxdn_tx_sb_impl.cc b/op25/gr-op25_repeater/lib/nxdn_tx_sb_impl.cc new file mode 100644 index 0000000..b9b11ca --- /dev/null +++ b/op25/gr-op25_repeater/lib/nxdn_tx_sb_impl.cc @@ -0,0 +1,373 @@ +/* -*- c++ -*- */ +/* + * NXDN Encoder (C) Copyright 2017 Max H. Parke KA1RBI + * + * This file is part of OP25 + * + * This 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 3, or (at your option) + * any later version. + * + * This software 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 software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include <gnuradio/io_signature.h> +#include "nxdn.h" +#include "mbelib.h" +#include "p25p2_vf.h" +#include "nxdn_tx_sb_impl.h" +#include "nxdn_const.h" +#include "op25_imbe_frame.h" + +#include <vector> +#include <stdint.h> +#include <stdio.h> +#include <string.h> +#include <errno.h> +#include <sys/socket.h> +#include <netinet/in.h> +#include <arpa/inet.h> +#include <assert.h> + +static const uint8_t nxdn_fs[] = {3, 0, 3, 1, 3, 3, 1, 1, 2, 1}; + +#if 0 +static inline void print_result(char title[], const uint8_t r[], int len) { + uint8_t buf[256]; + for (int i=0; i<len; i++){ + buf[i] = r[i] + '0'; + } + buf[len] = 0; + printf("%s: %s\n", title, buf); +} +#endif + +static inline void store_i(int reg, uint8_t val[], int len) { + for (int i=0; i<len; i++){ + val[i] = (reg >> (len-1-i)) & 1; + } +} + +static inline void bits_to_dibits(uint8_t* dest, const uint8_t* src, int n_dibits) { + for (int i=0; i<n_dibits; i++) { + dest[i] = src[i*2] * 2 + src[i*2+1]; + } +} + +static inline void bool_to_dibits(uint8_t* dest, const std::vector<bool> src, int n_dibits) { + for (int i=0; i<n_dibits; i++) { + int l = src[i*2] ? 1 : 0; + int r = src[i*2+1] ? 1 : 0; + dest[i] = l * 2 + r; + } +} + +static inline int load_i(const uint8_t val[], int len) { + int acc = 0; + for (int i=0; i<len; i++){ + acc = (acc << 1) + (val[i] & 1); + } + return acc; +} + +// unpacks bytes into bits, len is length of result +static inline void unpack_bytes(uint8_t result[], const char src[], int len) { + static const int nbytes = len / 8; + int outp = 0; + for (int i=0; i < len; i++) { + result[i] = (src[i>>3] >> (7-(i%8))) & 1; + } +} + +static inline int crc6(const uint8_t * bits, int len) { + uint8_t s[6] = {1,1,1,1,1,1}; + for (int i=0; i<len; i++) { + int bit = bits[i]; + int a = bit ^ s[0]; + s[0] = a ^ s[1]; + s[1] = s[2]; + s[2] = s[3]; + s[3] = a ^ s[4]; + s[4] = a ^ s[5]; + s[5] = a; + } + return (load_i(s, 6)); +} + +static inline int crc12(uint8_t bits[], int len) { + uint8_t s[] = {1,1,1,1,1,1,1,1,1,1,1,1}; + for (int i=0; i<len; i++) { + int bit = bits[i]; + int a = bit ^ s[0]; + s[0] = a ^ s[1]; + s[1] = s[2]; + s[2] = s[3]; + s[3] = s[4]; + s[4] = s[5]; + s[5] = s[6]; + s[6] = s[7]; + s[7] = s[8]; + s[8] = a ^ s[9]; + s[9] = a ^ s[10]; + s[10] = a ^ s[11]; + s[11] = a; + } + return (load_i(s, 12)); +} + +static const uint8_t trellis_PC[] = {0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1}; +static inline void trellis_encode(const uint8_t src_bits[], uint8_t dest_bits[], const int dest_len) { + int reg = 0; + for (int i=0; i<dest_len/2; i++) { + int bit = src_bits[i]; + reg = (reg << 1) | bit; + dest_bits[i*2] = trellis_PC[reg & 0x19]; + dest_bits[i*2+1] = trellis_PC[reg & 0x17]; + } +} + +static inline uint16_t crc16(const uint8_t buf[], int len) { + uint32_t poly = (1<<12) + (1<<5) + (1<<0); + uint32_t crc = 0; + for(int i=0; i<len; i++) { + uint8_t bit = buf[i] & 1; + crc = ((crc << 1) | bit) & 0x1ffff; + if (crc & 0x10000) + crc = (crc & 0xffff) ^ poly; + } + crc = crc ^ 0xffff; + return crc & 0xffff; +} + +static inline void encode_sacch_chunk(const uint8_t src_bits[18], uint8_t dest_dibits[30], int structure, int ran) { + uint8_t buf1[60]; + uint8_t buf2[72]; + uint8_t str_field[8]; + int str; + + str = (structure & 3) << 6; + str |= ran & 0x3f; + store_i(str, str_field, 8); + + memcpy(buf1, str_field, 8); + memcpy(buf1+8, src_bits, 18); + + int crc = crc6(buf1, 26); + store_i(crc, buf1+26, 6); + store_i(0, buf1+32, 4); + trellis_encode(buf1, buf2, sizeof(buf2)); + for (int i=0, op=0; i<sizeof(buf2); i += 12) { + buf1[op++] = buf2[i+0]; + buf1[op++] = buf2[i+1]; + buf1[op++] = buf2[i+2]; + buf1[op++] = buf2[i+3]; + buf1[op++] = buf2[i+4]; + buf1[op++] = buf2[i+6]; + buf1[op++] = buf2[i+7]; + buf1[op++] = buf2[i+8]; + buf1[op++] = buf2[i+9]; + buf1[op++] = buf2[i+10]; + } + for (int i=0; i<60; i++) { + buf2[i] = buf1[PERM_12_5[i]]; + } + bits_to_dibits(dest_dibits, buf2, 30); +} + +static inline void encode_facch(const uint8_t src_bits[80], uint8_t dest_dibits[72]) { + uint8_t buf1[144]; + uint8_t buf2[192]; + memcpy(buf1, src_bits, 80); + int crc = crc12(buf1, 80); + store_i(crc, buf1+80, 12); + store_i(0, buf1+92, 4); + trellis_encode(buf1, buf2, sizeof(buf2)); + for (int i=0, op=0; i<sizeof(buf2); i += 4) { + buf1[op++] = buf2[i+0]; + buf1[op++] = buf2[i+2]; + buf1[op++] = buf2[i+3]; + } + for (int i=0; i<144; i++) { + buf2[i] = buf1[PERM_16_9[i]]; + } + bits_to_dibits(dest_dibits, buf2, 72); +} + +static inline void encode_lich(const int lich, uint8_t lich_dibits[8]) { + uint8_t b[8]; + store_i(lich & 0x7f, b, 7); + b[8] = (b[0] + b[1] + b[2] + b[3]) & 1; + for (int i=0; i<8; i++) + lich_dibits[i] = (b[i]) ? 3 : 1; +} + +namespace gr { + namespace op25_repeater { + + nxdn_tx_sb::sptr + nxdn_tx_sb::make(int verbose_flag, const char * config_file, bool nxdn96_mode) + { + return gnuradio::get_initial_sptr + (new nxdn_tx_sb_impl(verbose_flag, config_file, nxdn96_mode)); + } + +////////////////////////////////////////////////////////////////////////// + +static const int MIN_IN = 1; +static const int MAX_IN = 1; + +static const int MIN_OUT = 1; +static const int MAX_OUT = 1; + + /* + * The private constructor + */ + nxdn_tx_sb_impl::nxdn_tx_sb_impl(int verbose_flag, const char * config_file, bool nxdn96_mode) + : gr::block("nxdn_tx_sb", + gr::io_signature::make (MIN_IN, MAX_IN, sizeof(short)), + gr::io_signature::make (MIN_OUT, MAX_OUT, sizeof(char))), + d_verbose_flag(verbose_flag), + d_config_file(config_file), + d_nxdn96_mode(nxdn96_mode), + d_output_amount((nxdn96_mode) ? 384 : 192), + d_sacch_seq(0), + d_lich(0), + d_ran(0) + { + set_output_multiple(d_output_amount); + memset(d_acch, 0, sizeof(d_acch)); + config(); + } + + /* + * Our virtual destructor. + */ + nxdn_tx_sb_impl::~nxdn_tx_sb_impl() + { + } + +void +nxdn_tx_sb_impl::config() +{ + FILE * fp1 = fopen(d_config_file, "r"); + char line[256]; + char * cp; + unsigned int li[10]; + long int ran; + long int lich, lich2; + if (!fp1) { + fprintf(stderr, "nxdn_tx_sb_impl:config: failed to open %s\n", d_config_file); + return; + } + for (;;) { + cp = fgets(line, sizeof(line) - 2, fp1); + if (!cp) break; + if (line[0] == '#') continue; + if (memcmp(line, "ran=", 4) == 0) { + ran = strtol(line+4, 0, 0); + d_ran = ran; + } else if (memcmp(line, "lich=", 5) == 0) { + lich = strtol(line+5, 0, 0); + d_lich = lich; + } else if (memcmp(line, "lich2=", 6) == 0) { + lich2 = strtol(line+6, 0, 0); + d_lich2 = lich2; + } else if (memcmp(line, "acch=", 5) == 0) { + sscanf(&line[5], "%x %x %x %x %x %x %x %x %x %x", &li[0], &li[1], &li[2], &li[3], &li[4], &li[5], &li[6], &li[7], &li[8], &li[9]); + for (int i=0; i<10; i++) { + store_i(li[i], d_acch+i*8, 8); + } + } + } + encode_lich(d_lich, d_lich_x1); + encode_lich(d_lich2, d_lich_x2); + encode_facch(d_acch, d_facch1); + for (int i=0; i<4; i++) + encode_sacch_chunk(d_acch+18*i, d_sacch[i], 3-i, d_ran); + fclose(fp1); +} + +void +nxdn_tx_sb_impl::forecast(int nof_output_items, gr_vector_int &nof_input_items_reqd) +{ + // each 192-dibit output frame contains four voice code words=640 samples + // for nxdn96 we output 384 dibits per four voice code words + const size_t nof_inputs = nof_input_items_reqd.size(); + const int nof_vcw = nof_output_items / d_output_amount; + const int nof_samples_reqd = nof_vcw * 4 * 160; + std::fill(&nof_input_items_reqd[0], &nof_input_items_reqd[nof_inputs], nof_samples_reqd); +} + +int +nxdn_tx_sb_impl::general_work (int noutput_items, + gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items) +{ + + int nconsumed = 0; + int16_t *in; + in = (int16_t *) input_items[0]; + uint8_t *out = reinterpret_cast<uint8_t*>(output_items[0]); + int nframes=0; + int16_t frame_vector[8]; + voice_codeword cw(voice_codeword_sz); + uint8_t ambe_codeword[36]; // dibits + std::vector <bool> interleaved_buf(144); + + for (int n=0;n < (noutput_items/d_output_amount);n++) { + // need (at least) four voice codewords worth of samples + if (ninput_items[0] - nconsumed < 4*160) break; + memcpy(out, nxdn_fs, sizeof(nxdn_fs)); + memcpy(out+10, d_lich_x1, 8); + memcpy(out+18, d_sacch[d_sacch_seq++ % 4], 30); + // TODO: would be nice to multithread these + for (int vcw = 0; vcw < 4; vcw++) { + d_halfrate_encoder.encode(in, ambe_codeword); + memcpy(out+48+36*vcw, ambe_codeword, sizeof(ambe_codeword)); + in += 160; + nconsumed += 160; + } + nxdn_descramble(out+10, 182); + if (d_nxdn96_mode) { + memcpy(out+192, nxdn_fs, sizeof(nxdn_fs)); + memcpy(out+192+10, d_lich_x2, 8); + memcpy(out+192+18, d_sacch[d_sacch_seq++ % 4], 30); + memcpy(out+192+48, d_facch1, 72); + memcpy(out+192+120, d_facch1, 72); + nxdn_descramble(out+192+10, 182); + } + nframes += 1; + out += d_output_amount; + } + + // Tell runtime system how many input items we consumed on + // each input stream. + + if (nconsumed) + consume_each(nconsumed); + + // Tell runtime system how many output items we produced. + return (nframes * d_output_amount); +} + +void +nxdn_tx_sb_impl::set_gain_adjust(float gain_adjust) { + d_halfrate_encoder.set_gain_adjust(gain_adjust); +} + + } /* namespace op25_repeater */ +} /* namespace gr */ diff --git a/op25/gr-op25_repeater/lib/nxdn_tx_sb_impl.h b/op25/gr-op25_repeater/lib/nxdn_tx_sb_impl.h new file mode 100644 index 0000000..4d2590a --- /dev/null +++ b/op25/gr-op25_repeater/lib/nxdn_tx_sb_impl.h @@ -0,0 +1,80 @@ +/* -*- c++ -*- */ +/* + * NXDN Encoder (C) Copyright 2020 Max H. Parke KA1RBI + * + * This file is part of OP25 + * + * This 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 3, or (at your option) + * any later version. + * + * This software 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 software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + +#ifndef INCLUDED_OP25_REPEATER_NXDN_TX_SB_IMPL_H +#define INCLUDED_OP25_REPEATER_NXDN_TX_SB_IMPL_H + +#include <op25_repeater/nxdn_tx_sb.h> + +#include <sys/time.h> +#include <netinet/in.h> +#include <stdint.h> +#include <vector> +#include <deque> +#include <algorithm> + +#include "imbe_vocoder/imbe_vocoder.h" +#include "ambe_encoder.h" + +namespace gr { + namespace op25_repeater { + + class nxdn_tx_sb_impl : public nxdn_tx_sb + { + private: + void config(void); + + public: + nxdn_tx_sb_impl(int verbose_flag, const char * config_file, bool nxdn96_mode); + ~nxdn_tx_sb_impl(); + + void forecast (int noutput_items, gr_vector_int &ninput_items_required); + + int general_work(int noutput_items, + gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items); + void set_gain_adjust(float gain_adjust); + + private: + int d_verbose_flag; + int d_nxdn96_mode; + const char * d_config_file; + // imbe_vocoder d_fullrate_encoder; + ambe_encoder d_halfrate_encoder; + int d_output_amount; + int d_sacch_seq; + + uint8_t d_lich; + uint8_t d_lich2; + uint16_t d_ran; + uint8_t d_acch[80]; + uint8_t d_lich_x1[8]; + uint8_t d_lich_x2[8]; + uint8_t d_sacch[4][30]; + uint8_t d_facch1[72]; + }; + + } // namespace op25_repeater +} // namespace gr + +#endif /* INCLUDED_OP25_REPEATER_NXDN_TX_SB_IMPL_H */ diff --git a/op25/gr-op25_repeater/lib/op25_audio.cc b/op25/gr-op25_repeater/lib/op25_audio.cc new file mode 100644 index 0000000..ed5963c --- /dev/null +++ b/op25/gr-op25_repeater/lib/op25_audio.cc @@ -0,0 +1,247 @@ +/* -*- c++ -*- */ +/* + * Copyright 2017 Graham J Norbury, gnorbury@bondcar.com + * from op25_audio; rewrite Nov 2017 Copyright 2017 Max H. Parke KA1RBI + * + * This 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 3, or (at your option) + * any later version. + * + * This software 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 software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include <unistd.h> +#include <stdio.h> +#include <string.h> +#include <errno.h> +#include <stdint.h> +#include <sys/types.h> +#include <sys/stat.h> +#include <fcntl.h> +#include <netdb.h> + +#include "op25_audio.h" + +// convert hostname to ip address +static int hostname_to_ip(const char *hostname , char *ip) +{ + int sockfd; + struct addrinfo hints, *servinfo, *p; + struct sockaddr_in *h; + int rv; + + memset(&hints, 0, sizeof hints); + hints.ai_family = AF_UNSPEC; // use AF_INET6 to force IPv6 + hints.ai_socktype = SOCK_DGRAM; + + if ( (rv = getaddrinfo( hostname , NULL , &hints , &servinfo)) != 0) + { + fprintf(stderr, "op25_audio::hostname_to_ip() getaddrinfo: %s\n", gai_strerror(rv)); + return -1; + } + + // loop through all the results and connect to the first we can + for(p = servinfo; p != NULL; p = p->ai_next) + { + h = (struct sockaddr_in *) p->ai_addr; + if (h->sin_addr.s_addr != 0) + { + strcpy(ip , inet_ntoa( h->sin_addr ) ); + break; + } + } + + freeaddrinfo(servinfo); // all done with this structure + return 0; + +} + +// constructor +op25_audio::op25_audio(const char* udp_host, int port, int debug) : + d_udp_enabled(false), + d_debug(debug), + d_write_port(port), + d_audio_port(port), + d_write_sock(0), + d_file_enabled(false) +{ + char ip[20]; + if (hostname_to_ip(udp_host, ip) == 0) + { + strncpy(d_udp_host, ip, sizeof(d_udp_host)); + d_udp_host[sizeof(d_udp_host)-1] = 0; + if ( port ) + open_socket(); + } +} + +// destructor +op25_audio::~op25_audio() +{ + if (d_file_enabled) + close(d_write_sock); + close_socket(); +} + +// constructor +op25_audio::op25_audio(const char* destination, int debug) : + d_udp_enabled(false), + d_debug(debug), + d_write_port(0), + d_audio_port(0), + d_write_sock(0), + d_file_enabled(false) +{ + static const int DEFAULT_UDP_PORT = 23456; + static const char P_UDP[] = "udp://"; + static const char P_FILE[] = "file://"; + int port = DEFAULT_UDP_PORT; + + if (memcmp(destination, P_UDP, strlen(P_UDP)) == 0) { + char ip[20]; + char host[64]; + const char * p1 = destination+strlen(P_UDP); + strncpy(host, p1, sizeof(host)); + char * pc = index(host, ':'); + if (pc) { + sscanf(pc+1, "%d", &port); + *pc = 0; + } + if (hostname_to_ip(host, ip) == 0) { + strncpy(d_udp_host, ip, sizeof(d_udp_host)); + d_udp_host[sizeof(d_udp_host)-1] = 0; + d_write_port = d_audio_port = port; + open_socket(); + } + } else if (memcmp(destination, P_FILE, strlen(P_FILE)) == 0) { + const char * filename = destination+strlen(P_FILE); + size_t l = strlen(filename); + if (l > 4 && (strcmp(&filename[l-4], ".wav") == 0 || strcmp(&filename[l-4], ".WAV") == 0)) { + fprintf (stderr, "Warning! Output file %s will be written, but in raw form ***without*** a WAV file header!\n", filename); + } + d_write_sock = open(filename, O_WRONLY | O_CREAT, 0644); + if (d_write_sock < 0) { + fprintf(stderr, "op25_audio::open file %s: error: %d (%s)\n", filename, errno, strerror(errno)); + d_write_sock = 0; + return; + } + d_file_enabled = true; + } +} +// open socket and set up data structures +void op25_audio::open_socket() +{ + memset (&d_sock_addr, 0, sizeof(d_sock_addr)); + + // open handle to socket + d_write_sock = socket(PF_INET, SOCK_DGRAM, 17); // UDP socket + if ( d_write_sock < 0 ) + { + fprintf(stderr, "op25_audio::open_socket(): error: %d\n", errno); + d_write_sock = 0; + return; + } + + // set up data structure for generic udp host/port + if ( !inet_aton(d_udp_host, &d_sock_addr.sin_addr) ) + { + fprintf(stderr, "op25_audio::open_socket(): inet_aton: bad IP address\n"); + close(d_write_sock); + d_write_sock = 0; + return; + } + d_sock_addr.sin_family = AF_INET; + + fprintf(stderr, "op25_audio::open_socket(): enabled udp host(%s), wireshark(%d), audio(%d)\n", d_udp_host, d_write_port, d_audio_port); + d_udp_enabled = true; +} + +// close socket +void op25_audio::close_socket() +{ + if (!d_udp_enabled) + return; + close(d_write_sock); + d_write_sock = 0; + d_udp_enabled = false; +} + +ssize_t op25_audio::do_send(const void * buf, size_t len, int port, bool is_ctrl ) const { + ssize_t rc = 0; + struct sockaddr_in tmp_sockaddr; + if (len <= 0) + return 0; + if (d_udp_enabled) { + memcpy(&tmp_sockaddr, &d_sock_addr, sizeof(struct sockaddr)); + tmp_sockaddr.sin_port = htons(port); + rc = sendto(d_write_sock, buf, len, 0, (struct sockaddr *)&tmp_sockaddr, sizeof(struct sockaddr_in)); + if (rc == -1) + { + fprintf(stderr, "op25_audio::do_send(length %lu): error(%d): %s\n", len, errno, strerror(errno)); + rc = 0; + } + } else if (d_file_enabled && !is_ctrl) { + size_t amt_written = 0; + for (;;) { + rc = write(d_write_sock, amt_written + (char*)buf, len - amt_written); + if (rc < 0) { + fprintf(stderr, "op25_audio::write(length %lu): error(%d): %s\n", len, errno, strerror(errno)); + rc = 0; + } else if (rc == 0) { + fprintf(stderr, "op25_audio::write(length %lu): error, write rc zero\n", len); + } else { + amt_written += rc; + } + if (rc <= 0 || amt_written >= len) + break; + } /* end of for() */ + rc = amt_written; + } + return rc; +} + +// send generic data to destination +ssize_t op25_audio::send_to(const void *buf, size_t len) const +{ + return do_send(buf, len, d_write_port, false); +} + +// send audio data to destination +ssize_t op25_audio::send_audio(const void *buf, size_t len) const +{ + return do_send(buf, len, d_audio_port, false); +} + +// send audio data on specifed channel to destination +ssize_t op25_audio::send_audio_channel(const void *buf, size_t len, ssize_t slot_id) const +{ + return do_send(buf, len, d_audio_port + slot_id*2, false); +} + +// send flag to audio destination +ssize_t op25_audio::send_audio_flag_channel(const udpFlagEnumType udp_flag, ssize_t slot_id) const +{ + char audio_flag[2]; + // 16 bit little endian encoding + audio_flag[0] = (udp_flag & 0x00ff); + audio_flag[1] = ((udp_flag & 0xff00) >> 8); + return do_send(audio_flag, 2, d_audio_port+slot_id, true); +} + +ssize_t op25_audio::send_audio_flag(const op25_audio::udpFlagEnumType udp_flag) const +{ + return send_audio_flag_channel(udp_flag, 0); +} diff --git a/op25/gr-op25_repeater/lib/op25_audio.h b/op25/gr-op25_repeater/lib/op25_audio.h new file mode 100644 index 0000000..dc715a0 --- /dev/null +++ b/op25/gr-op25_repeater/lib/op25_audio.h @@ -0,0 +1,70 @@ +/* -*- c++ -*- */ +/* + * Copyright 2017 Graham J Norbury, gnorbury@bondcar.com + * from op25_audio; rewrite Nov 2017 Copyright 2017 Max H. Parke KA1RBI + * + * This 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 3, or (at your option) + * any later version. + * + * This software 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 software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + +#ifndef INCLUDED_OP25_AUDIO_H +#define INCLUDED_OP25_AUDIO_H + +#include <stdint.h> +#include <sys/socket.h> +#include <netinet/in.h> +#include <arpa/inet.h> + +class op25_audio +{ +public: + enum udpFlagEnumType + { + DRAIN = 0x0000, // play queued pcm frames + DROP = 0x0001 // discard queued pcm frames + }; + +private: + bool d_udp_enabled; + int d_debug; + int d_write_port; + int d_audio_port; + char d_udp_host[64]; + int d_write_sock; + bool d_file_enabled; + struct sockaddr_in d_sock_addr; + + void open_socket(); + void close_socket(); + ssize_t do_send(const void * bufp, size_t len, int port, bool is_ctrl) const; + +public: + op25_audio(const char* udp_host, int port, int debug); + op25_audio(const char* destination, int debug); + ~op25_audio(); + + inline bool enabled() const { return d_udp_enabled; } + + ssize_t send_to(const void *buf, size_t len) const; + + ssize_t send_audio(const void *buf, size_t len) const; + ssize_t send_audio_flag(const udpFlagEnumType udp_flag) const; + + ssize_t send_audio_channel(const void *buf, size_t len, ssize_t slot_id) const; + ssize_t send_audio_flag_channel(const udpFlagEnumType udp_flag, ssize_t slot_id) const; + +}; // class op25_audio + +#endif /* INCLUDED_OP25_AUDIO_H */ diff --git a/op25/gr-op25_repeater/lib/p25_frame.h b/op25/gr-op25_repeater/lib/p25_frame.h index 8afbaf0..6c47b01 100644 --- a/op25/gr-op25_repeater/lib/p25_frame.h +++ b/op25/gr-op25_repeater/lib/p25_frame.h @@ -18,8 +18,8 @@ * Boston, MA 02110-1301, USA. */ -#ifndef INCLUDED_OP25_P25_FRAME_H -#define INCLUDED_OP25_P25_FRAME_H 1 +#ifndef INCLUDED_P25_FRAME_H +#define INCLUDED_P25_FRAME_H 1 #include <vector> typedef std::vector<bool> bit_vector; @@ -62,4 +62,4 @@ p25_setup_frame_header(bit_vector& frame_body, uint64_t hw) { } // namespace op25_repeater } // namespace gr -#endif /* INCLUDED_OP25_P25_FRAME_H */ +#endif /* INCLUDED_P25_FRAME_H */ diff --git a/op25/gr-op25_repeater/lib/p25_frame_assembler_impl.cc b/op25/gr-op25_repeater/lib/p25_frame_assembler_impl.cc index 1c1acf7..43086c2 100644 --- a/op25/gr-op25_repeater/lib/p25_frame_assembler_impl.cc +++ b/op25/gr-op25_repeater/lib/p25_frame_assembler_impl.cc @@ -39,12 +39,23 @@ namespace gr { void p25_frame_assembler_impl::p25p2_queue_msg(int duid) { - static const char wbuf[2] = {0xff, 0xff}; // dummy NAC + unsigned char wbuf[8]; + int p=0; + if (!d_do_msgq) return; if (d_msg_queue->full_p()) return; - gr::message::sptr msg = gr::message::make_from_string(std::string(wbuf, 2), duid, 0, 0); + if (!d_nac) + return; + + wbuf[p++] = 0xaa; + wbuf[p++] = 0x55; + wbuf[p++] = (d_msgq_id >> 8) & 0xff; + wbuf[p++] = d_msgq_id & 0xff; + wbuf[p++] = (d_nac >> 8) & 0xff; + wbuf[p++] = d_nac & 0xff; + gr::message::sptr msg = gr::message::make_from_string(std::string((const char *)wbuf, p), duid, 0, 0); d_msg_queue->insert_tail(msg); } @@ -52,15 +63,20 @@ namespace gr { p2tdma.set_xormask(p); } + void p25_frame_assembler_impl::set_nac(int nac) { + d_nac = nac; + p2tdma.set_nac(nac); + } + void p25_frame_assembler_impl::set_slotid(int slotid) { p2tdma.set_slotid(slotid); } p25_frame_assembler::sptr - p25_frame_assembler::make(const char* udp_host, int port, int debug, bool do_imbe, bool do_output, bool do_msgq, gr::msg_queue::sptr queue, bool do_audio_output, bool do_phase2_tdma) + p25_frame_assembler::make(const char* udp_host, int port, int debug, bool do_imbe, bool do_output, bool do_msgq, gr::msg_queue::sptr queue, bool do_audio_output, bool do_phase2_tdma, int msgq_id) { return gnuradio::get_initial_sptr - (new p25_frame_assembler_impl(udp_host, port, debug, do_imbe, do_output, do_msgq, queue, do_audio_output, do_phase2_tdma)); + (new p25_frame_assembler_impl(udp_host, port, debug, do_imbe, do_output, do_msgq, queue, do_audio_output, do_phase2_tdma, msgq_id)); } /* @@ -80,24 +96,24 @@ static const int MAX_IN = 1; // maximum number of input streams /* * The private constructor */ - p25_frame_assembler_impl::p25_frame_assembler_impl(const char* udp_host, int port, int debug, bool do_imbe, bool do_output, bool do_msgq, gr::msg_queue::sptr queue, bool do_audio_output, bool do_phase2_tdma) + p25_frame_assembler_impl::p25_frame_assembler_impl(const char* udp_host, int port, int debug, bool do_imbe, bool do_output, bool do_msgq, gr::msg_queue::sptr queue, bool do_audio_output, bool do_phase2_tdma, int msgq_id) : gr::block("p25_frame_assembler", gr::io_signature::make (MIN_IN, MAX_IN, sizeof (char)), gr::io_signature::make ((do_output) ? 1 : 0, (do_output) ? 1 : 0, (do_audio_output && do_output) ? sizeof(int16_t) : ((do_output) ? sizeof(char) : 0 ))), d_do_imbe(do_imbe), d_do_output(do_output), output_queue(), - p1fdma(udp_host, port, debug, do_imbe, do_output, do_msgq, queue, output_queue, do_audio_output), + op25audio(udp_host, port, debug), + p1fdma(op25audio, debug, do_imbe, do_output, do_msgq, queue, output_queue, do_audio_output, msgq_id), d_do_audio_output(do_audio_output), d_do_phase2_tdma(do_phase2_tdma), - p2tdma(udp_host, port, 0, debug, output_queue), + p2tdma(op25audio, 0, debug, do_msgq, queue, output_queue, do_audio_output, msgq_id), d_do_msgq(do_msgq), - d_msg_queue(queue) + d_msg_queue(queue), + d_nac(0), + d_msgq_id(msgq_id) { - if (d_do_audio_output && !d_do_imbe) - fprintf(stderr, "p25_frame_assembler: error: do_imbe must be enabled if do_audio_output is enabled\n"); - if (d_do_phase2_tdma && !d_do_audio_output) - fprintf(stderr, "p25_frame_assembler: error: do_audio_output must be enabled if do_phase2_tdma is enabled\n"); + fprintf(stderr, "p25_frame_assembler_impl: do_imbe[%d], do_output[%d], do_audio_output[%d], do_phase2_tdma[%d]\n", do_imbe, do_output, do_audio_output, do_phase2_tdma); } void @@ -113,6 +129,7 @@ p25_frame_assembler_impl::forecast(int nof_output_items, gr_vector_int &nof_inpu nof_samples_reqd = nof_output_items; if (d_do_audio_output) nof_samples_reqd = 0.6 * nof_output_items; + nof_samples_reqd = std::max(nof_samples_reqd, 256); std::fill(&nof_input_items_reqd[0], &nof_input_items_reqd[nof_inputs], nof_samples_reqd); } @@ -130,9 +147,10 @@ p25_frame_assembler_impl::general_work (int noutput_items, for (int i = 0; i < ninput_items[0]; i++) { if(p2tdma.rx_sym(in[i])) { int rc = p2tdma.handle_frame(); - if (rc > -1) + if (rc > -1) { p25p2_queue_msg(rc); p1fdma.reset_timer(); // prevent P1 timeouts due to long TDMA transmissions + } } } } diff --git a/op25/gr-op25_repeater/lib/p25_frame_assembler_impl.h b/op25/gr-op25_repeater/lib/p25_frame_assembler_impl.h index 9763e79..5c53a77 100644 --- a/op25/gr-op25_repeater/lib/p25_frame_assembler_impl.h +++ b/op25/gr-op25_repeater/lib/p25_frame_assembler_impl.h @@ -32,6 +32,7 @@ #include "p25p1_fdma.h" #include "p25p2_tdma.h" +#include "op25_audio.h" typedef std::deque<uint8_t> dibit_queue; @@ -49,11 +50,14 @@ namespace gr { p25p2_tdma p2tdma; bool d_do_msgq; gr::msg_queue::sptr d_msg_queue; + int d_nac; + int d_msgq_id; // internal functions void p25p2_queue_msg(int duid); void set_xormask(const char*p) ; + void set_nac(int nac); void set_slotid(int slotid) ; typedef std::vector<bool> bit_vector; std::deque<int16_t> output_queue; @@ -63,9 +67,11 @@ namespace gr { // Nothing to declare in this block. public: - p25_frame_assembler_impl(const char* udp_host, int port, int debug, bool do_imbe, bool do_output, bool do_msgq, gr::msg_queue::sptr queue, bool do_audio_output, bool do_phase2_tdma); + p25_frame_assembler_impl(const char* udp_host, int port, int debug, bool do_imbe, bool do_output, bool do_msgq, gr::msg_queue::sptr queue, bool do_audio_output, bool do_phase2_tdma, int msgq_id); ~p25_frame_assembler_impl(); + op25_audio op25audio; + // Where all the action really happens int general_work(int noutput_items, diff --git a/op25/gr-op25_repeater/lib/p25_framer.cc b/op25/gr-op25_repeater/lib/p25_framer.cc index 20338b4..32d92a5 100644 --- a/op25/gr-op25_repeater/lib/p25_framer.cc +++ b/op25/gr-op25_repeater/lib/p25_framer.cc @@ -7,16 +7,16 @@ #include <vector> #include <stdio.h> #include <stdint.h> -#include <bch.h> +#include "bch.h" #include <sys/time.h> -#include <op25_p25_frame.h> -#include <p25_framer.h> +#include "op25_p25_frame.h" +#include "p25_framer.h" #include "check_frame_sync.h" static const int max_frame_lengths[16] = { // lengths are in bits, not symbols - 792, // 0 - pdu + 792, // 0 - hdu 0, 0, // 1, 2 - undef 144, // 3 - tdu 0, // 4 - undef @@ -27,17 +27,22 @@ static const int max_frame_lengths[16] = { 0, // 9 - VSELP "voice PDU" P25_VOICE_FRAME_SIZE, // a - ldu2 0, // b - undef - 720, // c - VSELP "voice PDU" + 962, // c - pdu (triple data block MBT) 0, 0, // d, e - undef 432 // f - tdu }; // constructor -p25_framer::p25_framer() : +p25_framer::p25_framer(int debug, int msgq_id) : + d_debug(debug), + d_msgq_id(msgq_id), reverse_p(0), nid_syms(0), next_bit(0), nid_accum(0), + nac(0), + duid(0), + parity(0), frame_size_limit(0), symbols_received(0), frame_body(P25_VOICE_FRAME_SIZE) @@ -57,35 +62,41 @@ p25_framer::~p25_framer () */ bool p25_framer::nid_codeword(uint64_t acc) { bit_vector cw(64); - bool low = acc & 1; - // for bch, split bits into codeword vector - for (int i=0; i < 64; i++) { + uint64_t save_acc = acc; + + // save the parity lsb, not used by BCH` + int acc_parity = acc & 1; + + // for bch, split bits into codeword vector (lsb first) + for (int i = 0; i <= 63; i++) { acc >>= 1; cw[i] = acc & 1; } // do bch decode - int rc = bchDec(cw); - - // check if bch decode unsuccessful - if (rc < 0) { - return false; - } - - bch_errors = rc; - - // load corrected bch bits into acc + int ec = bchDec(cw); + + // load corrected bch bits into acc (msb first) acc = 0; - for (int i=63; i>=0; i--) { + for (int i = 63; i >= 0; i--) { acc |= cw[i]; acc <<= 1; } - acc |= low; // FIXME + + // put the parity lsb back + acc |= acc_parity; + + // check if bch decode unsuccessful + if (ec < 0) + return false; + + bch_errors = ec; nid_word = acc; // reconstructed NID // extract nac and duid nac = (acc >> 52) & 0xfff; duid = (acc >> 48) & 0x00f; + parity = acc_parity; return true; } @@ -100,6 +111,7 @@ bool p25_framer::nid_codeword(uint64_t acc) { * Returns true when complete frame received, else false */ bool p25_framer::rx_sym(uint8_t dibit) { + struct timeval currtime; symbols_received++; bool rc = false; dibit ^= reverse_p; @@ -125,7 +137,7 @@ bool p25_framer::rx_sym(uint8_t dibit) { if (nid_syms > 0) // if nid accumulation in progress nid_syms++; // count symbols in nid - if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ P25_FRAME_SYNC_MAGIC, 6, 48)) { + if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ P25_FRAME_SYNC_MAGIC, 3, 48)) { nid_syms = 1; } if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ P25_FRAME_SYNC_REV_P, 0, 48)) { @@ -134,13 +146,16 @@ bool p25_framer::rx_sym(uint8_t dibit) { fprintf(stderr, "Reversed FS polarity detected - autocorrecting\n"); } if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0x001050551155LL, 0, 48)) { - fprintf(stderr, "tuning error -1200\n"); + gettimeofday(&currtime, 0); + fprintf(stderr, "%010lu.%06lu channel %d tuning error -1200\n", currtime.tv_sec, currtime.tv_usec, d_msgq_id); } if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xFFEFAFAAEEAALL, 0, 48)) { - fprintf(stderr, "tuning error +1200\n"); + gettimeofday(&currtime, 0); + fprintf(stderr, "%010lu.%06lu channel %d tuning error +1200\n", currtime.tv_sec, currtime.tv_usec, d_msgq_id); } if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xAA8A0A008800LL, 0, 48)) { - fprintf(stderr, "tuning error +/- 2400\n"); + gettimeofday(&currtime, 0); + fprintf(stderr, "%010lu.%06lu channel %d tuning error +/- 2400\n", currtime.tv_sec, currtime.tv_usec, d_msgq_id); } if (next_bit > 0) { frame_body[next_bit++] = (dibit >> 1) & 1; diff --git a/op25/gr-op25_repeater/lib/p25_framer.h b/op25/gr-op25_repeater/lib/p25_framer.h index ddaf71c..1a7fe55 100644 --- a/op25/gr-op25_repeater/lib/p25_framer.h +++ b/op25/gr-op25_repeater/lib/p25_framer.h @@ -26,9 +26,11 @@ private: uint64_t nid_accum; uint32_t frame_size_limit; + int d_debug; + int d_msgq_id; public: - p25_framer(); // constructor + p25_framer(int debug = 0, int msgq_id=0); ~p25_framer (); // destructor bool rx_sym(uint8_t dibit) ; @@ -38,9 +40,10 @@ public: uint64_t nid_word; // received NID word uint32_t nac; // extracted NAC uint32_t duid; // extracted DUID + uint8_t parity; // extracted DUID parity bit_vector frame_body; // all bits in frame - uint32_t frame_size; // number of bits in frame_body - uint32_t bch_errors; // number of errors detected in bch + uint32_t frame_size; // number of bits in frame_body + uint32_t bch_errors; // number of errors detected in bch }; #endif /* INCLUDED_P25_FRAMER_H */ diff --git a/op25/gr-op25_repeater/lib/p25p1_fdma.cc b/op25/gr-op25_repeater/lib/p25p1_fdma.cc index 336e0e8..396fb55 100644 --- a/op25/gr-op25_repeater/lib/p25p1_fdma.cc +++ b/op25/gr-op25_repeater/lib/p25p1_fdma.cc @@ -1,6 +1,8 @@ /* -*- c++ -*- */ /* - * Copyright 2010, 2011, 2012, 2013, 2014 Max H. Parke KA1RBI + * Copyright 2010, 2011, 2012, 2013, 2014, 2018 Max H. Parke KA1RBI + * Copyright 2017 Graham J. Norbury (modularization rewrite) + * Copyright 2008, Michael Ossmann <mike@ossmann.com> * * This is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -37,6 +39,7 @@ #include "p25_frame.h" #include "p25_framer.h" #include "rs.h" +#include "value_string.h" namespace gr { namespace op25_repeater { @@ -45,12 +48,12 @@ static const int64_t TIMEOUT_THRESHOLD = 1000000; p25p1_fdma::~p25p1_fdma() { - if (write_sock > 0) - close(write_sock); delete framer; } static uint16_t crc16(uint8_t buf[], int len) { + if (buf == 0) + return -1; uint32_t poly = (1<<12) + (1<<5) + (1<<0); uint32_t crc = 0; for(int i=0; i<len; i++) { @@ -146,8 +149,6 @@ block_deinterleave(bit_vector& bv, unsigned int start, uint8_t* buf) int state = 0; uint8_t codeword; uint16_t crc; - uint32_t crc1; - uint32_t crc2; static const uint8_t next_words[4][4] = { {0x2, 0xC, 0x1, 0xF}, @@ -187,38 +188,31 @@ block_deinterleave(bit_vector& bv, unsigned int start, uint8_t* buf) buf[d >> 2] |= state << (6 - ((d%4) * 2)); } } - crc = crc16(buf, 12); - if (crc == 0) - return 0; // return OK code - crc1 = crc32(buf, 8*8); // try crc32 - crc2 = (buf[8] << 24) + (buf[9] << 16) + (buf[10] << 8) + buf[11]; - if (crc1 == crc2) - return 0; // return OK code - return -2; // trellis decode OK, but CRC error occurred + return 0; } -p25p1_fdma::p25p1_fdma(const char* udp_host, int port, int debug, bool do_imbe, bool do_output, bool do_msgq, gr::msg_queue::sptr queue, std::deque<int16_t> &output_queue, bool do_audio_output) : +p25p1_fdma::p25p1_fdma(const op25_audio& udp, int debug, bool do_imbe, bool do_output, bool do_msgq, gr::msg_queue::sptr queue, std::deque<int16_t> &output_queue, bool do_audio_output, int msgq_id) : + op25audio(udp), write_bufp(0), - write_sock(0), - d_udp_host(udp_host), - d_port(port), d_debug(debug), d_do_imbe(do_imbe), d_do_output(do_output), d_do_msgq(do_msgq), d_msg_queue(queue), output_queue(output_queue), - framer(new p25_framer()), + framer(new p25_framer(debug, msgq_id)), d_do_audio_output(do_audio_output), - p1voice_decode((debug > 0), udp_host, port, output_queue) + ess_algid(0x80), + ess_keyid(0), + vf_tgid(0), + d_msgq_id(msgq_id), + p1voice_decode((debug > 0), udp, output_queue) { gettimeofday(&last_qtime, 0); - if (port > 0) - init_sock(d_udp_host, d_port); } void -p25p1_fdma::process_duid(uint32_t const duid, uint32_t const nac, uint8_t const buf[], int const len) +p25p1_fdma::process_duid(uint32_t const duid, uint32_t const nac, const uint8_t* buf, const int len) { char wbuf[256]; int p = 0; @@ -226,7 +220,11 @@ p25p1_fdma::process_duid(uint32_t const duid, uint32_t const nac, uint8_t const return; if (d_msg_queue->full_p()) return; - assert (len+2 <= sizeof(wbuf)); + assert (len+6 <= sizeof(wbuf)); + wbuf[p++] = 0xaa; + wbuf[p++] = 0x55; + wbuf[p++] = (d_msgq_id >> 8) & 0xff; + wbuf[p++] = d_msgq_id & 0xff; wbuf[p++] = (nac >> 8) & 0xff; wbuf[p++] = nac & 0xff; if (buf) { @@ -236,110 +234,507 @@ p25p1_fdma::process_duid(uint32_t const duid, uint32_t const nac, uint8_t const gr::message::sptr msg = gr::message::make_from_string(std::string(wbuf, p), duid, 0, 0); d_msg_queue->insert_tail(msg); gettimeofday(&last_qtime, 0); -// msg.reset(); } void -p25p1_fdma::reset_timer() +p25p1_fdma::process_HDU(const bit_vector& A) { - //update last_qtime with current time - gettimeofday(&last_qtime, 0); + uint32_t MFID; + int i, j, k, ec; + uint32_t gly; + std::vector<uint8_t> HB(63,0); // hexbit vector + std::string s = ""; + int failures = 0; + k = 0; + if (d_debug >= 10) { + fprintf (stderr, "%s NAC 0x%03x HDU: ", logts.get(), framer->nac); + } + + for (i = 0; i < 36; i ++) { + uint32_t CW = 0; + for (j = 0; j < 18; j++) { // 18 bits / cw + CW = (CW << 1) + A [ hdu_codeword_bits[k++] ]; + } + gly = gly24128Dec(CW); + HB[27 + i] = gly & 63; + if (CW ^ gly24128Enc(gly)) + /* "failures" in this context means any mismatch, + * disregarding how "recoverable" the error may be + * and disregarding how many bits may mismatch */ + failures += 1; + } + ec = rs16.decode(HB); // Reed Solomon (36,20,17) error correction + if (failures > 6) { + if (d_debug >= 10) { + fprintf (stderr, "ESS computation suppressed: failed %d of 36, ec=%d\n", failures, ec); + } + return; + } + if (ec >= 0) { + j = 27; // 72 bit MI + for (i = 0; i < 9;) { + ess_mi[i++] = (uint8_t) (HB[j ] << 2) + (HB[j+1] >> 4); + ess_mi[i++] = (uint8_t) ((HB[j+1] & 0x0f) << 4) + (HB[j+2] >> 2); + ess_mi[i++] = (uint8_t) ((HB[j+2] & 0x03) << 6) + HB[j+3]; + j += 4; + } + MFID = (HB[j ] << 2) + (HB[j+1] >> 4); // 8 bit MfrId + ess_algid = ((HB[j+1] & 0x0f) << 4) + (HB[j+2] >> 2); // 8 bit AlgId + ess_keyid = ((HB[j+2] & 0x03) << 14) + (HB[j+3] << 8) + (HB[j+4] << 2) + (HB[j+5] >> 4); // 16 bit KeyId + vf_tgid = ((HB[j+5] & 0x0f) << 12) + (HB[j+6] << 6) + HB[j+7]; // 16 bit TGID + + if (ess_algid == 0) { // workaround spurious algid + ess_algid = 128; // unenc + if (d_debug) + fprintf(stderr, "***HDU detected zero algid, msgq_id %d***\n", d_msgq_id); + } + + if (d_debug >= 10) { + fprintf (stderr, "ESS: tgid=%d, mfid=%x, algid=%x, keyid=%x, mi=", vf_tgid, MFID, ess_algid, ess_keyid); + for (i = 0; i < 9; i++) { + fprintf(stderr, "%02x ", ess_mi[i]); + } + fprintf(stderr, ", Golay failures=%d of 36, ec=%d", failures, ec); + } + s = "{\"nac\" : " + std::to_string(framer->nac) + ", \"algid\" : " + std::to_string(ess_algid) + ", \"alg\" : \"" + lookup(ess_algid, ALGIDS, ALGIDS_SZ) + "\", \"keyid\": " + std::to_string(ess_keyid) + "}"; + send_msg(s, -3); + } + + if (d_debug >= 10) { + fprintf (stderr, "\n"); + } } -void -p25p1_fdma::rx_sym (const uint8_t *syms, int nsyms) +int +p25p1_fdma::process_LLDU(const bit_vector& A, std::vector<uint8_t>& HB) +{ // return count of hamming failures + process_duid(framer->duid, framer->nac, NULL, 0); + + int i, j, k; + int failures = 0; + k = 0; + for (i = 0; i < 24; i ++) { // 24 10-bit codewords + uint32_t CW = 0; + for (j = 0; j < 10; j++) { // 10 bits / cw + CW = (CW << 1) + A[ imbe_ldu_ls_data_bits[k++] ]; + } + HB[39 + i] = hmg1063Dec( CW >> 4, CW & 0x0f ); + if (CW ^ ((HB[39+i] << 4) + hmg1063EncTbl[HB[39+i]])) + failures += 1; + } + return failures; +} + +void +p25p1_fdma::process_LDU1(const bit_vector& A) { - struct timeval currtime; - for (int i1 = 0; i1 < nsyms; i1++){ - if(framer->rx_sym(syms[i1])) { // complete frame was detected + int hmg_failures; + if (d_debug >= 10) { + fprintf (stderr, "%s NAC 0x%03x LDU1: ", logts.get(), framer->nac); + } + + std::vector<uint8_t> HB(63,0); // hexbit vector + hmg_failures = process_LLDU(A, HB); + if (hmg_failures < 6) + process_LCW(HB); + else { if (d_debug >= 10) { - fprintf (stderr, "NAC 0x%X DUID 0x%X len %u errs %u ", framer->nac, framer->duid, framer->frame_size >> 1, framer->bch_errors); - } - if ((framer->duid == 0x03) || - (framer->duid == 0x05) || - (framer->duid == 0x0A) || - (framer->duid == 0x0F)) { - process_duid(framer->duid, framer->nac, NULL, 0); + fprintf (stderr, " (LCW computation suppressed"); } - if ((framer->duid == 0x07 || framer->duid == 0x0c)) { - unsigned int d, b; - int rc[3]; - bit_vector bv1(720); - int sizes[3] = {360, 576, 720}; - uint8_t deinterleave_buf[3][12]; - - if (framer->frame_size > 720) { - fprintf(stderr, "warning trunk frame size %u exceeds maximum\n", framer->frame_size); - framer->frame_size = 720; + } + + if (d_debug >= 10) { + fprintf(stderr, ", Hamming failures=%d of 24", hmg_failures); + fprintf (stderr, "\n"); + } + + // LDUx frames with exactly 20 failures seem to be not uncommon (and deliberate?) + // a TDU almost always immediately follows these code violations + if (hmg_failures < 16) + process_voice(A); +} + +void +p25p1_fdma::process_LDU2(const bit_vector& A) +{ + std::string s = ""; + int hmg_failures; + if (d_debug >= 10) { + fprintf (stderr, "%s NAC 0x%03x LDU2: ", logts.get(), framer->nac); + } + + std::vector<uint8_t> HB(63,0); // hexbit vector + hmg_failures = process_LLDU(A, HB); + + int i, j, ec=0; + if (hmg_failures < 6) { + ec = rs8.decode(HB); // Reed Solomon (24,16,9) error correction + if (ec >= 0) { // save info if good decode + j = 39; // 72 bit MI + for (i = 0; i < 9;) { + ess_mi[i++] = (uint8_t) (HB[j ] << 2) + (HB[j+1] >> 4); + ess_mi[i++] = (uint8_t) ((HB[j+1] & 0x0f) << 4) + (HB[j+2] >> 2); + ess_mi[i++] = (uint8_t) ((HB[j+2] & 0x03) << 6) + HB[j+3]; + j += 4; } - for (d=0, b=0; d < framer->frame_size >> 1; d++) { - if ((d+1) % 36 == 0) - continue; // skip SS - bv1[b++] = framer->frame_body[d*2]; - bv1[b++] = framer->frame_body[d*2+1]; + ess_algid = (HB[j ] << 2) + (HB[j+1] >> 4); // 8 bit AlgId + ess_keyid = ((HB[j+1] & 0x0f) << 12) + (HB[j+2] << 6) + HB[j+3]; // 16 bit KeyId + + if (ess_algid == 0) { // workaround spurious algid + ess_algid = 128; // unenc + if (d_debug) + fprintf(stderr, "***LDU2 detected zero algid, msgq_id %d***\n", d_msgq_id); } - for(int sz=0; sz < 3; sz++) { - if (framer->frame_size >= sizes[sz]) { - rc[sz] = block_deinterleave(bv1,48+64+sz*196 , deinterleave_buf[sz]); - if (framer->duid == 0x07 && rc[sz] == 0) - process_duid(framer->duid, framer->nac, deinterleave_buf[sz], 10); + + if (d_debug >= 10) { + fprintf(stderr, "ESS: algid=%x, keyid=%x, mi=", ess_algid, ess_keyid); + for (int i = 0; i < 9; i++) { + fprintf(stderr, "%02x ", ess_mi[i]); + } + } + s = "{\"nac\" : " + std::to_string(framer->nac) + ", \"algid\" : " + std::to_string(ess_algid) + ", \"alg\" : \"" + lookup(ess_algid, ALGIDS, ALGIDS_SZ) + "\", \"keyid\": " + std::to_string(ess_keyid) + "}"; + send_msg(s, -3); + } + } else { + if (d_debug >= 10) { + fprintf (stderr, " (ESS computation suppressed)"); + } + } + + if (d_debug >= 10) { + fprintf(stderr, ", Hamming failures=%d of 24, ec=%d", hmg_failures, ec); + fprintf (stderr, "\n"); + } + + if (hmg_failures < 16) + process_voice(A); +} + +void +p25p1_fdma::process_TTDU() +{ + process_duid(framer->duid, framer->nac, NULL, 0); + + if ((d_do_imbe || d_do_audio_output) && (framer->duid == 0x3 || framer->duid == 0xf)) { // voice termination + op25audio.send_audio_flag(op25_audio::DRAIN); + } +} + +void +p25p1_fdma::process_TDU3() +{ + if (d_debug >= 10) { + fprintf (stderr, "%s NAC 0x%03x TDU3: ", logts.get(), framer->nac); + } + + process_TTDU(); + + if (d_debug >= 10) { + fprintf (stderr, "\n"); + } +} + +void +p25p1_fdma::process_TDU15(const bit_vector& A) +{ + if (d_debug >= 10) { + fprintf (stderr, "%s NAC 0x%03x TDU15: ", logts.get(), framer->nac); + } + + process_TTDU(); + + int i, j, k; + std::vector<uint8_t> HB(63,0); // hexbit vector + k = 0; + for (i = 0; i <= 22; i += 2) { + uint32_t CW = 0; + for (j = 0; j < 12; j++) { // 12 24-bit codewords + CW = (CW << 1) + A [ hdu_codeword_bits[k++] ]; + CW = (CW << 1) + A [ hdu_codeword_bits[k++] ]; + } + uint32_t D = gly24128Dec(CW); + HB[39 + i] = D >> 6; + HB[40 + i] = D & 63; + } + process_LCW(HB); + + if (d_debug >= 10) { + fprintf (stderr, "\n"); + } +} + +void +p25p1_fdma::process_LCW(std::vector<uint8_t>& HB) +{ + int ec = rs12.decode(HB); // Reed Solomon (24,12,13) error correction + if (ec < 0) { + if (d_debug >= 10) + fprintf(stderr, "p25p1_fdma::process_LCW: rs decode failure\n"); + return; // failed CRC + } + + int i, j; + std::vector<uint8_t> lcw(9,0); // Convert hexbits to bytes + j = 0; + for (i = 0; i < 9;) { + lcw[i++] = (uint8_t) (HB[j+39] << 2) + (HB[j+40] >> 4); + lcw[i++] = (uint8_t) ((HB[j+40] & 0x0f) << 4) + (HB[j+41] >> 2); + lcw[i++] = (uint8_t) ((HB[j+41] & 0x03) << 6) + HB[j+42]; + j += 4; + } + + int pb = (lcw[0] >> 7); + int sf = ((lcw[0] & 0x40) >> 6); + int lco = lcw[0] & 0x3f; + std::string s = ""; + + if (d_debug >= 10) + fprintf(stderr, "LCW: ec=%d, pb=%d, sf=%d, lco=%d", ec, pb, sf, lco); + + char s_msg[9]; + for (i=0; i<9; i++) { + s_msg[i] = lcw[i]; + } + char nac[2]; + nac[0] = (framer->nac >> 8) & 0xff; + nac[1] = (framer->nac ) & 0xff; + send_msg(std::string(nac, 2) + std::string(s_msg, 9), -7); + + if (pb == 0) { // only decode if unencrypted + if ((sf == 0) && ((lcw[1] == 0x00) || (lcw[1] == 0x01) || (lcw[1] == 0x90))) { // sf=0, explicit MFID in standard or Motorola format + switch (lco) { + case 0x00: { // Group Voice Channel User + uint16_t grpaddr = (lcw[4] << 8) + lcw[5]; + uint32_t srcaddr = (lcw[6] << 16) + (lcw[7] << 8) + lcw[8]; + s = "{\"srcaddr\" : " + std::to_string(srcaddr) + ", \"grpaddr\": " + std::to_string(grpaddr) + ", \"nac\" : " + std::to_string(framer->nac) + "}"; + send_msg(s, -3); + if (d_debug >= 10) + fprintf(stderr, ", srcaddr=%d, grpaddr=%d", srcaddr, grpaddr); + break; } } - // two-block mbt is the only format currently supported - if (framer->duid == 0x0c - && framer->frame_size == 576 - && rc[0] == 0 - && rc[1] == 0) { - // we copy first 10 bytes from first and - // first 8 from second (removes CRC's) - uint8_t mbt_block[18]; - memcpy(mbt_block, deinterleave_buf[0], 10); - memcpy(&mbt_block[10], deinterleave_buf[1], 8); - process_duid(framer->duid, framer->nac, mbt_block, sizeof(mbt_block)); + } else if (sf == 1) { // sf=1, implicit MFID + switch (lco) { + case 0x02: { // Group Voice Channel Update + uint16_t ch_A = (lcw[1] << 8) + lcw[2]; + uint16_t grp_A = (lcw[3] << 8) + lcw[4]; + uint16_t ch_B = (lcw[5] << 8) + lcw[6]; + uint16_t grp_B = (lcw[7] << 8) + lcw[8]; + if (d_debug >= 10) + fprintf(stderr, ", ch_A=%d, grp_A=%d, ch_B=%d, grp_B=%d", ch_A, grp_A, ch_B, grp_B); + break; + } + case 0x04: { // Group Voice Channel Update Explicit + uint8_t svcopts = (lcw[2] ) ; + uint16_t grpaddr = (lcw[3] << 8) + lcw[4]; + uint16_t ch_T = (lcw[5] << 8) + lcw[6]; + uint16_t ch_R = (lcw[7] << 8) + lcw[8]; + if (d_debug >= 10) + fprintf(stderr, ", svcopts=0x%02x, grpaddr=%d, ch_T=%d, ch_R=%d", svcopts, grpaddr, ch_T, ch_R); + break; + } + } } - if (d_debug >= 10 && framer->duid == 0x00) { - ProcHDU(framer->frame_body); - } else if (d_debug > 10 && framer->duid == 0x05) { - ProcLDU1(framer->frame_body); - } else if (d_debug >= 10 && framer->duid == 0x0a) { - ProcLDU2(framer->frame_body); - } else if (d_debug > 10 && framer->duid == 0x0f) { - ProcTDU(framer->frame_body); + } + if (d_debug >= 10) { + fprintf(stderr, " : "); + for (i = 0; i < 9; i++) + fprintf(stderr, " %02x", lcw[i]); + } +} + +void +p25p1_fdma::process_TSBK(const bit_vector& fr, uint32_t fr_len) +{ + uint8_t op, lb = 0; + block_vector deinterleave_buf; + if (process_blocks(fr, fr_len, deinterleave_buf) == 0) { + for (int j = 0; (j < deinterleave_buf.size()) && (lb == 0); j++) { + if (crc16(deinterleave_buf[j].data(), 12) != 0) // validate CRC + return; + + lb = deinterleave_buf[j][0] >> 7; // last block flag + op = deinterleave_buf[j][0] & 0x3f; // opcode + process_duid(framer->duid, framer->nac, deinterleave_buf[j].data(), 10); + + if (d_debug >= 10) { + fprintf (stderr, "%s NAC 0x%03x TSBK: op=%02x : ", logts.get(), framer->nac, op); + for (int i = 0; i < 12; i++) { + fprintf(stderr, "%02x ", deinterleave_buf[j][i]); + } + fprintf(stderr, "\n"); + } } - if (d_debug >= 10) - fprintf(stderr, "\n"); - if ((d_do_imbe || d_do_audio_output) && (framer->duid == 0x5 || framer->duid == 0xa)) { // if voice - ldu1 or ldu2 - for(size_t i = 0; i < nof_voice_codewords; ++i) { - voice_codeword cw(voice_codeword_sz); - uint32_t E0, ET; - uint32_t u[8]; - char s[128]; - imbe_deinterleave(framer->frame_body, cw, i); - // recover 88-bit IMBE voice code word - imbe_header_decode(cw, u[0], u[1], u[2], u[3], u[4], u[5], u[6], u[7], E0, ET); - // output one 32-byte msg per 0.020 sec. - // also, 32*9 = 288 byte pkts (for use via UDP) - sprintf(s, "%03x %03x %03x %03x %03x %03x %03x %03x\n", u[0], u[1], u[2], u[3], u[4], u[5], u[6], u[7]); - if (d_do_audio_output) - p1voice_decode.rxframe(u); - if (d_do_output && !d_do_audio_output) { - for (size_t j=0; j < strlen(s); j++) { - output_queue.push_back(s[j]); - } + } +} + +void +p25p1_fdma::process_PDU(const bit_vector& fr, uint32_t fr_len) +{ + uint8_t fmt, sap, blks, op = 0; + block_vector deinterleave_buf; + if ((process_blocks(fr, fr_len, deinterleave_buf) == 0) && + (deinterleave_buf.size() > 0)) { // extract all blocks associated with this PDU + if (crc16(deinterleave_buf[0].data(), 12) != 0) // validate PDU header + return; + + fmt = deinterleave_buf[0][0] & 0x1f; + sap = deinterleave_buf[0][1] & 0x3f; + blks = deinterleave_buf[0][6] & 0x7f; + + if ((sap == 61) && ((fmt == 0x17) || (fmt == 0x15))) { // Multi Block Trunking messages + if (blks > deinterleave_buf.size()) + return; // insufficient blocks available + + uint32_t crc1 = crc32(deinterleave_buf[1].data(), ((blks * 12) - 4) * 8); + uint32_t crc2 = (deinterleave_buf[blks][8] << 24) + (deinterleave_buf[blks][9] << 16) + + (deinterleave_buf[blks][10] << 8) + deinterleave_buf[blks][11]; + + if (crc1 != crc2) + return; // payload crc check failed + + process_duid(framer->duid, framer->nac, deinterleave_buf[0].data(), ((blks + 1) * 12) - 4); + + if (d_debug >= 10) { + if (fmt == 0x15) { + op = deinterleave_buf[1][0] & 0x3f; // Unconfirmed MBT format + } else if (fmt == 0x17) { + op = deinterleave_buf[0][7] & 0x3f; // Alternate MBT format } - if (d_do_output && write_sock > 0) { - memcpy(&write_buf[write_bufp], s, strlen(s)); - write_bufp += strlen(s); - if (write_bufp >= 288) { // 9 * 32 = 288 - sendto(write_sock, write_buf, 288, 0, (struct sockaddr *)&write_sock_addr, sizeof(write_sock_addr)); - // FIXME check sendto() rc - write_bufp = 0; + + fprintf (stderr, "%s NAC 0x%03x PDU: fmt=%02x, op=0x%02x : ", logts.get(), framer->nac, fmt, op); + for (int j = 0; (j < blks+1) && (j < 3); j++) { + for (int i = 0; i < 12; i++) { + fprintf(stderr, "%02x ", deinterleave_buf[j][i]); } } + fprintf(stderr, "\n"); } - } // end of imbe/voice - if (!d_do_imbe) { + } else if (d_debug >= 10) { + fprintf(stderr, "%s NAC 0x%03x PDU: non-MBT message ignored\n", logts.get(), framer->nac); + } + + } +} + +int +p25p1_fdma::process_blocks(const bit_vector& fr, uint32_t& fr_len, block_vector& dbuf) +{ + bit_vector bv; + bv.reserve(fr_len >> 1); + for (unsigned int d=0; d < fr_len >> 1; d++) { // eliminate status bits from frame + if ((d+1) % 36 == 0) + continue; + bv.push_back(fr[d*2]); + bv.push_back(fr[d*2+1]); + } + + int bl_cnt = 0; + int bl_len = (bv.size() - (48+64)) / 196; + for (bl_cnt = 0; bl_cnt < bl_len; bl_cnt++) { // deinterleave, decode trellis1_2, save 12 byte block + dbuf.push_back({0,0,0,0,0,0,0,0,0,0,0,0}); + if(block_deinterleave(bv, 48+64+bl_cnt*196, dbuf[bl_cnt].data()) != 0) { + dbuf.pop_back(); + return -1; + } + } + return (bl_cnt > 0) ? 0 : -1; +} + +void +p25p1_fdma::process_voice(const bit_vector& A) +{ + if (d_do_imbe || d_do_audio_output) { + for(size_t i = 0; i < nof_voice_codewords; ++i) { + voice_codeword cw(voice_codeword_sz); + uint32_t E0, ET; + uint32_t u[8]; + char s[128]; + imbe_deinterleave(A, cw, i); + // recover 88-bit IMBE voice code word + imbe_header_decode(cw, u[0], u[1], u[2], u[3], u[4], u[5], u[6], u[7], E0, ET); + + if (d_debug >= 10) { + packed_codeword p_cw; + imbe_pack(p_cw, u[0], u[1], u[2], u[3], u[4], u[5], u[6], u[7]); + sprintf(s,"%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x", + p_cw[0], p_cw[1], p_cw[2], p_cw[3], p_cw[4], p_cw[5], + p_cw[6], p_cw[7], p_cw[8], p_cw[9], p_cw[10]); + fprintf(stderr, "%s IMBE %s\n", logts.get(), s); // print to log in one operation + } + // output one 32-byte msg per 0.020 sec. + // also, 32*9 = 288 byte pkts (for use via UDP) + sprintf(s, "%03x %03x %03x %03x %03x %03x %03x %03x\n", u[0], u[1], u[2], u[3], u[4], u[5], u[6], u[7]); + if (d_do_audio_output) { + if (!encrypted()) + p1voice_decode.rxframe(u); + } + + if (d_do_output && !d_do_audio_output) { + for (size_t j=0; j < strlen(s); j++) { + output_queue.push_back(s[j]); + } + } + } + } +} + +void +p25p1_fdma::reset_timer() +{ + //update last_qtime with current time + gettimeofday(&last_qtime, 0); +} + +void p25p1_fdma::send_msg(const std::string msg_str, long msg_type) +{ + unsigned char hdr[4] = {0xaa, 0x55, (unsigned char)((d_msgq_id >> 8) & 0xff), (unsigned char) (d_msgq_id & 0xff)}; + if (!d_do_msgq || d_msg_queue->full_p()) + return; + + gr::message::sptr msg = gr::message::make_from_string(std::string((char*)hdr, 4) + msg_str, msg_type, 0, 0); + d_msg_queue->insert_tail(msg); +} + +void +p25p1_fdma::rx_sym (const uint8_t *syms, int nsyms) +{ + struct timeval currtime; + unsigned char hdr[4] = {0xaa, 0x55, (unsigned char)((d_msgq_id >> 8) & 0xff), (unsigned char) (d_msgq_id & 0xff)}; + for (int i1 = 0; i1 < nsyms; i1++){ + if(framer->rx_sym(syms[i1])) { // complete frame was detected + + if (framer->nac == 0) { // discard frame if NAC is invalid + return; + } + + // extract additional signalling information and voice codewords + switch(framer->duid) { + case 0x00: + process_HDU(framer->frame_body); + break; + case 0x03: + process_TDU3(); + break; + case 0x05: + process_LDU1(framer->frame_body); + break; + case 0x07: + process_TSBK(framer->frame_body, framer->frame_size); + break; + case 0x0a: + process_LDU2(framer->frame_body); + break; + case 0x0c: + process_PDU(framer->frame_body, framer->frame_size); + break; + case 0x0f: + process_TDU15(framer->frame_body); + break; + } + + if (!d_do_imbe) { // send raw frame to wireshark // pack the bits into bytes, MSB first size_t obuf_ct = 0; uint8_t obuf[P25_VOICE_FRAME_SIZE/2]; @@ -355,16 +750,15 @@ p25p1_fdma::rx_sym (const uint8_t *syms, int nsyms) (framer->frame_body[i+7] ); obuf[obuf_ct++] = b; } - if (write_sock > 0) { - sendto(write_sock, obuf, obuf_ct, 0, (struct sockaddr*)&write_sock_addr, sizeof(write_sock_addr)); - } + op25audio.send_to(obuf, obuf_ct); + if (d_do_output) { for (size_t j=0; j < obuf_ct; j++) { output_queue.push_back(obuf[j]); } } } - } // end of complete frame + } // end of complete frame } if (d_do_msgq && !d_msg_queue->full_p()) { // check for timeout @@ -377,31 +771,19 @@ p25p1_fdma::rx_sym (const uint8_t *syms, int nsyms) } diff_usec += diff_sec * 1000000; if (diff_usec >= TIMEOUT_THRESHOLD) { + if (d_debug > 0) + fprintf(stderr, "%010lu.%06lu p25p1_fdma::rx_sym() timeout, channel %d\n", currtime.tv_sec, currtime.tv_usec, d_msgq_id); + + if (d_do_audio_output) { + op25audio.send_audio_flag(op25_audio::DRAIN); + } + gettimeofday(&last_qtime, 0); - gr::message::sptr msg = gr::message::make(-1, 0, 0); + gr::message::sptr msg = gr::message::make_from_string(std::string((char*)hdr, 4), -1, 0, 0); d_msg_queue->insert_tail(msg); } } } -void p25p1_fdma::init_sock(const char* udp_host, int udp_port) -{ - memset (&write_sock_addr, 0, sizeof(write_sock_addr)); - write_sock = socket(PF_INET, SOCK_DGRAM, 17); // UDP socket - if (write_sock < 0) { - fprintf(stderr, "op25_imbe_vocoder: socket: %d\n", errno); - write_sock = 0; - return; - } - if (!inet_aton(udp_host, &write_sock_addr.sin_addr)) { - fprintf(stderr, "op25_imbe_vocoder: inet_aton: bad IP address\n"); - close(write_sock); - write_sock = 0; - return; - } - write_sock_addr.sin_family = AF_INET; - write_sock_addr.sin_port = htons(udp_port); -} - } // namespace } // namespace diff --git a/op25/gr-op25_repeater/lib/p25p1_fdma.h b/op25/gr-op25_repeater/lib/p25p1_fdma.h index 3595699..e5f3292 100644 --- a/op25/gr-op25_repeater/lib/p25p1_fdma.h +++ b/op25/gr-op25_repeater/lib/p25p1_fdma.h @@ -22,12 +22,13 @@ #define INCLUDED_OP25_REPEATER_P25P1_FDMA_H #include <gnuradio/msg_queue.h> -#include <sys/socket.h> #include <sys/time.h> -#include <netinet/in.h> -#include <arpa/inet.h> #include <deque> +#include "ezpwd/rs" + +#include "log_ts.h" +#include "op25_audio.h" #include "p25_framer.h" #include "p25p1_voice_encode.h" #include "p25p1_voice_decode.h" @@ -37,37 +38,59 @@ namespace gr { class p25p1_fdma { private: - - void init_sock(const char* udp_host, int udp_port); + typedef std::vector<bool> bit_vector; + typedef std::array<uint8_t, 12> block_array; + typedef std::vector<block_array> block_vector; // internal functions - typedef std::vector<bool> bit_vector; bool header_codeword(uint64_t acc, uint32_t& nac, uint32_t& duid); - void proc_voice_unit(bit_vector& frame_body) ; - void process_duid(uint32_t const duid, uint32_t const nac, uint8_t const buf[], int const len); + void process_duid(uint32_t const duid, uint32_t const nac, const uint8_t* buf, const int len); + void process_HDU(const bit_vector& A); + void process_LCW(std::vector<uint8_t>& HB); + int process_LLDU(const bit_vector& A, std::vector<uint8_t>& HB); + void process_LDU1(const bit_vector& A); + void process_LDU2(const bit_vector& A); + void process_TTDU(); + void process_TDU15(const bit_vector& A); + void process_TDU3(); + void process_TSBK(const bit_vector& fr, uint32_t fr_len); + void process_PDU(const bit_vector& fr, uint32_t fr_len); + void process_voice(const bit_vector& A); + int process_blocks(const bit_vector& fr, uint32_t& fr_len, block_vector& dbuf); + inline bool encrypted() { return (ess_algid != 0x80); } + void send_msg(const std::string msg_str, long msg_type); + // internal instance variables and state int write_bufp; - int write_sock; - struct sockaddr_in write_sock_addr; char write_buf[512]; - const char* d_udp_host; - int d_port; int d_debug; bool d_do_imbe; bool d_do_output; bool d_do_msgq; + bool d_do_audio_output; gr::msg_queue::sptr d_msg_queue; std::deque<int16_t> &output_queue; p25_framer* framer; struct timeval last_qtime; - bool d_do_audio_output; p25p1_voice_decode p1voice_decode; + const op25_audio& op25audio; + log_ts logts; + + ezpwd::RS<63,55> rs8; // Reed-Solomon decoders for 8, 12 and 16 bit parity + ezpwd::RS<63,51> rs12; + ezpwd::RS<63,47> rs16; + + uint16_t ess_keyid; + uint16_t ess_algid; + uint8_t ess_mi[9] = {0}; + uint16_t vf_tgid; + int d_msgq_id; public: void reset_timer(); void rx_sym (const uint8_t *syms, int nsyms); - p25p1_fdma(const char* udp_host, int port, int debug, bool do_imbe, bool do_output, bool do_msgq, gr::msg_queue::sptr queue, std::deque<int16_t> &output_queue, bool do_audio_output); - ~p25p1_fdma(); + p25p1_fdma(const op25_audio& udp, int debug, bool do_imbe, bool do_output, bool do_msgq, gr::msg_queue::sptr queue, std::deque<int16_t> &output_queue, bool do_audio_output, int msgq_id); + ~p25p1_fdma(); // Where all the action really happens diff --git a/op25/gr-op25_repeater/lib/p25p1_voice_decode.cc b/op25/gr-op25_repeater/lib/p25p1_voice_decode.cc index 798b373..bf30b4f 100644 --- a/op25/gr-op25_repeater/lib/p25p1_voice_decode.cc +++ b/op25/gr-op25_repeater/lib/p25p1_voice_decode.cc @@ -50,18 +50,13 @@ static void clear_bits(bit_vector& v) { } } -p25p1_voice_decode::p25p1_voice_decode(bool verbose_flag, const char* udp_host, int udp_port, std::deque<int16_t> &_output_queue) : - write_sock(0), +p25p1_voice_decode::p25p1_voice_decode(bool verbose_flag, const op25_audio& udp, std::deque<int16_t> &_output_queue) : + op25audio(udp), write_bufp(0), rxbufp(0), output_queue(_output_queue), - opt_verbose(verbose_flag), - opt_udp_port(udp_port) + opt_verbose(verbose_flag) { - if (opt_udp_port != 0) - // remote UDP output - init_sock(udp_host, opt_udp_port); - const char *p = getenv("IMBE"); if (p && strcasecmp(p, "soft") == 0) d_software_imbe_decoder = true; @@ -101,8 +96,8 @@ void p25p1_voice_decode::rxframe(const uint32_t u[]) /* TEST*/ frame_vector[7] >>= 1; vocoder.imbe_decode(frame_vector, snd); } - if (opt_udp_port > 0) { - sendto(write_sock, snd, FRAME * sizeof(int16_t), 0, (struct sockaddr*)&write_sock_addr, sizeof(write_sock_addr)); + if (op25audio.enabled()) { + op25audio.send_audio(snd, FRAME * sizeof(int16_t)); } else { // add generated samples to output queue for (int i = 0; i < FRAME; i++) { @@ -132,24 +127,5 @@ void p25p1_voice_decode::rxchar(const char* c, int len) } /* end of for() */ } -void p25p1_voice_decode::init_sock(const char* udp_host, int udp_port) -{ - memset (&write_sock_addr, 0, sizeof(write_sock_addr)); - write_sock = socket(PF_INET, SOCK_DGRAM, 17); // UDP socket - if (write_sock < 0) { - fprintf(stderr, "vocoder: socket: %d\n", errno); - write_sock = 0; - return; - } - if (!inet_aton(udp_host, &write_sock_addr.sin_addr)) { - fprintf(stderr, "vocoder: bad IP address\n"); - close(write_sock); - write_sock = 0; - return; - } - write_sock_addr.sin_family = AF_INET; - write_sock_addr.sin_port = htons(udp_port); -} - } /* namespace op25_repeater */ } /* namespace gr */ diff --git a/op25/gr-op25_repeater/lib/p25p1_voice_decode.h b/op25/gr-op25_repeater/lib/p25p1_voice_decode.h index 9c49b9d..159cba6 100644 --- a/op25/gr-op25_repeater/lib/p25p1_voice_decode.h +++ b/op25/gr-op25_repeater/lib/p25p1_voice_decode.h @@ -22,11 +22,11 @@ #define INCLUDED_OP25_REPEATER_P25P1_VOICE_DECODE_H #include <sys/time.h> -#include <netinet/in.h> #include <stdint.h> #include <vector> #include <deque> +#include "op25_audio.h" #include "imbe_vocoder/imbe_vocoder.h" #include "imbe_decoder.h" @@ -42,7 +42,7 @@ namespace gr { // Nothing to declare in this block. public: - p25p1_voice_decode(bool verbose_flag, const char* udp_host, int udp_port, std::deque<int16_t> &_output_queue); + p25p1_voice_decode(bool verbose_flag, const op25_audio& udp, std::deque<int16_t> &_output_queue); ~p25p1_voice_decode(); void rxframe(const uint32_t u[]); void rxchar(const char* c, int len); @@ -51,8 +51,6 @@ namespace gr { static const int RXBUF_MAX = 80; /* data items */ - int write_sock; - struct sockaddr_in write_sock_addr; int write_bufp; char write_buf[512]; char rxbuf[RXBUF_MAX]; @@ -60,13 +58,12 @@ namespace gr { imbe_vocoder vocoder; software_imbe_decoder software_decoder; bool d_software_imbe_decoder; + const op25_audio& op25audio; std::deque<int16_t> &output_queue; bool opt_verbose; - int opt_udp_port; /* local methods */ - void init_sock(const char* udp_host, int udp_port); }; } // namespace op25_repeater diff --git a/op25/gr-op25_repeater/lib/p25p1_voice_encode.cc b/op25/gr-op25_repeater/lib/p25p1_voice_encode.cc index f2ca445..d2fda13 100644 --- a/op25/gr-op25_repeater/lib/p25p1_voice_encode.cc +++ b/op25/gr-op25_repeater/lib/p25p1_voice_encode.cc @@ -33,9 +33,6 @@ #include <stdio.h> #include <string.h> #include <errno.h> -#include <sys/socket.h> -#include <netinet/in.h> -#include <arpa/inet.h> #include "imbe_vocoder/imbe_vocoder.h" #include "p25_frame.h" @@ -153,9 +150,9 @@ static void clear_bits(bit_vector& v) { } } -p25p1_voice_encode::p25p1_voice_encode(bool verbose_flag, int stretch_amt, char* udp_host, int udp_port, bool raw_vectors_flag, std::deque<uint8_t> &_output_queue) : +p25p1_voice_encode::p25p1_voice_encode(bool verbose_flag, int stretch_amt, const op25_audio& udp, bool raw_vectors_flag, std::deque<uint8_t> &_output_queue) : + op25audio(udp), frame_cnt(0), - write_sock(0), write_bufp(0), peak_amplitude(0), peak(0), @@ -166,8 +163,7 @@ p25p1_voice_encode::p25p1_voice_encode(bool verbose_flag, int stretch_amt, char* output_queue(_output_queue), f_body(P25_VOICE_FRAME_SIZE), opt_dump_raw_vectors(raw_vectors_flag), - opt_verbose(verbose_flag), - opt_udp_port(udp_port) + opt_verbose(verbose_flag) { opt_stretch_amt = 0; if (stretch_amt < 0) { @@ -178,10 +174,6 @@ p25p1_voice_encode::p25p1_voice_encode(bool verbose_flag, int stretch_amt, char* opt_stretch_amt = stretch_amt; } - if (opt_udp_port != 0) - // remote UDP output - init_sock(udp_host, opt_udp_port); - clear_bits(f_body); } @@ -216,7 +208,7 @@ void p25p1_voice_encode::append_imbe_codeword(bit_vector& frame_body, int16_t fr frame_body[i] = frame_body[i] | ldu_preset[i]; } // finally, output the frame - if (opt_udp_port > 0) { + if (op25audio.enabled()) { // pack the bits into bytes, MSB first size_t obuf_ct = 0; for (uint32_t i = 0; i < P25_VOICE_FRAME_SIZE; i += 8) { @@ -231,7 +223,7 @@ void p25p1_voice_encode::append_imbe_codeword(bit_vector& frame_body, int16_t fr (frame_body[i+7] ); obuf[obuf_ct++] = b; } - sendto(write_sock, obuf, obuf_ct, 0, (struct sockaddr*)&write_sock_addr, sizeof(write_sock_addr)); + op25audio.send_to(obuf, obuf_ct); } else { for (uint32_t i = 0; i < P25_VOICE_FRAME_SIZE; i += 2) { uint8_t dibit = @@ -274,7 +266,7 @@ void p25p1_voice_encode::compress_frame(int16_t snd[]) memcpy(&write_buf[write_bufp], s, strlen(s)); write_bufp += strlen(s); if (write_bufp >= 288) { - sendto(write_sock, write_buf, 288, 0, (struct sockaddr*)&write_sock_addr, sizeof(write_sock_addr)); + op25audio.send_to(write_buf, 288); write_bufp = 0; } return; @@ -324,28 +316,10 @@ void p25p1_voice_encode::compress_samp(const int16_t * samp, int len) } } -void p25p1_voice_encode::init_sock(char* udp_host, int udp_port) -{ - memset (&write_sock_addr, 0, sizeof(write_sock_addr)); - write_sock = socket(PF_INET, SOCK_DGRAM, 17); // UDP socket - if (write_sock < 0) { - fprintf(stderr, "vocoder: socket: %d\n", errno); - write_sock = 0; - return; - } - if (!inet_aton(udp_host, &write_sock_addr.sin_addr)) { - fprintf(stderr, "vocoder: bad IP address\n"); - close(write_sock); - write_sock = 0; - return; - } - write_sock_addr.sin_family = AF_INET; - write_sock_addr.sin_port = htons(udp_port); -} - void p25p1_voice_encode::set_gain_adjust(float gain_adjust) { vocoder.set_gain_adjust(gain_adjust); } + } /* namespace op25_repeater */ } /* namespace gr */ diff --git a/op25/gr-op25_repeater/lib/p25p1_voice_encode.h b/op25/gr-op25_repeater/lib/p25p1_voice_encode.h index e3663ec..379eda5 100644 --- a/op25/gr-op25_repeater/lib/p25p1_voice_encode.h +++ b/op25/gr-op25_repeater/lib/p25p1_voice_encode.h @@ -22,11 +22,11 @@ #define INCLUDED_OP25_REPEATER_P25P1_VOICE_ENCODE_H #include <sys/time.h> -#include <netinet/in.h> #include <stdint.h> #include <vector> #include <deque> +#include "op25_audio.h" #include "imbe_vocoder/imbe_vocoder.h" #include "imbe_decoder.h" @@ -42,7 +42,7 @@ namespace gr { // Nothing to declare in this block. public: - p25p1_voice_encode(bool verbose_flag, int stretch_amt, char* udp_host, int udp_port, bool raw_vectors_flag, std::deque<uint8_t> &_output_queue); + p25p1_voice_encode(bool verbose_flag, int stretch_amt, const op25_audio& udp, bool raw_vectors_flag, std::deque<uint8_t> &_output_queue); ~p25p1_voice_encode(); void compress_samp(const int16_t * samp, int len); void set_gain_adjust(float gain_adjust); @@ -51,8 +51,6 @@ namespace gr { /* data items */ int frame_cnt ; - int write_sock; - struct sockaddr_in write_sock_addr; int write_bufp; char write_buf[512]; struct timeval tv; @@ -68,6 +66,7 @@ namespace gr { int stretch_count ; bit_vector f_body; imbe_vocoder vocoder; + const op25_audio& op25audio; std::deque<uint8_t> &output_queue; @@ -75,12 +74,10 @@ namespace gr { bool opt_verbose; int opt_stretch_amt; int opt_stretch_sign; - int opt_udp_port; /* local methods */ void append_imbe_codeword(bit_vector& frame_body, int16_t frame_vector[], unsigned int& codeword_ct); void compress_frame(int16_t snd[]); void add_sample(int16_t samp); - void init_sock(char* udp_host, int udp_port); }; } // namespace op25_repeater diff --git a/op25/gr-op25_repeater/lib/p25p2_framer.cc b/op25/gr-op25_repeater/lib/p25p2_framer.cc index 819603d..7de5431 100644 --- a/op25/gr-op25_repeater/lib/p25p2_framer.cc +++ b/op25/gr-op25_repeater/lib/p25p2_framer.cc @@ -8,7 +8,7 @@ #include <stdio.h> #include <stdint.h> #include <sys/time.h> -#include <p25p2_framer.h> +#include "p25p2_framer.h" #include "check_frame_sync.h" diff --git a/op25/gr-op25_repeater/lib/p25p2_tdma.cc b/op25/gr-op25_repeater/lib/p25p2_tdma.cc index 8bc1afe..18c04e6 100644 --- a/op25/gr-op25_repeater/lib/p25p2_tdma.cc +++ b/op25/gr-op25_repeater/lib/p25p2_tdma.cc @@ -1,4 +1,5 @@ -// P25 TDMA Decoder (C) Copyright 2013, 2014 Max H. Parke KA1RBI +// P25 TDMA Decoder (C) Copyright 2013, 2014, 2021 Max H. Parke KA1RBI +// Copyright 2017 Graham J. Norbury (modularization rewrite) // // This file is part of OP25 // @@ -24,6 +25,7 @@ #include <iostream> #include <assert.h> #include <errno.h> +#include <sys/time.h> #include "p25p2_duid.h" #include "p25p2_sync.h" @@ -31,9 +33,12 @@ #include "p25p2_vf.h" #include "mbelib.h" #include "ambe.h" +#include "value_string.h" +#include "crc16.h" static const int BURST_SIZE = 180; static const int SUPERFRAME_SIZE = (12*BURST_SIZE); +static const int which_slot[] = {0,1,0,1,0,1,0,1,0,1,1,0}; static uint16_t crc12(const uint8_t bits[], unsigned int len) { uint16_t crc=0; @@ -66,23 +71,28 @@ static bool crc12_ok(const uint8_t bits[], unsigned int len) { return (crc == crc12(bits,len)); } -p25p2_tdma::p25p2_tdma(const char* udp_host, int port, int slotid, int debug, std::deque<int16_t> &qptr) : // constructor +p25p2_tdma::p25p2_tdma(const op25_audio& udp, int slotid, int debug, bool do_msgq, gr::msg_queue::sptr queue, std::deque<int16_t> &qptr, bool do_audio_output, int msgq_id) : // constructor + op25audio(udp), write_bufp(0), - write_sock(0), - d_udp_host(udp_host), - d_port(port), tdma_xormask(new uint8_t[SUPERFRAME_SIZE]), symbols_received(0), packets(0), d_slotid(slotid), + d_nac(0), + d_do_msgq(do_msgq), + d_msg_queue(queue), output_queue_decode(qptr), d_debug(debug), - crc_errors(0), + d_do_audio_output(do_audio_output), + burst_id(-1), + ESS_A(28,0), + ESS_B(16,0), + ess_algid(0x80), + ess_keyid(0), + d_msgq_id(msgq_id), p2framer() { - if (port > 0) - init_sock(d_udp_host, d_port); - + memset(tdma_xormask, 0, SUPERFRAME_SIZE); assert (slotid == 0 || slotid == 1); mbe_initMbeParms (&cur_mp, &prev_mp, &enh_mp); } @@ -101,9 +111,6 @@ void p25p2_tdma::set_slotid(int slotid) p25p2_tdma::~p25p2_tdma() // destructor { - if (write_sock > 0) - close(write_sock); - delete[](tdma_xormask); } @@ -113,77 +120,220 @@ p25p2_tdma::set_xormask(const char*p) { tdma_xormask[i] = p[i] & 3; } -int p25p2_tdma::process_mac_pdu(const uint8_t byte_buf[], unsigned int len) +int p25p2_tdma::process_mac_pdu(const uint8_t byte_buf[], const unsigned int len) { unsigned int opcode = (byte_buf[0] >> 5) & 0x7; unsigned int offset = (byte_buf[0] >> 2) & 0x7; + bool my_slot = (which_slot[sync.tdma_slotid()] == d_slotid); + + if (d_debug >= 10) { + fprintf(stderr, "%s process_mac_pdu: opcode %d len %d buf %02x %02x %02x\n", logts.get(), opcode, len, byte_buf[0], byte_buf[1], byte_buf[2]); + } + + if (opcode == 2) { // MAC_END_PTT + uint16_t colorcd = ((byte_buf[1] & 0x0f) << 8) + byte_buf[2]; + if (colorcd != d_nac && d_debug > 0) + fprintf(stderr, "p25p2_tdma_ process_mac_pdu: MAC_END_PTT color code 0x%x does not match d_nac 0x%x channel %d\n", colorcd, d_nac, d_msgq_id); + } + + if (opcode == 3 || opcode == 4 || opcode == 6) { // send msg for MAC_IDLE, MAC_ACTIVE, MAC_HANGTIME + char nac_color[2]; + nac_color[0] = d_nac >> 8; + nac_color[1] = d_nac & 0xff; + send_msg(std::string(nac_color, 2) + std::string((const char *)byte_buf, len), -6); + } + + if (opcode != 0 && !my_slot) // for all except MAC_SIGNAL, ignore if on oppo. slot + return -1; + + switch (opcode) + { + case 0: // MAC_SIGNAL + handle_mac_signal(byte_buf, len); + return -1; + break; + + case 1: // MAC_PTT + handle_mac_ptt(byte_buf, len); + break; + + case 2: // MAC_END_PTT + handle_mac_end_ptt(byte_buf, len); + break; + + case 3: // MAC_IDLE + op25audio.send_audio_flag(op25_audio::DRAIN); + break; + + case 4: // MAC_ACTIVE + break; + + case 6: // MAC_HANGTIME + op25audio.send_audio_flag(op25_audio::DRAIN); + break; + default: + if (d_debug > 0) + fprintf(stderr, "p25p2_tdma_ process_mac_pdu: unrecognized opcode 0x%x channel %d\n", opcode, d_msgq_id); + break; + } // maps sacch opcodes into phase I duid values - // 0, 5, 7 - Reserved - // 1 - MAC_PTT - // 2 - MAC_END_PTT - // 3 - MAC_IDLE - // 4 - MAC_ACTIVE - // 6 - MAC_HANGTIME - static const int opcode_map[8] = {3, 5, 15, 15, 5, 3, 3, 3}; + static const int opcode_map[8] = {7, 5, 15, 15, 5, 3, 3, 3}; return opcode_map[opcode]; - // TODO: decode MAC PDU's } -int p25p2_tdma::handle_acch_frame(const uint8_t dibits[], bool fast) +void p25p2_tdma::handle_mac_signal(const uint8_t byte_buf[], const unsigned int len) { - int rc = -1; + char nac_color[2]; + int i; + i = (byte_buf[19] << 4) + ((byte_buf[20] >> 4) & 0xf); + nac_color[0] = i >> 8; + nac_color[1] = i & 0xff; + send_msg(std::string(nac_color, 2) + std::string((const char *)byte_buf, len), -6); +} + +void p25p2_tdma::handle_mac_ptt(const uint8_t byte_buf[], const unsigned int len) +{ + uint32_t srcaddr = (byte_buf[13] << 16) + (byte_buf[14] << 8) + byte_buf[15]; + uint16_t grpaddr = (byte_buf[16] << 8) + byte_buf[17]; + std::string s = "{\"srcaddr\" : " + std::to_string(srcaddr) + ", \"grpaddr\": " + std::to_string(grpaddr) + ", \"nac\" : " + std::to_string(d_nac) + "}"; + send_msg(s, -3); + + if (d_debug >= 10) { + fprintf(stderr, "%s MAC_PTT: srcaddr=%u, grpaddr=%u", logts.get(), srcaddr, grpaddr); + } + for (int i = 0; i < 9; i++) { + ess_mi[i] = byte_buf[i+1]; + } + ess_algid = byte_buf[10]; + ess_keyid = (byte_buf[11] << 8) + byte_buf[12]; + + if (ess_algid == 0) { // workaround spurious algid + ess_algid = 128; // unenc + if (d_debug) + fprintf(stderr, "***mac_ptt detected zero algid, msgq_id %d***\n", d_msgq_id); + } + + if (d_debug >= 10) { + fprintf(stderr, ", algid=%x, keyid=%x, mi=", ess_algid, ess_keyid); + for (int i = 0; i < 9; i++) { + fprintf(stderr,"%02x ", ess_mi[i]); + } + } + s = "{\"nac\" : " + std::to_string(d_nac) + ", \"algid\" : " + std::to_string(ess_algid) + ", \"alg\" : \"" + lookup(ess_algid, ALGIDS, ALGIDS_SZ) + "\", \"keyid\": " + std::to_string(ess_keyid) + "}"; + send_msg(s, -3); + + if (d_debug >= 10) { + fprintf(stderr, "\n"); + } + + reset_vb(); +} + +void p25p2_tdma::handle_mac_end_ptt(const uint8_t byte_buf[], const unsigned int len) +{ + uint16_t colorcd = ((byte_buf[1] & 0x0f) << 8) + byte_buf[2]; + uint32_t srcaddr = (byte_buf[13] << 16) + (byte_buf[14] << 8) + byte_buf[15]; + uint16_t grpaddr = (byte_buf[16] << 8) + byte_buf[17]; + + if (d_debug >= 10) + fprintf(stderr, "%s MAC_END_PTT: colorcd=0x%03x, srcaddr=%u, grpaddr=%u\n", logts.get(), colorcd, srcaddr, grpaddr); + + //std::string s = "{\"srcaddr\" : " + std::to_string(srcaddr) + ", \"grpaddr\": " + std::to_string(grpaddr) + "}"; + //send_msg(s, -3); // can cause data display issues if this message is processed after the DUID15 + op25audio.send_audio_flag(op25_audio::DRAIN); +} + +int p25p2_tdma::handle_acch_frame(const uint8_t dibits[], bool fast, bool is_lcch) +{ + int i, j, rc; uint8_t bits[512]; + std::vector<uint8_t> HB(63,0); + std::vector<int> Erasures; uint8_t byte_buf[32]; unsigned int bufl=0; unsigned int len=0; if (fast) { - for (int i=11; i < 11+36; i++) { + for (i=11; i < 11+36; i++) { bits[bufl++] = (dibits[i] >> 1) & 1; bits[bufl++] = dibits[i] & 1; } - for (int i=48; i < 48+31; i++) { + for (i=48; i < 48+31; i++) { bits[bufl++] = (dibits[i] >> 1) & 1; bits[bufl++] = dibits[i] & 1; } - for (int i=100; i < 100+32; i++) { + for (i=100; i < 100+32; i++) { bits[bufl++] = (dibits[i] >> 1) & 1; bits[bufl++] = dibits[i] & 1; } - for (int i=133; i < 133+36; i++) { + for (i=133; i < 133+36; i++) { bits[bufl++] = (dibits[i] >> 1) & 1; bits[bufl++] = dibits[i] & 1; } } else { - for (int i=11; i < 11+36; i++) { + for (i=11; i < 11+36; i++) { bits[bufl++] = (dibits[i] >> 1) & 1; bits[bufl++] = dibits[i] & 1; } - for (int i=48; i < 48+84; i++) { + for (i=48; i < 48+84; i++) { bits[bufl++] = (dibits[i] >> 1) & 1; bits[bufl++] = dibits[i] & 1; } - for (int i=133; i < 133+36; i++) { + for (i=133; i < 133+36; i++) { bits[bufl++] = (dibits[i] >> 1) & 1; bits[bufl++] = dibits[i] & 1; } } - // FIXME: TODO: add RS decode - if (fast) + + // Reed-Solomon + if (fast) { + j = 9; + len = 270; + Erasures = {0,1,2,3,4,5,6,7,8,54,55,56,57,58,59,60,61,62}; + } + else { + j = 5; + len = 312; + Erasures = {0,1,2,3,4,57,58,59,60,61,62}; + } + + for (i = 0; i < len; i += 6) { // convert bits to hexbits + HB[j] = (bits[i] << 5) + (bits[i+1] << 4) + (bits[i+2] << 3) + (bits[i+3] << 2) + (bits[i+4] << 1) + bits[i+5]; + j++; + } + rc = rs28.decode(HB, Erasures); +// if (d_debug >= 10) +// fprintf(stderr, "p25p2_tdma: rc28: rc %d\n", rc); + if (rc < 0) + return -1; + + if (fast) { + j = 9; len = 144; - else - len = 168; - if (crc12_ok(bits, len)) { - for (int i=0; i<len/8; i++) { + } + else { + j = 5; + len = (is_lcch) ? 180 : 168; + } + for (i = 0; i < len; i += 6) { // convert hexbits back to bits + bits[i] = (HB[j] & 0x20) >> 5; + bits[i+1] = (HB[j] & 0x10) >> 4; + bits[i+2] = (HB[j] & 0x08) >> 3; + bits[i+3] = (HB[j] & 0x04) >> 2; + bits[i+4] = (HB[j] & 0x02) >> 1; + bits[i+5] = (HB[j] & 0x01); + j++; + } + + bool crc_ok = (is_lcch) ? (crc16(bits, len) == 0) : crc12_ok(bits, len); + int olen = (is_lcch) ? 23 : len/8; + if (d_debug >= 10) + fprintf(stderr, "p25p2_tdma: crc%d result: %s, length %d\n", (is_lcch) ? 16 : 12, (crc_ok) ? "ok" : "failed", olen); + rc = -1; + if (crc_ok) { // TODO: rewrite crc12 so we don't have to do so much bit manipulation + for (int i=0; i<olen; i++) { byte_buf[i] = (bits[i*8 + 0] << 7) + (bits[i*8 + 1] << 6) + (bits[i*8 + 2] << 5) + (bits[i*8 + 3] << 4) + (bits[i*8 + 4] << 3) + (bits[i*8 + 5] << 2) + (bits[i*8 + 6] << 1) + (bits[i*8 + 7] << 0); } - rc = process_mac_pdu(byte_buf, len/8); - } else { - crc_errors++; - } - // write a zero audio sample (2 bytes) at end of voice to trigger pcm drain - if (((rc == 3) || (rc == 15)) && (write_sock > 0)) { - memset(write_buf, 0, 2); - sendto(write_sock, write_buf, 2, 0, (struct sockaddr *)&write_sock_addr, sizeof(write_sock_addr)); + rc = process_mac_pdu(byte_buf, olen); } return rc; } @@ -197,7 +347,7 @@ void p25p2_tdma::handle_voice_frame(const uint8_t dibits[]) int rc = -1; vf.process_vcw(dibits, b); - if (b[0] < 120) + if (b[0] < 120) // anything above 120 is an erasure or special frame rc = mbe_dequantizeAmbe2250Parms (&cur_mp, &prev_mp, b); /* FIXME: check RC */ K = 12; @@ -216,15 +366,10 @@ void p25p2_tdma::handle_voice_frame(const uint8_t dibits[]) } write_buf[write_bufp++] = snd & 0xFF ; write_buf[write_bufp++] = snd >> 8; -#if 0 - output_queue_decode.push_back(snd); -#endif } - if (write_sock > 0) { - if (write_bufp >= 0) { - sendto(write_sock, write_buf, write_bufp, 0, (struct sockaddr *)&write_sock_addr, sizeof(write_sock_addr)); - write_bufp = 0; - } + if (d_do_audio_output && (write_bufp >= 0)) { + op25audio.send_audio(write_buf, write_bufp); + write_bufp = 0; } mbe_moveMbeParms (&cur_mp, &prev_mp); @@ -241,42 +386,61 @@ int p25p2_tdma::handle_frame(void) return rc; } +static inline bool null_codeword(const uint8_t*bp) { + static const int l = 36; // dibits per voice frame + for (int i=0; i<l; i++) { + if (bp[i]) + return false; + } + return true; +} + /* returns true if in sync and slot matches current active slot d_slotid */ int p25p2_tdma::handle_packet(const uint8_t dibits[]) { int rc = -1; - static const int which_slot[] = {0,1,0,1,0,1,0,1,0,1,1,0}; packets++; sync.check_confidence(dibits); if (!sync.in_sync()) return -1; const uint8_t* burstp = &dibits[10]; uint8_t xored_burst[BURST_SIZE - 10]; + bool my_slot = (which_slot[sync.tdma_slotid()] == d_slotid); int burst_type = duid.duid_lookup(duid.extract_duid(burstp)); - if (which_slot[sync.tdma_slotid()] != d_slotid) // active slot? - return -1; for (int i=0; i<BURST_SIZE - 10; i++) { xored_burst[i] = burstp[i] ^ tdma_xormask[sync.tdma_slotid() * BURST_SIZE + i]; } - if (d_debug) { - fprintf(stderr, "p25p2_tdma: burst type %d symbols %u packets %u\n", burst_type, symbols_received, packets); + if (d_debug >= 10) { + fprintf(stderr, "%s TDMA burst type=%d\n", logts.get(), burst_type); } - if (burst_type == 0 || burst_type == 6) { // 4v or 2v (voice) ? - handle_voice_frame(&xored_burst[11]); - handle_voice_frame(&xored_burst[48]); - if (burst_type == 0) { // 4v ? - handle_voice_frame(&xored_burst[96]); - handle_voice_frame(&xored_burst[133]); - } + if (burst_type == 0 || burst_type == 6) { // 4V or 2V burst + if (!my_slot) // ignore if on oppo. slot + return -1; + track_vb(burst_type); + handle_4V2V_ess(&xored_burst[84]); + if ( !encrypted() ) { + if(!null_codeword(&burstp[11])) + handle_voice_frame(&xored_burst[11]); + if(!null_codeword(&burstp[48])) + handle_voice_frame(&xored_burst[48]); + if (burst_type == 0) { + if(!null_codeword(&burstp[96])) + handle_voice_frame(&xored_burst[96]); + if(!null_codeword(&burstp[133])) + handle_voice_frame(&xored_burst[133]); + } + } return -1; - } else if (burst_type == 3) { // scrambled sacch - rc = handle_acch_frame(xored_burst, 0); - } else if (burst_type == 9) { // scrambled facch - rc = handle_acch_frame(xored_burst, 1); - } else if (burst_type == 12) { // unscrambled sacch - rc = handle_acch_frame(burstp, 0); - } else if (burst_type == 15) { // unscrambled facch - rc = handle_acch_frame(burstp, 1); + } else if (burst_type == 3) { // scrambled sacch + rc = handle_acch_frame(xored_burst, 0, false); + } else if (burst_type == 9) { // scrambled facch + rc = handle_acch_frame(xored_burst, 1, false); + } else if (burst_type == 12) { // unscrambled sacch + rc = handle_acch_frame(burstp, 0, false); + } else if (burst_type == 13) { // TDMA CC OECI + rc = handle_acch_frame(burstp, 0, true); + } else if (burst_type == 15) { // unscrambled facch + rc = handle_acch_frame(burstp, 1, false); } else { // unsupported type duid return -1; @@ -284,22 +448,66 @@ int p25p2_tdma::handle_packet(const uint8_t dibits[]) return rc; } -void p25p2_tdma::init_sock(const char* udp_host, int udp_port) +void p25p2_tdma::handle_4V2V_ess(const uint8_t dibits[]) { - memset (&write_sock_addr, 0, sizeof(write_sock_addr)); - write_sock = socket(PF_INET, SOCK_DGRAM, 17); // UDP socket - if (write_sock < 0) { - fprintf(stderr, "op25_ambe_vocoder: socket: %d\n", errno); - write_sock = 0; - return; - } - if (!inet_aton(udp_host, &write_sock_addr.sin_addr)) { - fprintf(stderr, "op25_ambe_vocoder: inet_aton: bad IP address\n"); - close(write_sock); - write_sock = 0; - return; + std::string s = ""; + if (d_debug >= 10) { + fprintf(stderr, "%s %s_BURST ", logts.get(), (burst_id < 4) ? "4V" : "2V"); } - write_sock_addr.sin_family = AF_INET; - write_sock_addr.sin_port = htons(udp_port); + + if (burst_id < 4) { + for (int i=0; i < 12; i += 3) { // ESS-B is 4 hexbits / 12 dibits + ESS_B[(4 * burst_id) + (i / 3)] = (uint8_t) ((dibits[i] << 4) + (dibits[i+1] << 2) + dibits[i+2]); + } + } + else { + int i, j, k, ec; + + j = 0; + for (i = 0; i < 28; i++) { // ESS-A is 28 hexbits / 84 dibits + ESS_A[i] = (uint8_t) ((dibits[j] << 4) + (dibits[j+1] << 2) + dibits[j+2]); + j = (i == 15) ? (j + 4) : (j + 3); // skip dibit containing DUID#3 + } + + ec = rs28.decode(ESS_B, ESS_A); + + if (ec >= 0) { // save info if good decode + ess_algid = (ESS_B[0] << 2) + (ESS_B[1] >> 4); + ess_keyid = ((ESS_B[1] & 15) << 12) + (ESS_B[2] << 6) + ESS_B[3]; + + if (ess_algid == 0) { // workaround spurious algid + ess_algid = 128; // unenc + if (d_debug) + fprintf(stderr, "***burst detected zero algid, msgq_id %d***\n", d_msgq_id); + } + + j = 0; + for (i = 0; i < 9;) { + ess_mi[i++] = (uint8_t) (ESS_B[j+4] << 2) + (ESS_B[j+5] >> 4); + ess_mi[i++] = (uint8_t) ((ESS_B[j+5] & 0x0f) << 4) + (ESS_B[j+6] >> 2); + ess_mi[i++] = (uint8_t) ((ESS_B[j+6] & 0x03) << 6) + ESS_B[j+7]; + j += 4; + } + s = "{\"nac\" : " + std::to_string(d_nac) + ", \"algid\" : " + std::to_string(ess_algid) + ", \"alg\" : \"" + lookup(ess_algid, ALGIDS, ALGIDS_SZ) + "\", \"keyid\": " + std::to_string(ess_keyid) + "}"; + send_msg(s, -3); + } + } + + if (d_debug >= 10) { + fprintf(stderr, "ESS: algid=%x, keyid=%x, mi=", ess_algid, ess_keyid); + for (int i = 0; i < 9; i++) { + fprintf(stderr,"%02x ", ess_mi[i]); + } + fprintf(stderr, "\n"); + } } +void p25p2_tdma::send_msg(const std::string msg_str, long msg_type) +{ + unsigned char hdr[4] = {0xaa, 0x55, (unsigned char)((d_msgq_id >> 8) & 0xff), (unsigned char)(d_msgq_id & 0xff)}; + if (!d_do_msgq || d_msg_queue->full_p()) + return; + + gr::message::sptr msg = gr::message::make_from_string(std::string((char*)hdr, 4) + msg_str, msg_type, 0, 0); + d_msg_queue->insert_tail(msg); +} diff --git a/op25/gr-op25_repeater/lib/p25p2_tdma.h b/op25/gr-op25_repeater/lib/p25p2_tdma.h index d7e30c3..1b9966c 100644 --- a/op25/gr-op25_repeater/lib/p25p2_tdma.h +++ b/op25/gr-op25_repeater/lib/p25p2_tdma.h @@ -22,9 +22,8 @@ #include <stdint.h> #include <deque> -#include <sys/socket.h> -#include <netinet/in.h> -#include <arpa/inet.h> +#include <vector> +#include <gnuradio/msg_queue.h> #include "mbelib.h" #include "imbe_decoder.h" #include "software_imbe_decoder.h" @@ -32,12 +31,16 @@ #include "p25p2_sync.h" #include "p25p2_vf.h" #include "p25p2_framer.h" +#include "op25_audio.h" +#include "log_ts.h" -class p25p2_tdma; +#include "ezpwd/rs" + +//class p25p2_tdma; class p25p2_tdma { public: - p25p2_tdma(const char* udp_host, int port, int slotid, int debug, std::deque<int16_t> &qptr) ; // constructor + p25p2_tdma(const op25_audio& udp, int slotid, int debug, bool do_msgq, gr::msg_queue::sptr queue, std::deque<int16_t> &qptr, bool do_audio_output, int msgq_id) ; // constructor int handle_packet(const uint8_t dibits[]) ; void set_slotid(int slotid); uint8_t* tdma_xormask; @@ -45,6 +48,7 @@ public: uint32_t packets; ~p25p2_tdma(); // destructor void set_xormask(const char*p); + void set_nac(int nac) { d_nac = nac; } bool rx_sym(uint8_t sym); int handle_frame(void) ; private: @@ -52,26 +56,49 @@ private: p25p2_duid duid; p25p2_vf vf; int write_bufp; - int write_sock; - struct sockaddr_in write_sock_addr; char write_buf[512]; - const char* d_udp_host; - int d_port; int d_slotid; + int d_nac; mbe_parms cur_mp; mbe_parms prev_mp; mbe_parms enh_mp; software_imbe_decoder software_decoder; + gr::msg_queue::sptr d_msg_queue; std::deque<int16_t> &output_queue_decode; + bool d_do_msgq; + bool d_do_audio_output; + const op25_audio& op25audio; + log_ts logts; int d_debug; - unsigned long int crc_errors; + + int burst_id; + inline int track_vb(int burst_type) { return burst_id = (burst_type == 0) ? (++burst_id % 5) : 4; } + inline void reset_vb(void) { burst_id = -1; } + + ezpwd::RS<63,35> rs28; // Reed-Solomon decoder object + std::vector<uint8_t> ESS_A; // ESS_A and ESS_B are hexbits vectors + std::vector<uint8_t> ESS_B; + + uint16_t ess_keyid; + uint16_t ess_algid; + uint8_t ess_mi[9] = {0}; + int d_msgq_id; p25p2_framer p2framer; - int handle_acch_frame(const uint8_t dibits[], bool fast) ; + int handle_acch_frame(const uint8_t dibits[], bool fast, bool is_lcch) ; void handle_voice_frame(const uint8_t dibits[]) ; - int process_mac_pdu(const uint8_t byte_buf[], unsigned int len) ; - void init_sock(const char* udp_host, int udp_port); + int process_mac_pdu(const uint8_t byte_buf[], const unsigned int len) ; + void handle_mac_signal(const uint8_t byte_buf[], const unsigned int len) ; + void handle_mac_ptt(const uint8_t byte_buf[], const unsigned int len) ; + void handle_mac_end_ptt(const uint8_t byte_buf[], const unsigned int len) ; + void handle_mac_idle(const uint8_t byte_buf[], const unsigned int len) ; + void handle_mac_active(const uint8_t byte_buf[], const unsigned int len) ; + void handle_mac_hangtime(const uint8_t byte_buf[], const unsigned int len) ; + void handle_4V2V_ess(const uint8_t dibits[]); + inline bool encrypted() { return (ess_algid != 0x80); } + + void send_msg(const std::string msg_str, long msg_type); }; #endif /* INCLUDED_P25P2_TDMA_H */ diff --git a/op25/gr-op25_repeater/lib/p25p2_vf.cc b/op25/gr-op25_repeater/lib/p25p2_vf.cc index cab6f7d..592befe 100644 --- a/op25/gr-op25_repeater/lib/p25p2_vf.cc +++ b/op25/gr-op25_repeater/lib/p25p2_vf.cc @@ -20,6 +20,7 @@ #include <stdio.h> #include <stdint.h> #include <string.h> +#include "op25_golay.h" #include "p25p2_vf.h" #include "rs.h" @@ -817,9 +818,11 @@ static const int m_list[] = {0, 1, 2, 3, 4, 5, 11, 12, 13, 14, 17, 18, 19, 20, 2 static const int d_list[] = {7, 1, 11, 21, 31, 25, 35, 45, 55, 49, 59, 69, 6, 0, 10, 20, 30, 24, 34, 44, 54, 48, 58, 68, 5, 15, 9, 19, 29, 39, 33, 43, 53, 63, 57, 67, 4, 14, 8, 18, 28, 38, 32, 42, 52, 62, 56, 66, 3, 13, 23, 17, 27, 37, 47, 41, 51, 61, 71, 65, 2, 12, 22, 16, 26, 36, 46, 40, 50, 60, 70, 64}; +static const int alt_d_list[] = {0, 12, 24, 36, 48, 60, 1, 13, 25, 37, 49, 61, 2, 14, 26, 38, 50, 62, 3, 15, 27, 39, 51, 63, 4, 16, 28, 40, 52, 64, 5, 17, 29, 41, 53, 65, 6, 18, 30, 42, 54, 66, 7, 19, 31, 43, 55, 67, 8, 20, 32, 44, 56, 68, 9, 21, 33, 45, 57, 69, 10, 22, 34, 46, 58, 70, 11, 23, 35, 47, 59, 71}; + static const int b_lengths[] = {7,4,6,9,7,4,4,4,3}; -void p25p2_vf::encode_dstar(uint8_t result[72], const int b[9]) { +void p25p2_vf::encode_dstar(uint8_t result[72], const int b[9], bool alt_dstar_interleave) { uint8_t pbuf[48]; uint8_t tbuf[48]; @@ -842,15 +845,21 @@ void p25p2_vf::encode_dstar(uint8_t result[72], const int b[9]) { store_reg(c1, pre_buf+24, 24); memcpy(pre_buf+48, pbuf+24, 24); for (int i=0; i < 72; i++) - result[d_list[i]] = pre_buf[i]; + if (alt_dstar_interleave) + result[i] = pre_buf[alt_d_list[i]]; + else + result[d_list[i]] = pre_buf[i]; } -void p25p2_vf::decode_dstar(const uint8_t codeword[72], int b[9]) { +void p25p2_vf::decode_dstar(const uint8_t codeword[72], int b[9], bool alt_dstar_interleave) { uint8_t pre_buf[72]; uint8_t post_buf[48]; uint8_t tbuf[48]; for (int i=0; i < 72; i++) - pre_buf[i] = codeword[d_list[i]]; + if (alt_dstar_interleave) + pre_buf[alt_d_list[i]] = codeword[i]; + else + pre_buf[i] = codeword[d_list[i]]; uint32_t c0 = load_reg(pre_buf, 24); uint32_t c1 = load_reg(pre_buf+24, 24); diff --git a/op25/gr-op25_repeater/lib/p25p2_vf.h b/op25/gr-op25_repeater/lib/p25p2_vf.h index b86d500..9b9b63e 100644 --- a/op25/gr-op25_repeater/lib/p25p2_vf.h +++ b/op25/gr-op25_repeater/lib/p25p2_vf.h @@ -26,8 +26,8 @@ class p25p2_vf { public: void process_vcw(const uint8_t vf[], int* b); void encode_vcw(uint8_t vf[], const int* b); - void encode_dstar(uint8_t result[72], const int b[9]); - void decode_dstar(const uint8_t codeword[72], int b[9]); + void encode_dstar(uint8_t result[72], const int b[9], bool alt_dstar_interleave); + void decode_dstar(const uint8_t codeword[72], int b[9], bool alt_dstar_interleave); private: void extract_vcw(const uint8_t _vf[], int& _c0, int& _c1, int& _c2, int& _c3); void interleave_vcw(uint8_t _vf[], int _c0, int _c1, int _c2, int _c3); diff --git a/op25/gr-op25_repeater/lib/rs.cc b/op25/gr-op25_repeater/lib/rs.cc index 2a5b774..272776e 100644 --- a/op25/gr-op25_repeater/lib/rs.cc +++ b/op25/gr-op25_repeater/lib/rs.cc @@ -4,7 +4,6 @@ #include <stdint.h> #include <vector> #include <assert.h> -#include <op25_imbe_frame.h> #ifdef DEBUG /* @@ -27,52 +26,6 @@ dump_cw(const_bit_vector cw, int len, FILE* fp) // len in bytes } #endif // DEBUG -// this table used for HDU and also for TDU codeword mapping. -// FIXME: possible dup of GOLAY_CODEWORDS in hdu.cc ? -static const uint16_t hdu_codeword_bits[658] = { // 329 symbols = 324 + 5 pad - 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, - 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 144, 145, 146, 147, - 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, - 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, - 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, - 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, - 212, 213, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, - 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, - 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 256, 257, 258, 259, 260, 261, - 262, 263, 264, 265, 266, 267, 268, 269, 270, 271, 272, 273, 274, 275, 276, 277, - 278, 279, 280, 281, 282, 283, 284, 285, 288, 289, 290, 291, 292, 293, 294, 295, - 296, 297, 298, 299, 300, 301, 302, 303, 304, 305, 306, 307, 308, 309, 310, 311, - 312, 313, 314, 315, 316, 317, 318, 319, 320, 321, 322, 323, 324, 325, 326, 327, - 328, 329, 330, 331, 332, 333, 334, 335, 336, 337, 338, 339, 340, 341, 342, 343, - 344, 345, 346, 347, 348, 349, 350, 351, 352, 353, 354, 355, 356, 357, 360, 361, - 362, 363, 364, 365, 366, 367, 368, 369, 370, 371, 372, 373, 374, 375, 376, 377, - 378, 379, 380, 381, 382, 383, 384, 385, 386, 387, 388, 389, 390, 391, 392, 393, - 394, 395, 396, 397, 398, 399, 400, 401, 402, 403, 404, 405, 406, 407, 408, 409, - 410, 411, 412, 413, 414, 415, 416, 417, 418, 419, 420, 421, 422, 423, 424, 425, - 426, 427, 428, 429, 432, 433, 434, 435, 436, 437, 438, 439, 440, 441, 442, 443, - 444, 445, 446, 447, 448, 449, 450, 451, 452, 453, 454, 455, 456, 457, 458, 459, - 460, 461, 462, 463, 464, 465, 466, 467, 468, 469, 470, 471, 472, 473, 474, 475, - 476, 477, 478, 479, 480, 481, 482, 483, 484, 485, 486, 487, 488, 489, 490, 491, - 492, 493, 494, 495, 496, 497, 498, 499, 500, 501, 504, 505, 506, 507, 508, 509, - 510, 511, 512, 513, 514, 515, 516, 517, 518, 519, 520, 521, 522, 523, 524, 525, - 526, 527, 528, 529, 530, 531, 532, 533, 534, 535, 536, 537, 538, 539, 540, 541, - 542, 543, 544, 545, 546, 547, 548, 549, 550, 551, 552, 553, 554, 555, 556, 557, - 558, 559, 560, 561, 562, 563, 564, 565, 566, 567, 568, 569, 570, 571, 572, 573, - 576, 577, 578, 579, 580, 581, 582, 583, 584, 585, 586, 587, 588, 589, 590, 591, - 592, 593, 594, 595, 596, 597, 598, 599, 600, 601, 602, 603, 604, 605, 606, 607, - 608, 609, 610, 611, 612, 613, 614, 615, 616, 617, 618, 619, 620, 621, 622, 623, - 624, 625, 626, 627, 628, 629, 630, 631, 632, 633, 634, 635, 636, 637, 638, 639, - 640, 641, 642, 643, 644, 645, 648, 649, 650, 651, 652, 653, 654, 655, 656, 657, - 658, 659, 660, 661, 662, 663, 664, 665, 666, 667, 668, 669, 670, 671, 672, 673, - 674, 675, 676, 677, 678, 679, 680, 681, 682, 683, 684, 685, 686, 687, 688, 689, - 690, 691, 692, 693, 694, 695, 696, 697, 698, 699, 700, 701, 702, 703, 704, 705, - 706, 707, 708, 709, 710, 711, 712, 713, 714, 715, 716, 717, 720, 721, 722, 723, - 724, 725, 726, 727, 728, 729, 730, 731, 732, 733, 734, 735, 736, 737, 738, 739, - 740, 741, 742, 743, 744, 745, 746, 747, 748, 749, 750, 751, 752, 753, 754, 755, - 756, 757, 758, 759, 760, 761, 762, 763, 764, 765, 766, 767, 768, 769, 770, 771, - 772, 773, 774, 775, 776, 777, 778, 779, 780, 781, 782, 783, 784, 785, 786, 787, - 788, 789 }; - static const uint32_t gly23127DecTbl[2048] = { 0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 147459, 1, 2, 2, 3, 2, 3, 3, 4268035, 2, 3, 3, 1574915, 3, 2097155, 294915, 4099, @@ -203,260 +156,543 @@ static const uint32_t gly23127DecTbl[2048] = { 4718595, 16387, 16387, 16386, 1048579, 2138115, 65539, 16387, 2099203, 69635, 1343491, 16387, 131075, 262147, 4206595, 526339, 1048579, 69635, 141315, 16387, 1048578, 1048579, 1048579, 4456451, 69635, 69634, 524291, 69635, 1048579, 69635, 2113539, 163843 }; -static const uint32_t hmg1063EncTbl[64] = { - 0, 12, 3, 15, 7, 11, 4, 8, 11, 7, 8, 4, 12, 0, 15, 3, - 13, 1, 14, 2, 10, 6, 9, 5, 6, 10, 5, 9, 1, 13, 2, 14, - 14, 2, 13, 1, 9, 5, 10, 6, 5, 9, 6, 10, 2, 14, 1, 13, - 3, 15, 0, 12, 4, 8, 7, 11, 8, 4, 11, 7, 15, 3, 12, 0 }; -static const uint32_t hmg1063DecTbl[16] = { - 0, 0, 0, 2, 0, 0, 0, 4, 0, 0, 0, 8, 1, 16, 32, 0 }; - -static const uint32_t rsGFexp[64] = { - 1, 2, 4, 8, 16, 32, 3, 6, 12, 24, 48, 35, 5, 10, 20, 40, - 19, 38, 15, 30, 60, 59, 53, 41, 17, 34, 7, 14, 28, 56, 51, 37, - 9, 18, 36, 11, 22, 44, 27, 54, 47, 29, 58, 55, 45, 25, 50, 39, - 13, 26, 52, 43, 21, 42, 23, 46, 31, 62, 63, 61, 57, 49, 33, 0 }; -static const uint32_t rsGFlog[64] = { - 63, 0, 1, 6, 2, 12, 7, 26, 3, 32, 13, 35, 8, 48, 27, 18, - 4, 24, 33, 16, 14, 52, 36, 54, 9, 45, 49, 38, 28, 41, 19, 56, - 5, 62, 25, 11, 34, 31, 17, 47, 15, 23, 53, 51, 37, 44, 55, 40, - 10, 61, 46, 30, 50, 22, 39, 43, 29, 60, 42, 21, 20, 59, 57, 58 }; - -int hmg1063Dec (uint32_t Dat, uint32_t Par) { - assert ((Dat < 64) && (Par < 16)); - return Dat ^ hmg1063DecTbl [ hmg1063EncTbl[Dat] ^ Par]; -} - -int -rsDec (int nroots, int FirstInfo, uint8_t HB[]) { - -//RS (63,63-nroots,nroots+1) decoder where nroots = number of parity bits -// rsDec(8, 39) rsDec(16, 27) rsDec(12, 39) - - -int lambda[18] ;//Err+Eras Locator poly -int S[17] ;//syndrome poly -int b[18] ; -int t[18] ; -int omega[18] ; -int root[17] ; -int reg[18] ; -int locn[17] ; - -int i,j, count, r, el, SynError, DiscrR, q, DegOmega, tmp, num1, num2, den, DegLambda; - -//form the syndromes; i.e., evaluate HB(x) at roots of g(x) - -for (i = 0; i <= nroots - 1; i++) { - S[i] = HB[0]; -} -for (j = 1; j <= 62; j++) { - for (i = 0; i <= nroots - 1; i++) { - if (S[i] == 0) { - S[i] = HB[j]; - } else { - S[i] = HB[j] ^ rsGFexp[(rsGFlog[S[i]] + i + 1) % 63]; - } - } -} - - - -//convert syndromes to index form, checking for nonzero condition - -SynError = 0; - -for (i = 0; i <= nroots - 1; i++) { - SynError = SynError | S[i]; - S[i] = rsGFlog[S[i]]; -} - -if (SynError == 0) { - //if syndrome is zero, rsData[] is a codeword and there are - //no errors to correct. So return rsData[] unmodified - count = 0; - goto rsDecFinish; -} - -for (i = 1; i <= nroots; i++) { - lambda[i] = 0; -} -lambda[0] = 1; - -for (i = 0; i <= nroots; i++) { - b[i] = rsGFlog[lambda[i]]; -} - - - -//begin Berlekamp-Massey algorithm to determine error+erasure -//locator polynomial - -r = 0; -el = 0; -while ( r < nroots) { //r is the step number - r = r + 1; - //compute discrepancy at the r-th step in poly-form - DiscrR = 0; - for (i = 0; i <= r - 1; i++) { - if ((lambda[i] != 0) && (S[r - i - 1] != 63)) { - DiscrR = DiscrR ^ rsGFexp[(rsGFlog[lambda[i]] + S[r - i - 1]) % 63]; - } - } - DiscrR = rsGFlog[DiscrR] ;//index form - - if (DiscrR == 63) { - //shift elements upward one step - for (i = nroots; i >= 1; i += -1){b[i] = b[i - 1]; } b[0] = 63; - } else { - //t(x) <-- lambda(x) - DiscrR*x*b(x) - t[0] = lambda[0]; - for (i = 0; i <= nroots - 1; i++) { - if (b[i] != 63) { - t[i + 1] = lambda[i + 1] ^ rsGFexp[(DiscrR + b[i]) % 63]; - } else { - t[i + 1] = lambda[i + 1]; - } - } - if (2 * el <= r - 1) { - el = r - el; - //b(x) <-- inv(DiscrR) * lambda(x) - for (i = 0; i <= nroots; i++) { - if (lambda[i]) { b[i] = (rsGFlog[lambda[i]] - DiscrR + 63) % 63; } else { b[i] = 63; } - } - } else { - //shift elements upward one step - for (i = nroots; i >= 1; i += -1){b[i] = b[i - 1]; } b[0] = 63; - } - for (i = 0; i <= nroots; i++) { lambda[i] = t[i]; } - } -} /* end while() */ - - - -//convert lambda to index form and compute deg(lambda(x)) - -DegLambda = 0; -for (i = 0; i <= nroots; i++) { - lambda[i] = rsGFlog[lambda[i]]; - if (lambda[i] != 63) { DegLambda = i; } +static const uint32_t gly24128EncTbl[4096] = { + 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, + 132813, 138790, 143347, 145176, 150618, 152753, 157028, 163215, + 166667, 168928, 172597, 178910, 180636, 186743, 190626, 192585, + 198058, 204097, 208020, 210047, 216893, 219094, 222723, 229096, + 231532, 233607, 237906, 244153, 246523, 252432, 256965, 258862, + 265625, 267634, 271527, 277580, 280334, 286693, 290352, 292571, + 295007, 301236, 305505, 307594, 314056, 315939, 320502, 326429, + 331518, 333333, 337856, 343851, 345193, 351362, 355671, 357820, + 361272, 367571, 371206, 373485, 379311, 381252, 385169, 391290, + 396116, 398271, 402026, 408193, 410051, 416040, 420093, 421910, + 427666, 433785, 438188, 440135, 445445, 447726, 451899, 458192, + 460851, 463064, 467213, 473574, 475812, 481871, 486298, 488305, + 493045, 498974, 502987, 504864, 511842, 513929, 517724, 523959, + 525274, 531249, 535268, 537103, 543053, 545190, 548979, 555160, + 560668, 562935, 567074, 573385, 574603, 580704, 585141, 587102, + 590013, 596054, 600451, 602472, 608810, 611009, 615188, 621567, + 626043, 628112, 631877, 638126, 641004, 646919, 650962, 652857, + 656663, 663036, 666665, 668866, 675712, 677739, 681662, 687701, + 690385, 692282, 696815, 702724, 705094, 711341, 715640, 717715, + 722544, 728731, 733006, 735141, 740583, 742412, 746969, 752946, + 756662, 758621, 762504, 768611, 770337, 776650, 780319, 782580, + 790083, 792232, 796541, 802710, 804052, 810047, 814570, 816385, + 820101, 826222, 830139, 832080, 837906, 840185, 843820, 850119, + 855332, 857551, 861210, 867569, 870323, 876376, 880269, 882278, + 884962, 890889, 895452, 897335, 903797, 905886, 910155, 916384, + 919694, 921701, 926128, 932187, 934425, 940786, 944935, 947148, + 951624, 957859, 961654, 963741, 970719, 972596, 976609, 982538, + 986089, 987906, 991959, 997948, 999806, 1005973, 1009728, 1011883, + 1017391, 1023684, 1027857, 1030138, 1035448, 1037395, 1041798, 1047917, + 1050548, 1056607, 1060490, 1062497, 1068323, 1070536, 1074205, 1080566, + 1084018, 1086105, 1090380, 1096615, 1097957, 1103886, 1108443, 1110320, + 1115347, 1121336, 1125869, 1127686, 1134148, 1136303, 1140602, 1146769, + 1149205, 1151486, 1155115, 1161408, 1164162, 1170281, 1174204, 1176151, + 1180025, 1186194, 1189959, 1192108, 1199086, 1200901, 1204944, 1210939, + 1215679, 1217620, 1222017, 1228138, 1230376, 1236675, 1240854, 1243133, + 1245726, 1252085, 1256224, 1258443, 1263753, 1265762, 1270199, 1276252, + 1282008, 1283891, 1287910, 1293837, 1295695, 1301924, 1305713, 1307802, + 1313325, 1315526, 1319699, 1326072, 1327290, 1333329, 1337732, 1339759, + 1345515, 1351424, 1355477, 1357374, 1363324, 1365399, 1369154, 1375401, + 1378634, 1380769, 1384564, 1390751, 1393629, 1399606, 1403619, 1405448, + 1410188, 1416295, 1420722, 1422681, 1429019, 1431280, 1435429, 1441742, + 1445088, 1446923, 1451486, 1457461, 1459831, 1466012, 1470281, 1472418, + 1474854, 1481165, 1484824, 1487091, 1493937, 1495898, 1499791, 1505892, + 1511303, 1513324, 1517241, 1523282, 1525008, 1531387, 1535022, 1537221, + 1540673, 1546922, 1551231, 1553300, 1558742, 1560637, 1565160, 1571075, + 1573998, 1580165, 1584464, 1586619, 1593081, 1594898, 1599431, 1605420, + 1608104, 1610051, 1613974, 1620093, 1622847, 1629140, 1632769, 1635050, + 1640201, 1646562, 1650231, 1652444, 1658270, 1660277, 1664160, 1670219, + 1673935, 1675812, 1680369, 1686298, 1687640, 1693875, 1698150, 1700237, + 1704611, 1710664, 1715101, 1717110, 1722420, 1724639, 1728778, 1735137, + 1740645, 1742734, 1746523, 1752752, 1754610, 1760537, 1764556, 1766439, + 1769924, 1775919, 1779962, 1781777, 1788755, 1790904, 1794669, 1800838, + 1805314, 1807593, 1811772, 1818071, 1820309, 1826430, 1830827, 1832768, + 1837559, 1839388, 1843401, 1849378, 1852256, 1858443, 1862238, 1864373, + 1868849, 1875162, 1879311, 1881572, 1887910, 1889869, 1894296, 1900403, + 1903248, 1905275, 1909678, 1915717, 1916935, 1923308, 1927481, 1929682, + 1935190, 1941437, 1945192, 1947267, 1953217, 1955114, 1959167, 1965076, + 1969978, 1972177, 1975812, 1982191, 1983917, 1989958, 1993875, 1995896, + 1999612, 2005527, 2010050, 2011945, 2017387, 2019456, 2023765, 2030014, + 2034781, 2036918, 2041187, 2047368, 2049738, 2055713, 2060276, 2062111, + 2064795, 2070896, 2074789, 2076750, 2083596, 2085863, 2089522, 2095833, + 2101096, 2103171, 2106966, 2113213, 2115071, 2120980, 2124993, 2126890, + 2130606, 2136645, 2141072, 2143099, 2148409, 2150610, 2154759, 2161132, + 2165775, 2168036, 2172209, 2178522, 2180760, 2186867, 2191270, 2193229, + 2195913, 2201890, 2205943, 2207772, 2214750, 2216885, 2220640, 2226827, + 2230693, 2232654, 2236571, 2242672, 2245426, 2251737, 2255372, 2257639, + 2262115, 2268296, 2272605, 2274742, 2281204, 2283039, 2287562, 2293537, + 2296514, 2298409, 2302972, 2308887, 2310229, 2316478, 2320747, 2322816, + 2328324, 2334703, 2338362, 2340561, 2346387, 2348408, 2352301, 2358342, + 2360049, 2365978, 2370511, 2372388, 2377830, 2379917, 2384216, 2390451, + 2395959, 2398172, 2401801, 2408162, 2409888, 2415947, 2419870, 2421877, + 2425238, 2431357, 2435240, 2437187, 2444033, 2446314, 2449983, 2456276, + 2460752, 2462907, 2467182, 2473349, 2475719, 2481708, 2486265, 2488082, + 2491452, 2497751, 2501890, 2504169, 2510507, 2512448, 2516885, 2523006, + 2525690, 2527505, 2531524, 2537519, 2540397, 2546566, 2550355, 2552504, + 2557787, 2564016, 2567781, 2569870, 2575820, 2577703, 2581746, 2587673, + 2591389, 2593398, 2597795, 2603848, 2605066, 2611425, 2615604, 2617823, + 2624690, 2626649, 2631052, 2637159, 2639397, 2645710, 2649883, 2652144, + 2654580, 2660767, 2664522, 2666657, 2673635, 2675464, 2679517, 2685494, + 2691029, 2692926, 2696939, 2702848, 2704706, 2710953, 2714748, 2716823, + 2720275, 2726648, 2730797, 2732998, 2738308, 2740335, 2744762, 2750801, + 2755199, 2757268, 2761537, 2767786, 2769128, 2775043, 2779606, 2781501, + 2787257, 2793298, 2797191, 2799212, 2805038, 2807237, 2810896, 2817275, + 2820376, 2822643, 2826278, 2832589, 2835343, 2841444, 2845361, 2847322, + 2852062, 2858037, 2862560, 2864395, 2870857, 2872994, 2877303, 2883484, + 2883883, 2890176, 2893845, 2896126, 2902972, 2904919, 2908802, 2914921, + 2919661, 2921478, 2926035, 2932024, 2934394, 2940561, 2944836, 2946991, + 2949708, 2955943, 2960242, 2962329, 2967771, 2969648, 2974181, 2980110, + 2985866, 2987873, 2991796, 2997855, 2999581, 3005942, 3009571, 3011784, + 3016678, 3022605, 3026648, 3028531, 3034481, 3036570, 3040335, 3046564, + 3050016, 3052235, 3056414, 3062773, 3063991, 3070044, 3074441, 3076450, + 3081345, 3087466, 3091903, 3093844, 3100182, 3102461, 3106600, 3112899, + 3115335, 3117484, 3121273, 3127442, 3130320, 3136315, 3140334, 3142149, + 3147996, 3149879, 3154402, 3160329, 3162699, 3168928, 3173237, 3175326, + 3179802, 3186161, 3189796, 3192015, 3198861, 3200870, 3204787, 3210840, + 3214267, 3216208, 3220101, 3226222, 3227948, 3234247, 3237906, 3240185, + 3245693, 3251862, 3256131, 3258280, 3263722, 3265537, 3270100, 3276095, + 3280401, 3282682, 3286831, 3293124, 3294342, 3300461, 3304888, 3306835, + 3310551, 3316540, 3320553, 3322370, 3328320, 3330475, 3334270, 3340437, + 3345782, 3347869, 3351624, 3357859, 3360737, 3366666, 3370719, 3372596, + 3375280, 3381339, 3385742, 3387749, 3394087, 3396300, 3400473, 3406834, + 3409221, 3415470, 3419259, 3421328, 3428306, 3430201, 3434220, 3440135, + 3442819, 3444840, 3449277, 3455318, 3457556, 3463935, 3468074, 3470273, + 3474978, 3481289, 3485468, 3487735, 3493045, 3495006, 3499403, 3505504, + 3509220, 3511055, 3515098, 3521073, 3522931, 3529112, 3532877, 3535014, + 3539848, 3545955, 3549878, 3551837, 3557663, 3559924, 3563553, 3569866, + 3575374, 3577509, 3581808, 3587995, 3589337, 3595314, 3599847, 3601676, + 3604719, 3610628, 3615185, 3617082, 3623544, 3625619, 3629894, 3636141, + 3640617, 3642818, 3646487, 3652860, 3655614, 3661653, 3665536, 3667563, + 3672838, 3675117, 3678776, 3685075, 3686801, 3692922, 3696815, 3698756, + 3704512, 3710507, 3715070, 3716885, 3722327, 3724476, 3728745, 3734914, + 3737697, 3739786, 3744095, 3750324, 3752694, 3758621, 3763144, 3765027, + 3769767, 3775820, 3779737, 3781746, 3788592, 3790811, 3794446, 3800805, + 3804619, 3806496, 3810549, 3816478, 3819356, 3825591, 3829346, 3831433, + 3833869, 3840230, 3844403, 3846616, 3852954, 3854961, 3859364, 3865423, + 3870380, 3872327, 3876754, 3882873, 3884091, 3890384, 3894533, 3896814, + 3900266, 3906433, 3910228, 3912383, 3918333, 3920150, 3924163, 3930152, + 3933855, 3939956, 3944353, 3946314, 3951624, 3953891, 3958070, 3964381, + 3967833, 3969970, 3973735, 3979916, 3981774, 3987749, 3991792, 3993627, + 3999224, 4005139, 4009158, 4011053, 4018031, 4020100, 4023889, 4030138, + 4032574, 4034773, 4038912, 4045291, 4047529, 4053570, 4058007, 4060028, + 4063314, 4069561, 4073836, 4075911, 4082373, 4084270, 4088827, 4094736, + 4099476, 4101503, 4105386, 4111425, 4114179, 4120552, 4124221, 4126422, + 4129589, 4135902, 4139531, 4141792, 4147618, 4149577, 4153500, 4159607, + 4165363, 4167192, 4171725, 4177702, 4179044, 4185231, 4189530, 4191665, + 4195899, 4202192, 4206341, 4208622, 4213932, 4215879, 4220306, 4226425, + 4230141, 4231958, 4235971, 4241960, 4243818, 4249985, 4253780, 4255935, + 4261212, 4267447, 4271202, 4273289, 4280267, 4282144, 4286197, 4292126, + 4294810, 4296817, 4301220, 4307279, 4309517, 4315878, 4320051, 4322264, + 4325622, 4331549, 4336072, 4337955, 4344417, 4346506, 4350815, 4357044, + 4361520, 4363739, 4367374, 4373733, 4376487, 4382540, 4386457, 4388466, + 4391825, 4397946, 4401839, 4403780, 4409606, 4411885, 4415544, 4421843, + 4427351, 4429500, 4433769, 4439938, 4441280, 4447275, 4451838, 4453653, + 4459426, 4461385, 4465308, 4471415, 4473141, 4479454, 4483083, 4485344, + 4490852, 4497039, 4501338, 4503473, 4508915, 4510744, 4515277, 4521254, + 4524229, 4526126, 4530683, 4536592, 4538962, 4545209, 4549484, 4551559, + 4556035, 4562408, 4566077, 4568278, 4575124, 4577151, 4581034, 4587073, + 4590959, 4593028, 4596817, 4603066, 4605944, 4611859, 4615878, 4617773, + 4620457, 4626498, 4630935, 4632956, 4639294, 4641493, 4645632, 4652011, + 4656648, 4658915, 4663094, 4669405, 4670623, 4676724, 4681121, 4683082, + 4686798, 4692773, 4696816, 4698651, 4704601, 4706738, 4710503, 4716684, + 4720097, 4726026, 4730079, 4731956, 4738934, 4741021, 4744776, 4751011, + 4753447, 4755660, 4759833, 4766194, 4768432, 4774491, 4778894, 4780901, + 4785798, 4791917, 4796344, 4798291, 4803601, 4805882, 4810031, 4816324, + 4819776, 4821931, 4825726, 4831893, 4833751, 4839740, 4843753, 4845570, + 4850476, 4856775, 4860434, 4862713, 4868539, 4870480, 4874373, 4880494, + 4886250, 4888065, 4892628, 4898623, 4899965, 4906134, 4910403, 4912552, + 4915275, 4921504, 4925813, 4927902, 4934364, 4936247, 4940770, 4946697, + 4951437, 4953446, 4957363, 4963416, 4966170, 4972529, 4976164, 4978383, + 4982904, 4984979, 4989254, 4995501, 4997871, 5003780, 5008337, 5010234, + 5014974, 5021013, 5024896, 5026923, 5033769, 5035970, 5039639, 5046012, + 5049119, 5051380, 5055009, 5061322, 5063048, 5069155, 5073078, 5075037, + 5080793, 5086770, 5091303, 5093132, 5098574, 5100709, 5105008, 5111195, + 5115573, 5117534, 5121931, 5128032, 5129250, 5135561, 5139740, 5142007, + 5145459, 5151640, 5155405, 5157542, 5163492, 5165327, 5169370, 5175345, + 5180882, 5182777, 5186796, 5192711, 5195589, 5201838, 5205627, 5207696, + 5210132, 5216511, 5220650, 5222849, 5229187, 5231208, 5235645, 5241686, + 5243279, 5249380, 5253297, 5255258, 5262104, 5264371, 5268006, 5274317, + 5278793, 5280930, 5285239, 5291420, 5293790, 5299765, 5304288, 5306123, + 5309160, 5315075, 5319638, 5321533, 5326975, 5329044, 5333313, 5339562, + 5345070, 5347269, 5350928, 5357307, 5359033, 5365074, 5368967, 5370988, + 5375810, 5382057, 5385852, 5387927, 5393877, 5395774, 5399787, 5405696, + 5409412, 5411439, 5415866, 5421905, 5423123, 5429496, 5433645, 5435846, + 5440549, 5446862, 5451035, 5453296, 5459634, 5461593, 5465996, 5472103, + 5474787, 5476616, 5480669, 5486646, 5489524, 5495711, 5499466, 5501601, + 5508118, 5510397, 5514536, 5520835, 5523073, 5529194, 5533631, 5535572, + 5538256, 5544251, 5548270, 5550085, 5557063, 5559212, 5563001, 5569170, + 5574513, 5576602, 5580367, 5586596, 5588454, 5594381, 5598424, 5600307, + 5604023, 5610076, 5614473, 5616482, 5621792, 5624011, 5628190, 5634549, + 5638875, 5640752, 5645285, 5651214, 5652556, 5658791, 5663090, 5665177, + 5670685, 5677046, 5680675, 5682888, 5688714, 5690721, 5694644, 5700703, + 5704124, 5706071, 5709954, 5716073, 5718827, 5725120, 5728789, 5731070, + 5735546, 5741713, 5745988, 5748143, 5754605, 5756422, 5760979, 5766968, + 5767765, 5774014, 5778283, 5780352, 5785794, 5787689, 5792252, 5798167, + 5803923, 5805944, 5809837, 5815878, 5817604, 5823983, 5827642, 5829841, + 5833010, 5839321, 5842956, 5845223, 5852069, 5854030, 5857947, 5864048, + 5868788, 5870623, 5875146, 5881121, 5883491, 5889672, 5893981, 5896118, + 5899416, 5905523, 5909926, 5911885, 5918223, 5920484, 5924657, 5930970, + 5933406, 5935541, 5939296, 5945483, 5948361, 5954338, 5958391, 5960220, + 5965823, 5971732, 5975745, 5977642, 5983592, 5985667, 5989462, 5995709, + 5999161, 6001362, 6005511, 6011884, 6013102, 6019141, 6023568, 6025595, + 6033356, 6035239, 6039282, 6045209, 6047067, 6053296, 6057061, 6059150, + 6062602, 6068961, 6073140, 6075359, 6080669, 6082678, 6087075, 6093128, + 6098091, 6100032, 6104469, 6110590, 6112828, 6119127, 6123266, 6125545, + 6127981, 6134150, 6137939, 6140088, 6147066, 6148881, 6152900, 6158895, + 6162689, 6164970, 6168639, 6174932, 6177686, 6183805, 6187688, 6189635, + 6194375, 6200364, 6204921, 6206738, 6213200, 6215355, 6219630, 6225797, + 6228582, 6230669, 6234968, 6241203, 6242545, 6248474, 6253007, 6254884, + 6260640, 6266699, 6270622, 6272629, 6278455, 6280668, 6284297, 6290658, + 6293843, 6295992, 6299757, 6305926, 6308804, 6314799, 6318842, 6320657, + 6325397, 6331518, 6335915, 6337856, 6344194, 6346473, 6350652, 6356951, + 6359604, 6361823, 6365962, 6372321, 6373539, 6379592, 6384029, 6386038, + 6391794, 6397721, 6401740, 6403623, 6409573, 6411662, 6415451, 6421680, + 6426526, 6428533, 6432416, 6438475, 6440201, 6446562, 6450231, 6452444, + 6455896, 6462131, 6466406, 6468493, 6473935, 6475812, 6480369, 6486298, + 6491385, 6493202, 6497735, 6503724, 6506094, 6512261, 6516560, 6518715, + 6521151, 6527444, 6531073, 6533354, 6540200, 6542147, 6546070, 6552189, + 6554826, 6560801, 6565364, 6567199, 6573661, 6575798, 6580067, 6586248, + 6588684, 6590951, 6594610, 6600921, 6603675, 6609776, 6613669, 6615630, + 6621101, 6627142, 6631059, 6633080, 6638906, 6641105, 6644740, 6651119, + 6654571, 6656640, 6660949, 6667198, 6668540, 6674455, 6678978, 6680873, + 6685191, 6691564, 6695737, 6697938, 6703248, 6705275, 6709678, 6715717, + 6721473, 6723370, 6727423, 6733332, 6735190, 6741437, 6745192, 6747267, + 6750560, 6756747, 6760542, 6762677, 6769655, 6771484, 6775497, 6781474, + 6786214, 6788173, 6792600, 6798707, 6800945, 6807258, 6811407, 6813668, + 6818441, 6820450, 6824887, 6830940, 6832158, 6838517, 6842656, 6844875, + 6850383, 6856612, 6860401, 6862490, 6868440, 6870323, 6874342, 6880269, + 6883822, 6885637, 6889680, 6895675, 6898553, 6904722, 6908487, 6910636, + 6915112, 6921411, 6925590, 6927869, 6934207, 6936148, 6940545, 6946666, + 6949956, 6952111, 6956410, 6962577, 6964947, 6970936, 6975469, 6977286, + 6979970, 6986089, 6990012, 6991959, 6998805, 7001086, 7004715, 7011008, + 7016227, 7018440, 7022109, 7028470, 7030196, 7036255, 7040138, 7042145, + 7045861, 7051790, 7056347, 7058224, 7063666, 7065753, 7070028, 7076263, + 7079696, 7086075, 7089710, 7091909, 7097735, 7099756, 7103673, 7109714, + 7113430, 7115325, 7119848, 7125763, 7127105, 7133354, 7137663, 7139732, + 7144567, 7150748, 7155017, 7157154, 7163616, 7165451, 7170014, 7175989, + 7178673, 7180634, 7184527, 7190628, 7193382, 7199693, 7203352, 7205619, + 7209437, 7215414, 7219427, 7221256, 7228234, 7230369, 7234164, 7240351, + 7244827, 7247088, 7251237, 7257550, 7259788, 7265895, 7270322, 7272281, + 7275194, 7281233, 7285636, 7287663, 7292973, 7295174, 7299347, 7305720, + 7311228, 7313303, 7317058, 7323305, 7325163, 7331072, 7335125, 7337022, + 7343847, 7345676, 7350233, 7356210, 7357552, 7363739, 7368014, 7370149, + 7373601, 7379914, 7383583, 7385844, 7391670, 7393629, 7397512, 7403619, + 7409024, 7411051, 7414974, 7421013, 7423767, 7430140, 7433769, 7435970, + 7438406, 7444653, 7448952, 7451027, 7457489, 7459386, 7463919, 7469828, + 7473194, 7475393, 7479572, 7485951, 7488189, 7494230, 7498627, 7500648, + 7505388, 7511303, 7515346, 7517241, 7524219, 7526288, 7530053, 7536302, + 7539533, 7541670, 7545459, 7551640, 7553498, 7559473, 7563492, 7565327, + 7571083, 7577184, 7581621, 7583582, 7588892, 7591159, 7595298, 7601609, + 7603070, 7609237, 7612992, 7615147, 7621097, 7622914, 7626967, 7632956, + 7638712, 7640659, 7645062, 7651181, 7652399, 7658692, 7662865, 7665146, + 7667737, 7674098, 7678247, 7680460, 7686798, 7688805, 7693232, 7699291, + 7704031, 7705908, 7709921, 7715850, 7718728, 7724963, 7728758, 7730845, + 7734707, 7740760, 7744653, 7746662, 7753508, 7755727, 7759386, 7765745, + 7768181, 7770270, 7774539, 7780768, 7783138, 7789065, 7793628, 7795511, + 7800532, 7806527, 7811050, 7812865, 7818307, 7820456, 7824765, 7830934, + 7834386, 7836665, 7840300, 7846599, 7848325, 7854446, 7858363, 7860304, + 7867709, 7869910, 7873539, 7879912, 7882666, 7888705, 7892628, 7894655, + 7897339, 7903248, 7907781, 7909678, 7916140, 7918215, 7922514, 7928761, + 7933530, 7935665, 7939940, 7946127, 7947469, 7953446, 7958003, 7959832, + 7963548, 7969655, 7973538, 7975497, 7981323, 7983584, 7987253, 7993566, + 7998448, 8000283, 8004302, 8010277, 8012135, 8018316, 8022105, 8024242, + 8029750, 8036061, 8040200, 8042467, 8047777, 8049738, 8054175, 8060276, + 8063127, 8065148, 8069545, 8075586, 8077824, 8084203, 8088382, 8090581, + 8095057, 8101306, 8105071, 8107140, 8114118, 8116013, 8120056, 8125971, + 8126628, 8132687, 8137114, 8139121, 8145459, 8147672, 8151821, 8158182, + 8162658, 8164745, 8168540, 8174775, 8177653, 8183582, 8187595, 8189472, + 8192963, 8198952, 8203005, 8204822, 8210772, 8212927, 8216682, 8222849, + 8228357, 8230638, 8234811, 8241104, 8242322, 8248441, 8252844, 8254791, + 8259177, 8265346, 8269655, 8271804, 8277246, 8279061, 8283584, 8289579, + 8293295, 8295236, 8299153, 8305274, 8307000, 8313299, 8316934, 8319213, + 8324366, 8330725, 8334384, 8336603, 8343449, 8345458, 8349351, 8355404, + 8358088, 8359971, 8364534, 8370461, 8372831, 8379060, 8383329, 8385418, + 8391797, 8393886, 8398155, 8404384, 8406754, 8412681, 8417244, 8419127, + 8421811, 8427864, 8431757, 8433766, 8440612, 8442831, 8446490, 8452849, + 8458002, 8460281, 8463916, 8470215, 8471941, 8478062, 8481979, 8483920, + 8487636, 8493631, 8498154, 8499969, 8505411, 8507560, 8511869, 8518038, + 8522424, 8524371, 8528774, 8534893, 8536111, 8542404, 8546577, 8548858, + 8554366, 8560533, 8564288, 8566443, 8572393, 8574210, 8578263, 8584252, + 8587743, 8589620, 8593633, 8599562, 8602440, 8608675, 8612470, 8614557, + 8619033, 8625394, 8629543, 8631756, 8638094, 8640101, 8644528, 8650587, + 8651244, 8657159, 8661202, 8663097, 8670075, 8672144, 8675909, 8682158, + 8686634, 8688833, 8693012, 8699391, 8701629, 8707670, 8712067, 8714088, + 8716939, 8723040, 8727477, 8729438, 8734748, 8737015, 8741154, 8747465, + 8752973, 8755110, 8758899, 8765080, 8766938, 8772913, 8776932, 8778767, + 8783649, 8789962, 8793631, 8795892, 8801718, 8803677, 8807560, 8813667, + 8817383, 8819212, 8823769, 8829746, 8831088, 8837275, 8841550, 8843685, + 8848454, 8854701, 8859000, 8861075, 8867537, 8869434, 8873967, 8879876, + 8882560, 8884587, 8888510, 8894549, 8897303, 8903676, 8907305, 8909506, + 8916911, 8918852, 8922769, 8928890, 8930616, 8936915, 8940550, 8942829, + 8946281, 8952450, 8956759, 8958908, 8964350, 8966165, 8970688, 8976683, + 8981704, 8983587, 8988150, 8994077, 8996447, 9002676, 9006945, 9009034, + 9011470, 9017829, 9021488, 9023707, 9030553, 9032562, 9036455, 9042508, + 9046370, 9048457, 9052252, 9058487, 9061365, 9067294, 9071307, 9073184, + 9077924, 9083983, 9088410, 9090417, 9096755, 9098968, 9103117, 9109478, + 9112069, 9114350, 9118523, 9124816, 9126034, 9132153, 9136556, 9138503, + 9144259, 9150248, 9154301, 9156118, 9162068, 9164223, 9167978, 9174145, + 9175606, 9181917, 9186056, 9188323, 9193633, 9195594, 9200031, 9206132, + 9211888, 9213723, 9217742, 9223717, 9225575, 9231756, 9235545, 9237682, + 9240913, 9247162, 9250927, 9252996, 9259974, 9261869, 9265912, 9271827, + 9276567, 9278588, 9282985, 9289026, 9291264, 9297643, 9301822, 9304021, + 9307387, 9313296, 9317829, 9319726, 9326188, 9328263, 9332562, 9338809, + 9341245, 9343446, 9347075, 9353448, 9356202, 9362241, 9366164, 9368191, + 9373596, 9379703, 9383586, 9385545, 9391371, 9393632, 9397301, 9403614, + 9407066, 9409201, 9413476, 9419663, 9421005, 9426982, 9431539, 9433368, + 9440193, 9442090, 9446143, 9452052, 9453910, 9460157, 9463912, 9465987, + 9471495, 9477868, 9482041, 9484242, 9489552, 9491579, 9495982, 9502021, + 9504934, 9506893, 9511320, 9517427, 9519665, 9525978, 9530127, 9532388, + 9536864, 9543051, 9546846, 9548981, 9555959, 9557788, 9561801, 9567778, + 9571596, 9573863, 9577522, 9583833, 9586587, 9592688, 9596581, 9598542, + 9601226, 9607201, 9611764, 9613599, 9620061, 9622198, 9626467, 9632648, + 9637483, 9639552, 9643861, 9650110, 9651452, 9657367, 9661890, 9663785, + 9667501, 9673542, 9677459, 9679480, 9685306, 9687505, 9691140, 9697519, + 9700952, 9707187, 9711462, 9713549, 9718991, 9720868, 9725425, 9731354, + 9735070, 9737077, 9740960, 9747019, 9748745, 9755106, 9758775, 9760988, + 9766207, 9772500, 9776129, 9778410, 9785256, 9787203, 9791126, 9797245, + 9799929, 9801746, 9806279, 9812268, 9814638, 9820805, 9825104, 9827259, + 9830549, 9836670, 9841067, 9843008, 9849346, 9851625, 9855804, 9862103, + 9866579, 9868728, 9872493, 9878662, 9881540, 9887535, 9891578, 9893393, + 9896946, 9902873, 9906892, 9908775, 9914725, 9916814, 9920603, 9926832, + 9932340, 9934559, 9938698, 9945057, 9946275, 9952328, 9956765, 9958774, + 9963547, 9965808, 9969957, 9976270, 9978508, 9984615, 9989042, 9991001, + 9995741, 10001718, 10005731, 10007560, 10014538, 10016673, 10020468, 10026655, + 10029948, 10032023, 10035778, 10042025, 10043883, 10049792, 10053845, 10055742, + 10061498, 10067537, 10071940, 10073967, 10079277, 10081478, 10085651, 10092024, + 10096342, 10098237, 10102760, 10108675, 10110017, 10116266, 10120575, 10122644, + 10126096, 10132475, 10136110, 10138309, 10144135, 10146156, 10150073, 10156114, + 10161585, 10163546, 10167439, 10173540, 10176294, 10182605, 10186264, 10188531, + 10190967, 10197148, 10201417, 10203554, 10210016, 10211851, 10216414, 10222389, + 10225026, 10231145, 10235068, 10237015, 10243861, 10246142, 10249771, 10256064, + 10258500, 10260655, 10264954, 10271121, 10273491, 10279480, 10284013, 10285830, + 10290917, 10296846, 10301403, 10303280, 10308722, 10310809, 10315084, 10321319, + 10324771, 10326984, 10330653, 10337014, 10338740, 10344799, 10348682, 10350689, + 10355535, 10361764, 10365553, 10367642, 10373592, 10375475, 10379494, 10385421, + 10391177, 10393186, 10397623, 10403676, 10404894, 10411253, 10415392, 10417611, + 10420264, 10426563, 10430742, 10433021, 10439359, 10441300, 10445697, 10451818, + 10456558, 10458373, 10462416, 10468411, 10471289, 10477458, 10481223, 10483372, + 10486557, 10492918, 10496547, 10498760, 10504586, 10506593, 10510516, 10516575, + 10522331, 10524208, 10528741, 10534670, 10536012, 10542247, 10546546, 10548633, + 10551418, 10557585, 10561860, 10564015, 10570477, 10572294, 10576851, 10582840, + 10587580, 10589527, 10593410, 10599529, 10602283, 10608576, 10612245, 10614526, + 10618320, 10624315, 10628334, 10630149, 10637127, 10639276, 10643065, 10649234, + 10651670, 10653949, 10658088, 10664387, 10666625, 10672746, 10677183, 10679124, + 10684087, 10690140, 10694537, 10696546, 10701856, 10704075, 10708254, 10714613, + 10718065, 10720154, 10723919, 10730148, 10732006, 10737933, 10741976, 10743859, + 10751620, 10753647, 10758074, 10764113, 10765331, 10771704, 10775853, 10778054, + 10781506, 10787753, 10791548, 10793623, 10799573, 10801470, 10805483, 10811392, + 10816995, 10818824, 10822877, 10828854, 10831732, 10837919, 10841674, 10843809, + 10846245, 10852558, 10856731, 10858992, 10865330, 10867289, 10871692, 10877799, + 10881097, 10883234, 10887543, 10893724, 10896094, 10902069, 10906592, 10908427, + 10913167, 10919268, 10923185, 10925146, 10931992, 10934259, 10937894, 10944205, + 10947374, 10949573, 10953232, 10959611, 10961337, 10967378, 10971271, 10973292, + 10979048, 10984963, 10989526, 10991421, 10996863, 10998932, 11003201, 11009450, + 11010247, 11016236, 11020793, 11022610, 11029072, 11031227, 11035502, 11041669, + 11046145, 11048426, 11052095, 11058388, 11061142, 11067261, 11071144, 11073091, + 11076512, 11082571, 11086494, 11088501, 11094327, 11096540, 11100169, 11106530, + 11112038, 11114125, 11118424, 11124659, 11126001, 11131930, 11136463, 11138340, + 11142666, 11149025, 11153204, 11155423, 11160733, 11162742, 11167139, 11173192, + 11176908, 11178791, 11182834, 11188761, 11190619, 11196848, 11200613, 11202702, + 11208045, 11214214, 11218003, 11220152, 11227130, 11228945, 11232964, 11238959, + 11241643, 11243584, 11248021, 11254142, 11256380, 11262679, 11266818, 11269097, + 11275614, 11277749, 11281504, 11287691, 11290569, 11296546, 11300599, 11302428, + 11305112, 11311219, 11315622, 11317581, 11323919, 11326180, 11330353, 11336666, + 11341369, 11343570, 11347719, 11354092, 11355310, 11361349, 11365776, 11367803, + 11371519, 11377428, 11381441, 11383338, 11389288, 11391363, 11395158, 11401405, + 11406227, 11408248, 11412141, 11418182, 11419908, 11426287, 11429946, 11432145, + 11437653, 11443902, 11448171, 11450240, 11455682, 11457577, 11462140, 11468055, + 11471092, 11472927, 11477450, 11483425, 11485795, 11491976, 11496285, 11498422, + 11502898, 11509209, 11512844, 11515111, 11521957, 11523918, 11527835, 11533936, + 11535529, 11541570, 11546007, 11548028, 11554366, 11556565, 11560704, 11567083, + 11569519, 11571588, 11575377, 11581626, 11584504, 11590419, 11594438, 11596333, + 11601870, 11607845, 11611888, 11613723, 11619673, 11621810, 11625575, 11631756, + 11635208, 11637475, 11641654, 11647965, 11649183, 11655284, 11659681, 11661642, + 11666020, 11672207, 11676506, 11678641, 11684083, 11685912, 11690445, 11696422, + 11702178, 11704137, 11708060, 11714167, 11715893, 11722206, 11725835, 11728096, + 11731203, 11737576, 11741245, 11743446, 11750292, 11752319, 11756202, 11762241, + 11766981, 11768878, 11773435, 11779344, 11781714, 11787961, 11792236, 11794311, + 11798832, 11801051, 11804686, 11811045, 11813799, 11819852, 11823769, 11825778, + 11830518, 11836445, 11840968, 11842851, 11849313, 11851402, 11855711, 11861940, + 11864663, 11866812, 11871081, 11877250, 11878592, 11884587, 11889150, 11890965, + 11896721, 11902842, 11906735, 11908676, 11914502, 11916781, 11920440, 11926739, + 11931645, 11933462, 11937475, 11943464, 11945322, 11951489, 11955284, 11957439, + 11960891, 11967184, 11971333, 11973614, 11978924, 11980871, 11985298, 11991417, + 11996314, 11998321, 12002724, 12008783, 12011021, 12017382, 12021555, 12023768, + 12026204, 12032439, 12036194, 12038281, 12045259, 12047136, 12051189, 12057118, + 12060531, 12066712, 12070477, 12072614, 12078564, 12080399, 12084442, 12090417, + 12094133, 12096094, 12100491, 12106592, 12107810, 12114121, 12118300, 12120567, + 12125204, 12131583, 12135722, 12137921, 12144259, 12146280, 12150717, 12156758, + 12159442, 12161337, 12165356, 12171271, 12174149, 12180398, 12184187, 12186256, + 12190142, 12196181, 12200064, 12202091, 12208937, 12211138, 12214807, 12221180, + 12225656, 12227731, 12232006, 12238253, 12240623, 12246532, 12251089, 12252986, + 12255961, 12261938, 12266471, 12268300, 12273742, 12275877, 12280176, 12286363, + 12291871, 12294132, 12297761, 12304074, 12305800, 12311907, 12315830, 12317789, + 12323562, 12325377, 12329940, 12335935, 12337277, 12343446, 12347715, 12349864, + 12355372, 12361671, 12365330, 12367609, 12373435, 12375376, 12379269, 12385390, + 12388749, 12390758, 12394675, 12400728, 12403482, 12409841, 12413476, 12415695, + 12420171, 12426400, 12430709, 12432798, 12439260, 12441143, 12445666, 12451593, + 12454951, 12457164, 12461337, 12467698, 12469936, 12475995, 12480398, 12482405, + 12485089, 12491018, 12495071, 12496948, 12503926, 12506013, 12509768, 12516003, + 12521280, 12523435, 12527230, 12533397, 12535255, 12541244, 12545257, 12547074, + 12550790, 12556909, 12561336, 12563283, 12568593, 12570874, 12575023, 12581316, + 12585550, 12587685, 12591984, 12598171, 12599513, 12605490, 12610023, 12611852, + 12617608, 12623715, 12627638, 12629597, 12635423, 12637684, 12641313, 12647626, + 12650793, 12652994, 12656663, 12663036, 12665790, 12671829, 12675712, 12677739, + 12682479, 12688388, 12692945, 12694842, 12701304, 12703379, 12707654, 12713901, + 12717187, 12719208, 12723645, 12729686, 12731924, 12738303, 12742442, 12744641, + 12747077, 12753326, 12757115, 12759184, 12766162, 12768057, 12772076, 12777991, + 12783588, 12785423, 12789466, 12795441, 12797299, 12803480, 12807245, 12809382, + 12812834, 12819145, 12823324, 12825591, 12830901, 12832862, 12837259, 12843360, + 12847063, 12853052, 12857065, 12858882, 12864832, 12866987, 12870782, 12876949, + 12880401, 12882682, 12886831, 12893124, 12894342, 12900461, 12904888, 12906835, + 12911792, 12917851, 12922254, 12924261, 12930599, 12932812, 12936985, 12943346, + 12945782, 12947869, 12951624, 12957859, 12960737, 12966666, 12970719, 12972596, + 12976410, 12982769, 12986404, 12988623, 12995469, 12997478, 13001395, 13007448, + 13012188, 13014071, 13018594, 13024521, 13026891, 13033120, 13037429, 13039518, + 13042301, 13048470, 13052739, 13054888, 13060330, 13062145, 13066708, 13072703, + 13078459, 13080400, 13084293, 13090414, 13092140, 13098439, 13102098, 13104377, + 13109652, 13111679, 13115562, 13121601, 13124355, 13130728, 13134397, 13136598, + 13141074, 13147321, 13151596, 13153671, 13160133, 13162030, 13166587, 13172496, + 13175539, 13177368, 13181901, 13187878, 13189220, 13195407, 13199706, 13201841, + 13207349, 13213662, 13217291, 13219552, 13225378, 13227337, 13231260, 13237367, + 13242201, 13244338, 13248103, 13254284, 13256142, 13262117, 13266160, 13267995, + 13271711, 13277812, 13282209, 13284170, 13289480, 13291747, 13295926, 13302237, + 13306942, 13309141, 13313280, 13319659, 13321897, 13327938, 13332375, 13334396, + 13337080, 13342995, 13347014, 13348909, 13355887, 13357956, 13361745, 13367994, + 13370381, 13376742, 13380915, 13383128, 13389466, 13391473, 13395876, 13401935, + 13404619, 13406496, 13410549, 13416478, 13419356, 13425591, 13429346, 13431433, + 13436778, 13442945, 13446740, 13448895, 13454845, 13456662, 13460675, 13466664, + 13470380, 13472327, 13476754, 13482873, 13484091, 13490384, 13494533, 13496814, + 13501120, 13507115, 13511678, 13513493, 13518935, 13521084, 13525353, 13531522, + 13537030, 13539309, 13542968, 13549267, 13550993, 13557114, 13561007, 13562948, + 13566375, 13572428, 13576345, 13578354, 13585200, 13587419, 13591054, 13597413, + 13601889, 13603978, 13608287, 13614516, 13616886, 13622813, 13627336, 13629219, + 13635066, 13636881, 13640900, 13646895, 13649773, 13655942, 13659731, 13661880, + 13664316, 13670615, 13674754, 13677033, 13683371, 13685312, 13689749, 13695870, + 13700765, 13702774, 13707171, 13713224, 13714442, 13720801, 13724980, 13727199, + 13730651, 13736880, 13740645, 13742734, 13748684, 13750567, 13754610, 13760537, + 13765431, 13767644, 13771273, 13777634, 13779360, 13785419, 13789342, 13791349, + 13797105, 13803034, 13807567, 13809444, 13814886, 13816973, 13821272, 13827507, + 13830224, 13832379, 13836654, 13842821, 13845191, 13851180, 13855737, 13857554, + 13862294, 13868413, 13872296, 13874243, 13881089, 13883370, 13887039, 13893332, + 13893731, 13899912, 13904221, 13906358, 13912820, 13914655, 13919178, 13925153, + 13929893, 13931854, 13935771, 13941872, 13944626, 13950937, 13954572, 13956839, + 13959940, 13966319, 13969978, 13972177, 13978003, 13980024, 13983917, 13989958, + 13995714, 13997609, 14002172, 14008087, 14009429, 14015678, 14019947, 14022016, + 14026414, 14032453, 14036880, 14038907, 14044217, 14046418, 14050567, 14056940, + 14060392, 14062467, 14066262, 14072509, 14074367, 14080276, 14084289, 14086186, + 14091721, 14097698, 14101751, 14103580, 14110558, 14112693, 14116448, 14122635, + 14125071, 14127332, 14131505, 14137818, 14140056, 14146163, 14150566, 14152525, + 14159392, 14161611, 14165790, 14172149, 14173367, 14179420, 14183817, 14185826, + 14189542, 14195469, 14199512, 14201395, 14207345, 14209434, 14213199, 14219428, + 14224711, 14226860, 14230649, 14236818, 14239696, 14245691, 14249710, 14251525, + 14254209, 14260330, 14264767, 14266708, 14273046, 14275325, 14279464, 14285763, + 14289133, 14290950, 14295507, 14301496, 14303866, 14310033, 14314308, 14316463, + 14320939, 14327232, 14330901, 14333182, 14340028, 14341975, 14345858, 14351977, + 14355338, 14357345, 14361268, 14367327, 14369053, 14375414, 14379043, 14381256, + 14386764, 14392999, 14397298, 14399385, 14404827, 14406704, 14411237, 14417166, + 14418873, 14424914, 14428807, 14430828, 14436654, 14438853, 14442512, 14448891, + 14454399, 14456468, 14460737, 14466986, 14468328, 14474243, 14478806, 14480701, + 14483678, 14489653, 14494176, 14496011, 14502473, 14504610, 14508919, 14515100, + 14519576, 14521843, 14525478, 14531789, 14534543, 14540644, 14544561, 14546522, + 14550388, 14556575, 14560330, 14562465, 14569443, 14571272, 14575325, 14581302, + 14583986, 14585945, 14590348, 14596455, 14598693, 14605006, 14609179, 14611440, + 14616083, 14622456, 14626605, 14628806, 14634116, 14636143, 14640570, 14646609, + 14650325, 14652222, 14656235, 14662144, 14664002, 14670249, 14674044, 14676119, + 14681382, 14687693, 14691352, 14693619, 14700465, 14702426, 14706319, 14712420, + 14715104, 14716939, 14721502, 14727477, 14729847, 14736028, 14740297, 14742434, + 14747201, 14753450, 14757759, 14759828, 14765270, 14767165, 14771688, 14777603, + 14781319, 14783340, 14787257, 14793298, 14795024, 14801403, 14805038, 14807237, + 14812139, 14818048, 14822101, 14823998, 14829948, 14832023, 14835778, 14842025, + 14847533, 14849734, 14853907, 14860280, 14861498, 14867537, 14871940, 14873967, + 14876812, 14882919, 14887346, 14889305, 14895643, 14897904, 14902053, 14908366, + 14912842, 14914977, 14918772, 14924959, 14927837, 14933814, 14937827, 14939656, + 14944447, 14946388, 14950785, 14956906, 14959144, 14965443, 14969622, 14971901, + 14976377, 14982546, 14986311, 14988460, 14995438, 14997253, 15001296, 15007291, + 15010776, 15012659, 15016678, 15022605, 15024463, 15030692, 15034481, 15036570, + 15042078, 15048437, 15052576, 15054795, 15060105, 15062114, 15066551, 15072604, + 15076978, 15079065, 15083340, 15089575, 15090917, 15096846, 15101403, 15103280, + 15106996, 15113055, 15116938, 15118945, 15124771, 15126984, 15130653, 15137014, + 15142165, 15144446, 15148075, 15154368, 15157122, 15163241, 15167164, 15169111, + 15171795, 15177784, 15182317, 15184134, 15190596, 15192751, 15197050, 15203217, + 15206140, 15212055, 15216578, 15218473, 15223915, 15225984, 15230293, 15236542, + 15239994, 15242193, 15245828, 15252207, 15253933, 15259974, 15263891, 15265912, + 15271323, 15277424, 15281317, 15283278, 15290124, 15292391, 15296050, 15302361, + 15304797, 15306934, 15311203, 15317384, 15319754, 15325729, 15330292, 15332127, + 15335473, 15341786, 15345935, 15348196, 15354534, 15356493, 15360920, 15367027, + 15371767, 15373596, 15377609, 15383586, 15386464, 15392651, 15396446, 15398581, + 15401814, 15408061, 15411816, 15413891, 15419841, 15421738, 15425791, 15431700, + 15437456, 15439483, 15443886, 15449925, 15451143, 15457516, 15461689, 15463890, + 15469413, 15471502, 15475291, 15481520, 15483378, 15489305, 15493324, 15495207, + 15500963, 15507016, 15511453, 15513462, 15518772, 15520991, 15525130, 15531489, + 15534082, 15536361, 15540540, 15546839, 15549077, 15555198, 15559595, 15561536, + 15566276, 15572271, 15576314, 15578129, 15585107, 15587256, 15591021, 15597190, + 15601064, 15603011, 15606934, 15613053, 15615807, 15622100, 15625729, 15628010, + 15630446, 15636613, 15640912, 15643067, 15649529, 15651346, 15655879, 15661868, + 15666895, 15668772, 15673329, 15679258, 15680600, 15686835, 15691110, 15693197, + 15696649, 15703010, 15706679, 15708892, 15714718, 15716725, 15720608, 15726667, + 15729298, 15735417, 15739820, 15741767, 15747077, 15749358, 15753531, 15759824, + 15765332, 15767487, 15771242, 15777409, 15779267, 15785256, 15789309, 15791126, + 15794677, 15800606, 15804619, 15806496, 15813474, 15815561, 15819356, 15825591, + 15830067, 15832280, 15836429, 15842790, 15845028, 15851087, 15855514, 15857521, + 15860831, 15867060, 15871329, 15873418, 15879880, 15881763, 15886326, 15892253, + 15894937, 15896946, 15900839, 15906892, 15909646, 15916005, 15919664, 15921883, + 15927096, 15933395, 15937030, 15939309, 15945135, 15947076, 15950993, 15957114, + 15960830, 15962645, 15967168, 15973163, 15974505, 15980674, 15984983, 15987132, + 15994635, 15996896, 16000565, 16006878, 16008604, 16014711, 16018594, 16020553, + 16024269, 16030246, 16034803, 16036632, 16042074, 16044209, 16048484, 16054671, + 16059500, 16061575, 16065874, 16072121, 16074491, 16080400, 16084933, 16086830, + 16089514, 16095553, 16099476, 16101503, 16108349, 16110550, 16114179, 16120552, + 16124358, 16126253, 16130296, 16136211, 16139089, 16145338, 16149103, 16151172, + 16155648, 16162027, 16166206, 16168405, 16174743, 16176764, 16181161, 16187202, + 16190113, 16192074, 16196511, 16202612, 16203830, 16210141, 16214280, 16216547, + 16222055, 16228236, 16232025, 16234162, 16240112, 16241947, 16245966, 16251941, + 16253256, 16259491, 16263286, 16265373, 16272351, 16274228, 16278241, 16284170, + 16288910, 16290917, 16295344, 16301403, 16303641, 16310002, 16314151, 16316364, + 16319023, 16325316, 16329489, 16331770, 16337080, 16339027, 16343430, 16349549, + 16355305, 16357122, 16361175, 16367164, 16369022, 16375189, 16378944, 16381099, + 16385925, 16392046, 16395963, 16397904, 16403730, 16406009, 16409644, 16415943, + 16419395, 16421544, 16425853, 16432022, 16433364, 16439359, 16443882, 16445697, + 16450786, 16456713, 16461276, 16463159, 16469621, 16471710, 16475979, 16482208, + 16484644, 16486863, 16490522, 16496881, 16499635, 16505688, 16509581, 16511590, + 16518353, 16520250, 16524783, 16530692, 16533062, 16539309, 16543608, 16545683, + 16548119, 16554492, 16558121, 16560322, 16567168, 16569195, 16573118, 16579157, + 16584630, 16586589, 16590472, 16596579, 16598305, 16604618, 16608287, 16610548, + 16614000, 16620187, 16624462, 16626597, 16632039, 16633868, 16638425, 16644402, + 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 +}; + +uint32_t gly24128Enc(uint32_t n) { + assert (n <= 4095); + return gly24128EncTbl[n]; } - - -//Find roots of the error+erasure locator polynomial by Chien search - -for (i = 1; i <= nroots; i++) { reg[i] = lambda[i]; } -count = 0 ;//number of roots of lambda(x) -for (i = 1; i <= 63; i++) { - q = 1 ;//lambda[0] is always 0 - for (j = DegLambda; j >= 1; j += -1) { - if (reg[j] != 63) { - reg[j] = (reg[j] + j) % 63; - q = q ^ rsGFexp[reg[j]]; - } - } - if (q == 0) { //it is a root - //store root (index-form) and error location number - root[count] = i; - locn[count] = i - 1; - //if wehave max possible roots, abort search to save time - count = count + 1; if (count == DegLambda) { break; } - } -} - -if (DegLambda != count) { - //deg(lambda) unequal to number of roots => uncorrectable error detected - count = -1; - goto rsDecFinish; -} - - - -//compute err+eras evaluator poly omega(x) -// = s(x)*lambda(x) (modulo x**nroots). in index form. Also find deg(omega). - -DegOmega = 0; -for (i = 0; i <= nroots - 1; i++) { - tmp = 0; - if (DegLambda < i) { j = DegLambda; } else { j = i; } - for ( /* j = j */ ; j >= 0; j += -1) { - if ((S[i - j] != 63) && (lambda[j] != 63)) { - tmp = tmp ^ rsGFexp[(S[i - j] + lambda[j]) % 63]; - } - } - if (tmp) { DegOmega = i; } - omega[i] = rsGFlog[tmp]; -} -omega[nroots] = 63; - - -//compute error values in poly-form: -// num1 = omega(inv(X(l))) -// num2 = inv(X(l))**(FCR - 1) -// den = lambda_pr(inv(X(l))) - -for (j = count - 1; j >= 0; j += -1) { - num1 = 0; - for (i = DegOmega; i >= 0; i += -1) { - if (omega[i] != 63) { - num1 = num1 ^ rsGFexp[(omega[i] + i * root[j]) % 63]; - } - } - num2 = rsGFexp[0]; - den = 0; - - // lambda[i+1] for i even is the formal derivative lambda_pr of lambda[i] - if (DegLambda < nroots) { i = DegLambda; } else { i = nroots; } - for (i = i & ~1; i >= 0; i += -2) { - if (lambda[i + 1] != 63) { - den = den ^ rsGFexp[(lambda[i + 1] + i * root[j]) % 63]; - } - } - if (den == 0) { count = -1; goto rsDecFinish; } - - // apply error to data - if (num1 != 0) { - if (locn[j] < FirstInfo) { count = -1; goto rsDecFinish ; } //added by me - HB[locn[j]] = HB[locn[j]] ^ (rsGFexp[(rsGFlog[num1] + rsGFlog[num2] + 63 - rsGFlog[den]) % 63]); - } -} - - -rsDecFinish: -return (count); - -} - -/*********************************************************************/ -/*********************************************************************/ -/*********************************************************************/ - uint32_t gly23127GetSyn (uint32_t pattern) { + uint32_t aux = 0x400000; -uint32_t aux = 0x400000; - -while(pattern & 0xFFFFF800) { - while ((aux & pattern) == 0) { - aux = aux >> 1; - } - pattern = pattern ^ (aux / 0x800 * 0xC75) ;//generator is C75 -} - -return pattern; - + while(pattern & 0xFFFFF800) { + while ((aux & pattern) == 0) { + aux = aux >> 1; + } + pattern = pattern ^ (aux / 0x800 * 0xC75) ;//generator is C75 + } + return pattern; } -uint32_t gly24128Dec (uint32_t n) { - -//based on gly23127Dec - -uint32_t CW = n >> 1 ; //toss the parity bit -uint32_t correction = gly23127DecTbl[gly23127GetSyn(CW)]; -CW = (CW ^ correction) >> 11; - -return CW; - +uint32_t gly24128Dec (uint32_t n) { //based on gly23127Dec + uint32_t CW = n >> 1 ; //toss the parity bit + uint32_t correction = gly23127DecTbl[gly23127GetSyn(CW)]; + CW = (CW ^ correction) >> 11; + return CW; } uint32_t gly23127Dec (uint32_t CW) { uint32_t correction = gly23127DecTbl[gly23127GetSyn(CW)]; @@ -464,109 +700,3 @@ uint32_t gly23127Dec (uint32_t CW) { return CW; } -void ProcHDU(const_bit_vector A) { -int i, j, k, ec; -uint8_t HB[63]; // "hexbit" array - -//header code word is 324 dibits (padded by 5 trailing zero dibits) -// 324 dibits = 648 bits = 36 18-bit Golay codewords - -//do (18,6,8) shortened Golay decode - make 36 hexbits for rs dec -for (i = 0; i <= 26; i++) { - HB[i] = 0; -} -k = 0; -for (i = 0; i < 36; i ++) { // 36 codewords - uint32_t CW = 0; - for (j = 0; j < 18; j++) { // 18 bits / cw - CW = (CW << 1) + A [ hdu_codeword_bits[k++] ]; - } - HB[27 + i] = gly24128Dec(CW) & 63; -} - -//do (36,20,17) RS decode -ec = rsDec(16, 27, HB); -//120 info bits = 20 hexbits: (27..46) - //72 bits MI: (27..38) - // 8 bits MFID - // 8 bits ALGID - //16 bits KID - //16 bits TGID - -uint32_t MFID = HB[39] * 4 + (HB[40] >> 4); -uint32_t ALGID = (HB[40] & 15) * 16 + (HB[41] >> 2); -uint32_t KID = (HB[41] & 3) * 16384 + HB[42] * 256 + HB[43] * 4 + (HB[44] >> 4); -uint32_t TGID = (HB[44] & 15) * 4096 + HB[45] * 64 + HB[46]; - -fprintf (stderr, " HDU: rc %d mfid %x alg %x kid %x tgid %d", ec, MFID, ALGID, KID, TGID); - -} - -void ProcLLDU(const_bit_vector A, uint8_t HB[]) { -int i, j, k; -for (i = 0; i <= 38; i++) { - HB[i] = 0; -} -k = 0; -for (i = 0; i < 24; i ++) { // 24 10-bit codewords - uint32_t CW = 0; - for (j = 0; j < 10; j++) { // 10 bits / cw - CW = (CW << 1) + A [ imbe_ldu_ls_data_bits[k++] ]; - } - HB[39 + i] = hmg1063Dec( CW >> 4, CW & 0xF ); -} - -} - -void ProcLC(uint8_t HB[]) { - int ec = rsDec(12, 39, HB); - int pb = HB[39] >> 5; - int sf = (HB[39] & 16) >> 4; - int lco = (HB[39] & 15) * 4 + (HB[40] >> 4); - fprintf(stderr, " LC: rc %d pb %d sf %d lco %d", ec, pb, sf, lco); -} - -void ProcLDU1(const_bit_vector A) { - uint8_t HB[63]; // "hexbit" array - - ProcLLDU(A, HB); - ProcLC(HB); -} - -void ProcLDU2(const_bit_vector A) { - uint8_t HB[63]; // "hexbit" array - - ProcLLDU(A, HB); - int ec = rsDec(8, 39, HB); - - uint32_t ALGID = HB[51] * 4 + (HB[52] >> 4); - uint32_t KID = (HB[52] & 15) * 4096 + HB[53] * 64 + HB[54]; - - fprintf(stderr, " LDU2: rc %d ALGID %x KID %x MI ", ec, ALGID, KID); - for (int i = 39; i <= 50; i++) { - fprintf(stderr, "%02x ", HB[ i ]); - } - // fprintf(stderr, "\n"); -} - -void ProcTDU(const_bit_vector A) { -uint8_t HB[63]; // "hexbit" array - -int i, j, k; -for (i = 0; i <= 38; i++) { - HB[i] = 0; -} -k = 0; -for (i = 0; i <= 22; i += 2) { - uint32_t CW = 0; - for (j = 0; j < 12; j++) { // 12 24-bit codewords - CW = (CW << 1) + A [ hdu_codeword_bits[k++] ]; - CW = (CW << 1) + A [ hdu_codeword_bits[k++] ]; - } - uint32_t D = gly24128Dec(CW); - HB[39 + i] = D >> 6; - HB[40 + i] = D & 63; -} -ProcLC(HB); -} - diff --git a/op25/gr-op25_repeater/lib/rs.h b/op25/gr-op25_repeater/lib/rs.h index 37c7c21..d45093f 100644 --- a/op25/gr-op25_repeater/lib/rs.h +++ b/op25/gr-op25_repeater/lib/rs.h @@ -5,12 +5,8 @@ #include <stdlib.h> #include <stdint.h> #include <vector> -#include <op25_imbe_frame.h> -void ProcHDU(const_bit_vector A); -void ProcTDU(const_bit_vector A); -void ProcLDU1(const_bit_vector A); -void ProcLDU2(const_bit_vector A); +uint32_t gly24128Enc (uint32_t n) ; uint32_t gly24128Dec (uint32_t n) ; uint32_t gly23127Dec (uint32_t n) ; diff --git a/op25/gr-op25_repeater/lib/rx_sync.cc b/op25/gr-op25_repeater/lib/rx_sync.cc new file mode 100644 index 0000000..bf6c359 --- /dev/null +++ b/op25/gr-op25_repeater/lib/rx_sync.cc @@ -0,0 +1,823 @@ +// P25 Decoder (C) Copyright 2013, 2014, 2015, 2016, 2017 Max H. Parke KA1RBI +// +// This file is part of OP25 +// +// OP25 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 3, or (at your option) +// any later version. +// +// OP25 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 OP25; see the file COPYING. If not, write to the Free +// Software Foundation, Inc., 51 Franklin Street, Boston, MA +// 02110-1301, USA. + +#include <stdio.h> +#include <stdint.h> +#include <stdlib.h> +#include <string.h> +#include <string> +#include <iostream> +#include <deque> +#include <assert.h> +#include <errno.h> +#include <unistd.h> + +#include "rx_sync.h" + +#include "bit_utils.h" + +#include "check_frame_sync.h" + +#include "p25p2_vf.h" +#include "mbelib.h" +#include "ambe.h" +#include "rs.h" +#include "crc16.h" + +#include "ysf_const.h" +#include "dmr_const.h" +#include "p25_frame.h" +#include "op25_imbe_frame.h" +#include "software_imbe_decoder.h" +#include "op25_audio.h" + +namespace gr{ + namespace op25_repeater{ + +void rx_sync::cbuf_insert(const uint8_t c) { + d_cbuf[d_cbuf_idx] = c; + d_cbuf[d_cbuf_idx + CBUF_SIZE] = c; + d_cbuf_idx = (d_cbuf_idx + 1) % CBUF_SIZE; +} + +void rx_sync::sync_reset(void) { + d_threshold = 0; + d_shift_reg = 0; + d_unmute_until[0] = 0; + d_unmute_until[1] = 0; +} + +static inline void debug_dump(const char* s, const uint8_t p[], int l) { + char buf[64]; + for (int i=0; i<l; i++) { + if (i*2+3 >= sizeof(buf)) + break; + sprintf(buf+i*2, "%02x", p[i]); + } + fprintf(stderr, "%s: %s\n", s, buf); +} + +static inline void cfill(uint8_t result[], const uint8_t src[], int len) { + for (int i=0; i<len; i++) + result[i] = load_i(src+i*8, 8); +} + +static int ysf_decode_fich(const uint8_t src[100], uint8_t dest[32]) { // input is 100 dibits, result is 32 bits +// return -1 on decode error, else 0 + static const int pc[] = {0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1}; + uint8_t buf[100]; + for (int i=0; i<20; i++) { + for (int j=0; j<5; j++) { + buf[j+i*5] = src[i+j*20]; + } + } + uint8_t dr = 0; + uint8_t ans[100]; + /* fake trellis decode */ + /* TODO: make less fake */ + for (int i=0; i<100; i++) { + uint8_t sym = buf[i]; + uint8_t d0 = ((dr << 1) | 0) & 0x1f; + uint8_t r0 = (pc[ d0 & 0x19 ] << 1) + pc[ d0 & 0x17]; + uint8_t d1 = ((dr << 1) | 1) & 0x1f; + uint8_t r1 = (pc[ d1 & 0x19 ] << 1) + pc[ d1 & 0x17]; + if (sym == r0) { + ans[i] = 0; + dr = d0; + } else if (sym == r1) { + ans[i] = 1; + dr = d1; + } else { + return -1; /* decode error */ + } + } + uint8_t fich_bits[12*4]; + store_i(gly24128Dec(load_i(ans+24*0, 24)), fich_bits+12*0, 12); + store_i(gly24128Dec(load_i(ans+24*1, 24)), fich_bits+12*1, 12); + store_i(gly24128Dec(load_i(ans+24*2, 24)), fich_bits+12*2, 12); + store_i(gly24128Dec(load_i(ans+24*3, 24)), fich_bits+12*3, 12); + uint16_t crc_result = crc16(fich_bits, 48); + if (crc_result != 0) + return -1; // crc failure + memcpy(dest, fich_bits, 32); + return 0; +} + +void rx_sync::ysf_sync(const uint8_t dibitbuf[], bool& ysf_fullrate, bool& unmute) { + uint8_t fich_buf[32]; + int rc = ysf_decode_fich(dibitbuf+20, fich_buf); + if (rc == 0) { + uint32_t fich = load_i(fich_buf, 32); + uint32_t dt = (fich >> 8) & 3; + d_shift_reg = dt; + } + switch(d_shift_reg) { + case 0: // voice/data mode 1 + unmute = false; + break; + case 1: // data mode + unmute = false; + break; + case 2: // voice/data mode 2 + unmute = true; + ysf_fullrate = false; + break; + case 3: // voice fr mode + unmute = true; + ysf_fullrate = true; + break; + } + if (d_debug > 5 && !unmute) + fprintf(stderr, "ysf_sync: muting audio: dt: %d, rc: %d\n", d_shift_reg, rc); +} + +static int decode_embedded(uint8_t result_lc[72], const uint8_t src[32*4], int srclen) { +// return code < 0 indicates decode failure + static const int lengths[] = {11, 11, 10, 10, 10, 10, 10}; + int s_index = 0; + uint8_t decode[16*8]; + uint8_t decode_lc[72+5]; + int srcp = 0; + if (srclen != 32*4) + return -4; + for (int i=0; i<16; i++) { + for (int j=0; j<8; j++){ + decode[i+16*j] = src[srcp++]; + } + } + for (int i=0; i<7; i++) { + int v = load_i(&decode[16*i], 16); + int rc = hamming_16_11_decode(v); + if (rc < 0) + return rc; + store_i(rc, &decode_lc[11*i], 11); + memcpy(&result_lc[s_index], &decode_lc[11*i], lengths[i]); + s_index += lengths[i]; + } + uint16_t r_csum = 0; + for (int i=0; i<5; i++) { + r_csum <<= 1; + r_csum |= decode_lc[(i+2)*11+10] & 1; + } + uint16_t c_csum = 0; + for (int i=0; i<9; i++) { + c_csum += load_i(&result_lc[i*8], 8); + } + c_csum = c_csum % 31; + if (r_csum != c_csum) + return -3; + return 0; // OK return +} + +static void init_xlist(int * lp) { + for (int i=0; i < XLIST_SIZE; i++) { + lp[i] = -1; + } +} + +static bool find_xlist(const int grp, const int * lp) { + for (int i=0; i < XLIST_SIZE; i++) { + if (lp[i] == grp && grp > 0) + return true; + } + return false; +} + +static bool add_xlist(const int grp, int * lp) { +// returns false if failed (grp bad, dup or list full), otherwise true + for (int i=0; i < XLIST_SIZE; i++) { + if (lp[i] == grp || grp < 1) + return false; // dup or group invalid + if (lp[i] == -1) { + lp[i] = grp; + return true; + } + } + return false; +} + +static int count_xlist(const int * lp) { + int res=0; + for (int i=0; i < XLIST_SIZE; i++) { + if (lp[i] > 0) + res += 1; + } + return res; +} + +void rx_sync::insert_whitelist(int grpaddr) { + bool rc = add_xlist(grpaddr, d_whitelist); + if (rc == false) + fprintf(stderr, "insert_whitelist failed for grp=%d- dup or list full\n", grpaddr); + else if (d_debug) + fprintf(stderr, "insert_whitelist complete for grp=%d\n", grpaddr); +} + +void rx_sync::insert_blacklist(int grpaddr) { + bool rc = add_xlist(grpaddr, d_blacklist); + if (rc == false) + fprintf(stderr, "insert_blacklist failed for grp=%d- dup or list full\n", grpaddr); + else if (d_debug) + fprintf(stderr, "insert_blacklist complete for grp=%d\n", grpaddr); +} + +void rx_sync::dmr_sync(const uint8_t bitbuf[], int& current_slot, bool& unmute) { + static const int slot_ids[] = {0, 1, 0, 0, 1, 1, 0, 1}; + static const uint32_t BURST_SZ = 32; // embedded burst size (bits) + int tact; + int chan; + int fstype; + uint8_t tactbuf[sizeof(cach_tact_bits)]; + uint8_t lc72[72]; + + for (size_t i=0; i<sizeof(cach_tact_bits); i++) + tactbuf[i] = bitbuf[cach_tact_bits[i]]; + tact = hamming_7_4_decode[load_i(tactbuf, 7)]; + chan = (tact>>2) & 1; + d_shift_reg = (d_shift_reg << 1) + chan; + current_slot = slot_ids[d_shift_reg & 7]; + + if (d_groupid_valid[current_slot] > 0) + d_groupid_valid[current_slot] -= 1; + + uint64_t sync = load_reg64(bitbuf + (MODE_DATA[RX_TYPE_DMR].sync_offset << 1), MODE_DATA[RX_TYPE_DMR].sync_len); + if (check_frame_sync(DMR_VOICE_SYNC_MAGIC ^ sync, d_threshold, MODE_DATA[RX_TYPE_DMR].sync_len)) + fstype = 1; + else if (check_frame_sync(DMR_IDLE_SYNC_MAGIC ^ sync, d_threshold, MODE_DATA[RX_TYPE_DMR].sync_len)) + fstype = 2; + else + fstype = 0; + if (fstype > 0) + d_expires = d_symbol_count + MODE_DATA[d_current_type].expiration; + if (fstype == 1) { + if (!d_unmute_until[current_slot] && d_debug > 5) + fprintf(stderr, "%d unmute slot %d\n", d_symbol_count, current_slot); + d_unmute_until[current_slot] = d_symbol_count + MODE_DATA[d_current_type].expiration; + } else if (fstype == 2) { + if (d_unmute_until[current_slot] && d_debug > 5) + fprintf(stderr, "%d mute slot %d\n", d_symbol_count, current_slot); + d_unmute_until[current_slot] = 0; + } + if (d_unmute_until[current_slot] <= d_symbol_count) { + d_unmute_until[current_slot] = 0; + } + unmute = d_unmute_until[current_slot] > 0; + if (fstype == 0) { + uint16_t emb = (load_i(bitbuf+132, 8) << 8) + load_i(bitbuf+172, 8); + int emb_decode = hamming_16_7_decode(emb); + if (emb_decode >= 0) { + uint8_t cc = emb_decode >> 3; + uint8_t pi = (emb_decode >> 2) & 1; + uint8_t lcss = emb_decode & 3; + switch (lcss) { + case 0: + break; + case 1: + memcpy(d_burstb[current_slot], bitbuf+140, BURST_SZ); + d_burstl[current_slot] = BURST_SZ; + break; + case 2: + if (d_burstl[current_slot] && d_burstl[current_slot]+BURST_SZ <= sizeof(d_burstb)) { + memcpy(d_burstb[current_slot] + d_burstl[current_slot], bitbuf+140, BURST_SZ); + d_burstl[current_slot] += BURST_SZ; + int rc = decode_embedded(lc72, d_burstb[current_slot], d_burstl[current_slot]); + if (rc >= 0) { + int opcode = load_i(lc72+2, 6); + if (opcode == 0) { // group voice channel user + int grpaddr = load_i(lc72+24, 24); + if (grpaddr > 0) { + d_groupid[current_slot] = grpaddr; + d_groupid_valid[current_slot] = 20; + } + } + } else { + if (d_debug) + fprintf(stderr, "decode_embedded failed, code %d\n", rc); + } + } + d_burstl[current_slot] = 0; + break; + case 3: + if (d_burstl[current_slot] && d_burstl[current_slot]+BURST_SZ <= sizeof(d_burstb)) { + memcpy(d_burstb[current_slot] + d_burstl[current_slot], bitbuf+140, BURST_SZ); + d_burstl[current_slot] += BURST_SZ; + } else { + d_burstl[current_slot] = 0; + } + break; + } + } else { + d_burstl[current_slot] = 0; + } + } + if (unmute && d_groupid_valid[current_slot] > 0) { + if (count_xlist(d_whitelist) > 0 && !find_xlist(d_groupid[current_slot], d_whitelist)) { + if (d_debug) + fprintf(stderr, "%d group %d not in whitelist, muting slot %d\n", d_symbol_count, d_groupid[current_slot], current_slot); + unmute = 0; + if (d_unmute_until[current_slot] && d_debug > 5) + fprintf(stderr, "%d mute slot %d\n", d_symbol_count, current_slot); + d_unmute_until[current_slot] = 0; + } + if (count_xlist(d_blacklist) > 0 && find_xlist(d_groupid[current_slot], d_blacklist)) { + if (d_debug) + fprintf(stderr, "group %d in blacklist, muting slot %d\n", d_groupid[current_slot], current_slot); + unmute = 0; + if (d_unmute_until[current_slot] && d_debug > 5) + fprintf(stderr, "%d mute slot %d\n", d_symbol_count, current_slot); + d_unmute_until[current_slot] = 0; + } + } +} + +rx_sync::rx_sync(const char * options, int debug, gr::msg_queue::sptr queue, int msgq_id) : // constructor + d_symbol_count(0), + d_sync_reg(0), + d_cbuf_idx(0), + d_current_type(RX_TYPE_NONE), + d_rx_count(0), + d_expires(0), + d_stereo(false), + d_debug(debug), + d_audio(options, debug), + d_msg_queue(queue), + d_previous_nxdn_sync(0), + d_previous_nxdn_sr_structure(-1), + d_previous_nxdn_sr_ran(-1), + d_msgq_id(msgq_id) +{ + mbe_initMbeParms (&cur_mp[0], &prev_mp[0], &enh_mp[0]); + mbe_initMbeParms (&cur_mp[1], &prev_mp[1], &enh_mp[1]); + sync_reset(); + d_burstl[0] = 0; + d_burstl[1] = 0; + init_xlist(d_whitelist); + init_xlist(d_blacklist); + d_groupid[0] = 0; + d_groupid_valid[0] = 0; + d_groupid[1] = 0; + d_groupid_valid[1] = 0; +} + +rx_sync::~rx_sync() // destructor +{ +} + + +void rx_sync::codeword(const uint8_t* cw, const enum codeword_types codeword_type, int slot_id) { + static const int x=4; + static const int y=26; + static const uint8_t majority[8] = {0,0,0,1,0,1,1,1}; + + int b[9]; + uint8_t buf[4*26]; + uint8_t tmp_codeword [144]; + uint32_t E0, ET; + uint32_t u[8]; + bool do_fullrate = false; + bool do_silence = false; + voice_codeword fullrate_cw(voice_codeword_sz); + + switch(codeword_type) { + case CODEWORD_DMR: + case CODEWORD_NXDN_EHR: // halfrate + interleaver.process_vcw(cw, b); + if (b[0] < 120) + mbe_dequantizeAmbe2250Parms(&cur_mp[slot_id], &prev_mp[slot_id], b); + break; + case CODEWORD_DSTAR: + interleaver.decode_dstar(cw, b, false); + if (b[0] < 120) + mbe_dequantizeAmbe2400Parms(&cur_mp[slot_id], &prev_mp[slot_id], b); + break; + case CODEWORD_YSF_HALFRATE: // 104 bits + for (int i=0; i<x; i++) { + for (int j=0; j<y; j++) + buf[j+i*y] = cw[i+j*x]; + } + ysf_scramble(buf, 104); + for (int i=0; i<27; i++) + tmp_codeword[i] = majority[ (buf[0+i*3] << 2) | (buf[1+i*3] << 1) | buf[2+i*3] ]; + + memcpy(tmp_codeword+27, buf+81, 22); + decode_49bit(b, tmp_codeword); + if (b[0] < 120) + mbe_dequantizeAmbe2250Parms(&cur_mp[slot_id], &prev_mp[slot_id], b); + break; + case CODEWORD_P25P2: + break; + case CODEWORD_P25P1: // 144 bits + for (int i=0; i<144; i++) + fullrate_cw[i] = cw[i]; + imbe_header_decode(fullrate_cw, u[0], u[1], u[2], u[3], u[4], u[5], u[6], u[7], E0, ET); + do_fullrate = true; + break; + case CODEWORD_YSF_FULLRATE: // 144 bits + for (int i=0; i<144; i++) + fullrate_cw[i] = cw[ysf_permutation[i]]; + imbe_header_decode(fullrate_cw, u[0], u[1], u[2], u[3], u[4], u[5], u[6], u[7], E0, ET); + do_fullrate = true; + break; + } + mbe_moveMbeParms (&cur_mp[slot_id], &prev_mp[slot_id]); + if (do_fullrate) { + d_software_decoder[slot_id].decode(fullrate_cw); + } else { /* halfrate */ + if (b[0] >= 120) { + do_silence = true; + } else { + d_software_decoder[slot_id].decode_tap(cur_mp[slot_id].L, 0, cur_mp[slot_id].w0, &cur_mp[slot_id].Vl[1], &cur_mp[slot_id].Ml[1]); + } + } + audio_samples *samples = d_software_decoder[slot_id].audio(); + float snd; + int16_t samp_buf[NSAMP_OUTPUT]; + for (int i=0; i < NSAMP_OUTPUT; i++) { + if ((!do_silence) && samples->size() > 0) { + snd = samples->front(); + samples->pop_front(); + } else { + snd = 0; + } + if (do_fullrate) + snd *= 32768.0; + samp_buf[i] = snd; + } + output(samp_buf, slot_id); +} + +void rx_sync::output(int16_t * samp_buf, const ssize_t slot_id) { + if (!d_stereo) { + d_audio.send_audio_channel(samp_buf, NSAMP_OUTPUT * sizeof(int16_t), slot_id); + return; + } +} + +bool rx_sync::nxdn_gate(enum rx_types sync_detected) { + // if nxdn sync is detected while another type is already active, + // we require two consecutive nxdn frames before allowing change to new type + // (to try to prevent falsing due to shortened nxdn sync signature size) + // returns false if sync is either not present or should be ignored + static const int NXDN_FRSIZE = 192; + bool rc; + if (sync_detected == RX_TYPE_NONE) + return false; + if (sync_detected == d_current_type) + return true; + if (sync_detected != RX_TYPE_NXDN) + return true; + if (d_current_type == RX_TYPE_NONE) + return true; + // trying to switch from another type to nxdn + if (d_symbol_count - d_previous_nxdn_sync != NXDN_FRSIZE) { + if (d_debug) + fprintf(stderr, "ignoring NXDN frame sync in state %s, count %d, symbol %d\n", MODE_DATA[d_current_type].type, d_symbol_count - d_previous_nxdn_sync, d_symbol_count); + rc = false; + } else { + if (d_debug) + fprintf(stderr, "changing to NXDN from state %s, symbol %d\n", MODE_DATA[d_current_type].type, d_symbol_count); + rc = true; + } + d_previous_nxdn_sync = d_symbol_count; + return rc; +} + +void rx_sync::rx_sym(const uint8_t sym) +{ + uint8_t bitbuf[864*2]; + enum rx_types sync_detected = RX_TYPE_NONE; + int current_slot; + bool unmute; + uint8_t tmpcw[144]; + bool ysf_fullrate; + + d_symbol_count ++; + d_sync_reg = (d_sync_reg << 2) | (sym & 3); + for (int i = 1; i < RX_N_TYPES; i++) { + if (check_frame_sync(MODE_DATA[i].sync ^ d_sync_reg, (i == d_current_type) ? d_threshold : 0, MODE_DATA[i].sync_len)) { + sync_detected = (enum rx_types) i; + break; + } + } + cbuf_insert(sym); + if (d_current_type == RX_TYPE_NONE && sync_detected == RX_TYPE_NONE) + return; + d_rx_count ++; + if (d_debug && sync_detected == RX_TYPE_NONE && d_rx_count == MODE_DATA[d_current_type].sync_offset + (MODE_DATA[d_current_type].sync_len >> 1)) + fprintf(stderr, "missing expected %s sync, symbol %d\n", MODE_DATA[d_current_type].type, d_symbol_count); + if (nxdn_gate(sync_detected)) { + if (d_current_type != sync_detected) { + d_current_type = sync_detected; + d_expires = d_symbol_count + MODE_DATA[d_current_type].expiration; + d_rx_count = 0; + } + if (d_rx_count != MODE_DATA[d_current_type].sync_offset + (MODE_DATA[d_current_type].sync_len >> 1)) { + if (d_debug) + fprintf(stderr, "resync at count %d symbol %d for protocol %s\n", d_rx_count, d_symbol_count, MODE_DATA[d_current_type].type); + sync_reset(); + d_rx_count = MODE_DATA[d_current_type].sync_offset + (MODE_DATA[d_current_type].sync_len >> 1); + } else { + d_threshold = std::min(d_threshold + 1, (d_current_type == RX_TYPE_NXDN) ? 0 : 2); + } + d_expires = d_symbol_count + MODE_DATA[d_current_type].expiration; + } + if (d_symbol_count >= d_expires) { + if (d_debug) + fprintf(stderr, "%s: timeout, symbol %d\n", MODE_DATA[d_current_type].type, d_symbol_count); + d_current_type = RX_TYPE_NONE; + return; + } + if (d_rx_count < MODE_DATA[d_current_type].fragment_len) + return; + d_rx_count = 0; + int start_idx = d_cbuf_idx + CBUF_SIZE - MODE_DATA[d_current_type].fragment_len; + assert (start_idx >= 0); + uint8_t * symbol_ptr = d_cbuf+start_idx; + uint8_t * bit_ptr = symbol_ptr; + if (d_current_type != RX_TYPE_DSTAR) { + dibits_to_bits(bitbuf, symbol_ptr, MODE_DATA[d_current_type].fragment_len); + bit_ptr = bitbuf; + } + switch (d_current_type) { + case RX_TYPE_NONE: + break; + case RX_TYPE_P25: + for (unsigned int codeword_ct=0; codeword_ct < nof_voice_codewords; codeword_ct++) { + for (unsigned int i=0; i<voice_codeword_sz; i++) + tmpcw[i] = bit_ptr[voice_codeword_bits[codeword_ct][i]]; + codeword(tmpcw, CODEWORD_P25P1, 0); // 144 bits + } + break; + case RX_TYPE_DMR: + dmr_sync(bit_ptr, current_slot, unmute); + if (!unmute) + break; + codeword(symbol_ptr+12, CODEWORD_DMR, current_slot); + memcpy(tmpcw, symbol_ptr+48, 18); + memcpy(tmpcw+18, symbol_ptr+90, 18); + codeword(tmpcw, CODEWORD_DMR, current_slot); + codeword(symbol_ptr+108, CODEWORD_DMR, current_slot); + break; + case RX_TYPE_DSTAR: + codeword(bit_ptr, CODEWORD_DSTAR, 0); // 72 bits = 72 symbols + break; + case RX_TYPE_YSF: + ysf_sync(symbol_ptr, ysf_fullrate, unmute); + if (!unmute) + break; + for (int vcw = 0; vcw < 5; vcw++) { + if (ysf_fullrate) { + codeword(bit_ptr + 2*(vcw*72 + 120), CODEWORD_YSF_FULLRATE, 0); // 144 bits + } else { /* halfrate */ + codeword(bit_ptr + 2*(vcw*72 + 120 + 20), CODEWORD_YSF_HALFRATE, 0); // 104 bits + } + } + break; + case RX_TYPE_NXDN: + nxdn_frame(symbol_ptr); + break; + case RX_N_TYPES: + assert(0==1); /* should not occur */ + break; + } +} + +static inline void qmsg(const gr::msg_queue::sptr msg_queue, const uint8_t s[], int len, int msgq_id) { + unsigned char hdr[4] = {0xaa, 0x55, (unsigned char)((msgq_id >> 8) & 0xff), (unsigned char)(msgq_id & 0xff)}; + if (!msg_queue->full_p()) { + gr::message::sptr msg = gr::message::make_from_string(std::string((char*)hdr, 4) + std::string((char*)s, len), -5, 0, 0); + msg_queue->insert_tail(msg); + } +} + +void rx_sync::nxdn_frame(const uint8_t symbol_ptr[]) +{ // length is implicitly 192, with frame sync in first 10 dibits + uint8_t dbuf[182]; + uint8_t lich; + int answer_len=0; + uint8_t answer[32]; + uint8_t sacch_answer[32]; + uint8_t lich_buf[8]; + int lich_parity_received; + int lich_parity_computed; + int voice=0; + int facch=0; + int facch2=0; + int sacch=0; + int cac=0; + int sr_structure; + int sr_ran; + + memcpy(lich_buf, symbol_ptr+10, sizeof(lich_buf)); + nxdn_descramble(lich_buf, sizeof(lich_buf)); + lich = 0; + for (int i=0; i<8; i++) + lich |= (lich_buf[i] >> 1) << (7-i); + lich_parity_received = lich & 1; + lich_parity_computed = ((lich >> 7) + (lich >> 6) + (lich >> 5) + (lich >> 4)) & 1; + lich = lich >> 1; + if (lich_parity_received != lich_parity_computed) { + if (d_debug) + fprintf(stderr, "NXDN lich parity error, ignoring frame at symbol %d\n", d_symbol_count); + return; + } + voice = 0; + facch = 0; + facch2 = 0; + sacch = 0; + cac = 0; + switch(lich) { + case 0x01: // CAC type + case 0x05: + cac = 1; + break; + case 0x28: + case 0x29: + case 0x2e: + case 0x2f: + case 0x48: + case 0x49: + case 0x4e: + case 0x4f: + case 0x69: + case 0x6f: + facch2 = 1; + break; + case 0x32: + case 0x33: + case 0x52: + case 0x53: + case 0x73: + voice = 2; // second half is voice + facch = 1; + sacch = 1; + break; + case 0x34: + case 0x35: + case 0x54: + case 0x55: + case 0x75: + voice = 1; // first half is voice + facch = 2; + sacch = 1; + break; + case 0x36: + case 0x37: + case 0x56: + case 0x57: + case 0x77: + voice = 3; // voice in both first and last + facch = 0; + sacch = 1; + break; + case 0x20: + case 0x21: + case 0x30: + case 0x31: + case 0x40: + case 0x41: + case 0x50: + case 0x51: + case 0x61: + case 0x71: + voice = 0; + facch = 3; // facch in both + sacch = 1; + break; + case 0x38: + case 0x39: + sacch = 1; + break; + default: + if (d_debug) + fprintf(stderr, "unsupported NXDN lich type 0x%x, symbol %d\n", lich, d_symbol_count); + voice = 0; + break; + } // end of switch(lich) + if (d_debug > 3) + fprintf(stderr, "nxdn lich %x voice %d facch %d sacch %d cac %d symbol %d\n", lich, voice, facch, sacch, cac, d_symbol_count); + if (voice || facch || facch2 || sacch || cac) { + memcpy(dbuf, symbol_ptr+10, sizeof(dbuf)); + nxdn_descramble(dbuf, sizeof(dbuf)); + } + if (voice & 1) + for (int vcw = 0; vcw < 2; vcw++) + codeword(dbuf+38+36*vcw, CODEWORD_NXDN_EHR, 0); + if (voice & 2) + for (int vcw = 2; vcw < 4; vcw++) + codeword(dbuf+38+36*vcw, CODEWORD_NXDN_EHR, 0); + if (sacch) { + bool non_superframe = (lich == 0x20 || lich == 0x21 || lich == 0x61 || lich == 0x40 || lich == 0x41) ? true : false; + answer_len = sizeof(sacch_answer); + nxdn_decode_sacch(dbuf+8, 30, sacch_answer, answer_len); // sacch size = 30 dibits, 26 bits returned if successful + sr_structure = load_i(sacch_answer, 2) & 3; + if (answer_len > 0 && non_superframe == true && sr_structure == 0) { + answer[0] = 's'; + answer[1] = lich; + int nbytes = (answer_len + 7) / 8; + cfill(answer+2, sacch_answer, nbytes); + qmsg(d_msg_queue, answer, nbytes+2, d_msgq_id); + if (d_debug > 2) + debug_dump("nxdn: sacch", answer, nbytes+2); + } else if (answer_len > 0 && non_superframe == false) { + sr_ran = load_i(sacch_answer+2, 6) & 0x3f; + bool ok = true; + if (d_previous_nxdn_sr_structure == -1 && sr_structure != 3) + ok = false; + else if (sr_structure < 3 && sr_structure+1 != d_previous_nxdn_sr_structure) + ok = false; + else if (sr_structure < 3 && d_previous_nxdn_sr_ran != sr_ran) + ok = false; + if (ok) { + int seg = 3 - sr_structure; + memcpy(d_sacch_buf + 18*seg, sacch_answer + 8, 18); + if (sr_structure > 0) { + d_previous_nxdn_sr_ran = sr_ran; + d_previous_nxdn_sr_structure = sr_structure; + } else { + answer[0] = 'S'; + answer[1] = lich; + answer[2] = sr_ran; + int nbytes = 9; + cfill(answer+3, d_sacch_buf, nbytes); + qmsg(d_msg_queue, answer, nbytes+3, d_msgq_id); + if (d_debug > 2) + debug_dump("nxdn: sacch", answer, nbytes+3); + d_previous_nxdn_sr_ran = -1; + d_previous_nxdn_sr_structure = -1; + } + } else { + d_previous_nxdn_sr_ran = -1; + d_previous_nxdn_sr_structure = -1; + } + } + } + if (facch & 1) { + answer_len = sizeof(answer)-2; + nxdn_decode_facch(dbuf+38, 72, answer+2, answer_len); // facch size = 72 dibits + if (answer_len > 0) { + answer[0] = 'f'; + answer[1] = lich; + qmsg(d_msg_queue, answer, answer_len+2, d_msgq_id); + if (d_debug > 2) + debug_dump("nxdn: facch", answer, answer_len+2); + } + } + if (facch & 2) { + if ((facch & 1) && !memcmp(dbuf+38, dbuf+38+72, 72)) { + if (d_debug > 5) + fprintf(stderr, "nxdn: skipping duplicate facch\n"); + } else { + answer_len = sizeof(answer)-2; + nxdn_decode_facch(dbuf+38+72, 72, answer+2, answer_len); + if (answer_len > 0) { + answer[0] = 'f'; + answer[1] = lich; + qmsg(d_msg_queue, answer, answer_len+2, d_msgq_id); + if (d_debug > 2) + debug_dump("nxdn: facch", answer, answer_len+2); + } + } + } + if (facch2) { + answer_len = sizeof(answer)-2; + nxdn_decode_facch2_udch(dbuf+8, 174, answer+2, answer_len); + if (answer_len > 0) { + answer[0] = 'u'; + answer[1] = lich; + qmsg(d_msg_queue, answer, answer_len+2, d_msgq_id); + if (d_debug > 2) + debug_dump("nxdn: facch2", answer, answer_len+2); + } + } + if (cac) { + answer_len = sizeof(answer)-2; + nxdn_decode_cac(dbuf+8, 150, answer+2, answer_len); + if (answer_len > 0) { + answer[0] = 'c'; + answer[1] = lich; + qmsg(d_msg_queue, answer, answer_len+2, d_msgq_id); + if (d_debug > 2) + debug_dump("nxdn: cac", answer, answer_len+2); + } + } +} + + } // end namespace op25_repeater +} // end namespace gr diff --git a/op25/gr-op25_repeater/lib/rx_sync.h b/op25/gr-op25_repeater/lib/rx_sync.h new file mode 100644 index 0000000..35d9eb5 --- /dev/null +++ b/op25/gr-op25_repeater/lib/rx_sync.h @@ -0,0 +1,146 @@ +// P25 Decoder (C) Copyright 2013, 2014, 2015, 2016, 2017 Max H. Parke KA1RBI +// +// This file is part of OP25 +// +// OP25 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 3, or (at your option) +// any later version. +// +// OP25 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 OP25; see the file COPYING. If not, write to the Free +// Software Foundation, Inc., 51 Franklin Street, Boston, MA +// 02110-1301, USA. + +#ifndef INCLUDED_RX_SYNC_H +#define INCLUDED_RX_SYNC_H + +#include <stdio.h> +#include <stdint.h> +#include <stdlib.h> +#include <string.h> +#include <string> +#include <iostream> +#include <deque> +#include <assert.h> +#include <gnuradio/msg_queue.h> + +#include "bit_utils.h" +#include "check_frame_sync.h" + +#include "p25p2_vf.h" +#include "mbelib.h" +#include "ambe.h" + +#include "ysf_const.h" +#include "dmr_const.h" +#include "p25_frame.h" +#include "op25_imbe_frame.h" +#include "software_imbe_decoder.h" +#include "op25_audio.h" +#include "nxdn_const.h" +#include "nxdn.h" + +namespace gr{ + namespace op25_repeater{ + +static const uint64_t DSTAR_FRAME_SYNC_MAGIC = 0x444445101440LL; // expanded into dibits + +enum rx_types { + RX_TYPE_NONE=0, + RX_TYPE_P25, + RX_TYPE_DMR, + RX_TYPE_DSTAR, + RX_TYPE_YSF, + RX_TYPE_NXDN, + RX_N_TYPES +}; // also used as array index + +static const struct _mode_data { + const char * type; + int sync_len; + int sync_offset; + int fragment_len; // symbols + int expiration; + uint64_t sync; +} MODE_DATA[RX_N_TYPES] = { + {"NONE", 0,0,0,0,0}, + {"P25", 48,0,864,1728, P25_FRAME_SYNC_MAGIC}, + {"DMR", 48,66,144,1728, DMR_VOICE_SYNC_MAGIC}, + {"DSTAR", 48,72,96,2016*2, DSTAR_FRAME_SYNC_MAGIC}, + {"YSF", 40,0,480,480*2, YSF_FRAME_SYNC_MAGIC}, + {"NXDN", 20,0,192,192*2, NXDN_SYNC_MAGIC} +}; // index order must match rx_types enum + +enum codeword_types { + CODEWORD_P25P1, + CODEWORD_P25P2, + CODEWORD_DMR, + CODEWORD_DSTAR, + CODEWORD_YSF_FULLRATE, + CODEWORD_YSF_HALFRATE, + CODEWORD_NXDN_EHR +}; + +#define XLIST_SIZE 256 + +class rx_sync { +public: + void rx_sym(const uint8_t sym); + void sync_reset(void); + rx_sync(const char * options, int debug, gr::msg_queue::sptr queue, int msgq_id); + ~rx_sync(); + void insert_whitelist(int grpaddr); + void insert_blacklist(int grpaddr); +private: + void cbuf_insert(const uint8_t c); + void dmr_sync(const uint8_t bitbuf[], int& current_slot, bool& unmute); + void ysf_sync(const uint8_t dibitbuf[], bool& ysf_fullrate, bool& unmute); + void codeword(const uint8_t* cw, const enum codeword_types codeword_type, int slot_id); + void output(int16_t * samp_buf, const ssize_t slot_id); + bool nxdn_gate(enum rx_types sync_detected); + void nxdn_frame(const uint8_t symbol_ptr[]); + static const int CBUF_SIZE=864; + static const int NSAMP_OUTPUT = 160; + + unsigned int d_symbol_count; + uint64_t d_sync_reg; + uint8_t d_cbuf[CBUF_SIZE*2]; + unsigned int d_cbuf_idx; + enum rx_types d_current_type; + int d_threshold; + int d_rx_count; + unsigned int d_expires; + int d_shift_reg; + unsigned int d_unmute_until[2]; + p25p2_vf interleaver; + mbe_parms cur_mp[2]; + mbe_parms prev_mp[2]; + mbe_parms enh_mp[2]; + software_imbe_decoder d_software_decoder[2]; + std::deque<int16_t> d_output_queue[2]; + bool d_stereo; + int d_debug; + op25_audio d_audio; + uint8_t d_burstb[2][32*4]; + int d_burstl[2]; // in units of bits + int d_groupid[2]; + unsigned int d_groupid_valid[2]; + int d_whitelist[XLIST_SIZE]; + int d_blacklist[XLIST_SIZE]; + gr::msg_queue::sptr d_msg_queue; + int d_previous_nxdn_sync; + int d_previous_nxdn_sr_structure; + int d_previous_nxdn_sr_ran; + uint8_t d_sacch_buf[72]; + int d_msgq_id; +}; + + } // end namespace op25_repeater +} // end namespace gr +#endif // INCLUDED_RX_SYNC_H diff --git a/op25/gr-op25_repeater/lib/software_imbe_decoder.cc b/op25/gr-op25_repeater/lib/software_imbe_decoder.cc index 77c0b24..86191c6 100644 --- a/op25/gr-op25_repeater/lib/software_imbe_decoder.cc +++ b/op25/gr-op25_repeater/lib/software_imbe_decoder.cc @@ -734,6 +734,7 @@ software_imbe_decoder::software_imbe_decoder() int i,j;
//initialize
OldL = 0;
+ L = 9;
Old = 1; New = 0;
psi1 = 0.0;
for(i=0; i < 58; i++) {
@@ -935,7 +936,6 @@ software_imbe_decoder::decode_tap(int _L, int _K, float _w0, const int * _v, con int en, tmp_f;
L = _L;
- int K = _K;
w0 = _w0;
for(ell = 1; ell <= L; ell++) {
vee[ell][ New] = _v[ell - 1];
diff --git a/op25/gr-op25_repeater/lib/software_imbe_decoder.h b/op25/gr-op25_repeater/lib/software_imbe_decoder.h index 7573bd4..cd22f9a 100644 --- a/op25/gr-op25_repeater/lib/software_imbe_decoder.h +++ b/op25/gr-op25_repeater/lib/software_imbe_decoder.h @@ -47,7 +47,7 @@ public: /** * Decode the compressed audio. * - * \cw in IMBE codeword (including parity check bits). + * cw in IMBE codeword (including parity check bits). */ virtual void decode(const voice_codeword& cw); diff --git a/op25/gr-op25_repeater/lib/value_string.cc b/op25/gr-op25_repeater/lib/value_string.cc new file mode 120000 index 0000000..2c72a9b --- /dev/null +++ b/op25/gr-op25_repeater/lib/value_string.cc @@ -0,0 +1 @@ +../../gr-op25/lib/value_string.cc
\ No newline at end of file diff --git a/op25/gr-op25_repeater/lib/value_string.h b/op25/gr-op25_repeater/lib/value_string.h new file mode 120000 index 0000000..78bca90 --- /dev/null +++ b/op25/gr-op25_repeater/lib/value_string.h @@ -0,0 +1 @@ +../../gr-op25/lib/value_string.h
\ No newline at end of file diff --git a/op25/gr-op25_repeater/lib/vocoder_impl.cc b/op25/gr-op25_repeater/lib/vocoder_impl.cc index 3f2032d..074f7df 100644 --- a/op25/gr-op25_repeater/lib/vocoder_impl.cc +++ b/op25/gr-op25_repeater/lib/vocoder_impl.cc @@ -40,6 +40,8 @@ namespace gr { namespace op25_repeater { + static const int FRAGMENT_SIZE = 864; + vocoder::sptr vocoder::make(bool encode_flag, bool verbose_flag, int stretch_amt, char* udp_host, int udp_port, bool raw_vectors_flag) { @@ -70,9 +72,12 @@ namespace gr { output_queue_decode(), opt_udp_port(udp_port), opt_encode_flag(encode_flag), - p1voice_encode(verbose_flag, stretch_amt, udp_host, udp_port, raw_vectors_flag, output_queue), - p1voice_decode(verbose_flag, udp_host, udp_port, output_queue_decode) + op25audio(udp_host, udp_port, 0), + p1voice_encode(verbose_flag, stretch_amt, op25audio, raw_vectors_flag, output_queue), + p1voice_decode(verbose_flag, op25audio, output_queue_decode) { + if (opt_encode_flag) + set_output_multiple(FRAGMENT_SIZE); } /* @@ -131,15 +136,20 @@ vocoder_impl::general_work_encode (int noutput_items, gr_vector_void_star &output_items) { const short *in = (const short *) input_items[0]; + const int noutput_fragments = noutput_items / FRAGMENT_SIZE; + const int fragments_available = output_queue.size() / FRAGMENT_SIZE; + const int nsamples_consume = std::min(ninput_items[0], std::max(0,(noutput_fragments - fragments_available) * 9 * 160)); - p1voice_encode.compress_samp(in, ninput_items[0]); + if (nsamples_consume > 0) { + p1voice_encode.compress_samp(in, nsamples_consume); - // Tell runtime system how many input items we consumed on - // each input stream. + // Tell runtime system how many input items we consumed on + // each input stream. - consume_each (ninput_items[0]); + consume_each (nsamples_consume); + } - if (opt_udp_port > 0) // in udp option, we are a gr sink only + if (op25audio.enabled()) // in udp option, we are a gr sink only return 0; uint8_t *out = reinterpret_cast<uint8_t*>(output_items[0]); diff --git a/op25/gr-op25_repeater/lib/vocoder_impl.h b/op25/gr-op25_repeater/lib/vocoder_impl.h index ce709fd..95dc94a 100644 --- a/op25/gr-op25_repeater/lib/vocoder_impl.h +++ b/op25/gr-op25_repeater/lib/vocoder_impl.h @@ -29,6 +29,7 @@ #include <vector> #include <deque> +#include "op25_audio.h" #include "p25p1_voice_encode.h" #include "p25p1_voice_decode.h" @@ -67,6 +68,7 @@ namespace gr { std::deque<int16_t> output_queue_decode; int opt_udp_port; bool opt_encode_flag; + op25_audio op25audio; p25p1_voice_encode p1voice_encode; p25p1_voice_decode p1voice_decode; 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..bac42cb 100644 --- a/op25/gr-op25_repeater/lib/ysf_tx_sb_impl.cc +++ b/op25/gr-op25_repeater/lib/ysf_tx_sb_impl.cc @@ -29,7 +29,7 @@ #include "p25p2_vf.h" #include "ysf_tx_sb_impl.h" #include "ysf_const.h" -#include <op25_imbe_frame.h> +#include "op25_imbe_frame.h" #include <vector> #include <stdint.h> @@ -52,7 +52,7 @@ static inline void print_result(char title[], const uint8_t r[], int len) { } #endif -static inline int store_i(int reg, uint8_t val[], int len) { +static inline void store_i(int reg, uint8_t val[], int len) { for (int i=0; i<len; i++){ val[i] = (reg >> (len-1-i)) & 1; } @@ -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; |