From 30513bb1d2da9504a90260c4981d5444d40dea7f Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Sat, 1 May 2021 22:32:17 +0000 Subject: [PATCH] add support for properly encoding/decoding the PI header LC; --- DVMHost.vcxproj | 2 + DVMHost.vcxproj.filters | 6 ++ Makefile | 1 + Makefile.arm | 1 + Makefile.rpi-arm | 1 + Utils.cpp | 2 - dmr/DMRDefines.h | 2 + dmr/Slot.cpp | 2 + dmr/Slot.h | 4 +- dmr/VoicePacket.cpp | 23 ++++- dmr/VoicePacket.h | 3 +- dmr/lc/FullLC.cpp | 44 ++++++++++ dmr/lc/FullLC.h | 7 ++ dmr/lc/LC.cpp | 9 +- dmr/lc/LC.h | 3 +- dmr/lc/PrivacyLC.cpp | 186 ++++++++++++++++++++++++++++++++++++++++ dmr/lc/PrivacyLC.h | 86 +++++++++++++++++++ 17 files changed, 371 insertions(+), 11 deletions(-) create mode 100644 dmr/lc/PrivacyLC.cpp create mode 100644 dmr/lc/PrivacyLC.h diff --git a/DVMHost.vcxproj b/DVMHost.vcxproj index 106a03d4..380e603c 100644 --- a/DVMHost.vcxproj +++ b/DVMHost.vcxproj @@ -163,6 +163,7 @@ + @@ -241,6 +242,7 @@ + diff --git a/DVMHost.vcxproj.filters b/DVMHost.vcxproj.filters index f7e151e2..02838de9 100644 --- a/DVMHost.vcxproj.filters +++ b/DVMHost.vcxproj.filters @@ -350,6 +350,9 @@ Header Files\modem\port + + Header Files\dmr\lc + @@ -562,6 +565,9 @@ Source Files\modem\port + + Source Files\dmr\lc + diff --git a/Makefile b/Makefile index 15ec92b5..a46e2bea 100644 --- a/Makefile +++ b/Makefile @@ -25,6 +25,7 @@ OBJECTS = \ dmr/lc/CSBK.o \ dmr/lc/FullLC.o \ dmr/lc/LC.o \ + dmr/lc/PrivacyLC.o \ dmr/lc/ShortLC.o \ dmr/Control.o \ dmr/ControlPacket.o \ diff --git a/Makefile.arm b/Makefile.arm index 853a6fbc..2cee2e35 100644 --- a/Makefile.arm +++ b/Makefile.arm @@ -25,6 +25,7 @@ OBJECTS = \ dmr/lc/CSBK.o \ dmr/lc/FullLC.o \ dmr/lc/LC.o \ + dmr/lc/PrivacyLC.o \ dmr/lc/ShortLC.o \ dmr/Control.o \ dmr/ControlPacket.o \ diff --git a/Makefile.rpi-arm b/Makefile.rpi-arm index 66719fc6..84f9fbdd 100644 --- a/Makefile.rpi-arm +++ b/Makefile.rpi-arm @@ -25,6 +25,7 @@ OBJECTS = \ dmr/lc/CSBK.o \ dmr/lc/FullLC.o \ dmr/lc/LC.o \ + dmr/lc/PrivacyLC.o \ dmr/lc/ShortLC.o \ dmr/Control.o \ dmr/ControlPacket.o \ diff --git a/Utils.cpp b/Utils.cpp index 56bdd807..2e5c2827 100644 --- a/Utils.cpp +++ b/Utils.cpp @@ -183,8 +183,6 @@ void Utils::symbols(const std::string& title, const uint8_t* data, uint32_t leng while (bufLen > 0U) { std::string output; - uint32_t bytes = (bufLen > 18U) ? 18U : bufLen; - uint32_t symOffset = offset; // iterate through bytes in groups of 2 diff --git a/dmr/DMRDefines.h b/dmr/DMRDefines.h index 84615b47..f0c86535 100644 --- a/dmr/DMRDefines.h +++ b/dmr/DMRDefines.h @@ -131,6 +131,8 @@ namespace dmr const uint32_t DMR_MAX_PDU_COUNT = 32U; const uint32_t DMR_MAX_PDU_LENGTH = 512U; + const uint32_t DMR_MI_LENGTH_BYTES = 4U; // This was guessed based on OTA data captures -- the message indicator seems to be the same length as a source/destination address + const uint8_t FID_ETSI = 0x00U; // ETSI Standard Feature Set const uint8_t FID_DMRA = 0x10U; // diff --git a/dmr/Slot.cpp b/dmr/Slot.cpp index 2aba1aa9..84ae80b0 100644 --- a/dmr/Slot.cpp +++ b/dmr/Slot.cpp @@ -113,8 +113,10 @@ Slot::Slot(uint32_t slotNo, uint32_t timeout, uint32_t tgHang, uint32_t queueSiz m_netState(RS_NET_IDLE), m_netLastDstId(0U), m_rfLC(NULL), + m_rfPrivacyLC(NULL), m_rfSeqNo(0U), m_netLC(NULL), + m_netPrivacyLC(NULL), m_networkWatchdog(1000U, 0U, 1500U), m_rfTimeoutTimer(1000U, timeout), m_rfTGHang(1000U, tgHang), diff --git a/dmr/Slot.h b/dmr/Slot.h index 67d3f393..f400fb89 100644 --- a/dmr/Slot.h +++ b/dmr/Slot.h @@ -12,7 +12,7 @@ // /* * Copyright (C) 2015,2016,2017 by Jonathan Naylor G4KLX -* Copyright (C) 2017-2020 by Bryan Biedenkapp N2PLL +* Copyright (C) 2017-2021 by Bryan Biedenkapp N2PLL * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -113,10 +113,12 @@ namespace dmr uint32_t m_netLastDstId; lc::LC* m_rfLC; + lc::PrivacyLC* m_rfPrivacyLC; uint8_t m_rfSeqNo; lc::LC* m_netLC; + lc::PrivacyLC* m_netPrivacyLC; Timer m_networkWatchdog; Timer m_rfTimeoutTimer; diff --git a/dmr/VoicePacket.cpp b/dmr/VoicePacket.cpp index 37d80161..22db188e 100644 --- a/dmr/VoicePacket.cpp +++ b/dmr/VoicePacket.cpp @@ -204,6 +204,14 @@ bool VoicePacket::process(uint8_t* data, uint32_t len) if (m_slot->m_rfState != RS_RF_AUDIO) return false; + lc::FullLC fullLC; + lc::PrivacyLC* lc = fullLC.decodePI(data + 2U); +/* + if (lc == NULL) + return false; +*/ + m_slot->m_rfPrivacyLC = lc; + // Regenerate the Slot Type slotType.encode(data + 2U); @@ -225,7 +233,7 @@ bool VoicePacket::process(uint8_t* data, uint32_t len) m_slot->writeNetworkRF(data, DT_VOICE_PI_HEADER); if (m_verbose) { - LogMessage(LOG_RF, DMR_DT_VOICE_PI_HEADER ", slot = %u", m_slot->m_slotNo); + LogMessage(LOG_RF, DMR_DT_VOICE_PI_HEADER ", slot = %u, algId = %u, kId = %u, dstId = %u", m_slot->m_slotNo, lc->getAlgId(), lc->getKId(), lc->getDstId()); } return true; @@ -729,6 +737,17 @@ void VoicePacket::processNetwork(const data::Data& dmrData) m_slot->m_slotNo, srcId, m_slot->m_netLC->getFLCO() == FLCO_GROUP ? "TG " : "", dstId); } + lc::FullLC fullLC; + lc::PrivacyLC* lc = fullLC.decodePI(data + 2U); +/* + if (lc == NULL) { + LogWarning(LOG_NET, "DMR Slot %u, DT_VOICE_PI_HEADER, bad LC received from the network, replacing", m_slot->m_slotNo); + lc = new lc::PrivacyLC(); + lc->setDstId(dmrData.getDstId()); + } +*/ + m_slot->m_netPrivacyLC = lc; + // Regenerate the Slot Type SlotType slotType; slotType.setColorCode(m_slot->m_colorCode); @@ -750,7 +769,7 @@ void VoicePacket::processNetwork(const data::Data& dmrData) m_slot->writeQueueNet(data); if (m_verbose) { - LogMessage(LOG_NET, DMR_DT_VOICE_PI_HEADER ", slot = %u", m_slot->m_slotNo); + LogMessage(LOG_NET, DMR_DT_VOICE_PI_HEADER ", slot = %u, algId = %u, kId = %u, dstId = %u", m_slot->m_slotNo, lc->getAlgId(), lc->getKId(), lc->getDstId()); } } else if (dataType == DT_VOICE_SYNC) { diff --git a/dmr/VoicePacket.h b/dmr/VoicePacket.h index ef7d5ad4..e8f7a77f 100644 --- a/dmr/VoicePacket.h +++ b/dmr/VoicePacket.h @@ -12,7 +12,7 @@ // /* * Copyright (C) 2015,2016,2017 by Jonathan Naylor G4KLX -* Copyright (C) 2017-2019 by Bryan Biedenkapp N2PLL +* Copyright (C) 2017-2021 by Bryan Biedenkapp N2PLL * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -35,6 +35,7 @@ #include "dmr/data/Data.h" #include "dmr/data/EmbeddedData.h" #include "dmr/lc/LC.h" +#include "dmr/lc/PrivacyLC.h" #include "dmr/Slot.h" #include "edac/AMBEFEC.h" #include "network/BaseNetwork.h" diff --git a/dmr/lc/FullLC.cpp b/dmr/lc/FullLC.cpp index 255d08e0..d2f45d4a 100644 --- a/dmr/lc/FullLC.cpp +++ b/dmr/lc/FullLC.cpp @@ -13,6 +13,7 @@ /* * Copyright (C) 2012 by Ian Wraith * Copyright (C) 2015,2016 by Jonathan Naylor G4KLX +* Copyright (C) 2021 Bryan Biedenkapp N2PLL * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -32,6 +33,7 @@ #include "dmr/DMRDefines.h" #include "dmr/lc/FullLC.h" #include "edac/RS129.h" +#include "edac/CRC.h" #include "Log.h" #include "Utils.h" @@ -138,3 +140,45 @@ void FullLC::encode(const LC& lc, uint8_t* data, uint8_t type) // encode BPTC (196,96) FEC m_bptc.encode(lcData, data); } + +/// +/// Decode DMR privacy control data. +/// +/// +/// +/// +PrivacyLC* FullLC::decodePI(const uint8_t* data) +{ + assert(data != NULL); + + // decode BPTC (196,96) FEC + uint8_t lcData[DMR_LC_HEADER_LENGTH_BYTES]; + m_bptc.decode(data, lcData); + + // check CRC-CCITT 16,2 +// if (!edac::CRC::checkCCITT162(lcData, DMR_LC_HEADER_LENGTH_BYTES)) +// return NULL; + + return new PrivacyLC(lcData); +} + +/// +/// Encode DMR privacy control data. +/// +/// +/// +/// +void FullLC::encodePI(const PrivacyLC& lc, uint8_t* data) +{ + assert(data != NULL); + + uint8_t lcData[DMR_LC_HEADER_LENGTH_BYTES]; + lc.getData(lcData); + + // encode CRC-CCITT 16,2 +// uint8_t parity[2U]; +// edac::CRC::addCCITT162(lcData, 9U); + + // encode BPTC (196,96) FEC + m_bptc.encode(lcData, data); +} diff --git a/dmr/lc/FullLC.h b/dmr/lc/FullLC.h index cb645480..e09ef72f 100644 --- a/dmr/lc/FullLC.h +++ b/dmr/lc/FullLC.h @@ -12,6 +12,7 @@ // /* * Copyright (C) 2015,2016 by Jonathan Naylor G4KLX +* Copyright (C) 2021 Bryan Biedenkapp N2PLL * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -32,6 +33,7 @@ #include "Defines.h" #include "dmr/lc/LC.h" +#include "dmr/lc/PrivacyLC.h" #include "dmr/SlotType.h" #include "edac/BPTC19696.h" @@ -56,6 +58,11 @@ namespace dmr /// Encode DMR full-link control data. void encode(const LC& lc, uint8_t* data, uint8_t type); + /// Decode DMR privacy control data. + PrivacyLC* decodePI(const uint8_t* data); + /// Encode DMR privacy control data. + void encodePI(const PrivacyLC& lc, uint8_t* data); + private: edac::BPTC19696 m_bptc; }; diff --git a/dmr/lc/LC.cpp b/dmr/lc/LC.cpp index b36f026a..d4d18591 100644 --- a/dmr/lc/LC.cpp +++ b/dmr/lc/LC.cpp @@ -12,6 +12,7 @@ // /* * Copyright (C) 2015,2016 by Jonathan Naylor G4KLX +* Copyright (C) 2020-2021 by Bryan Biedenkapp N2PLL * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -49,7 +50,7 @@ using namespace dmr; LC::LC(uint8_t flco, uint32_t srcId, uint32_t dstId) : m_PF(false), m_FLCO(flco), - m_FID(0U), + m_FID(FID_ETSI), m_srcId(srcId), m_dstId(dstId), m_emergency(false), @@ -68,7 +69,7 @@ LC::LC(uint8_t flco, uint32_t srcId, uint32_t dstId) : LC::LC(const uint8_t* bytes) : m_PF(false), m_FLCO(FLCO_GROUP), - m_FID(0U), + m_FID(FID_ETSI), m_srcId(0U), m_dstId(0U), m_emergency(false), @@ -103,7 +104,7 @@ LC::LC(const uint8_t* bytes) : LC::LC(const bool* bits) : m_PF(false), m_FLCO(FLCO_GROUP), - m_FID(0U), + m_FID(FID_ETSI), m_srcId(0U), m_dstId(0U), m_emergency(false), @@ -152,7 +153,7 @@ LC::LC(const bool* bits) : LC::LC() : m_PF(false), m_FLCO(FLCO_GROUP), - m_FID(0U), + m_FID(FID_ETSI), m_srcId(0U), m_dstId(0U), m_emergency(false), diff --git a/dmr/lc/LC.h b/dmr/lc/LC.h index 1448cc41..7c796e21 100644 --- a/dmr/lc/LC.h +++ b/dmr/lc/LC.h @@ -12,6 +12,7 @@ // /* * Copyright (C) 2015,2016 by Jonathan Naylor G4KLX +* Copyright (C) 2020-2021 by Bryan Biedenkapp N2PLL * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -67,7 +68,7 @@ namespace dmr /// Full-link control opcode. __PROPERTY(uint8_t, FLCO, FLCO); - /// CSBK feature ID. + /// Feature ID. __PROPERTY(uint8_t, FID, FID); /// Source ID. diff --git a/dmr/lc/PrivacyLC.cpp b/dmr/lc/PrivacyLC.cpp new file mode 100644 index 00000000..d328d159 --- /dev/null +++ b/dmr/lc/PrivacyLC.cpp @@ -0,0 +1,186 @@ +/** +* Digital Voice Modem - Host Software +* GPLv2 Open Source. Use is subject to license terms. +* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. +* +* @package DVM / Host Software +* +*/ +// +// Based on code from the MMDVMHost project. (https://github.com/g4klx/MMDVMHost) +// Licensed under the GPLv2 License (https://opensource.org/licenses/GPL-2.0) +// +/* +* Copyright (C) 2021 by Bryan Biedenkapp N2PLL +* +* This program is free software; you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation; either version 2 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program; if not, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ +#include "Defines.h" +#include "dmr/lc/PrivacyLC.h" +#include "edac/CRC.h" +#include "Utils.h" + +using namespace dmr::lc; +using namespace dmr; + +#include +#include + +// --------------------------------------------------------------------------- +// Public Class Members +// --------------------------------------------------------------------------- +/// +/// Initializes a new instance of the PrivacyLC class. +/// +/// +PrivacyLC::PrivacyLC(const uint8_t* bytes) : + m_FID(FID_ETSI), + m_dstId(0U), + m_group(false), + m_algId(0U), + m_kId(0U), + m_mi(NULL) +{ + assert(bytes != NULL); + + m_mi = new uint8_t[DMR_MI_LENGTH_BYTES]; + + m_group = (bytes[0U] & 0x20U) == 0x20U; + m_algId = bytes[0U] & 7; // Algorithm ID + + m_FID = bytes[1U]; + m_kId = bytes[2U]; + + m_mi[0U] = bytes[3U]; + m_mi[1U] = bytes[4U]; + m_mi[2U] = bytes[5U]; + m_mi[3U] = bytes[6U]; + + m_dstId = bytes[7U] << 16 | bytes[8U] << 8 | bytes[9U]; // Destination Address +} +/// +/// Initializes a new instance of the PrivacyLC class. +/// +/// +PrivacyLC::PrivacyLC(const bool* bits) : + m_FID(FID_ETSI), + m_dstId(0U), + m_group(false), + m_algId(0U), + m_kId(0U), + m_mi(NULL) +{ + assert(bits != NULL); + + m_mi = new uint8_t[DMR_MI_LENGTH_BYTES]; + + uint8_t temp1, temp2, temp3; + Utils::bitsToByteBE(bits + 0U, temp1); + + m_group = (temp1 & 0x20U) == 0x20U; + m_algId = temp1 & 7; // Algorithm ID + + Utils::bitsToByteBE(bits + 8U, temp2); + m_FID = temp2; + + Utils::bitsToByteBE(bits + 16U, temp3); + m_kId = temp3; + + uint8_t mi1, mi2, mi3, mi4; + Utils::bitsToByteBE(bits + 24U, mi1); + Utils::bitsToByteBE(bits + 32U, mi2); + Utils::bitsToByteBE(bits + 40U, mi3); + Utils::bitsToByteBE(bits + 48U, mi4); + + m_mi[0U] = mi1; + m_mi[1U] = mi2; + m_mi[2U] = mi3; + m_mi[3U] = mi4; + + uint8_t d1, d2, d3; + Utils::bitsToByteBE(bits + 56U, d1); + Utils::bitsToByteBE(bits + 64U, d2); + Utils::bitsToByteBE(bits + 72U, d3); + + m_dstId = d1 << 16 | d2 << 8 | d3; // Destination Address +} +/// +/// Initializes a new instance of the PrivacyLC class. +/// +PrivacyLC::PrivacyLC() : + m_FID(FID_ETSI), + m_dstId(0U), + m_group(false), + m_algId(0U), + m_kId(0U), + m_mi(NULL) +{ + /* stub */ +} + +/// +/// Finalizes a instance of the PrivacyLC class. +/// +PrivacyLC::~PrivacyLC() +{ + /* stub */ +} + +/// +/// +/// +/// +void PrivacyLC::getData(uint8_t* bytes) const +{ + assert(bytes != NULL); + + bytes[0U] = (m_group ? 0x20U : 0x00U) + + (m_algId & 0x07U); // Algorithm ID + + bytes[1U] = m_FID; + bytes[2U] = m_kId; + + bytes[3U] = m_mi[0U]; + bytes[4U] = m_mi[1U]; + bytes[5U] = m_mi[2U]; + bytes[6U] = m_mi[3U]; + + bytes[7U] = m_dstId >> 16; // Destination Address + bytes[8U] = m_dstId >> 8; // .. + bytes[9U] = m_dstId >> 0; // .. +} + +/// +/// +/// +/// +void PrivacyLC::getData(bool* bits) const +{ + assert(bits != NULL); + + uint8_t bytes[10U]; + getData(bytes); + + Utils::byteToBitsBE(bytes[0U], bits + 0U); + Utils::byteToBitsBE(bytes[1U], bits + 8U); + Utils::byteToBitsBE(bytes[2U], bits + 16U); + Utils::byteToBitsBE(bytes[3U], bits + 24U); + Utils::byteToBitsBE(bytes[4U], bits + 32U); + Utils::byteToBitsBE(bytes[5U], bits + 40U); + Utils::byteToBitsBE(bytes[6U], bits + 48U); + Utils::byteToBitsBE(bytes[7U], bits + 56U); + Utils::byteToBitsBE(bytes[8U], bits + 64U); + Utils::byteToBitsBE(bytes[9U], bits + 72U); +} diff --git a/dmr/lc/PrivacyLC.h b/dmr/lc/PrivacyLC.h new file mode 100644 index 00000000..60379984 --- /dev/null +++ b/dmr/lc/PrivacyLC.h @@ -0,0 +1,86 @@ +/** +* Digital Voice Modem - Host Software +* GPLv2 Open Source. Use is subject to license terms. +* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. +* +* @package DVM / Host Software +* +*/ +// +// Based on code from the MMDVMHost project. (https://github.com/g4klx/MMDVMHost) +// Licensed under the GPLv2 License (https://opensource.org/licenses/GPL-2.0) +// +/* +* Copyright (C) 2021 Bryan Biedenkapp N2PLL +* Copyright (C) 2020-2021 by Bryan Biedenkapp N2PLL +* +* This program is free software; you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation; either version 2 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program; if not, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ +#if !defined(__DMR_LC__PRIVACY_LC_H__) +#define __DMR_LC__PRIVACY_LC_H__ + +#include "Defines.h" +#include "dmr/DMRDefines.h" + +namespace dmr +{ + namespace lc + { + // --------------------------------------------------------------------------- + // Class Declaration + // Represents DMR privacy indicator link control data. + // --------------------------------------------------------------------------- + + class HOST_SW_API PrivacyLC { + public: + /// Initializes a new instance of the PrivacyLC class. + PrivacyLC(const uint8_t* bytes); + /// Initializes a new instance of the PrivacyLC class. + PrivacyLC(const bool* bits); + /// Initializes a new instance of the PrivacyLC class. + PrivacyLC(); + /// Finalizes a instance of the PrivacyLC class. + ~PrivacyLC(); + + /// Gets LC data as bytes. + void getData(uint8_t* bytes) const; + /// Gets LC data as bits. + void getData(bool* bits) const; + + public: + /// Feature ID. + __PROPERTY(uint8_t, FID, FID); + + /// Destination ID. + __PROPERTY(uint32_t, dstId, DstId); + + /** Service Options */ + /// Flag indicating a group/talkgroup operation. + __PROPERTY(bool, group, Group); + + /** Encryption data */ + /// Encryption algorithm ID. + __PROPERTY(uint8_t, algId, AlgId); + /// Encryption key ID. + __PROPERTY(uint32_t, kId, KId); + + private: + /** Encryption data */ + uint8_t* m_mi; + }; + } // namespace lc +} // namespace dmr + +#endif // __DMR_LC__PRIVACY_LC_H__