more progress on M17 support

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; }

core/src/gui/widgets/symbol_diagram.h

@@ -3,6 +3,7 @@
 #include <imgui.h>
 #include <imgui_internal.h>
 #include <dsp/stream.h>
+#include <vector>
 #include <mutex>
 
 namespace ImGui {
@@ -17,6 +18,8 @@ namespace ImGui {
 
         void releaseBuffer();
 
+        std::vector<float> lines;
+
     private:
         std::mutex bufferMtx;
         float* buffer;

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)

m17_decoder/src/base40.cpp

@@ -0,0 +1,16 @@
+#include <base40.h>
+
+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;
+}

m17_decoder/src/base40.h

@@ -0,0 +1,4 @@
+#pragma once
+#include <stdint.h>
+
+void decode_callsign_base40(uint64_t encoded, char* callsign);

m17_decoder/src/crc16.h

@@ -0,0 +1,72 @@
+// Copyright 2020 Mobilinkd LLC.
+
+#pragma once
+
+#include <cstdint>
+#include <array>
+#include <cstddef>
+
+namespace mobilinkd
+{
+
+template <uint16_t Poly = 0x5935, uint16_t Init = 0xFFFF>
+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<uint8_t, 2> get_bytes()
+    {
+        auto crc = get();
+        std::array<uint8_t, 2> result{uint8_t((crc >> 8) & 0xFF), uint8_t(crc & 0xFF)};
+        return result;
+    }
+};
+
+} // mobilinkd

m17_decoder/src/golay24.h

@@ -0,0 +1,233 @@
+// Copyright 2020 Rob Riggs <rob@mobilinkd.com>
+// All rights reserved.
+
+#pragma once
+
+#include <array>
+#include <cstdint>
+#include <algorithm>
+#include <utility>
+
+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<class T>
+void swap(T& l, T& r)
+{
+    T tmp = std::move(l);
+    l = std::move(r);
+    r = std::move(tmp);
+}
+
+template <typename T, size_t N>
+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 <typename T, size_t N>
+void sort_impl(array<T, N> &array, size_t left, size_t right)
+{
+    if (left < right)
+    {
+        size_t m = left;
+
+        for (size_t i = left + 1; i<right; i++)
+            if (array[i]<array[left])
+                swap(array[++m], array[i]);
+
+        swap(array[left], array[m]);
+
+        sort_impl(array, left, m);
+        sort_impl(array, m + 1, right);
+    }
+}
+
+template <typename T, size_t N>
+array<T, N> sort(array<T, N> 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<SyndromeMapEntry, LUT_SIZE> make_lut()
+{
+    constexpr size_t VECLEN=23;
+    detail::array<uint64_t, LUT_SIZE> 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<SyndromeMapEntry, LUT_SIZE> 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

m17_decoder/src/lsf_decode.cpp

@@ -0,0 +1,112 @@
+#include <lsf_decode.h>
+#include <string.h>
+#include <base40.h>
+#include <stdio.h>
+#include <inttypes.h>
+#include <crc16.h>
+#include <spdlog/spdlog.h>
+
+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;
+}

m17_decoder/src/lsf_decode.h

@@ -0,0 +1,42 @@
+#pragma once
+#include <string>
+#include <stdint.h>
+
+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);

m17_decoder/src/m17dsp.h

@@ -3,6 +3,8 @@
 #include <volk/volk.h>
 #include <codec2.h>
 #include <dsp/demodulator.h>
+#include <golay24.h>
+#include <lsf_decode.h>
 
 extern "C" {
     #include <correct.h>
@@ -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) {
+                        else if ((type == 1 || type == 2) && id < M17_LICH_SIZE) {
-                            lichOut.writeBuf[id/8] |= (delay[i++] ^ M17_SCRAMBLER[outCount - M17_SYNC_SIZE]) << (7 - (id%8));
+                            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<M17LSFDecoder> {
+    public:
+        M17LSFDecoder() {}
+
+        M17LSFDecoder(stream<uint8_t>* in, void (*handler)(M17LSF& lsf, void* ctx), void* ctx) { init(in, handler, ctx); }
+
+        ~M17LSFDecoder() {
+            if (!generic_block<M17LSFDecoder>::_block_init) { return; }
+            generic_block<M17LSFDecoder>::stop();
+            correct_convolutional_destroy(conv);
+        }
+
+        void init(stream<uint8_t>* 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<M17LSFDecoder>::registerInput(_in);
+            generic_block<M17LSFDecoder>::_block_init = true;
+        }
+
+        void setInput(stream<uint8_t>* in) {
+            assert(generic_block<M17LSFDecoder>::_block_init);
+            std::lock_guard<std::mutex> lck(generic_block<M17LSFDecoder>::ctrlMtx);
+            generic_block<M17LSFDecoder>::tempStop();
+            generic_block<M17LSFDecoder>::unregisterInput(_in);
+            _in = in;
+            generic_block<M17LSFDecoder>::registerInput(_in);
+            generic_block<M17LSFDecoder>::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<uint8_t>* _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<M17PayloadFEC> {
     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<M17LICHDecoder> {
+    public:
+        M17LICHDecoder() {}
+
+        M17LICHDecoder(stream<uint8_t>* in, void (*handler)(M17LSF& lsf, void* ctx), void* ctx) { init(in, handler, ctx); }
+
+        void init(stream<uint8_t>* in, void (*handler)(M17LSF& lsf, void* ctx), void* ctx) {
+            _in = in;
+            _handler = handler;
+            _ctx = ctx;
+            generic_block<M17LICHDecoder>::registerInput(_in);
+            generic_block<M17LICHDecoder>::_block_init = true;
+        }
+
+        void setInput(stream<uint8_t>* in) {
+            assert(generic_block<M17LICHDecoder>::_block_init);
+            std::lock_guard<std::mutex> lck(generic_block<M17LICHDecoder>::ctrlMtx);
+            generic_block<M17LICHDecoder>::tempStop();
+            generic_block<M17LICHDecoder>::unregisterInput(_in);
+            _in = in;
+            generic_block<M17LICHDecoder>::registerInput(_in);
+            generic_block<M17LICHDecoder>::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<uint8_t>* _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<M17Decoder> {
     public:
         M17Decoder() {}
 
-        M17Decoder(stream<complex_t>* input, float sampleRate) {
+        M17Decoder(stream<complex_t>* input, float sampleRate, void (*handler)(M17LSF& lsf, void* ctx), void* ctx) {
-            init(input, sampleRate);
+            init(input, sampleRate, handler, ctx);
         }
 
-        void init(stream<complex_t>* input, float sampleRate) {
+        void init(stream<complex_t>* 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<M17Decoder>::registerBlock(&doubler);
             generic_hier_block<M17Decoder>::registerBlock(&slice);
             generic_hier_block<M17Decoder>::registerBlock(&demux);
+            generic_hier_block<M17Decoder>::registerBlock(&lsfFEC);
             generic_hier_block<M17Decoder>::registerBlock(&payloadFEC);
+            generic_hier_block<M17Decoder>::registerBlock(&decodeLICH);
             generic_hier_block<M17Decoder>::registerBlock(&decodeAudio);
 
-            generic_hier_block<M17Decoder>::registerBlock(&ns0);
-            generic_hier_block<M17Decoder>::registerBlock(&ns1);
             generic_hier_block<M17Decoder>::registerBlock(&ns2);
 
             generic_hier_block<M17Decoder>::_block_init = true;
@@ -468,11 +653,11 @@ namespace dsp {
         StreamDoubler<float> doubler;
         M17Slice4FSK slice;
         M17FrameDemux demux;
+        M17LSFDecoder lsfFEC;
         M17PayloadFEC payloadFEC;
+        M17LICHDecoder decodeLICH;
         M17Codec2Decode decodeAudio;
 
-        NullSink<uint8_t> ns0;
-        NullSink<uint8_t> ns1;
         NullSink<uint8_t> ns2;
 
 

m17_decoder/src/main.cpp

@@ -19,6 +19,7 @@
 #include <gui/widgets/symbol_diagram.h>
 #include <m17dsp.h>
 #include <fstream>
+#include <chrono>
 
 #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<double>(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<std::chrono::milliseconds>(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<float> srChangeHandler;
     SinkManager::Stream stream;
 
+    bool showLines = false;
+
+    M17LSF lsf;
+    std::mutex lsfMtx;
+    std::chrono::steady_clock::time_point lastUpdated;
 };
 
 MOD_EXPORT void _INIT_() {