mirror of
https://github.com/f4exb/sdrangel.git
synced 2024-08-26 07:46:33 -04:00
410 lines
13 KiB
C++
410 lines
13 KiB
C++
// Copyright 2021 Mobilinkd LLC.
|
|
|
|
#pragma once
|
|
|
|
#include <QDebug>
|
|
#include <QString>
|
|
|
|
#include "M17Randomizer.h"
|
|
#include "PolynomialInterleaver.h"
|
|
#include "Trellis.h"
|
|
#include "Viterbi.h"
|
|
#include "CRC16.h"
|
|
#include "LinkSetupFrame.h"
|
|
#include "Golay24.h"
|
|
|
|
#include <algorithm>
|
|
#include <array>
|
|
#include <cstddef>
|
|
#include <functional>
|
|
#include <iostream>
|
|
#include <iomanip>
|
|
|
|
namespace modemm17
|
|
{
|
|
|
|
|
|
template <typename C, size_t N>
|
|
QString dump(const std::array<C,N>& data, char header = 'D')
|
|
{
|
|
QString s(header);
|
|
s += "=";
|
|
|
|
for (auto c : data) {
|
|
s += QString("%1 ").arg((int) c, 2, 16, QChar('0'));
|
|
}
|
|
|
|
return s;
|
|
}
|
|
|
|
struct M17FrameDecoder
|
|
{
|
|
static const size_t MAX_LICH_FRAGMENT = 5;
|
|
|
|
M17Randomizer derandomize_;
|
|
PolynomialInterleaver<45, 92, 368> interleaver_;
|
|
Trellis<4,2> trellis_{makeTrellis<4, 2>({031,027})};
|
|
Viterbi<decltype(trellis_), 4> viterbi_{trellis_};
|
|
CRC16 crc_;
|
|
|
|
enum class State { LSF, STREAM, BASIC_PACKET, FULL_PACKET, BERT };
|
|
enum class SyncWordType { LSF, STREAM, PACKET, BERT };
|
|
enum class DecodeResult { FAIL, OK, EOS, INCOMPLETE, PACKET_INCOMPLETE };
|
|
enum class FrameType { LSF, LICH, STREAM, BASIC_PACKET, FULL_PACKET, BERT };
|
|
|
|
State state_ = State::LSF;
|
|
|
|
using input_buffer_t = std::array<int8_t, 368>;
|
|
|
|
using lsf_conv_buffer_t = std::array<uint8_t, 46>;
|
|
using audio_conv_buffer_t = std::array<uint8_t, 34>;
|
|
|
|
using lsf_buffer_t = std::array<uint8_t, 30>;
|
|
using lich_buffer_t = std::array<uint8_t, 6>;
|
|
using audio_buffer_t = std::array<uint8_t, 18>;
|
|
using packet_buffer_t = std::array<uint8_t, 26>;
|
|
using bert_buffer_t = std::array<uint8_t, 25>;
|
|
|
|
using output_buffer_t = struct {
|
|
FrameType type;
|
|
union {
|
|
lich_buffer_t lich;
|
|
audio_buffer_t stream;
|
|
packet_buffer_t packet;
|
|
bert_buffer_t bert;
|
|
};
|
|
lsf_buffer_t lsf;
|
|
};
|
|
|
|
using depunctured_buffer_t = union {
|
|
std::array<int8_t, 488> lsf;
|
|
std::array<int8_t, 296> stream;
|
|
std::array<int8_t, 420> packet;
|
|
std::array<int8_t, 402> bert;
|
|
};
|
|
|
|
using decode_buffer_t = union {
|
|
std::array<uint8_t, 240> lsf;
|
|
std::array<uint8_t, 144> stream;
|
|
std::array<uint8_t, 206> packet;
|
|
std::array<uint8_t, 197> bert;
|
|
};
|
|
|
|
/**
|
|
* Callback function for frame types. The caller is expected to return
|
|
* true if the data was good or unknown and false if the data is known
|
|
* to be bad.
|
|
*/
|
|
using callback_t = std::function<bool(const output_buffer_t&, int)>;
|
|
|
|
callback_t callback_;
|
|
|
|
output_buffer_t output_buffer;
|
|
decode_buffer_t decode_buffer;
|
|
uint16_t frame_number = 0;
|
|
|
|
uint8_t lich_segments{0}; ///< one bit per received LICH fragment.
|
|
|
|
M17FrameDecoder(callback_t callback) :
|
|
crc_(0x5935, 0xFFFF),
|
|
callback_(callback)
|
|
{}
|
|
|
|
void update_state(std::array<uint8_t, 240>& lsf_output)
|
|
{
|
|
if (lsf_output[111]) // LSF type bit 0
|
|
{
|
|
if (lsf_output[109] != 0) {
|
|
state_ = State::STREAM;
|
|
}
|
|
}
|
|
else // packet frame comes next.
|
|
{
|
|
uint8_t packet_type = (lsf_output[109] << 1) | lsf_output[110];
|
|
switch (packet_type)
|
|
{
|
|
case 1: // RAW -- ignore LSF.
|
|
state_ = State::BASIC_PACKET;
|
|
break;
|
|
case 2: // ENCAPSULATED
|
|
state_ = State::FULL_PACKET;
|
|
break;
|
|
default:
|
|
state_ = State::FULL_PACKET;
|
|
}
|
|
}
|
|
}
|
|
|
|
void reset()
|
|
{
|
|
state_ = State::LSF;
|
|
frame_number = 0;
|
|
}
|
|
|
|
/**
|
|
* Decode the LSF and, if it is valid, transition to the next state.
|
|
*
|
|
* The LSF is returned for STREAM mode, dropped for BASIC_PACKET mode,
|
|
* and captured for FULL_PACKET mode.
|
|
*
|
|
* @param buffer
|
|
* @param viterbi_cost
|
|
* @return
|
|
*/
|
|
DecodeResult decode_lsf(input_buffer_t& buffer, int& viterbi_cost)
|
|
{
|
|
depunctured_buffer_t depuncture_buffer;
|
|
depuncture<368, 488, 61>(buffer, depuncture_buffer.lsf, P1);
|
|
viterbi_cost = viterbi_.decode(depuncture_buffer.lsf, decode_buffer.lsf);
|
|
to_byte_array(decode_buffer.lsf, output_buffer.lsf);
|
|
|
|
// qDebug() << "modemm17::M17FrameDecoder::decode_lsf: vierbi:" << viterbi_cost <<dump(output_buffer.lsf);
|
|
|
|
crc_.reset();
|
|
for (auto c : output_buffer.lsf) crc_(c);
|
|
auto checksum = crc_.get();
|
|
|
|
if (checksum == 0)
|
|
{
|
|
update_state(decode_buffer.lsf);
|
|
output_buffer.type = FrameType::LSF;
|
|
callback_(output_buffer, viterbi_cost);
|
|
return DecodeResult::OK;
|
|
}
|
|
else
|
|
{
|
|
qDebug() << "modemm17::M17FrameDecoder::decode_lsf: bad CRC:" << dump(output_buffer.lsf);
|
|
}
|
|
|
|
lich_segments = 0;
|
|
output_buffer.lsf.fill(0);
|
|
return DecodeResult::FAIL;
|
|
}
|
|
|
|
// Unpack & decode LICH fragments into tmp_buffer.
|
|
bool unpack_lich(input_buffer_t& buffer)
|
|
{
|
|
size_t index = 0;
|
|
// Read the 4 24-bit codewords from LICH
|
|
for (size_t i = 0; i != 4; ++i) // for each codeword
|
|
{
|
|
uint32_t codeword = 0;
|
|
for (size_t j = 0; j != 24; ++j) // for each bit in codeword
|
|
{
|
|
codeword <<= 1;
|
|
codeword |= (buffer[i * 24 + j] > 0);
|
|
}
|
|
uint32_t decoded = 0;
|
|
if (!Golay24::decode(codeword, decoded))
|
|
{
|
|
return false;
|
|
}
|
|
decoded >>= 12; // Remove check bits and parity.
|
|
// append codeword.
|
|
if (i & 1)
|
|
{
|
|
output_buffer.lich[index++] |= (decoded >> 8); // upper 4 bits
|
|
output_buffer.lich[index++] = (decoded & 0xFF); // lower 8 bits
|
|
}
|
|
else
|
|
{
|
|
output_buffer.lich[index++] |= (decoded >> 4); // upper 8 bits
|
|
output_buffer.lich[index] = (decoded & 0x0F) << 4; // lower 4 bits
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
DecodeResult decode_lich(input_buffer_t& buffer, int& viterbi_cost)
|
|
{
|
|
output_buffer.lich.fill(0);
|
|
// Read the 4 12-bit codewords from LICH into buffers.lich.
|
|
if (!unpack_lich(buffer)) return DecodeResult::FAIL;
|
|
|
|
output_buffer.type = FrameType::LICH;
|
|
callback_(output_buffer, 0);
|
|
|
|
uint8_t fragment_number = output_buffer.lich[5]; // Get fragment number.
|
|
fragment_number = (fragment_number >> 5) & 7;
|
|
|
|
if (fragment_number > MAX_LICH_FRAGMENT)
|
|
{
|
|
viterbi_cost = -1;
|
|
return DecodeResult::INCOMPLETE; // More to go...
|
|
}
|
|
|
|
// Copy decoded LICH to superframe buffer.
|
|
std::copy(output_buffer.lich.begin(), output_buffer.lich.begin() + 5,
|
|
output_buffer.lsf.begin() + (fragment_number * 5));
|
|
|
|
lich_segments |= (1 << fragment_number); // Indicate segment received.
|
|
if ((lich_segments & 0x3F) != 0x3F)
|
|
{
|
|
viterbi_cost = -1;
|
|
return DecodeResult::INCOMPLETE; // More to go...
|
|
}
|
|
|
|
crc_.reset();
|
|
for (auto c : output_buffer.lsf) crc_(c);
|
|
auto checksum = crc_.get();
|
|
|
|
if (checksum == 0)
|
|
{
|
|
lich_segments = 0;
|
|
state_ = State::STREAM;
|
|
viterbi_cost = 0;
|
|
output_buffer.type = FrameType::LSF;
|
|
callback_(output_buffer, viterbi_cost);
|
|
return DecodeResult::OK;
|
|
}
|
|
|
|
// Failed CRC... try again.
|
|
// lich_segments = 0;
|
|
// output_buffer.lsf.fill(0);
|
|
viterbi_cost = 128;
|
|
return DecodeResult::INCOMPLETE;
|
|
}
|
|
|
|
DecodeResult decode_bert(input_buffer_t& buffer, int& viterbi_cost)
|
|
{
|
|
depunctured_buffer_t depuncture_buffer;
|
|
depuncture<368, 402, 12>(buffer, depuncture_buffer.bert, P2);
|
|
viterbi_cost = viterbi_.decode(depuncture_buffer.bert, decode_buffer.bert);
|
|
to_byte_array(decode_buffer.bert, output_buffer.bert);
|
|
|
|
output_buffer.type = FrameType::BERT;
|
|
callback_(output_buffer, viterbi_cost);
|
|
|
|
return DecodeResult::OK;
|
|
}
|
|
|
|
DecodeResult decode_stream(input_buffer_t& buffer, int& viterbi_cost)
|
|
{
|
|
std::array<int8_t, 272> tmp;
|
|
std::copy(buffer.begin() + 96, buffer.end(), tmp.begin());
|
|
depunctured_buffer_t depuncture_buffer;
|
|
|
|
depuncture<272, 296, 12>(tmp, depuncture_buffer.stream, P2);
|
|
viterbi_cost = viterbi_.decode(depuncture_buffer.stream, decode_buffer.stream);
|
|
to_byte_array(decode_buffer.stream, output_buffer.stream);
|
|
|
|
if ((viterbi_cost < 60) && (output_buffer.stream[0] & 0x80))
|
|
{
|
|
// fputs("\nEOS\n", stderr);
|
|
state_ = State::LSF;
|
|
}
|
|
|
|
output_buffer.type = FrameType::STREAM;
|
|
callback_(output_buffer, viterbi_cost);
|
|
|
|
return state_ == State::LSF ? DecodeResult::EOS : DecodeResult::OK;
|
|
}
|
|
|
|
/**
|
|
* Capture packet frames until an EOF bit is found.
|
|
|
|
* @param buffer the demodulated M17 symbols in LLR format.
|
|
* @param viterbi_cost the cost of traversing the trellis.
|
|
* @param frame_type is either BASIC_PACKET or FULL_PACKET.
|
|
* @return the result of decoding the packet frame.
|
|
*/
|
|
DecodeResult decode_packet(input_buffer_t& buffer, int& viterbi_cost, FrameType type)
|
|
{
|
|
depunctured_buffer_t depuncture_buffer;
|
|
depuncture<368, 420, 8>(buffer, depuncture_buffer.packet, P3);
|
|
viterbi_cost = viterbi_.decode(depuncture_buffer.packet, decode_buffer.packet);
|
|
to_byte_array(decode_buffer.packet, output_buffer.packet);
|
|
|
|
output_buffer.type = type;
|
|
auto result = callback_(output_buffer, viterbi_cost);
|
|
|
|
if (output_buffer.packet[25] & 0x80) // last packet;
|
|
{
|
|
state_ = State::LSF;
|
|
return result ? DecodeResult::OK : DecodeResult::FAIL;
|
|
}
|
|
|
|
return DecodeResult::PACKET_INCOMPLETE;
|
|
}
|
|
|
|
/**
|
|
* Decode M17 frames. The decoder uses the sync word to determine frame
|
|
* type and to update its state machine.
|
|
*
|
|
* The decoder receives M17 frame type indicator (based on sync word) and
|
|
* frames from the M17 demodulator.
|
|
*
|
|
* If the frame is an LSF, the state immediately changes to LSF. When
|
|
* in LSF mode, the state machine can transition to:
|
|
*
|
|
* - LSF if the CRC is bad.
|
|
* - STREAM if the LSF type field indicates Stream.
|
|
* - BASIC_PACKET if the LSF type field indicates Packet and the packet
|
|
* type is RAW.
|
|
* - FULL_PACKET if the LSF type field indicates Packet and the packet
|
|
* type is ENCAPSULATED or RESERVED.
|
|
*
|
|
* When in LSF mode, if an LSF frame is received it is parsed as an LSF.
|
|
* When a STREAM frame is received, it attempts to recover an LSF from
|
|
* the LICH. PACKET frame types are ignored when state is LSF.
|
|
*
|
|
* When in STREAM mode, the state machine can transition to either:
|
|
*
|
|
* - STREAM when a any stream frame is received.
|
|
* - LSF when the EOS indicator is set, or when a packet frame is received.
|
|
*
|
|
* When in BASIC_PACKET mode, the state machine can transition to either:
|
|
*
|
|
* - BASIC_PACKET when any packet frame is received.
|
|
* - LSF when the EOS indicator is set, or when a stream frame is received.
|
|
*
|
|
* When in FULL_PACKET mode, the state machine can transition to either:
|
|
*
|
|
* - FULL_PACKET when any packet frame is received.
|
|
* - LSF when the EOS indicator is set, or when a stream frame is received.
|
|
*/
|
|
DecodeResult operator()(SyncWordType frame_type, input_buffer_t& buffer, int& viterbi_cost)
|
|
{
|
|
derandomize_(buffer);
|
|
interleaver_.deinterleave(buffer);
|
|
|
|
// This is out state machined.
|
|
switch(frame_type)
|
|
{
|
|
case SyncWordType::LSF:
|
|
state_ = State::LSF;
|
|
return decode_lsf(buffer, viterbi_cost);
|
|
case SyncWordType::STREAM:
|
|
switch (state_)
|
|
{
|
|
case State::LSF:
|
|
return decode_lich(buffer, viterbi_cost);
|
|
case State::STREAM:
|
|
return decode_stream(buffer, viterbi_cost);
|
|
default:
|
|
state_ = State::LSF;
|
|
}
|
|
break;
|
|
case SyncWordType::PACKET:
|
|
switch (state_)
|
|
{
|
|
case State::BASIC_PACKET:
|
|
return decode_packet(buffer, viterbi_cost, FrameType::BASIC_PACKET);
|
|
case State::FULL_PACKET:
|
|
return decode_packet(buffer, viterbi_cost, FrameType::FULL_PACKET);
|
|
default:
|
|
state_ = State::LSF;
|
|
}
|
|
break;
|
|
case SyncWordType::BERT:
|
|
state_ = State::BERT;
|
|
return decode_bert(buffer, viterbi_cost);
|
|
}
|
|
|
|
return DecodeResult::FAIL;
|
|
}
|
|
|
|
State state() const { return state_; }
|
|
};
|
|
|
|
} // modemm17
|