From efd3c47a6ce8ecb690d00695d66c0b7f925adfe1 Mon Sep 17 00:00:00 2001 From: AlexandreRouma Date: Tue, 28 Sep 2021 20:46:19 +0200 Subject: [PATCH] more progress on M17 support --- core/src/gui/widgets/symbol_diagram.cpp | 6 + core/src/gui/widgets/symbol_diagram.h | 3 + m17_decoder/CMakeLists.txt | 40 +++- m17_decoder/src/base40.cpp | 16 ++ m17_decoder/src/base40.h | 4 + m17_decoder/src/crc16.h | 72 ++++++++ m17_decoder/src/golay24.h | 233 ++++++++++++++++++++++++ m17_decoder/src/lsf_decode.cpp | 112 ++++++++++++ m17_decoder/src/lsf_decode.h | 42 +++++ m17_decoder/src/m17dsp.h | 221 ++++++++++++++++++++-- m17_decoder/src/main.cpp | 125 ++++++++++++- 11 files changed, 844 insertions(+), 30 deletions(-) create mode 100644 m17_decoder/src/base40.cpp create mode 100644 m17_decoder/src/base40.h create mode 100644 m17_decoder/src/crc16.h create mode 100644 m17_decoder/src/golay24.h create mode 100644 m17_decoder/src/lsf_decode.cpp create mode 100644 m17_decoder/src/lsf_decode.h diff --git a/core/src/gui/widgets/symbol_diagram.cpp b/core/src/gui/widgets/symbol_diagram.cpp index bbef51e1..c1606620 100644 --- a/core/src/gui/widgets/symbol_diagram.cpp +++ b/core/src/gui/widgets/symbol_diagram.cpp @@ -31,8 +31,14 @@ namespace ImGui { window->DrawList->AddRectFilled(min, ImVec2(min.x+size.x, min.y+size.y), IM_COL32(0,0,0,255)); ImU32 col = ImGui::GetColorU32(ImGuiCol_CheckMark, 0.7f); + ImU32 col2 = ImGui::GetColorU32(ImGuiCol_CheckMark, 0.7f); float increment = size.x / (float)sampleCount; float val; + + for (auto l : lines) { + window->DrawList->AddLine(ImVec2(min.x, (((l * _scale) + 1) * (size.y*0.5f)) + min.y), ImVec2(min.x + size.x, (((l * _scale) + 1) * (size.y*0.5f)) + min.y), IM_COL32(80, 80, 80, 255)); + } + for (int i = 0; i < sampleCount; i++) { val = buffer[i] * _scale; if (val > 1.0f || val < -1.0f) { continue; } diff --git a/core/src/gui/widgets/symbol_diagram.h b/core/src/gui/widgets/symbol_diagram.h index 43a9cc17..9ca6ed85 100644 --- a/core/src/gui/widgets/symbol_diagram.h +++ b/core/src/gui/widgets/symbol_diagram.h @@ -3,6 +3,7 @@ #include #include #include +#include #include namespace ImGui { @@ -17,6 +18,8 @@ namespace ImGui { void releaseBuffer(); + std::vector lines; + private: std::mutex bufferMtx; float* buffer; diff --git a/m17_decoder/CMakeLists.txt b/m17_decoder/CMakeLists.txt index b5fd29a7..33343692 100644 --- a/m17_decoder/CMakeLists.txt +++ b/m17_decoder/CMakeLists.txt @@ -1,8 +1,6 @@ cmake_minimum_required(VERSION 3.13) project(m17_decoder) -add_subdirectory("libcorrect") - if (MSVC) set(CMAKE_CXX_FLAGS "-O2 /std:c++17 /EHsc") elseif (CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -14,19 +12,45 @@ endif () file(GLOB_RECURSE SRC "src/*.cpp" "src/*.c") include_directories("src/") -include_directories("libcorrect/include") - -include_directories("C:/Users/ryzerth/Documents/Code/codec2/src") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -march=native") add_library(m17_decoder SHARED ${SRC}) target_link_libraries(m17_decoder PRIVATE sdrpp_core) -target_link_libraries(m17_decoder PRIVATE correct_static) set_target_properties(m17_decoder PROPERTIES PREFIX "") -target_link_directories(sdrpp_core PUBLIC "C:/Users/ryzerth/Documents/Code/codec2/build/src") -target_link_libraries(m17_decoder PRIVATE libcodec2) + + +if (MSVC) + # Lib path + target_include_directories(m17_decoder PUBLIC "C:/Users/ryzerth/Documents/Code/codec2/src") + target_link_directories(sdrpp_core PUBLIC "C:/Users/ryzerth/Documents/Code/codec2/build/src") + + target_include_directories(m17_decoder PUBLIC "C:/Program Files (x86)/Correct/include") + target_link_directories(sdrpp_core PUBLIC "C:/Program Files (x86)/Correct/lib") + + target_link_libraries(m17_decoder PRIVATE libcodec2) + target_link_libraries(m17_decoder PRIVATE correct) + +else (MSVC) + find_package(PkgConfig) + + pkg_check_modules(LIBCODEC2 REQUIRED codec2) + + target_include_directories(m17_decoder PUBLIC ${LIBCODEC2_INCLUDE_DIRS}) + target_link_directories(m17_decoder PUBLIC ${LIBCODEC2_LIBRARY_DIRS}) + target_link_libraries(m17_decoder PUBLIC ${LIBCODEC2_LIBRARIES}) + + target_link_libraries(m17_decoder PUBLIC correct) + + # Include it because for some reason pkgconfig doesn't look here? + if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + target_include_directories(m17_decoder PUBLIC "/usr/local/include") + endif() + +endif () + + # Install directives install(TARGETS m17_decoder DESTINATION lib/sdrpp/plugins) \ No newline at end of file diff --git a/m17_decoder/src/base40.cpp b/m17_decoder/src/base40.cpp new file mode 100644 index 00000000..61ae84c9 --- /dev/null +++ b/m17_decoder/src/base40.cpp @@ -0,0 +1,16 @@ +#include + +void decode_callsign_base40(uint64_t encoded, char *callsign) { + if (encoded >= 262144000000000) { // 40^9 + *callsign = 0; + return; + } + char *p = callsign; + for (; encoded > 0; p++) { + *p = " ABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789-/."[encoded % 40]; + encoded /= 40; + } + *p = 0; + + return; +} \ No newline at end of file diff --git a/m17_decoder/src/base40.h b/m17_decoder/src/base40.h new file mode 100644 index 00000000..94a79373 --- /dev/null +++ b/m17_decoder/src/base40.h @@ -0,0 +1,4 @@ +#pragma once +#include + +void decode_callsign_base40(uint64_t encoded, char* callsign); \ No newline at end of file diff --git a/m17_decoder/src/crc16.h b/m17_decoder/src/crc16.h new file mode 100644 index 00000000..467f5a4e --- /dev/null +++ b/m17_decoder/src/crc16.h @@ -0,0 +1,72 @@ +// Copyright 2020 Mobilinkd LLC. + +#pragma once + +#include +#include +#include + +namespace mobilinkd +{ + +template +struct CRC16 +{ + static constexpr uint16_t MASK = 0xFFFF; + static constexpr uint16_t LSB = 0x0001; + static constexpr uint16_t MSB = 0x8000; + + uint16_t reg_ = Init; + + void reset() + { + reg_ = Init; + + for (size_t i = 0; i != 16; ++i) + { + auto bit = reg_ & LSB; + if (bit) reg_ ^= Poly; + reg_ >>= 1; + if (bit) reg_ |= MSB; + } + + reg_ &= MASK; + } + + void operator()(uint8_t byte) + { + reg_ = crc(byte, reg_); + } + + uint16_t crc(uint8_t byte, uint16_t reg) + { + for (size_t i = 0; i != 8; ++i) + { + auto msb = reg & MSB; + reg = ((reg << 1) & MASK) | ((byte >> (7 - i)) & LSB); + if (msb) reg ^= Poly; + } + return reg & MASK; + } + + uint16_t get() + { + auto reg = reg_; + for (size_t i = 0; i != 16; ++i) + { + auto msb = reg & MSB; + reg = ((reg << 1) & MASK); + if (msb) reg ^= Poly; + } + return reg; + } + + std::array get_bytes() + { + auto crc = get(); + std::array result{uint8_t((crc >> 8) & 0xFF), uint8_t(crc & 0xFF)}; + return result; + } +}; + +} // mobilinkd \ No newline at end of file diff --git a/m17_decoder/src/golay24.h b/m17_decoder/src/golay24.h new file mode 100644 index 00000000..92a22de2 --- /dev/null +++ b/m17_decoder/src/golay24.h @@ -0,0 +1,233 @@ +// Copyright 2020 Rob Riggs +// All rights reserved. + +#pragma once + +#include +#include +#include +#include + +namespace mobilinkd { + +// Parts are adapted from: +// http://aqdi.com/articles/using-the-golay-error-detection-and-correction-code-3/ + +namespace Golay24 +{ + + int popcount(uint32_t n) { + int count = 0; + for (int i = 0; i < 32; i++) { + count += ((n >> i) & 1); + } + return count; + } + +namespace detail +{ + +// Need a constexpr sort. +// https://stackoverflow.com/a/40030044/854133 +template +void swap(T& l, T& r) +{ + T tmp = std::move(l); + l = std::move(r); + r = std::move(tmp); +} + +template +struct array +{ + constexpr T& operator[](size_t i) + { + return arr[i]; + } + + constexpr const T& operator[](size_t i) const + { + return arr[i]; + } + + constexpr const T* begin() const + { + return arr; + } + constexpr const T* end() const + { + return arr + N; + } + + T arr[N]; +}; + +template +void sort_impl(array &array, size_t left, size_t right) +{ + if (left < right) + { + size_t m = left; + + for (size_t i = left + 1; i +array sort(array array) +{ + auto sorted = array; + sort_impl(sorted, 0, N); + return sorted; +} + +} // detail + +// static constexpr uint16_t POLY = 0xAE3; +constexpr uint16_t POLY = 0xC75; + +#pragma pack(push, 1) +struct SyndromeMapEntry +{ + uint32_t a{0}; + uint16_t b{0}; +}; +#pragma pack(pop) + +/** + * Calculate the syndrome of a [23,12] Golay codeword. + * + * @return the 11-bit syndrome of the codeword in bits [22:12]. + */ +uint32_t syndrome(uint32_t codeword) +{ + codeword &= 0xffffffl; + for (size_t i = 0; i != 12; ++i) + { + if (codeword & 1) + codeword ^= POLY; + codeword >>= 1; + } + return (codeword << 12); +} + +bool parity(uint32_t codeword) +{ + return popcount(codeword) & 1; +} + +SyndromeMapEntry makeSyndromeMapEntry(uint64_t val) +{ + return SyndromeMapEntry{uint32_t(val >> 16), uint16_t(val & 0xFFFF)}; +} + +uint64_t makeSME(uint64_t syndrome, uint32_t bits) +{ + return (syndrome << 24) | (bits & 0xFFFFFF); +} + +constexpr size_t LUT_SIZE = 2048; + +std::array make_lut() +{ + constexpr size_t VECLEN=23; + detail::array result{}; + + size_t index = 0; + result[index++] = makeSME(syndrome(0), 0); + + for (size_t i = 0; i != VECLEN; ++i) + { + auto v = (1 << i); + result[index++] = makeSME(syndrome(v), v); + } + + for (size_t i = 0; i != VECLEN - 1; ++i) + { + for (size_t j = i + 1; j != VECLEN; ++j) + { + auto v = (1 << i) | (1 << j); + result[index++] = makeSME(syndrome(v), v); + } + } + + for (size_t i = 0; i != VECLEN - 2; ++i) + { + for (size_t j = i + 1; j != VECLEN - 1; ++j) + { + for (size_t k = j + 1; k != VECLEN; ++k) + { + auto v = (1 << i) | (1 << j) | (1 << k); + result[index++] = makeSME(syndrome(v), v); + } + } + } + + result = detail::sort(result); + + std::array tmp; + for (size_t i = 0; i != LUT_SIZE; ++i) + { + tmp[i] = makeSyndromeMapEntry(result[i]); + } + + return tmp; +} + +inline auto LUT = make_lut(); + +/** + * Calculate [23,12] Golay codeword. + * + * @return checkbits(11)|data(12). + */ +uint32_t encode23(uint16_t data) +{ + // data &= 0xfff; + uint32_t codeword = data; + for (size_t i = 0; i != 12; ++i) + { + if (codeword & 1) + codeword ^= POLY; + codeword >>= 1; + } + return codeword | (data << 11); +} + +uint32_t encode24(uint16_t data) +{ + auto codeword = encode23(data); + return ((codeword << 1) | parity(codeword)); +} + +bool decode(uint32_t input, uint32_t& output) +{ + auto syndrm = syndrome(input >> 1); + auto it = std::lower_bound(LUT.begin(), LUT.end(), syndrm, + [](const SyndromeMapEntry& sme, uint32_t val){ + return (sme.a >> 8) < val; + }); + + if ((it->a >> 8) == syndrm) + { + // Build the correction from the compressed entry. + auto correction = ((((it->a & 0xFF) << 16) | it->b) << 1); + // Apply the correction to the input. + output = input ^ correction; + // Only test parity for 3-bit errors. + return popcount(syndrm) < 3 || !parity(output); + } + + return false; +} + +} // Golay24 + +} // mobilinkd \ No newline at end of file diff --git a/m17_decoder/src/lsf_decode.cpp b/m17_decoder/src/lsf_decode.cpp new file mode 100644 index 00000000..a2687652 --- /dev/null +++ b/m17_decoder/src/lsf_decode.cpp @@ -0,0 +1,112 @@ +#include +#include +#include +#include +#include +#include +#include + +bool M17CheckCRC(uint8_t* data, int len) { + // TODO: Implement + return true; +} + +const char* M17DataTypesTxt[4] = { + "Unknown", + "Data", + "Voice", + "Voice & Data" +}; + +const char* M17EncryptionTypesTxt[4] = { + "None", + "AES", + "Scrambler", + "Unknown" +}; + +M17LSF M17DecodeLSF(uint8_t* _lsf) { + M17LSF lsf; + + // Extract CRC + lsf.rawCRC = 0; + for (int i = 0; i < 16; i++) { + lsf.rawCRC |= (((uint16_t)_lsf[(i+48+48+16+112) / 8] >> (7 - (i%8))) & 1) << (15 - i); + } + + // Check CRC + mobilinkd::CRC16 crc16; + crc16.reset(); + for (int i = 0; i < 28; i++) { + crc16(_lsf[i]); + } + if (crc16.get() != lsf.rawCRC) { + lsf.valid = false; + return lsf; + } + lsf.valid = true; + + + // Extract DST + lsf.rawDst = 0; + for (int i = 0; i < 48; i++) { + lsf.rawDst |= (((uint64_t)_lsf[i / 8] >> (7 - (i%8))) & 1) << (47 - i); + } + + // Extract SRC + lsf.rawSrc = 0; + for (int i = 0; i < 48; i++) { + lsf.rawSrc |= (((uint64_t)_lsf[(i+48) / 8] >> (7 - (i%8))) & 1) << (47 - i); + } + + // Extract TYPE + lsf.rawType = 0; + for (int i = 0; i < 16; i++) { + lsf.rawType |= (((uint16_t)_lsf[(i+48+48) / 8] >> (7 - (i%8))) & 1) << (15 - i); + } + + // Extract META + memcpy(lsf.meta, &_lsf[14], 14); + + // Decode DST + if (lsf.rawDst == 0) { + lsf.dst = "Invalid"; + } + else if (lsf.rawDst <= 262143999999999) { + char buf[128]; + decode_callsign_base40(lsf.rawDst, buf); + lsf.dst = buf; + } + else if (lsf.rawDst == 0xFFFFFFFFFFFF) { + lsf.dst = "Broadcast"; + } + else { + char buf[128]; + sprintf(buf, "%" PRIX64, lsf.rawDst); + lsf.dst = buf; + } + + // Decode SRC + if (lsf.rawSrc == 0 || lsf.rawSrc == 0xFFFFFFFFFFFF) { + lsf.src = "Invalid"; + } + else if (lsf.rawSrc <= 262143999999999) { + char buf[128]; + decode_callsign_base40(lsf.rawSrc, buf); + lsf.src = buf; + } + else { + char buf[128]; + sprintf(buf, "%" PRIX64, lsf.rawSrc); + lsf.src = buf; + } + + // Decode TYPE + lsf.isStream = (lsf.rawType >> 0) & 0b1; + lsf.dataType = (M17DataType)((lsf.rawType >> 1) & 0b11); + lsf.encryptionType = (M17EncryptionType)((lsf.rawType >> 3) & 0b11); + lsf.encryptionSubType = (lsf.rawType >> 5) & 0b11; + lsf.channelAccessNum = (lsf.rawType >> 7) & 0b1111; + + return lsf; +} \ No newline at end of file diff --git a/m17_decoder/src/lsf_decode.h b/m17_decoder/src/lsf_decode.h new file mode 100644 index 00000000..4ee6d7df --- /dev/null +++ b/m17_decoder/src/lsf_decode.h @@ -0,0 +1,42 @@ +#pragma once +#include +#include + +enum M17DataType { + M17_DATATYPE_UNKNOWN = 0b00, + M17_DATATYPE_DATA = 0b01, + M17_DATATYPE_VOICE = 0b10, + M17_DATATYPE_DATA_VOICE = 0b11 +}; + +enum M17EncryptionType { + M17_ENCRYPTION_NONE = 0b00, + M17_ENCRYPTION_AES = 0b01, + M17_ENCRYPTION_SCRAMBLE = 0b10, + M17_ENCRYPTION_UNKNOWN = 0b11 +}; + +extern const char* M17DataTypesTxt[4]; +extern const char* M17EncryptionTypesTxt[4]; + +struct M17LSF { + uint64_t rawDst; + uint64_t rawSrc; + uint16_t rawType; + uint8_t meta[14]; + uint16_t rawCRC; + + std::string dst; + std::string src; + bool isStream; + M17DataType dataType; + M17EncryptionType encryptionType; + uint8_t encryptionSubType; + uint8_t channelAccessNum; + + bool valid; +}; + +bool M17CheckCRC(uint8_t* data, int len); + +M17LSF M17DecodeLSF(uint8_t* _lsf); \ No newline at end of file diff --git a/m17_decoder/src/m17dsp.h b/m17_decoder/src/m17dsp.h index ceebc053..337a38ca 100644 --- a/m17_decoder/src/m17dsp.h +++ b/m17_decoder/src/m17dsp.h @@ -3,6 +3,8 @@ #include #include #include +#include +#include extern "C" { #include @@ -11,12 +13,14 @@ extern "C" { #define M17_DEVIATION 2400.0f #define M17_BAUDRATE 4800.0f #define M17_RRC_ALPHA 0.5f -#define M17_4FSK_HIGH_CUT 0.52f +#define M17_4FSK_HIGH_CUT 0.5f #define M17_SYNC_SIZE 16 #define M17_LICH_SIZE 96 #define M17_PAYLOAD_SIZE 144 #define M17_ENCODED_PAYLOAD_SIZE 296 +#define M17_LSF_SIZE 240 +#define M17_ENCODED_LSF_SIZE 488 #define M17_RAW_FRAME_SIZE 384 #define M17_CUT_FRAME_SIZE 368 @@ -179,14 +183,14 @@ namespace dsp { if (type == 0) { linkSetupOut.writeBuf[id] = delay[i++] ^ M17_SCRAMBLER[outCount - M17_SYNC_SIZE]; } - else if (type == 1 && id < M17_LICH_SIZE) { - lichOut.writeBuf[id/8] |= (delay[i++] ^ M17_SCRAMBLER[outCount - M17_SYNC_SIZE]) << (7 - (id%8)); + else if ((type == 1 || type == 2) && id < M17_LICH_SIZE) { + lichOut.writeBuf[id] = delay[i++] ^ M17_SCRAMBLER[outCount - M17_SYNC_SIZE]; } else if (type == 1) { streamOut.writeBuf[id - M17_LICH_SIZE] = (delay[i++] ^ M17_SCRAMBLER[outCount - M17_SYNC_SIZE]); } else if (type == 2) { - packetOut.writeBuf[id] = (delay[i++] ^ M17_SCRAMBLER[outCount - M17_SYNC_SIZE]); + packetOut.writeBuf[id - M17_LICH_SIZE] = (delay[i++] ^ M17_SCRAMBLER[outCount - M17_SYNC_SIZE]); } outCount++; @@ -198,10 +202,11 @@ namespace dsp { if (!linkSetupOut.swap(M17_CUT_FRAME_SIZE)) { return -1; } } else if (type == 1) { - if (!lichOut.swap(12)) {return -1; } + if (!lichOut.swap(M17_LICH_SIZE)) {return -1; } if (!streamOut.swap(M17_CUT_FRAME_SIZE)) {return -1; } } else if (type == 2) { + if (!lichOut.swap(M17_LICH_SIZE)) {return -1; } if (!packetOut.swap(M17_CUT_FRAME_SIZE)) { return -1; } } } @@ -213,7 +218,7 @@ namespace dsp { detect = true; outCount = 0; type = 0; - spdlog::warn("Found sync frame"); + //spdlog::warn("Found sync frame"); continue; } @@ -222,8 +227,7 @@ namespace dsp { detect = true; outCount = 0; type = 1; - memset(lichOut.writeBuf, 0, 12); - spdlog::warn("Found stream frame"); + //spdlog::warn("Found stream frame"); continue; } @@ -232,7 +236,7 @@ namespace dsp { detect = true; outCount = 0; type = 2; - spdlog::warn("Found packet frame"); + //spdlog::warn("Found packet frame"); continue; } @@ -263,6 +267,85 @@ namespace dsp { }; + class M17LSFDecoder : public generic_block { + public: + M17LSFDecoder() {} + + M17LSFDecoder(stream* in, void (*handler)(M17LSF& lsf, void* ctx), void* ctx) { init(in, handler, ctx); } + + ~M17LSFDecoder() { + if (!generic_block::_block_init) { return; } + generic_block::stop(); + correct_convolutional_destroy(conv); + } + + void init(stream* in, void (*handler)(M17LSF& lsf, void* ctx), void* ctx) { + _in = in; + _handler = handler; + _ctx = ctx; + + conv = correct_convolutional_create(2, 5, correct_conv_m17_polynomial); + + generic_block::registerInput(_in); + generic_block::_block_init = true; + } + + void setInput(stream* in) { + assert(generic_block::_block_init); + std::lock_guard lck(generic_block::ctrlMtx); + generic_block::tempStop(); + generic_block::unregisterInput(_in); + _in = in; + generic_block::registerInput(_in); + generic_block::tempStart(); + } + + int run() { + int count = _in->read(); + if (count < 0) { return -1; } + + // Depuncture the data + int inOffset = 0; + for (int i = 0; i < M17_ENCODED_LSF_SIZE; i++) { + if (!M17_PUNCTURING_P1[i % 61]) { + depunctured[i] = 0; + continue; + } + depunctured[i] = _in->readBuf[inOffset++]; + } + + _in->flush(); + + // Pack into bytes + memset(packed, 0, 61); + for (int i = 0; i < M17_ENCODED_LSF_SIZE; i++) { + packed[i/8] |= depunctured[i] << (7 - (i%8)); + } + + // Run through convolutional decoder + correct_convolutional_decode(conv, packed, M17_ENCODED_LSF_SIZE, lsf); + + // Decode it and call the handler + M17LSF decLsf = M17DecodeLSF(lsf); + if (decLsf.valid) { _handler(decLsf, _ctx); } + + return count; + } + + private: + stream* _in; + + void (*_handler)(M17LSF& lsf, void* ctx); + void* _ctx; + + uint8_t depunctured[488]; + uint8_t packed[61]; + uint8_t lsf[30]; + + correct_convolutional* conv; + + }; + class M17PayloadFEC : public generic_block { public: M17PayloadFEC() {} @@ -310,6 +393,7 @@ namespace dsp { } // Pack into bytes + memset(packed, 0, 37); for (int i = 0; i < M17_ENCODED_PAYLOAD_SIZE; i++) { if (!(i%8)) { packed[i/8] = 0; } packed[i/8] |= depunctured[i] << (7 - (i%8)); @@ -408,15 +492,116 @@ namespace dsp { }; + class M17LICHDecoder : public generic_block { + public: + M17LICHDecoder() {} + + M17LICHDecoder(stream* in, void (*handler)(M17LSF& lsf, void* ctx), void* ctx) { init(in, handler, ctx); } + + void init(stream* in, void (*handler)(M17LSF& lsf, void* ctx), void* ctx) { + _in = in; + _handler = handler; + _ctx = ctx; + generic_block::registerInput(_in); + generic_block::_block_init = true; + } + + void setInput(stream* in) { + assert(generic_block::_block_init); + std::lock_guard lck(generic_block::ctrlMtx); + generic_block::tempStop(); + generic_block::unregisterInput(_in); + _in = in; + generic_block::registerInput(_in); + generic_block::tempStart(); + } + + int run() { + int count = _in->read(); + if (count < 0) { return -1; } + + // Zero out block + memset(chunk, 0, 6); + + // Decode the 4 Golay(24, 12) blocks + uint32_t encodedBlock; + uint32_t decodedBlock; + for (int b = 0; b < 4; b++) { + // Pack the 24bit block into a byte + encodedBlock = 0; + decodedBlock = 0; + for (int i = 0; i < 24; i++) { encodedBlock |= _in->readBuf[(b * 24) + i] << (23 - i); } + + // Decode + if (!mobilinkd::Golay24::decode(encodedBlock, decodedBlock)) { + _in->flush(); + return count; + } + + // Pack the decoded bits into the output + int id = 0; + uint8_t temp; + for (int i = 0; i < 12; i++) { + id = (b * 12) + i; + chunk[id / 8] |= ((decodedBlock >> (23 - i)) & 1) << (7 - (id%8)); + } + } + + _in->flush(); + + int partId = chunk[5] >> 5; + + // If the ID of the chunk is zero, start a new LSF + if (partId == 0) { + newFrame = true; + lastId = 0; + memcpy(&lsf[partId*5], chunk, 5); + return count; + } + + // If we're recording a LSF and a discontinuity shows up, cancel + if (newFrame && partId != lastId + 1) { + newFrame = false; + return count; + } + + // If we're recording and there's no discontinuity (see above), add the data to the full frame + if (newFrame) { + lastId = partId; + memcpy(&lsf[partId*5], chunk, 5); + + // If the lsf is complete, send it out + if (partId == 5) { + newFrame = false; + M17LSF decLsf = M17DecodeLSF(lsf); + if (decLsf.valid) { _handler(decLsf, _ctx); } + } + } + + return count; + } + + private: + stream* _in; + void (*_handler)(M17LSF& lsf, void* ctx); + void* _ctx; + + uint8_t chunk[6]; + uint8_t lsf[240]; + bool newFrame = false; + int lastId = 0; + + }; + class M17Decoder : public generic_hier_block { public: M17Decoder() {} - M17Decoder(stream* input, float sampleRate) { - init(input, sampleRate); + M17Decoder(stream* input, float sampleRate, void (*handler)(M17LSF& lsf, void* ctx), void* ctx) { + init(input, sampleRate, handler, ctx); } - void init(stream* input, float sampleRate) { + void init(stream* input, float sampleRate, void (*handler)(M17LSF& lsf, void* ctx), void* ctx) { _sampleRate = sampleRate; demod.init(input, _sampleRate, M17_DEVIATION); @@ -426,11 +611,11 @@ namespace dsp { doubler.init(&recov.out); slice.init(&doubler.outA); demux.init(&slice.out); + lsfFEC.init(&demux.linkSetupOut, handler, ctx); payloadFEC.init(&demux.streamOut); + decodeLICH.init(&demux.lichOut, handler, ctx); decodeAudio.init(&payloadFEC.out); - ns0.init(&demux.linkSetupOut); - ns1.init(&demux.lichOut); ns2.init(&demux.packetOut); diagOut = &doubler.outB; @@ -442,11 +627,11 @@ namespace dsp { generic_hier_block::registerBlock(&doubler); generic_hier_block::registerBlock(&slice); generic_hier_block::registerBlock(&demux); + generic_hier_block::registerBlock(&lsfFEC); generic_hier_block::registerBlock(&payloadFEC); + generic_hier_block::registerBlock(&decodeLICH); generic_hier_block::registerBlock(&decodeAudio); - generic_hier_block::registerBlock(&ns0); - generic_hier_block::registerBlock(&ns1); generic_hier_block::registerBlock(&ns2); generic_hier_block::_block_init = true; @@ -468,11 +653,11 @@ namespace dsp { StreamDoubler doubler; M17Slice4FSK slice; M17FrameDemux demux; + M17LSFDecoder lsfFEC; M17PayloadFEC payloadFEC; + M17LICHDecoder decodeLICH; M17Codec2Decode decodeAudio; - NullSink ns0; - NullSink ns1; NullSink ns2; diff --git a/m17_decoder/src/main.cpp b/m17_decoder/src/main.cpp index 4454cd9d..2f60c9c0 100644 --- a/m17_decoder/src/main.cpp +++ b/m17_decoder/src/main.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #define CONCAT(a, b) ((std::string(a) + b).c_str()) @@ -36,16 +37,31 @@ ConfigManager config; class M17DecoderModule : public ModuleManager::Instance { public: - M17DecoderModule(std::string name) : diag(1.0f, 480) { + M17DecoderModule(std::string name) : diag(0.8, 480) { this->name = name; + lsf.valid = false; // Load config + config.acquire(); + if (!config.conf.contains(name)) { + config.conf[name]["showLines"] = false; + } + showLines = config.conf[name]["showLines"]; + if (showLines) { + diag.lines.push_back(-0.75f); + diag.lines.push_back(-0.25f); + diag.lines.push_back(0.25f); + diag.lines.push_back(0.75f); + } + config.release(true); + + // Initialize VFO vfo = sigpath::vfoManager.createVFO(name, ImGui::WaterfallVFO::REF_CENTER, 0, 9600, INPUT_SAMPLE_RATE, 9600, 9600, true); - vfo->setSnapInterval(500); + vfo->setSnapInterval(250); // Intialize DSP here - decoder.init(vfo->output, INPUT_SAMPLE_RATE); + decoder.init(vfo->output, INPUT_SAMPLE_RATE, lsfHandler, this); resampWin.init(4000, 4000, audioSampRate); resamp.init(decoder.out, &resampWin, 8000, audioSampRate); @@ -90,7 +106,7 @@ public: void enable() { double bw = gui::waterfall.getBandwidth(); vfo = sigpath::vfoManager.createVFO(name, ImGui::WaterfallVFO::REF_CENTER, std::clamp(0, -bw/2.0, bw/2.0), 9600, INPUT_SAMPLE_RATE, 9600, 9600, true); - vfo->setSnapInterval(500); + vfo->setSnapInterval(250); // Set Input of demod here decoder.setInput(vfo->output); @@ -130,6 +146,95 @@ private: ImGui::SetNextItemWidth(menuWidth); _this->diag.draw(); + { + std::lock_guard lck(_this->lsfMtx); + + auto now = std::chrono::high_resolution_clock::now(); + if (std::chrono::duration_cast(now-_this->lastUpdated).count() > 1000) { + _this->lsf.valid = false; + } + + ImGui::BeginTable(CONCAT("##m17_info_tbl_", _this->name), 2, ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_RowBg | ImGuiTableFlags_Borders); + if (!_this->lsf.valid) { + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::Text("Source"); + ImGui::TableSetColumnIndex(1); + ImGui::Text("--"); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::Text("Destination"); + ImGui::TableSetColumnIndex(1); + ImGui::Text("--"); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::Text("Data Type"); + ImGui::TableSetColumnIndex(1); + ImGui::Text("--"); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::Text("Encryption"); + ImGui::TableSetColumnIndex(1); + ImGui::Text("-- (Subtype --)"); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::Text("CAN"); + ImGui::TableSetColumnIndex(1); + ImGui::Text("--"); + } + else { + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::Text("Source"); + ImGui::TableSetColumnIndex(1); + ImGui::Text(_this->lsf.src.c_str()); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::Text("Destination"); + ImGui::TableSetColumnIndex(1); + ImGui::Text(_this->lsf.dst.c_str()); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::Text("Data Type"); + ImGui::TableSetColumnIndex(1); + ImGui::Text(M17DataTypesTxt[_this->lsf.dataType]); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::Text("Encryption"); + ImGui::TableSetColumnIndex(1); + ImGui::Text("%s (Subtype %d)", M17EncryptionTypesTxt[_this->lsf.encryptionType], _this->lsf.encryptionSubType); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::Text("CAN"); + ImGui::TableSetColumnIndex(1); + ImGui::Text("%d", _this->lsf.channelAccessNum); + } + ImGui::EndTable(); + } + + if (ImGui::Checkbox(CONCAT("Show Reference Lines##m17_showlines_", _this->name), &_this->showLines)) { + if (_this->showLines) { + _this->diag.lines.push_back(-0.75f); + _this->diag.lines.push_back(-0.25f); + _this->diag.lines.push_back(0.25f); + _this->diag.lines.push_back(0.75f); + } + else { + _this->diag.lines.clear(); + } + config.acquire(); + config.conf[_this->name]["showLines"] = _this->showLines; + config.release(true); + } + if (!_this->enabled) { style::endDisabled(); } } @@ -152,6 +257,13 @@ private: _this->resamp.tempStart(); } + static void lsfHandler(M17LSF& lsf, void* ctx) { + M17DecoderModule* _this = (M17DecoderModule*)ctx; + std::lock_guard lck(_this->lsfMtx); + _this->lastUpdated = std::chrono::high_resolution_clock::now(); + _this->lsf = lsf; + } + std::string name; bool enabled = true; @@ -172,6 +284,11 @@ private: EventHandler srChangeHandler; SinkManager::Stream stream; + bool showLines = false; + + M17LSF lsf; + std::mutex lsfMtx; + std::chrono::steady_clock::time_point lastUpdated; }; MOD_EXPORT void _INIT_() {