refactor DMR PDU processing to be more in-line with how P25 PDU processing is done; implement support on the FNE to inspect and dispatch DMR PDUs properly;
Bryan Biedenkapp
2 years ago
parent
b343473a87
commit
dc29208e70
@ -0,0 +1,369 @@
|
||||
// SPDX-License-Identifier: GPL-2.0-only
|
||||
/*
|
||||
* Digital Voice Modem - Common Library
|
||||
* GPLv2 Open Source. Use is subject to license terms.
|
||||
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
|
||||
*
|
||||
* Copyright (C) 2018-2024 Bryan Biedenkapp, N2PLL
|
||||
*
|
||||
*/
|
||||
#include "Defines.h"
|
||||
#include "dmr/DMRDefines.h"
|
||||
#include "dmr/data/DataBlock.h"
|
||||
#include "edac/CRC.h"
|
||||
#include "Log.h"
|
||||
#include "Utils.h"
|
||||
|
||||
using namespace dmr;
|
||||
using namespace dmr::defines;
|
||||
using namespace dmr::data;
|
||||
|
||||
#include <cassert>
|
||||
#include <cstring>
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Public Class Members
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
/* Initializes a new instance of the DataBlock class. */
|
||||
|
||||
DataBlock::DataBlock() :
|
||||
m_serialNo(0U),
|
||||
m_lastBlock(false),
|
||||
m_bptc(),
|
||||
m_trellis(),
|
||||
m_dataType(DataType::RATE_34_DATA),
|
||||
m_DPF(DPF::CONFIRMED_DATA),
|
||||
m_data(nullptr)
|
||||
{
|
||||
m_data = new uint8_t[DMR_PDU_UNCODED_LENGTH_BYTES];
|
||||
}
|
||||
|
||||
/* Finalizes a instance of the DataBlock class. */
|
||||
|
||||
DataBlock::~DataBlock()
|
||||
{
|
||||
delete[] m_data;
|
||||
}
|
||||
|
||||
/* Decodes DMR PDU data block. */
|
||||
|
||||
bool DataBlock::decode(const uint8_t* data, const DataHeader& header)
|
||||
{
|
||||
assert(data != nullptr);
|
||||
assert(m_data != nullptr);
|
||||
|
||||
uint8_t buffer[DMR_PDU_UNCODED_LENGTH_BYTES];
|
||||
::memset(buffer, 0x00U, DMR_PDU_UNCODED_LENGTH_BYTES);
|
||||
|
||||
m_DPF = header.getDPF();
|
||||
|
||||
// set these to reasonable defaults
|
||||
m_serialNo = 0U;
|
||||
m_lastBlock = false;
|
||||
|
||||
if (m_DPF == DPF::CONFIRMED_DATA) {
|
||||
try {
|
||||
if (m_dataType == DataType::RATE_34_DATA) {
|
||||
bool valid = m_trellis.decode34(data, buffer, true);
|
||||
if (!valid) {
|
||||
LogError(LOG_DMR, "DataBlock::decode(), failed to decode Trellis 3/4 rate coding");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else if (m_dataType == DataType::RATE_12_DATA) {
|
||||
m_bptc.decode(data, buffer);
|
||||
}
|
||||
else {
|
||||
LogError(LOG_DMR, "DataBlock::decode(), cowardly refusing to decode confirmed full-rate (rate 1) data");
|
||||
return false;
|
||||
}
|
||||
|
||||
#if DEBUG_DMR_PDU_DATA
|
||||
Utils::dump(1U, "DMR, DataBlock::decode(), Confirmed PDU Data Block", buffer, DMR_PDU_UNCODED_LENGTH_BYTES);
|
||||
#endif
|
||||
|
||||
m_serialNo = (buffer[0] & 0xFEU) >> 1; // Confirmed Data Serial No.
|
||||
uint16_t crc = ((buffer[0] & 0x01U) << 8) + buffer[1]; // CRC-9 Check Sum
|
||||
|
||||
::memset(m_data, 0x00U, DMR_PDU_UNCODED_LENGTH_BYTES);
|
||||
if (m_dataType == DataType::RATE_34_DATA)
|
||||
::memcpy(m_data, buffer + 2U, DMR_PDU_CONFIRMED_DATA_LENGTH_BYTES); // Payload Data
|
||||
else if (m_dataType == DataType::RATE_12_DATA)
|
||||
::memcpy(m_data, buffer + 2U, DMR_PDU_UNCONFIRMED_LENGTH_BYTES); // Payload Data
|
||||
else {
|
||||
LogError(LOG_DMR, "DataBlock::decode(), failed to decode block, invalid dataType = $%02X", m_dataType);
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t crcBuffer[DMR_PDU_UNCODED_LENGTH_BYTES];
|
||||
::memset(crcBuffer, 0x00U, DMR_PDU_UNCODED_LENGTH_BYTES);
|
||||
|
||||
// generate CRC buffer
|
||||
uint32_t bufferLen = DMR_PDU_CONFIRMED_DATA_LENGTH_BYTES;
|
||||
if (m_dataType == DataType::RATE_12_DATA)
|
||||
bufferLen = DMR_PDU_UNCONFIRMED_LENGTH_BYTES;
|
||||
uint32_t crcBitLength = 144U;
|
||||
if (m_dataType == DataType::RATE_12_DATA)
|
||||
crcBitLength = 96U;
|
||||
|
||||
for (uint32_t i = 16U; i < crcBitLength; i++) {
|
||||
bool b = READ_BIT(buffer, i);
|
||||
WRITE_BIT(crcBuffer, i - 16U, b);
|
||||
}
|
||||
|
||||
for (uint32_t i = 0U; i < 6U; i++) {
|
||||
bool b = READ_BIT(buffer, i);
|
||||
WRITE_BIT(crcBuffer, i + (crcBitLength - 16U), b);
|
||||
}
|
||||
|
||||
// compute CRC-9 for the packet
|
||||
uint16_t calculated = edac::CRC::createCRC9(crcBuffer, crcBitLength - 9U);
|
||||
calculated = ~calculated & 0x1FFU;
|
||||
|
||||
if ((crc ^ calculated) != 0) {
|
||||
LogWarning(LOG_DMR, "DMR, dataType = $%02X, invalid crc = $%04X != $%04X (computed)", m_dataType, crc, calculated);
|
||||
}
|
||||
|
||||
#if DEBUG_DMR_PDU_DATA
|
||||
LogDebug(LOG_DMR, "DMR, dataType = $%02X, crc = $%04X, calculated = $%04X", m_dataType, crc, calculated);
|
||||
#endif
|
||||
}
|
||||
catch (...) {
|
||||
Utils::dump(2U, "DMR, decoding excepted with input data", data, DMR_PDU_UNCODED_LENGTH_BYTES);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else if (m_DPF == DPF::UNCONFIRMED_DATA || m_DPF == DPF::RESPONSE || m_DPF == DPF::DEFINED_RAW ||
|
||||
m_DPF == DPF::DEFINED_SHORT || m_DPF == DPF::UDT) {
|
||||
try {
|
||||
if (m_dataType == DataType::RATE_34_DATA) {
|
||||
bool valid = m_trellis.decode34(data, buffer, true);
|
||||
if (!valid) {
|
||||
LogError(LOG_DMR, "DataBlock::decode(), failed to decode Trellis 3/4 rate coding");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else if (m_dataType == DataType::RATE_12_DATA) {
|
||||
m_bptc.decode(data, buffer);
|
||||
}
|
||||
else {
|
||||
::memcpy(buffer, data, DMR_PDU_UNCODED_LENGTH_BYTES);
|
||||
return true; // never do any further processing for uncoded
|
||||
}
|
||||
|
||||
::memset(m_data, 0x00U, DMR_PDU_UNCODED_LENGTH_BYTES);
|
||||
if (m_dataType == DataType::RATE_34_DATA)
|
||||
::memcpy(m_data, buffer, DMR_PDU_CONFIRMED_DATA_LENGTH_BYTES); // Payload Data
|
||||
else if (m_dataType == DataType::RATE_12_DATA)
|
||||
::memcpy(m_data, buffer, DMR_PDU_UNCONFIRMED_LENGTH_BYTES); // Payload Data
|
||||
else {
|
||||
LogError(LOG_DMR, "DataBlock::decode(), failed to decode block, invalid dataType = $%02X", m_dataType);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
catch (...) {
|
||||
Utils::dump(2U, "DMR, decoding excepted with input data", data, DMR_PDU_UNCODED_LENGTH_BYTES);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else {
|
||||
LogError(LOG_P25, "unknown DPF value in PDU, dpf = $%02X", m_DPF);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/* Encodes a DMR PDU data block. */
|
||||
|
||||
void DataBlock::encode(uint8_t* data)
|
||||
{
|
||||
assert(data != nullptr);
|
||||
assert(m_data != nullptr);
|
||||
|
||||
if (m_DPF == DPF::CONFIRMED_DATA) {
|
||||
if (m_dataType == DataType::RATE_34_DATA) {
|
||||
uint8_t buffer[DMR_PDU_CONFIRMED_LENGTH_BYTES];
|
||||
::memset(buffer, 0x00U, DMR_PDU_CONFIRMED_LENGTH_BYTES);
|
||||
|
||||
buffer[0U] = ((m_serialNo << 1) & 0xFEU); // Confirmed Data Serial No.
|
||||
|
||||
::memcpy(buffer + 2U, m_data, DMR_PDU_CONFIRMED_DATA_LENGTH_BYTES); // Payload Data
|
||||
|
||||
uint8_t crcBuffer[DMR_PDU_UNCODED_LENGTH_BYTES];
|
||||
::memset(crcBuffer, 0x00U, DMR_PDU_UNCODED_LENGTH_BYTES);
|
||||
|
||||
// generate CRC buffer
|
||||
uint32_t bufferLen = DMR_PDU_CONFIRMED_DATA_LENGTH_BYTES;
|
||||
uint32_t crcBitLength = 144U;
|
||||
|
||||
for (uint32_t i = 2U; i < bufferLen; i++)
|
||||
crcBuffer[i - 2U] = buffer[2U];
|
||||
for (uint32_t i = 0; i < 6U; i++) {
|
||||
bool b = READ_BIT(buffer, i);
|
||||
WRITE_BIT(crcBuffer, i + (crcBitLength - 15U), b);
|
||||
}
|
||||
|
||||
uint16_t crc = edac::CRC::createCRC9(crcBuffer, 135U);
|
||||
buffer[0U] = buffer[0U] + ((crc >> 8) & 0x01U); // CRC-9 Check Sum (b8)
|
||||
buffer[1U] = (crc & 0xFFU); // CRC-9 Check Sum (b0 - b7)
|
||||
|
||||
#if DEBUG_DMR_PDU_DATA
|
||||
Utils::dump(1U, "DMR, DataBlock::encode(), Confirmed PDU Data Block", buffer, DMR_PDU_CONFIRMED_LENGTH_BYTES);
|
||||
#endif
|
||||
|
||||
m_trellis.encode34(buffer, data, true);
|
||||
}
|
||||
else if (m_dataType == DataType::RATE_12_DATA) {
|
||||
uint8_t buffer[DMR_PDU_UNCONFIRMED_LENGTH_BYTES];
|
||||
::memset(buffer, 0x00U, DMR_PDU_UNCONFIRMED_LENGTH_BYTES);
|
||||
|
||||
buffer[0U] = ((m_serialNo << 1) & 0xFEU); // Confirmed Data Serial No.
|
||||
|
||||
::memcpy(buffer + 2U, m_data, DMR_PDU_CONFIRMED_HALFRATE_DATA_LENGTH_BYTES); // Payload Data
|
||||
|
||||
uint8_t crcBuffer[DMR_PDU_UNCODED_LENGTH_BYTES];
|
||||
::memset(crcBuffer, 0x00U, DMR_PDU_UNCODED_LENGTH_BYTES);
|
||||
|
||||
// generate CRC buffer
|
||||
uint32_t bufferLen = DMR_PDU_CONFIRMED_HALFRATE_DATA_LENGTH_BYTES;
|
||||
uint32_t crcBitLength = 96U;
|
||||
|
||||
for (uint32_t i = 2U; i < bufferLen; i++)
|
||||
crcBuffer[i - 2U] = buffer[2U];
|
||||
for (uint32_t i = 0; i < 6U; i++) {
|
||||
bool b = READ_BIT(buffer, i);
|
||||
WRITE_BIT(crcBuffer, i + (crcBitLength - 15U), b);
|
||||
}
|
||||
|
||||
uint16_t crc = edac::CRC::createCRC9(crcBuffer, 87U);
|
||||
buffer[0U] = buffer[0U] + ((crc >> 8) & 0x01U); // CRC-9 Check Sum (b8)
|
||||
buffer[1U] = (crc & 0xFFU); // CRC-9 Check Sum (b0 - b7)
|
||||
|
||||
::memcpy(buffer, m_data, DMR_PDU_UNCONFIRMED_LENGTH_BYTES);
|
||||
|
||||
#if DEBUG_DMR_PDU_DATA
|
||||
Utils::dump(1U, "DMR, DataBlock::encode(), Unconfirmed PDU Data Block", buffer, DMR_PDU_UNCONFIRMED_LENGTH_BYTES);
|
||||
#endif
|
||||
|
||||
m_bptc.encode(buffer, data);
|
||||
}
|
||||
else {
|
||||
LogError(LOG_DMR, "DataBlock::encode(), cowardly refusing to encode confirmed full-rate (rate 1) data");
|
||||
return;
|
||||
}
|
||||
}
|
||||
else if (m_DPF == DPF::UNCONFIRMED_DATA || m_DPF == DPF::RESPONSE || m_DPF == DPF::DEFINED_RAW ||
|
||||
m_DPF == DPF::DEFINED_SHORT || m_DPF == DPF::UDT) {
|
||||
if (m_dataType == DataType::RATE_34_DATA) {
|
||||
uint8_t buffer[DMR_PDU_CONFIRMED_LENGTH_BYTES];
|
||||
::memset(buffer, 0x00U, DMR_PDU_CONFIRMED_LENGTH_BYTES);
|
||||
|
||||
::memcpy(buffer, m_data, DMR_PDU_CONFIRMED_LENGTH_BYTES);
|
||||
|
||||
#if DEBUG_DMR_PDU_DATA
|
||||
Utils::dump(1U, "DMR, DataBlock::encode(), Unconfirmed PDU Data Block", buffer, DMR_PDU_CONFIRMED_LENGTH_BYTES);
|
||||
#endif
|
||||
|
||||
m_trellis.encode34(buffer, data, true);
|
||||
}
|
||||
else if (m_dataType == DataType::RATE_12_DATA) {
|
||||
uint8_t buffer[DMR_PDU_UNCONFIRMED_LENGTH_BYTES];
|
||||
::memset(buffer, 0x00U, DMR_PDU_UNCONFIRMED_LENGTH_BYTES);
|
||||
|
||||
::memcpy(buffer, m_data, DMR_PDU_UNCONFIRMED_LENGTH_BYTES);
|
||||
|
||||
#if DEBUG_DMR_PDU_DATA
|
||||
Utils::dump(1U, "DMR, DataBlock::encode(), Unconfirmed PDU Data Block", buffer, DMR_PDU_UNCONFIRMED_LENGTH_BYTES);
|
||||
#endif
|
||||
|
||||
m_bptc.encode(buffer, data);
|
||||
}
|
||||
else {
|
||||
uint8_t buffer[DMR_PDU_UNCODED_LENGTH_BYTES];
|
||||
::memset(buffer, 0x00U, DMR_PDU_UNCODED_LENGTH_BYTES);
|
||||
|
||||
::memcpy(buffer, m_data, DMR_PDU_UNCODED_LENGTH_BYTES);
|
||||
|
||||
::memcpy(data, buffer, DMR_PDU_UNCODED_LENGTH_BYTES);
|
||||
}
|
||||
}
|
||||
else {
|
||||
LogError(LOG_P25, "unknown DPF value in PDU, dpf = $%02X", m_DPF);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
/* Sets the data type. */
|
||||
|
||||
void DataBlock::setDataType(const DataType::E& dataType)
|
||||
{
|
||||
m_dataType = dataType;
|
||||
}
|
||||
|
||||
/* Gets the data type. */
|
||||
|
||||
DataType::E DataBlock::getDataType() const
|
||||
{
|
||||
return m_dataType;
|
||||
}
|
||||
|
||||
/* Sets the data format. */
|
||||
|
||||
void DataBlock::setFormat(const DPF::E fmt)
|
||||
{
|
||||
m_DPF = fmt;
|
||||
}
|
||||
|
||||
/* Sets the data format from the data header. */
|
||||
|
||||
void DataBlock::setFormat(const DataHeader& header)
|
||||
{
|
||||
m_DPF = header.getDPF();
|
||||
}
|
||||
|
||||
/* Gets the data format. */
|
||||
|
||||
DPF::E DataBlock::getFormat() const
|
||||
{
|
||||
return m_DPF;
|
||||
}
|
||||
|
||||
/* Sets the raw data stored in the data block. */
|
||||
|
||||
void DataBlock::setData(const uint8_t* buffer)
|
||||
{
|
||||
assert(buffer != nullptr);
|
||||
assert(m_data != nullptr);
|
||||
|
||||
if (m_dataType == DataType::RATE_34_DATA)
|
||||
::memcpy(m_data, buffer, DMR_PDU_CONFIRMED_DATA_LENGTH_BYTES);
|
||||
else if (m_dataType == DataType::RATE_12_DATA)
|
||||
::memcpy(m_data, buffer, DMR_PDU_UNCONFIRMED_LENGTH_BYTES);
|
||||
else if (m_dataType == DataType::RATE_1_DATA)
|
||||
::memcpy(m_data, buffer, DMR_PDU_UNCODED_LENGTH_BYTES);
|
||||
else
|
||||
LogError(LOG_DMR, "unknown dataType value in PDU, dataType = $%02X", m_dataType);
|
||||
}
|
||||
|
||||
/* Gets the raw data stored in the data block. */
|
||||
|
||||
uint32_t DataBlock::getData(uint8_t* buffer) const
|
||||
{
|
||||
assert(buffer != nullptr);
|
||||
assert(m_data != nullptr);
|
||||
|
||||
if (m_dataType == DataType::RATE_34_DATA) {
|
||||
::memcpy(buffer, m_data, DMR_PDU_CONFIRMED_DATA_LENGTH_BYTES);
|
||||
return DMR_PDU_CONFIRMED_DATA_LENGTH_BYTES;
|
||||
} else if (m_dataType == DataType::RATE_12_DATA) {
|
||||
::memcpy(buffer, m_data, DMR_PDU_UNCONFIRMED_LENGTH_BYTES);
|
||||
return DMR_PDU_UNCONFIRMED_LENGTH_BYTES;
|
||||
} else if (m_dataType == DataType::RATE_1_DATA) {
|
||||
::memcpy(buffer, m_data, DMR_PDU_UNCODED_LENGTH_BYTES);
|
||||
return DMR_PDU_UNCODED_LENGTH_BYTES;
|
||||
} else {
|
||||
LogError(LOG_DMR, "unknown dataType value in PDU, dataType = $%02X", m_dataType);
|
||||
return 0U;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,124 @@
|
||||
// SPDX-License-Identifier: GPL-2.0-only
|
||||
/*
|
||||
* Digital Voice Modem - Common Library
|
||||
* GPLv2 Open Source. Use is subject to license terms.
|
||||
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
|
||||
*
|
||||
* Copyright (C) 2024 Bryan Biedenkapp, N2PLL
|
||||
*
|
||||
*/
|
||||
/**
|
||||
* @file DataBlock.h
|
||||
* @ingroup dmr_pdu
|
||||
* @file DataBlock.cpp
|
||||
* @ingroup dmr_pdu
|
||||
*/
|
||||
#if !defined(__DMR_DATA__DATA_BLOCK_H__)
|
||||
#define __DMR_DATA__DATA_BLOCK_H__
|
||||
|
||||
#include "common/Defines.h"
|
||||
#include "common/dmr/DMRDefines.h"
|
||||
#include "common/dmr/data/DataHeader.h"
|
||||
#include "common/edac/BPTC19696.h"
|
||||
#include "common/edac/Trellis.h"
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace dmr
|
||||
{
|
||||
namespace data
|
||||
{
|
||||
// ---------------------------------------------------------------------------
|
||||
// Class Declaration
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
/**
|
||||
* @brief Represents a data block for PDU DMR packets.
|
||||
* @ingroup dmr_pdu
|
||||
*/
|
||||
class HOST_SW_API DataBlock {
|
||||
public:
|
||||
/**
|
||||
* @brief Initializes a new instance of the DataBlock class.
|
||||
*/
|
||||
DataBlock();
|
||||
/**
|
||||
* @brief Finalizes a instance of the DataBlock class.
|
||||
*/
|
||||
~DataBlock();
|
||||
|
||||
/**
|
||||
* @brief Decodes DMR PDU data block.
|
||||
* @param[in] data Buffer containing a PDU data block to decode.
|
||||
* @param header DMR PDU data header.
|
||||
* @returns bool True, if PDU data block decoded, otherwise false.
|
||||
*/
|
||||
bool decode(const uint8_t* data, const DataHeader& header);
|
||||
/**
|
||||
* @brief Encodes a DMR PDU data block.
|
||||
* @param[out] data Buffer to encode a PDU data block.
|
||||
*/
|
||||
void encode(uint8_t* data);
|
||||
|
||||
/**
|
||||
* @brief Sets the data format from the data header.
|
||||
* @param dataType DMR data type.
|
||||
*/
|
||||
void setDataType(const defines::DataType::E& dataType);
|
||||
/**
|
||||
* @brief Gets the data type.
|
||||
* @returns defines::DataType::E DMR data type.
|
||||
*/
|
||||
defines::DataType::E getDataType() const;
|
||||
/**
|
||||
* @brief Sets the data format.
|
||||
* @param fmt PDU data foramt.
|
||||
*/
|
||||
void setFormat(const defines::DPF::E fmt);
|
||||
/**
|
||||
* @brief Sets the data format from the data header.
|
||||
* @param header P25 PDU data header.
|
||||
*/
|
||||
void setFormat(const DataHeader& header);
|
||||
/**
|
||||
* @brief Gets the data format.
|
||||
* @returns defines::DPF::E PDU data format.
|
||||
*/
|
||||
defines::DPF::E getFormat() const;
|
||||
|
||||
/**
|
||||
* @brief Sets the raw data stored in the data block.
|
||||
* @param[in] buffer Buffer containing bytes to store in data block.
|
||||
*/
|
||||
void setData(const uint8_t* buffer);
|
||||
/**
|
||||
* @brief Gets the raw data stored in the data block.
|
||||
* @param[out] buffer Buffer to copy bytes in data block to.
|
||||
* @returns uint32_t Number of bytes copied.
|
||||
*/
|
||||
uint32_t getData(uint8_t* buffer) const;
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief Sets the data block serial number.
|
||||
*/
|
||||
__PROPERTY(uint8_t, serialNo, SerialNo);
|
||||
|
||||
/**
|
||||
* @brief Flag indicating this is the last block in a sequence of block.
|
||||
*/
|
||||
__PROPERTY(bool, lastBlock, LastBlock);
|
||||
|
||||
private:
|
||||
edac::BPTC19696 m_bptc;
|
||||
edac::Trellis m_trellis;
|
||||
|
||||
defines::DataType::E m_dataType;
|
||||
defines::DPF::E m_DPF;
|
||||
|
||||
uint8_t* m_data;
|
||||
};
|
||||
} // namespace data
|
||||
} // namespace dmr
|
||||
|
||||
#endif // __P25_DATA__DATA_BLOCK_H__
|
||||
@ -0,0 +1,327 @@
|
||||
// SPDX-License-Identifier: GPL-2.0-only
|
||||
/*
|
||||
* Digital Voice Modem - Converged FNE Software
|
||||
* GPLv2 Open Source. Use is subject to license terms.
|
||||
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
|
||||
*
|
||||
* Copyright (C) 2024 Bryan Biedenkapp, N2PLL
|
||||
*
|
||||
*/
|
||||
#include "fne/Defines.h"
|
||||
#include "common/dmr/data/DataBlock.h"
|
||||
#include "common/dmr/SlotType.h"
|
||||
#include "common/dmr/Sync.h"
|
||||
#include "common/edac/CRC.h"
|
||||
#include "common/Clock.h"
|
||||
#include "common/Log.h"
|
||||
#include "common/Thread.h"
|
||||
#include "common/Utils.h"
|
||||
#include "network/FNENetwork.h"
|
||||
#include "network/callhandler/packetdata/DMRPacketData.h"
|
||||
#include "HostFNE.h"
|
||||
|
||||
using namespace system_clock;
|
||||
using namespace network;
|
||||
using namespace network::callhandler;
|
||||
using namespace network::callhandler::packetdata;
|
||||
using namespace dmr;
|
||||
using namespace dmr::defines;
|
||||
|
||||
#include <cassert>
|
||||
#include <chrono>
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Constants
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
const uint8_t DATA_CALL_COLL_TIMEOUT = 60U;
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Public Class Members
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
/* Initializes a new instance of the DMRPacketData class. */
|
||||
|
||||
DMRPacketData::DMRPacketData(FNENetwork* network, TagDMRData* tag, bool debug) :
|
||||
m_network(network),
|
||||
m_tag(tag),
|
||||
m_status(),
|
||||
m_debug(debug)
|
||||
{
|
||||
assert(network != nullptr);
|
||||
assert(tag != nullptr);
|
||||
}
|
||||
|
||||
/* Finalizes a instance of the DMRPacketData class. */
|
||||
|
||||
DMRPacketData::~DMRPacketData() = default;
|
||||
|
||||
/* Process a data frame from the network. */
|
||||
|
||||
bool DMRPacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t peerId, uint16_t pktSeq, uint32_t streamId, bool external)
|
||||
{
|
||||
hrc::hrc_t pktTime = hrc::now();
|
||||
|
||||
uint8_t seqNo = data[4U];
|
||||
|
||||
uint32_t srcId = __GET_UINT16(data, 5U);
|
||||
uint32_t dstId = __GET_UINT16(data, 8U);
|
||||
|
||||
FLCO::E flco = (data[15U] & 0x40U) == 0x40U ? FLCO::PRIVATE : FLCO::GROUP;
|
||||
|
||||
uint32_t slotNo = (data[15U] & 0x80U) == 0x80U ? 2U : 1U;
|
||||
|
||||
DataType::E dataType = (DataType::E)(data[15U] & 0x0FU);
|
||||
|
||||
data::NetData dmrData;
|
||||
dmrData.setSeqNo(seqNo);
|
||||
dmrData.setSlotNo(slotNo);
|
||||
dmrData.setSrcId(srcId);
|
||||
dmrData.setDstId(dstId);
|
||||
dmrData.setFLCO(flco);
|
||||
|
||||
bool dataSync = (data[15U] & 0x20U) == 0x20U;
|
||||
bool voiceSync = (data[15U] & 0x10U) == 0x10U;
|
||||
|
||||
if (dataSync) {
|
||||
dmrData.setData(data + 20U);
|
||||
dmrData.setDataType(dataType);
|
||||
dmrData.setN(0U);
|
||||
}
|
||||
|
||||
uint8_t frame[DMR_FRAME_LENGTH_BYTES];
|
||||
dmrData.getData(frame);
|
||||
|
||||
// is the stream valid?
|
||||
if (m_tag->validate(peerId, dmrData, streamId)) {
|
||||
// is this peer ignored?
|
||||
if (!m_tag->isPeerPermitted(peerId, dmrData, streamId)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
auto it = std::find_if(m_status.begin(), m_status.end(), [&](StatusMapPair x) { return x.second->peerId == peerId; });
|
||||
if (it != m_status.end()) {
|
||||
RxStatus* status = m_status[peerId];
|
||||
if (streamId != status->streamId) {
|
||||
LogWarning(LOG_NET, "DMR, Data Call Collision, peer = %u, streamId = %u, rxPeer = %u, rxLlId = %u, rxSlotNo = %u, rxStreamId = %u, external = %u",
|
||||
peerId, streamId, status->peerId, status->srcId, status->slotNo, status->streamId, external);
|
||||
|
||||
uint64_t duration = hrc::diff(pktTime, status->callStartTime);
|
||||
|
||||
if ((duration / 1000) > DATA_CALL_COLL_TIMEOUT) {
|
||||
LogWarning(LOG_NET, "DMR, force clearing stuck data call, timeout, peer = %u, streamId = %u, rxPeer = %u, rxLlId = %u, rxStreamId = %u, external = %u",
|
||||
peerId, streamId, status->peerId, status->srcId, status->slotNo, status->streamId, external);
|
||||
|
||||
delete status;
|
||||
m_status.erase(peerId);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
if (dataSync && (dataType == DataType::DATA_HEADER)) {
|
||||
// this is a new call stream
|
||||
RxStatus* status = new RxStatus();
|
||||
status->callStartTime = pktTime;
|
||||
status->srcId = srcId;
|
||||
status->dstId = dstId;
|
||||
status->slotNo = slotNo;
|
||||
status->streamId = streamId;
|
||||
status->peerId = peerId;
|
||||
|
||||
bool ret = status->header.decode(frame);
|
||||
if (!ret) {
|
||||
LogError(LOG_NET, "DMR Slot %u, DataType::DATA_HEADER, unable to decode the network data header", status->slotNo);
|
||||
Utils::dump(1U, "Unfixable PDU Data", frame, DMR_FRAME_LENGTH_BYTES);
|
||||
|
||||
delete status;
|
||||
m_status.erase(peerId);
|
||||
return false;
|
||||
}
|
||||
|
||||
status->frames = status->header.getBlocksToFollow();
|
||||
status->dataBlockCnt = 0U;
|
||||
|
||||
bool gi = status->header.getGI();
|
||||
uint32_t srcId = status->header.getSrcId();
|
||||
uint32_t dstId = status->header.getDstId();
|
||||
|
||||
LogMessage(LOG_NET, DMR_DT_DATA_HEADER ", peerId = %u, slot = %u, dpf = $%02X, ack = %u, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %u, packetLength = %u, seqNo = %u, dstId = %u, srcId = %u, group = %u",
|
||||
peerId, status->slotNo, status->header.getDPF(), status->header.getA(), status->header.getSAP(), status->header.getFullMesage(), status->header.getBlocksToFollow(), status->header.getPadLength(), status->header.getPacketLength(),
|
||||
status->header.getFSN(), dstId, srcId, gi);
|
||||
|
||||
// make sure we don't get a PDU with more blocks then we support
|
||||
if (status->header.getBlocksToFollow() >= MAX_PDU_COUNT) {
|
||||
LogError(LOG_NET, P25_PDU_STR ", too many PDU blocks to process, %u > %u", status->header.getBlocksToFollow(), MAX_PDU_COUNT);
|
||||
return false;
|
||||
}
|
||||
|
||||
m_status[peerId] = status;
|
||||
|
||||
LogMessage(LOG_NET, "DMR, Data Call Start, peer = %u, slot = %u, srcId = %u, dstId = %u, streamId = %u, external = %u", peerId, status->slotNo, status->srcId, status->dstId, streamId, external);
|
||||
dispatch(peerId, dmrData, data, len, seqNo, pktSeq, streamId);
|
||||
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
RxStatus* status = m_status[peerId];
|
||||
|
||||
// a PDU header only with no blocks to follow is usually a response header
|
||||
if (status->header.getBlocksToFollow() == 0U) {
|
||||
LogMessage(LOG_NET, "DMR, Data Call End, peer = %u, slot = %u, srcId = %u, dstId = %u, streamId = %u, external = %u",
|
||||
peerId, status->slotNo, status->srcId, status->dstId, streamId, external);
|
||||
|
||||
delete status;
|
||||
m_status.erase(peerId);
|
||||
return true;
|
||||
}
|
||||
|
||||
data::DataBlock dataBlock;
|
||||
dataBlock.setDataType(dataType);
|
||||
|
||||
bool ret = dataBlock.decode(frame, status->header);
|
||||
if (ret) {
|
||||
uint32_t len = dataBlock.getData(status->pduUserData + status->pduDataOffset);
|
||||
status->pduDataOffset += len;
|
||||
|
||||
status->frames--;
|
||||
if (status->frames == 0U)
|
||||
dataBlock.setLastBlock(true);
|
||||
|
||||
if (dataType == DataType::RATE_34_DATA) {
|
||||
LogMessage(LOG_NET, DMR_DT_RATE_34_DATA ", ISP, block %u, peer = %u, dataType = $%02X, dpf = $%02X", status->dataBlockCnt, peerId, dataBlock.getDataType(), dataBlock.getFormat());
|
||||
} else if (dataType == DataType::RATE_12_DATA) {
|
||||
LogMessage(LOG_NET, DMR_DT_RATE_12_DATA ", ISP, block %u, peer = %u, dataType = $%02X, dpf = $%02X", status->dataBlockCnt, peerId, dataBlock.getDataType(), dataBlock.getFormat());
|
||||
}
|
||||
else {
|
||||
LogMessage(LOG_NET, DMR_DT_RATE_1_DATA ", ISP, block %u, peer = %u, dataType = $%02X, dpf = $%02X", status->dataBlockCnt, peerId, dataBlock.getDataType(), dataBlock.getFormat());
|
||||
}
|
||||
|
||||
dispatch(peerId, dmrData, data, len, seqNo, pktSeq, streamId);
|
||||
status->dataBlockCnt++;
|
||||
}
|
||||
|
||||
// dispatch the PDU data
|
||||
if (status->dataBlockCnt > 0U && status->frames == 0U) {
|
||||
if (status->header.getBlocksToFollow() > 0U && status->frames == 0U) {
|
||||
bool crcRet = edac::CRC::checkCRC32(status->pduUserData, status->pduDataOffset);
|
||||
if (!crcRet) {
|
||||
LogWarning(LOG_NET, P25_PDU_STR ", failed CRC-32 check, blocks %u, len %u", status->header.getBlocksToFollow(), status->pduDataOffset);
|
||||
}
|
||||
|
||||
if (m_network->m_dumpDataPacket) {
|
||||
Utils::dump(1U, "PDU Packet", status->pduUserData, status->pduDataOffset);
|
||||
}
|
||||
}
|
||||
|
||||
uint64_t duration = hrc::diff(pktTime, status->callStartTime);
|
||||
bool gi = status->header.getGI();
|
||||
uint32_t srcId = status->header.getSrcId();
|
||||
uint32_t dstId = status->header.getDstId();
|
||||
LogMessage(LOG_NET, "P25, Data Call End, peer = %u, slot = %u, srcId = %u, dstId = %u, blocks = %u, duration = %u, streamId = %u, external = %u",
|
||||
peerId, srcId, dstId, status->header.getBlocksToFollow(), duration / 1000, streamId, external);
|
||||
|
||||
// report call event to InfluxDB
|
||||
if (m_network->m_enableInfluxDB) {
|
||||
influxdb::QueryBuilder()
|
||||
.meas("call_event")
|
||||
.tag("peerId", std::to_string(peerId))
|
||||
.tag("mode", "DMR")
|
||||
.tag("streamId", std::to_string(streamId))
|
||||
.tag("srcId", std::to_string(srcId))
|
||||
.tag("dstId", std::to_string(dstId))
|
||||
.field("duration", duration)
|
||||
.field("slot", slotNo)
|
||||
.timestamp(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count())
|
||||
.request(m_network->m_influxServer);
|
||||
}
|
||||
|
||||
delete status;
|
||||
m_status.erase(peerId);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private Class Members
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
/* Helper to dispatch PDU user data. */
|
||||
|
||||
void DMRPacketData::dispatch(uint32_t peerId, dmr::data::NetData& dmrData, const uint8_t* data, uint32_t len, uint8_t seqNo, uint16_t pktSeq, uint32_t streamId)
|
||||
{
|
||||
RxStatus* status = m_status[peerId];
|
||||
|
||||
bool gi = status->header.getGI();
|
||||
uint32_t srcId = status->header.getSrcId();
|
||||
uint32_t dstId = status->header.getDstId();
|
||||
|
||||
// repeat traffic to the connected peers
|
||||
if (m_network->m_peers.size() > 0U) {
|
||||
uint32_t i = 0U;
|
||||
for (auto peer : m_network->m_peers) {
|
||||
if (peerId != peer.first) {
|
||||
// is this peer ignored?
|
||||
if (!m_tag->isPeerPermitted(peer.first, dmrData, status->streamId)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// every 5 peers flush the queue
|
||||
if (i % 5U == 0U) {
|
||||
m_network->m_frameQueue->flushQueue();
|
||||
}
|
||||
|
||||
m_network->writePeer(peer.first, { NET_FUNC::PROTOCOL, NET_SUBFUNC::PROTOCOL_SUBFUNC_DMR }, data, len, pktSeq, streamId, true);
|
||||
if (m_network->m_debug) {
|
||||
LogDebug(LOG_NET, "DMR, srcPeer = %u, dstPeer = %u, seqNo = %u, srcId = %u, dstId = %u, slotNo = %u, len = %u, pktSeq = %u, stream = %u",
|
||||
peerId, peer.first, seqNo, srcId, dstId, status->slotNo, len, pktSeq, streamId);
|
||||
}
|
||||
|
||||
if (!m_network->m_callInProgress)
|
||||
m_network->m_callInProgress = true;
|
||||
i++;
|
||||
}
|
||||
}
|
||||
m_network->m_frameQueue->flushQueue();
|
||||
}
|
||||
|
||||
// repeat traffic to external peers
|
||||
if (m_network->m_host->m_peerNetworks.size() > 0U) {
|
||||
for (auto peer : m_network->m_host->m_peerNetworks) {
|
||||
uint32_t dstPeerId = peer.second->getPeerId();
|
||||
|
||||
// don't try to repeat traffic to the source peer...if this traffic
|
||||
// is coming from a external peer
|
||||
if (dstPeerId != peerId) {
|
||||
// is this peer ignored?
|
||||
if (!m_tag->isPeerPermitted(dstPeerId, dmrData, status->streamId, true)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// check if the source peer is blocked from sending to this peer
|
||||
if (peer.second->checkBlockedPeer(peerId)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// skip peer if it isn't enabled
|
||||
if (!peer.second->isEnabled()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
peer.second->writeMaster({ NET_FUNC::PROTOCOL, NET_SUBFUNC::PROTOCOL_SUBFUNC_DMR }, data, len, pktSeq, streamId);
|
||||
if (m_network->m_debug) {
|
||||
LogDebug(LOG_NET, "DMR, srcPeer = %u, dstPeer = %u, seqNo = %u, srcId = %u, dstId = %u, slotNo = %u, len = %u, pktSeq = %u, stream = %u",
|
||||
peerId, dstPeerId, seqNo, srcId, dstId, status->slotNo, len, pktSeq, streamId);
|
||||
}
|
||||
|
||||
if (!m_network->m_callInProgress)
|
||||
m_network->m_callInProgress = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,138 @@
|
||||
// SPDX-License-Identifier: GPL-2.0-only
|
||||
/*
|
||||
* Digital Voice Modem - Converged FNE Software
|
||||
* GPLv2 Open Source. Use is subject to license terms.
|
||||
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
|
||||
*
|
||||
* Copyright (C) 2024 Bryan Biedenkapp, N2PLL
|
||||
*
|
||||
*/
|
||||
/**
|
||||
* @file DMRPacketData.h
|
||||
* @ingroup fne_callhandler
|
||||
* @file DMRPacketData.cpp
|
||||
* @ingroup fne_callhandler
|
||||
*/
|
||||
#if !defined(__PACKETDATA__DMR_PACKET_DATA_H__)
|
||||
#define __PACKETDATA__DMR_PACKET_DATA_H__
|
||||
|
||||
#include "fne/Defines.h"
|
||||
#include "common/Clock.h"
|
||||
#include "common/dmr/DMRDefines.h"
|
||||
#include "common/dmr/data/DataHeader.h"
|
||||
#include "network/FNENetwork.h"
|
||||
#include "network/PeerNetwork.h"
|
||||
#include "network/callhandler/TagDMRData.h"
|
||||
|
||||
#include <deque>
|
||||
|
||||
namespace network
|
||||
{
|
||||
namespace callhandler
|
||||
{
|
||||
namespace packetdata
|
||||
{
|
||||
// ---------------------------------------------------------------------------
|
||||
// Class Declaration
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
/**
|
||||
* @brief Implements the DMR packet data handler.
|
||||
* @ingroup fne_callhandler
|
||||
*/
|
||||
class HOST_SW_API DMRPacketData {
|
||||
public:
|
||||
/**
|
||||
* @brief Initializes a new instance of the DMRPacketData class.
|
||||
* @param network Instance of the FNENetwork class.
|
||||
* @param tag Instance of the TagDMRData class.
|
||||
* @param debug Flag indicating whether network debug is enabled.
|
||||
*/
|
||||
DMRPacketData(FNENetwork* network, TagDMRData* tag, bool debug);
|
||||
/**
|
||||
* @brief Finalizes a instance of the P25PacketData class.
|
||||
*/
|
||||
~DMRPacketData();
|
||||
|
||||
/**
|
||||
* @brief Process a data frame from the network.
|
||||
* @param data Network data buffer.
|
||||
* @param len Length of data.
|
||||
* @param peerId Peer ID.
|
||||
* @param pktSeq RTP packet sequence.
|
||||
* @param streamId Stream ID.
|
||||
* @param external Flag indicating traffic is from an external peer.
|
||||
* @returns bool True, if frame is processed, otherwise false.
|
||||
*/
|
||||
bool processFrame(const uint8_t* data, uint32_t len, uint32_t peerId, uint16_t pktSeq, uint32_t streamId, bool external = false);
|
||||
|
||||
private:
|
||||
FNENetwork* m_network;
|
||||
TagDMRData *m_tag;
|
||||
|
||||
/**
|
||||
* @brief Represents the receive status of a call.
|
||||
*/
|
||||
class RxStatus {
|
||||
public:
|
||||
system_clock::hrc::hrc_t callStartTime;
|
||||
uint32_t srcId;
|
||||
uint32_t dstId;
|
||||
uint8_t slotNo;
|
||||
uint32_t streamId;
|
||||
uint32_t peerId;
|
||||
|
||||
dmr::data::DataHeader header;
|
||||
uint8_t dataBlockCnt;
|
||||
uint8_t frames;
|
||||
|
||||
uint8_t* pduUserData;
|
||||
uint32_t pduDataOffset;
|
||||
|
||||
/**
|
||||
* @brief Initializes a new instance of the RxStatus class
|
||||
*/
|
||||
RxStatus() :
|
||||
srcId(0U),
|
||||
dstId(0U),
|
||||
slotNo(0U),
|
||||
streamId(0U),
|
||||
peerId(0U),
|
||||
header(),
|
||||
dataBlockCnt(0U),
|
||||
pduUserData(nullptr),
|
||||
pduDataOffset(0U)
|
||||
{
|
||||
pduUserData = new uint8_t[DMRDEF::MAX_PDU_COUNT * DMRDEF::DMR_PDU_UNCODED_LENGTH_BYTES + 2U];
|
||||
::memset(pduUserData, 0x00U, DMRDEF::MAX_PDU_COUNT * DMRDEF::DMR_PDU_UNCODED_LENGTH_BYTES + 2U);
|
||||
}
|
||||
/**
|
||||
* @brief Finalizes a instance of the RxStatus class
|
||||
*/
|
||||
~RxStatus()
|
||||
{
|
||||
delete[] pduUserData;
|
||||
}
|
||||
};
|
||||
typedef std::pair<const uint32_t, RxStatus*> StatusMapPair;
|
||||
std::unordered_map<uint32_t, RxStatus*> m_status;
|
||||
|
||||
bool m_debug;
|
||||
|
||||
/**
|
||||
* @brief Helper to dispatch PDU user data.
|
||||
* @param peerId Peer ID.
|
||||
* @param dmrData Instance of data::NetData DMR data container class.
|
||||
* @param data Network data buffer.
|
||||
* @param len Length of data.
|
||||
* @param seqNo
|
||||
* @param pktSeq RTP packet sequence.
|
||||
* @param streamId Stream ID.
|
||||
*/
|
||||
void dispatch(uint32_t peerId, dmr::data::NetData& dmrData, const uint8_t* data, uint32_t len, uint8_t seqNo, uint16_t pktSeq, uint32_t streamId);
|
||||
};
|
||||
} // namespace packetdata
|
||||
} // namespace callhandler
|
||||
} // namespace network
|
||||
|
||||
#endif // __PACKETDATA__DMR_PACKET_DATA_H__
|
||||
Loading…
Reference in new issue