From 98c9a5790012033fcd1e7e0a8856eaf0272936bd Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Wed, 3 Jan 2024 15:03:32 -0500 Subject: [PATCH] implement P25 packet interleaver; migrate the partial FneSystemBase from downstream projects into the core as an abstract class as it useful helper functions for implementors; --- FneSystemBase.cs | 528 ++++++++++++++++++++++++++++++++++++++++++ P25/P25Defines.cs | 6 + P25/P25Interleaver.cs | 128 ++++++++++ 3 files changed, 662 insertions(+) create mode 100644 FneSystemBase.cs create mode 100644 P25/P25Interleaver.cs diff --git a/FneSystemBase.cs b/FneSystemBase.cs new file mode 100644 index 0000000..38b70b2 --- /dev/null +++ b/FneSystemBase.cs @@ -0,0 +1,528 @@ +/** +* Digital Voice Modem - Fixed Network Equipment +* AGPLv3 Open Source. Use is subject to license terms. +* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. +* +* @package DVM / Fixed Network Equipment +* +*/ +/* +* Copyright (C) 2024 by Bryan Biedenkapp N2PLL +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU Affero General Public License as published by +* the Free Software Foundation, either version 3 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 Affero General Public License for more details. +*/ + +using System; +using System.Collections.Generic; +using System.Linq; +using System.Net; +using System.Threading; +using System.Threading.Tasks; + +using fnecore.EDAC; +using fnecore.DMR; +using fnecore.P25; +using fnecore.NXDN; + +namespace fnecore +{ + /// + /// Metadata class containing remote call data. + /// + public abstract class RemoteCallData + { + /// + /// Source ID. + /// + public uint SrcId = 0; + /// + /// Destination ID. + /// + public uint DstId = 0; + + /// + /// Link-Control Opcode. + /// + public byte LCO = 0; + /// + /// Manufacturer ID. + /// + public byte MFId = 0; + /// + /// Service Options. + /// + public byte ServiceOptions = 0; + + /// + /// Low-speed Data Byte 1 + /// + public byte LSD1 = 0; + /// + /// Low-speed Data Byte 2 + /// + public byte LSD2 = 0; + + /// + /// Encryption Message Indicator + /// + public byte[] MessageIndicator = new byte[P25Defines.P25_MI_LENGTH]; + + /// + /// Algorithm ID. + /// + public byte AlgorithmId = P25Defines.P25_ALGO_UNENCRYPT; + /// + /// Key ID. + /// + public ushort KeyId = 0; + + /// + /// + /// + public uint TxStreamID = 0; + + /// + /// + /// + public FrameType FrameType = FrameType.TERMINATOR; + /// + /// + /// + public byte Slot = 0; + + /* + ** Methods + */ + + /// + /// Reset values. + /// + public virtual void Reset() + { + SrcId = 0; + DstId = 0; + + LCO = 0; + MFId = 0; + ServiceOptions = 0; + + LSD1 = 0; + LSD2 = 0; + + MessageIndicator = new byte[P25Defines.P25_MI_LENGTH]; + + AlgorithmId = P25Defines.P25_ALGO_UNENCRYPT; + KeyId = 0; + + FrameType = FrameType.TERMINATOR; + Slot = 0; + } + } // public abstract class RemoteCallData + + /// + /// Implements a FNE system. + /// + public abstract class FneSystemBase + { + protected FneBase fne; + + protected const int DMR_FRAME_LENGTH_BYTES = 33; + protected const int DMR_PACKET_SIZE = 55; + + protected static readonly byte[] DMR_SILENCE_DATA = { 0x01, 0x00, + 0xB9, 0xE8, 0x81, 0x52, 0x61, 0x73, 0x00, 0x2A, 0x6B, 0xB9, 0xE8, + 0x81, 0x52, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x73, 0x00, + 0x2A, 0x6B, 0xB9, 0xE8, 0x81, 0x52, 0x61, 0x73, 0x00, 0x2A, 0x6B }; + + protected const int P25_MSG_HDR_SIZE = 24; + + /* + ** Properties + */ + + /// + /// Gets the system name for this . + /// + public string SystemName + { + get + { + if (fne != null) + return fne.SystemName; + return string.Empty; + } + } + + /// + /// Gets the peer ID for this . + /// + public uint PeerId + { + get + { + if (fne != null) + return fne.PeerId; + return uint.MaxValue; + } + } + + /// + /// Flag indicating whether this is running. + /// + public bool IsStarted + { + get + { + if (fne != null) + return fne.IsStarted; + return false; + } + } + + /// + /// Gets the this is. + /// + public FneType FneType + { + get + { + if (fne != null) + return fne.FneType; + return FneType.UNKNOWN; + } + } + + /* + ** Methods + */ + + /// + /// Initializes a new instance of the class. + /// + /// Instance of or + /// + public FneSystemBase(FneBase fne, LogLevel fneLogLevel = LogLevel.INFO) + { + this.fne = fne; + + // hook various FNE network callbacks + this.fne.DMRDataValidate = DMRDataValidate; + this.fne.DMRDataReceived += DMRDataReceived; + + this.fne.P25DataValidate = P25DataValidate; + this.fne.P25DataPreprocess += P25DataPreprocess; + this.fne.P25DataReceived += P25DataReceived; + + this.fne.NXDNDataValidate = NXDNDataValidate; + this.fne.NXDNDataReceived += NXDNDataReceived; + + this.fne.PeerIgnored = PeerIgnored; + this.fne.PeerConnected += PeerConnected; + + // hook logger callback + this.fne.LogLevel = fneLogLevel; + } + + /// + /// Starts the main execution loop for this . + /// + public virtual void Start() + { + if (!fne.IsStarted) + fne.Start(); + } + + /// + /// Stops the main execution loop for this . + /// + public virtual void Stop() + { + if (fne.IsStarted) + fne.Stop(); + } + + /// + /// Callback used to validate incoming DMR data. + /// + /// Peer ID + /// Source Address + /// Destination Address + /// Slot Number + /// Call Type (Group or Private) + /// Frame Type + /// DMR Data Type + /// Stream ID + /// Raw message data + /// True, if data stream is valid, otherwise false. + protected abstract bool DMRDataValidate(uint peerId, uint srcId, uint dstId, byte slot, CallType callType, FrameType frameType, DMRDataType dataType, uint streamId, byte[] message); + + /// + /// Event handler used to process incoming DMR data. + /// + /// + /// + protected abstract void DMRDataReceived(object sender, DMRDataReceivedEvent e); + + /// + /// Callback used to validate incoming P25 data. + /// + /// Peer ID + /// Source Address + /// Destination Address + /// Call Type (Group or Private) + /// P25 DUID + /// Frame Type + /// Stream ID + /// Raw message data + /// True, if data stream is valid, otherwise false. + protected abstract bool P25DataValidate(uint peerId, uint srcId, uint dstId, CallType callType, P25DUID duid, FrameType frameType, uint streamId, byte[] message); + + /// + /// Event handler used to pre-process incoming P25 data. + /// + /// + /// + protected abstract void P25DataPreprocess(object sender, P25DataReceivedEvent e); + + /// + /// Event handler used to process incoming P25 data. + /// + /// + /// + protected abstract void P25DataReceived(object sender, P25DataReceivedEvent e); + + /// + /// Callback used to validate incoming NXDN data. + /// + /// Peer ID + /// Source Address + /// Destination Address + /// Call Type (Group or Private) + /// NXDN Message Type + /// Frame Type + /// Stream ID + /// Raw message data + /// True, if data stream is valid, otherwise false. + protected abstract bool NXDNDataValidate(uint peerId, uint srcId, uint dstId, CallType callType, NXDNMessageType messageType, FrameType frameType, uint streamId, byte[] message); + + /// + /// Event handler used to process incoming NXDN data. + /// + /// + /// + protected abstract void NXDNDataReceived(object sender, NXDNDataReceivedEvent e); + + /// + /// Callback used to process whether or not a peer is being ignored for traffic. + /// + /// Peer ID + /// Source Address + /// Destination Address + /// Slot Number + /// Call Type (Group or Private) + /// Frame Type + /// DMR Data Type + /// Stream ID + /// True, if peer is ignored, otherwise false. + protected abstract bool PeerIgnored(uint peerId, uint srcId, uint dstId, byte slot, CallType callType, FrameType frameType, DMRDataType dataType, uint streamId); + + /// + /// Event handler used to handle a peer connected event. + /// + /// + /// + protected abstract void PeerConnected(object sender, PeerConnectedEvent e); + + /// + /// Creates an DMR frame message. + /// + /// + /// + /// + protected void CreateDMRMessage(ref byte[] data, RemoteCallData callData, byte seqNo, byte n) + { + FneUtils.StringToBytes(Constants.TAG_DMR_DATA, data, 0, Constants.TAG_DMR_DATA.Length); + + FneUtils.Write3Bytes(callData.SrcId, ref data, 5); // Source Address + FneUtils.Write3Bytes(callData.DstId, ref data, 8); // Destination Address + + data[15U] = (byte)((callData.Slot == 1) ? 0x00 : 0x80); // Slot Number + data[15U] |= 0x00; // Group + + if (callData.FrameType == FrameType.VOICE_SYNC) + data[15U] |= 0x10; + else if (callData.FrameType == FrameType.VOICE) + data[15U] |= n; + else + data[15U] |= (byte)(0x20 | (byte)callData.FrameType); + + data[4U] = seqNo; + } + + /// + /// Helper to send a DMR terminator with LC message. + /// + /// + /// + /// + /// + protected virtual void SendDMRTerminator(RemoteCallData callData, ref int seqNo, ref byte dmrN, EmbeddedData embeddedData) + { + byte n = (byte)((seqNo - 3U) % 6U); + uint fill = 6U - n; + + FnePeer peer = (FnePeer)fne; + ushort pktSeq = peer.pktSeq(true); + + byte[] data = null, dmrpkt = null; + if (n > 0U) + { + for (uint i = 0U; i < fill; i++) + { + // generate DMR AMBE data + data = new byte[DMR_FRAME_LENGTH_BYTES]; + Buffer.BlockCopy(DMR_SILENCE_DATA, 0, data, 0, DMR_FRAME_LENGTH_BYTES); + + byte lcss = embeddedData.GetData(ref data, n); + + // generated embedded signalling + EMB emb = new EMB(); + emb.ColorCode = 0; + emb.LCSS = lcss; + emb.Encode(ref data); + + // generate DMR network frame + dmrpkt = new byte[DMR_PACKET_SIZE]; + callData.FrameType = FrameType.DATA_SYNC; + + CreateDMRMessage(ref dmrpkt, callData, (byte)seqNo, n); + Buffer.BlockCopy(data, 0, dmrpkt, 20, DMR_FRAME_LENGTH_BYTES); + + peer.SendMaster(new Tuple(Constants.NET_FUNC_PROTOCOL, Constants.NET_PROTOCOL_SUBFUNC_DMR), dmrpkt, pktSeq, callData.TxStreamID); + + seqNo++; + dmrN++; + } + } + + data = new byte[DMR_FRAME_LENGTH_BYTES]; + + // generate DMR LC + LC dmrLC = new LC(); + dmrLC.FLCO = (byte)DMRFLCO.FLCO_GROUP; + dmrLC.SrcId = callData.SrcId; + dmrLC.DstId = callData.DstId; + + // generate the Slot TYpe + SlotType slotType = new SlotType(); + slotType.DataType = (byte)DMRDataType.TERMINATOR_WITH_LC; + slotType.GetData(ref data); + + FullLC.Encode(dmrLC, ref data, DMRDataType.TERMINATOR_WITH_LC); + + // generate DMR network frame + dmrpkt = new byte[DMR_PACKET_SIZE]; + callData.FrameType = FrameType.DATA_SYNC; + + CreateDMRMessage(ref dmrpkt, callData, (byte)seqNo, 0); + Buffer.BlockCopy(data, 0, dmrpkt, 20, DMR_FRAME_LENGTH_BYTES); + + peer.SendMaster(new Tuple(Constants.NET_FUNC_PROTOCOL, Constants.NET_PROTOCOL_SUBFUNC_DMR), dmrpkt, pktSeq, callData.TxStreamID); + + seqNo = 0; + dmrN = 0; + } + + + /// + /// Creates an P25 frame message header. + /// + /// + /// + protected void CreateP25MessageHdr(byte duid, RemoteCallData callData, ref byte[] data) + { + FneUtils.StringToBytes(Constants.TAG_P25_DATA, data, 0, Constants.TAG_P25_DATA.Length); + + data[4U] = callData.LCO; // LCO + + FneUtils.Write3Bytes(callData.SrcId, ref data, 5); // Source Address + FneUtils.Write3Bytes(callData.DstId, ref data, 8); // Destination Address + + data[11U] = 0; // System ID + data[12U] = 0; + + data[14U] = 0; // Control Byte + + data[15U] = callData.MFId; // MFId + + data[16U] = 0; // Network ID + data[17U] = 0; + data[18U] = 0; + + data[20U] = callData.LSD1; // LSD 1 + data[21U] = callData.LSD2; // LSD 2 + + data[22U] = duid; // DUID + + data[180U] = 0; // Frame Type + } + + /// + /// Helper to send a P25 TSDU message. + /// + /// + public virtual void SendP25TSBK(RemoteCallData callData, byte[] tsbk) + { + if (tsbk.Length != P25Defines.P25_TSBK_LENGTH_BYTES) + throw new InvalidOperationException($"TSBK length must be {P25Defines.P25_TSBK_LENGTH_BYTES}, passed length is {tsbk.Length}"); + + Trellis trellis = new Trellis(); + FnePeer peer = (FnePeer)fne; + ushort pktSeq = peer.pktSeq(true); + + byte[] payload = new byte[200]; + CreateP25MessageHdr((byte)P25DUID.TSDU, callData, ref payload); + + // pack raw P25 TSDU bytes + byte[] tsbkTrellis = new byte[P25Defines.P25_TSBK_FEC_LENGTH_BYTES]; + trellis.Encode12(tsbk, ref tsbkTrellis); + + byte[] raw = new byte[P25Defines.P25_TSDU_FRAME_LENGTH_BYTES]; + P25Interleaver.Encode(tsbkTrellis, ref raw, 114, 318); + + Buffer.BlockCopy(raw, 0, payload, 24, raw.Length); + payload[23U] = (byte)(P25_MSG_HDR_SIZE + raw.Length); + + peer.SendMaster(new Tuple(Constants.NET_FUNC_PROTOCOL, Constants.NET_PROTOCOL_SUBFUNC_P25), payload, pktSeq, callData.TxStreamID); + } + + /// + /// Helper to send a P25 TDU message. + /// + /// + /// + public virtual void SendP25TDU(RemoteCallData callData, bool grantDemand = false) + { + FnePeer peer = (FnePeer)fne; + ushort pktSeq = peer.pktSeq(true); + + byte[] payload = new byte[200]; + CreateP25MessageHdr((byte)P25DUID.TDU, callData, ref payload); + payload[23U] = P25_MSG_HDR_SIZE; + + // if this TDU is demanding a grant, set the grant demand control bit + if (grantDemand) + payload[14U] |= 0x80; + + peer.SendMaster(new Tuple(Constants.NET_FUNC_PROTOCOL, Constants.NET_PROTOCOL_SUBFUNC_P25), payload, pktSeq, callData.TxStreamID); + } + } // public abstract class FneSystemBase +} // namespace fnecore diff --git a/P25/P25Defines.cs b/P25/P25Defines.cs index 0aee5d0..7e871b3 100644 --- a/P25/P25Defines.cs +++ b/P25/P25Defines.cs @@ -145,8 +145,14 @@ namespace fnecore.P25 public const byte P25_MI_LENGTH = 9; + public const byte P25_TSDU_FRAME_LENGTH_BYTES = 45; + public const byte P25_LDU_FRAME_LENGTH_BYTES = 216; + public const byte P25_TSBK_FEC_LENGTH_BYTES = 25; + public const byte P25_TSBK_FEC_LENGTH_BITS = P25_TSBK_FEC_LENGTH_BYTES * 8 - 4; // Trellis is actually 196 bits + public const byte P25_TSBK_LENGTH_BYTES = 12; + public const byte P25_MAX_PDU_COUNT = 32; public const uint P25_MAX_PDU_LENGTH = 512; diff --git a/P25/P25Interleaver.cs b/P25/P25Interleaver.cs new file mode 100644 index 0000000..5be7061 --- /dev/null +++ b/P25/P25Interleaver.cs @@ -0,0 +1,128 @@ +/** +* Digital Voice Modem - Fixed Network Equipment +* AGPLv3 Open Source. Use is subject to license terms. +* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. +* +* @package DVM / Fixed Network Equipment +* +*/ +// +// 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) 2024 by Bryan Biedenkapp N2PLL +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU Affero General Public License as published by +* the Free Software Foundation, either version 3 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 Affero General Public License for more details. +*/ + +using System; + +namespace fnecore.P25 +{ + /// + /// + /// + public sealed class P25Interleaver + { + private const uint P25_SS0_START = 70U; + private const uint P25_SS1_START = 71U; + private const uint P25_SS_INCREMENT = 72U; + + /* + ** Methods + */ + + /// + /// Decode bit interleaving. + /// + /// + /// + /// + /// + /// + public static uint Decode(byte[] _in, ref byte[] _out, uint start, uint stop) + { + if (_in == null) + throw new NullReferenceException("_in"); + if (_out == null) + throw new NullReferenceException("_out"); + + // Move the SSx positions to the range needed + uint ss0Pos = P25_SS0_START; + uint ss1Pos = P25_SS1_START; + + while (ss0Pos < start) { + ss0Pos += P25_SS_INCREMENT; + ss1Pos += P25_SS_INCREMENT; + } + + uint n = 0U; + for (uint i = start; i < stop; i++) { + if (i == ss0Pos) { + ss0Pos += P25_SS_INCREMENT; + } + else if (i == ss1Pos) { + ss1Pos += P25_SS_INCREMENT; + } + else { + bool b = FneUtils.ReadBit(_in, i); + FneUtils.WriteBit(ref _out, n, b); + n++; + } + } + + return n; + } + + /// + /// Encode bit interleaving. + /// + /// + /// + /// + /// + /// + public static uint Encode(byte[] _in, ref byte[] _out, uint start, uint stop) + { + if (_in == null) + throw new NullReferenceException("_in"); + if (_out == null) + throw new NullReferenceException("_out"); + + // Move the SSx positions to the range needed + uint ss0Pos = P25_SS0_START; + uint ss1Pos = P25_SS1_START; + + while (ss0Pos < start) { + ss0Pos += P25_SS_INCREMENT; + ss1Pos += P25_SS_INCREMENT; + } + + uint n = 0U; + for (uint i = start; i < stop; i++) { + if (i == ss0Pos) { + ss0Pos += P25_SS_INCREMENT; + } + else if (i == ss1Pos) { + ss1Pos += P25_SS_INCREMENT; + } + else { + bool b = FneUtils.ReadBit(_in, n); + FneUtils.WriteBit(ref _out, i, b); + n++; + } + } + + return n; + } + } // public sealed class P25Interleaver +} // namespace fnecore.P25