From f68532ed330c3bef939e3a2124d66187b3bcebe6 Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Fri, 23 Aug 2024 00:44:47 -0400 Subject: [PATCH] [EXPERIMENTAL] add possible support to convertToAir V.24 PDU frames; --- src/common/p25/dfsi/DFSIDefines.h | 1 + src/common/p25/dfsi/frames/Frames.h | 1 + src/common/p25/dfsi/frames/MotPDUFrame.cpp | 108 +++++++++++++++++++++ src/common/p25/dfsi/frames/MotPDUFrame.h | 93 ++++++++++++++++++ src/host/modem/ModemV24.cpp | 75 +++++++++++++- 5 files changed, 276 insertions(+), 2 deletions(-) create mode 100644 src/common/p25/dfsi/frames/MotPDUFrame.cpp create mode 100644 src/common/p25/dfsi/frames/MotPDUFrame.h diff --git a/src/common/p25/dfsi/DFSIDefines.h b/src/common/p25/dfsi/DFSIDefines.h index 4a2a51f0..bf78aff6 100644 --- a/src/common/p25/dfsi/DFSIDefines.h +++ b/src/common/p25/dfsi/DFSIDefines.h @@ -114,6 +114,7 @@ namespace p25 LDU2_VOICE17 = 0x72U, // IMBE LDU2 - Voice 17 + Encryption Sync LDU2_VOICE18 = 0x73U, // IMBE LDU2 - Voice 18 + Low Speed Data + PDU = 0x87U, // PDU TSBK = 0xA1U // TSBK }; } diff --git a/src/common/p25/dfsi/frames/Frames.h b/src/common/p25/dfsi/frames/Frames.h index 0732a1d4..b529d58d 100644 --- a/src/common/p25/dfsi/frames/Frames.h +++ b/src/common/p25/dfsi/frames/Frames.h @@ -26,6 +26,7 @@ #include "common/p25/dfsi/frames/MotVoiceHeader1.h" #include "common/p25/dfsi/frames/MotVoiceHeader2.h" #include "common/p25/dfsi/frames/MotTSBKFrame.h" +#include "common/p25/dfsi/frames/MotPDUFrame.h" // FSC #include "common/p25/dfsi/frames/fsc/FSCMessage.h" diff --git a/src/common/p25/dfsi/frames/MotPDUFrame.cpp b/src/common/p25/dfsi/frames/MotPDUFrame.cpp new file mode 100644 index 00000000..8ed36bca --- /dev/null +++ b/src/common/p25/dfsi/frames/MotPDUFrame.cpp @@ -0,0 +1,108 @@ +// 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 + * + */ +#include "common/p25/P25Defines.h" +#include "common/p25/dfsi/frames/MotPDUFrame.h" +#include "common/p25/dfsi/DFSIDefines.h" +#include "common/Utils.h" +#include "common/Log.h" + +#include +#include + +using namespace p25; +using namespace p25::defines; +using namespace p25::dfsi; +using namespace p25::dfsi::defines; +using namespace p25::dfsi::frames; + +// --------------------------------------------------------------------------- +// Public Class Members +// --------------------------------------------------------------------------- + +/* Initializes a instance of the MotPDUFrame class. */ + +MotPDUFrame::MotPDUFrame() : + startOfStream(nullptr), + pduHeaderData(nullptr) +{ + pduHeaderData = new uint8_t[P25_PDU_HEADER_LENGTH_BYTES]; + ::memset(pduHeaderData, 0x00U, P25_PDU_HEADER_LENGTH_BYTES); + + startOfStream = new MotStartOfStream(); +} + +/* Initializes a instance of the MotPDUFrame class. */ + +MotPDUFrame::MotPDUFrame(uint8_t* data) : + startOfStream(nullptr), + pduHeaderData(nullptr) +{ + pduHeaderData = new uint8_t[P25_PDU_HEADER_LENGTH_BYTES]; + ::memset(pduHeaderData, 0x00U, P25_PDU_HEADER_LENGTH_BYTES); + + decode(data); +} + +/* Finalizes a instance of the MotPDUFrame class. */ + +MotPDUFrame::~MotPDUFrame() +{ + if (startOfStream != nullptr) + delete startOfStream; + if (pduHeaderData != nullptr) + delete[] pduHeaderData; +} + +/* Decode a TSBK frame. */ + +bool MotPDUFrame::decode(const uint8_t* data) +{ + assert(data != nullptr); + + // create a new start of stream + if (startOfStream != nullptr) + delete startOfStream; + startOfStream = new MotStartOfStream(); + + // create a buffer to decode the start record skipping the 10th byte (adjMM) + uint8_t startBuffer[MotStartOfStream::LENGTH]; + ::memset(startBuffer, 0x00U, MotStartOfStream::LENGTH); + ::memcpy(startBuffer + 1U, data, 4U); + + // decode start of stream + startOfStream->decode(startBuffer); + + ::memcpy(pduHeaderData, data + 9U, P25_PDU_HEADER_LENGTH_BYTES); + + return true; +} + +/* Encode a TSBK frame. */ + +void MotPDUFrame::encode(uint8_t* data) +{ + assert(data != nullptr); + assert(startOfStream != nullptr); + + // encode start of stream - scope is intentional + { + uint8_t buffer[MotStartOfStream::LENGTH]; + startOfStream->encode(buffer); + + // copy to data array (skipping first and last bytes) + ::memcpy(data + 1U, buffer + 1U, 4U); + } + + // encode TSBK - scope is intentional + { + data[0U] = DFSIFrameType::PDU; + ::memcpy(data + 9U, pduHeaderData, P25_PDU_HEADER_LENGTH_BYTES); + } +} diff --git a/src/common/p25/dfsi/frames/MotPDUFrame.h b/src/common/p25/dfsi/frames/MotPDUFrame.h new file mode 100644 index 00000000..1cb14868 --- /dev/null +++ b/src/common/p25/dfsi/frames/MotPDUFrame.h @@ -0,0 +1,93 @@ +// 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 MotPDUFrame.h + * @ingroup dfsi_frames + * @file MotPDUFrame.cpp + * @ingroup dfsi_frames + */ +#if !defined(__MOT_PDU_FRAME_H__) +#define __MOT_PDU_FRAME_H__ + +#include "Defines.h" +#include "common/Defines.h" +#include "common/Log.h" +#include "common/Utils.h" +#include "common/p25/dfsi/frames/FrameDefines.h" +#include "common/p25/dfsi/frames/MotStartOfStream.h" +#include "common/p25/dfsi/frames/MotFullRateVoice.h" + +namespace p25 +{ + namespace dfsi + { + namespace frames + { + // --------------------------------------------------------------------------- + // Class Declaration + // --------------------------------------------------------------------------- + + /** + * @brief Implements a P25 Motorola PDU frame. + * \code{.unparsed} + * Byte 0 1 2 3 + * Bit 7 6 5 4 3 2 1 0 7 6 5 4 3 2 1 0 7 6 5 4 3 2 1 0 7 6 5 4 3 2 1 0 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Encoded Motorola Start of Stream | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Reserved ? | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | PDU Header | + * + + + * | | + * + + + * | | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * \endcode + * @ingroup dfsi_frames + */ + class HOST_SW_API MotPDUFrame { + public: + static const uint8_t LENGTH = 20; + + /** + * @brief Initializes a copy instance of the MotPDUFrame class. + */ + MotPDUFrame(); + /** + * @brief Initializes a copy instance of the MotPDUFrame class. + * @param data Buffer to containing MotPDUFrame to decode. + */ + MotPDUFrame(uint8_t* data); + /** + * @brief Finalizes a instance of the MotPDUFrame class. + */ + ~MotPDUFrame(); + + /** + * @brief Decode a PDU frame. + * @param[in] data Buffer to containing MotPDUFrame to decode. + */ + bool decode(const uint8_t* data); + /** + * @brief Encode a TSBK frame. + * @param[out] data Buffer to encode a MotPDUFrame. + */ + void encode(uint8_t* data); + + public: + MotStartOfStream* startOfStream; // ?? - this should probably be private with getters/setters + uint8_t* pduHeaderData; // ?? - this should probably be private with getters/setters + }; + } // namespace frames + } // namespace dfsi +} // namespace p25 + +#endif // __MOT_PDU_FRAME_H__ \ No newline at end of file diff --git a/src/host/modem/ModemV24.cpp b/src/host/modem/ModemV24.cpp index cc080e35..202dddcf 100644 --- a/src/host/modem/ModemV24.cpp +++ b/src/host/modem/ModemV24.cpp @@ -10,6 +10,8 @@ */ #include "Defines.h" #include "common/p25/P25Defines.h" +#include "common/p25/data/DataHeader.h" +#include "common/p25/data/DataBlock.h" #include "common/p25/data/LowSpeedData.h" #include "common/p25/dfsi/LC.h" #include "common/p25/dfsi/frames/Frames.h" @@ -516,8 +518,11 @@ void ModemV24::storeConvertedRx(const uint8_t* buffer, uint32_t length) { // store converted frame into the Rx modem queue uint8_t storedLen[2U]; - storedLen[0U] = 0x00U; - storedLen[1U] = length; + if (length > 255U) + storedLen[0U] = (length >> 8U) & 0xFFU; + else + storedLen[0U] = 0x00U; + storedLen[1U] = length & 0xFFU; m_rxP25Queue.addData(storedLen, 2U); //Utils::dump("Storing converted RX data", buffer, length); @@ -703,6 +708,72 @@ void ModemV24::convertToAir(const uint8_t *data, uint32_t length) } break; + case DFSIFrameType::PDU: + { + // bryanb: this is gonna be a complete clusterfuck... + MotPDUFrame pf = MotPDUFrame(dfsiData); + data::DataHeader dataHeader = data::DataHeader(); + if (!dataHeader.decode(pf.pduHeaderData, true)) { + LogError(LOG_MODEM, "V.24/DFSI traffic failed to decode PDU FEC"); + } else { + uint32_t bitLength = ((dataHeader.getBlocksToFollow() + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS; + uint32_t offset = P25_PREAMBLE_LENGTH_BITS; + + UInt8Array __data = std::make_unique((bitLength / 8U) + 1U); + uint8_t* data = __data.get(); + + ::memset(data, 0x00U, bitLength / 8U); + uint8_t block[P25_PDU_FEC_LENGTH_BYTES]; + ::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES); + + uint32_t blocksToFollow = dataHeader.getBlocksToFollow(); + + if (blocksToFollow > 0U) { + uint32_t dataOffset = MotPDUFrame::LENGTH; + + // generate the PDU data + for (uint32_t i = 0U; i < blocksToFollow; i++) { + data::DataBlock dataBlock = data::DataBlock(); + dataBlock.setFormat(dataHeader); + dataBlock.setSerialNo(i); + dataBlock.setData(dfsiData + dataOffset); + + ::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES); + dataBlock.encode(block); + Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS); + + offset += P25_PDU_FEC_LENGTH_BITS; + dataOffset += (dataHeader.getFormat() == PDUFormatType::CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES; + } + } + + uint8_t buffer[P25_PDU_FRAME_LENGTH_BYTES + 2U]; + ::memset(buffer, 0x00U, P25_PDU_FRAME_LENGTH_BYTES + 2U); + + // Add the data + uint32_t newBitLength = P25Utils::encode(data, buffer + 2U, bitLength); + uint32_t newByteLength = newBitLength / 8U; + if ((newBitLength % 8U) > 0U) + newByteLength++; + + // Regenerate Sync + Sync::addP25Sync(buffer + 2U); + + // Regenerate NID + m_nid->encode(buffer + 2U, DUID::PDU); + + // Add status bits + P25Utils::addStatusBits(buffer + 2U, newBitLength, false); + P25Utils::addIdleStatusBits(buffer + 2U, newBitLength); + + // Set first busy bits to 1,1 + P25Utils::setStatusBits(buffer + 2U, P25_SS0_START, true, true); + + storeConvertedRx(buffer, P25_PDU_FRAME_LENGTH_BYTES + 2U); + } + } + break; + case DFSIFrameType::TSBK: { MotTSBKFrame tf = MotTSBKFrame(dfsiData);