From d2c7be9cc8558ca5f7f585fb30f6de18609c40b8 Mon Sep 17 00:00:00 2001
From: Silvano Seva <silseva@fastwebnet.it>
Date: Sat, 8 Jan 2022 22:25:40 +0100
Subject: [PATCH] Fixed bug affecting LSF reassembling from LICH segments
 inside M17 frame decoder class

---
 openrtx/src/protocols/M17/M17FrameDecoder.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/openrtx/src/protocols/M17/M17FrameDecoder.cpp b/openrtx/src/protocols/M17/M17FrameDecoder.cpp
index 946e26f5..e901e3c9 100644
--- a/openrtx/src/protocols/M17/M17FrameDecoder.cpp
+++ b/openrtx/src/protocols/M17/M17FrameDecoder.cpp
@@ -94,15 +94,17 @@ void M17FrameDecoder::decodeStream(const std::array< uint8_t, 46 >& data)
     if(decodeOk)
     {
         // Append LICH segment
-        uint8_t segmentNum = lsfSegment[5];
+        uint8_t segmentNum  = lsfSegment[5];
+        uint8_t segmentSize = lsfSegment.size() - 1;
         uint8_t *ptr = reinterpret_cast < uint8_t * >(&lsfFromLich.data);
-        memcpy(ptr + segmentNum, lsfSegment.data(), 5);
+        ptr += segmentNum * segmentSize;
+        memcpy(ptr, lsfSegment.data(), segmentSize);
 
         // Mark this segment as present
         lsfSegmentMap |= 1 << segmentNum;
 
         // Check if we have received all the five LICH segments
-        if(lsfSegmentMap == 0x1F)
+        if(lsfSegmentMap == 0x3F)
         {
             if(lsfFromLich.valid()) lsf = lsfFromLich;
             lsfSegmentMap = 0;