From 900a43b932bafced32a38f2484c130b99b547d30 Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Tue, 5 Dec 2023 15:34:24 -0500 Subject: [PATCH] add support code to assemble P25 PDU data headers and blocks; --- EDAC/Trellis.cs | 76 ++++---- P25/DataBlock.cs | 249 +++++++++++++++++++++++++++ P25/DataHeader.cs | 428 ++++++++++++++++++++++++++++++++++++++++++++++ P25/P25Defines.cs | 48 ++++++ 4 files changed, 763 insertions(+), 38 deletions(-) create mode 100644 P25/DataBlock.cs create mode 100644 P25/DataHeader.cs diff --git a/EDAC/Trellis.cs b/EDAC/Trellis.cs index 2a663a6..679b772 100644 --- a/EDAC/Trellis.cs +++ b/EDAC/Trellis.cs @@ -76,7 +76,7 @@ namespace fnecore.EDAC /// Trellis symbol bytes. /// Output bytes. /// True, if decoded, otherwise false. - public bool decode34(byte[] data, ref byte[] payload) + public bool Decode34(byte[] data, ref byte[] payload) { if (data == null) throw new NullReferenceException("data"); @@ -84,16 +84,16 @@ namespace fnecore.EDAC throw new NullReferenceException("payload"); int[] dibits = new int[98U]; - deinterleave(data, ref dibits); + Deinterleave(data, ref dibits); byte[] points = new byte[49U]; - dibitsToPoints(dibits, ref points); + DibitsToPoints(dibits, ref points); // check the original code byte[] tribits = new byte[49U]; - uint failPos = checkCode34(points, ref tribits); + uint failPos = CheckCode34(points, ref tribits); if (failPos == 999U) { - tribitsToBits(tribits, ref payload); + TribitsToBits(tribits, ref payload); return true; } @@ -101,7 +101,7 @@ namespace fnecore.EDAC for (uint i = 0U; i< 49U; i++) savePoints[i] = points[i]; - bool ret = fixCode34(points, failPos, ref payload); + bool ret = FixCode34(points, failPos, ref payload); if (ret) return true; @@ -109,7 +109,7 @@ namespace fnecore.EDAC return false; // Backtrack one place for a last go - return fixCode34(savePoints, failPos - 1U, ref payload); + return FixCode34(savePoints, failPos - 1U, ref payload); } /// @@ -117,7 +117,7 @@ namespace fnecore.EDAC /// /// Input bytes. /// Trellis symbol bytes. - public void encode34(byte[] payload, ref byte[] data) + public void Encode34(byte[] payload, ref byte[] data) { if (data == null) throw new NullReferenceException("data"); @@ -125,7 +125,7 @@ namespace fnecore.EDAC throw new NullReferenceException("payload"); byte[] tribits = new byte[49U]; - bitsToTribits(payload, ref tribits); + BitsToTribits(payload, ref tribits); byte[] points = new byte[49U]; byte state = 0; @@ -140,9 +140,9 @@ namespace fnecore.EDAC } int[] dibits = new int[98U]; - pointsToDibits(points, ref dibits); + PointsToDibits(points, ref dibits); - interleave(dibits, ref data); + Interleave(dibits, ref data); } /// @@ -151,7 +151,7 @@ namespace fnecore.EDAC /// Trellis symbol bytes. /// Output bytes. /// True, if decoded, otherwise false. - public bool decode12(byte[] data, ref byte[] payload) + public bool Decode12(byte[] data, ref byte[] payload) { if (data == null) throw new NullReferenceException("data"); @@ -159,17 +159,17 @@ namespace fnecore.EDAC throw new NullReferenceException("payload"); int[] dibits = new int[98U]; - deinterleave(data, ref dibits); + Deinterleave(data, ref dibits); byte[] points = new byte[49U]; - dibitsToPoints(dibits, ref points); + DibitsToPoints(dibits, ref points); // Check the original code byte[] bits = new byte[49U]; - uint failPos = checkCode12(points, ref bits); + uint failPos = CheckCode12(points, ref bits); if (failPos == 999U) { - dibitsToBits(bits, ref payload); + DibitsToBits(bits, ref payload); return true; } @@ -177,7 +177,7 @@ namespace fnecore.EDAC for (uint i = 0U; i < 49U; i++) savePoints[i] = points[i]; - bool ret = fixCode12(points, failPos, ref payload); + bool ret = FixCode12(points, failPos, ref payload); if (ret) return true; @@ -185,7 +185,7 @@ namespace fnecore.EDAC return false; // Backtrack one place for a last go - return fixCode12(savePoints, failPos - 1U, ref payload); + return FixCode12(savePoints, failPos - 1U, ref payload); } /// @@ -193,7 +193,7 @@ namespace fnecore.EDAC /// /// Input bytes. /// Trellis symbol bytes. - public void encode12(byte[] payload, ref byte[] data) + public void Encode12(byte[] payload, ref byte[] data) { if (data == null) throw new NullReferenceException("data"); @@ -201,7 +201,7 @@ namespace fnecore.EDAC throw new NullReferenceException("payload"); byte[] bits = new byte[49U]; - bitsToDibits(payload, ref bits); + BitsToDibits(payload, ref bits); byte[] points = new byte[49U]; byte state = 0; @@ -216,9 +216,9 @@ namespace fnecore.EDAC } int[] dibits = new int[98U]; - pointsToDibits(points, ref dibits); + PointsToDibits(points, ref dibits); - interleave(dibits, ref data); + Interleave(dibits, ref data); } /// @@ -226,7 +226,7 @@ namespace fnecore.EDAC /// /// Trellis symbol bytes. /// Dibits. - private void deinterleave(byte[] data, ref int[] dibits) + private void Deinterleave(byte[] data, ref int[] dibits) { for (uint i = 0U; i < 98U; i++) { uint n = i * 2U + 0U; @@ -255,7 +255,7 @@ namespace fnecore.EDAC /// /// Dibits. /// Trellis symbol bytes. - private void interleave(int[] dibits, ref byte[] data) + private void Interleave(int[] dibits, ref byte[] data) { for (uint i = 0U; i < 98U; i++) { uint n = INTERLEAVE_TABLE[i]; @@ -294,7 +294,7 @@ namespace fnecore.EDAC /// /// Dibits. /// 4FSK constellation points. - private void dibitsToPoints(int[] dibits, ref byte[] points) + private void DibitsToPoints(int[] dibits, ref byte[] points) { for (uint i = 0U; i < 49U; i++) { if (dibits[i * 2U + 0U] == +1 && dibits[i * 2U + 1U] == -1) @@ -337,7 +337,7 @@ namespace fnecore.EDAC /// /// 4FSK Constellation points. /// Dibits. - private void pointsToDibits(byte[] points, ref int[] dibits) + private void PointsToDibits(byte[] points, ref int[] dibits) { for (uint i = 0U; i < 49U; i++) { switch (points[i]) @@ -415,7 +415,7 @@ namespace fnecore.EDAC /// /// Byte payload. /// Tribits. - private void bitsToTribits(byte[] payload, ref byte[] tribits) + private void BitsToTribits(byte[] payload, ref byte[] tribits) { for (uint i = 0U; i < 48U; i++) { uint n = i * 3U; @@ -442,7 +442,7 @@ namespace fnecore.EDAC /// /// Byte payload. /// Dibits. - private void bitsToDibits(byte[] payload, ref byte[] dibits) + private void BitsToDibits(byte[] payload, ref byte[] dibits) { for (uint i = 0U; i < 48U; i++) { uint n = i * 2U; @@ -466,7 +466,7 @@ namespace fnecore.EDAC /// /// Tribits. /// Byte payload. - private void tribitsToBits(byte[] tribits, ref byte[] payload) + private void TribitsToBits(byte[] tribits, ref byte[] payload) { for (uint i = 0U; i < 48U; i++) { byte tribit = tribits[i]; @@ -490,7 +490,7 @@ namespace fnecore.EDAC /// /// Dibits. /// Byte payload. - private void dibitsToBits(byte[] dibits, ref byte[] payload) + private void DibitsToBits(byte[] dibits, ref byte[] payload) { for (uint i = 0U; i < 48U; i++) { byte dibit = dibits[i]; @@ -513,7 +513,7 @@ namespace fnecore.EDAC /// /// Byte payload. /// True, if error corrected, otherwise false. - private bool fixCode34(byte[] points, uint failPos, ref byte[] payload) + private bool FixCode34(byte[] points, uint failPos, ref byte[] payload) { for (uint j = 0; j < 20; j++) { @@ -525,10 +525,10 @@ namespace fnecore.EDAC points[failPos] = i; byte[] tribits = new byte[49]; - uint pos = checkCode34(points, ref tribits); + uint pos = CheckCode34(points, ref tribits); if (pos == 999) { - tribitsToBits(tribits, ref payload); + TribitsToBits(tribits, ref payload); return true; } @@ -552,7 +552,7 @@ namespace fnecore.EDAC /// 4FSK constellation points. /// Tribits. /// - private uint checkCode34(byte[] points, ref byte[] tribits) + private uint CheckCode34(byte[] points, ref byte[] tribits) { byte state = 0; @@ -589,7 +589,7 @@ namespace fnecore.EDAC /// /// Byte payload. /// True, if error corrected, otherwise false. - private bool fixCode12(byte[] points, uint failPos, ref byte[] payload) + private bool FixCode12(byte[] points, uint failPos, ref byte[] payload) { for (uint j = 0; j < 20; j++) { uint bestPos = 0; @@ -600,10 +600,10 @@ namespace fnecore.EDAC points[failPos] = i; byte[] dibits = new byte[49]; - uint pos = checkCode12(points, ref dibits); + uint pos = CheckCode12(points, ref dibits); if (pos == 999) { - dibitsToBits(dibits, ref payload); + DibitsToBits(dibits, ref payload); return true; } @@ -627,7 +627,7 @@ namespace fnecore.EDAC /// 4FSK constellation points. /// Dibits. /// - private uint checkCode12(byte[] points, ref byte[] dibits) + private uint CheckCode12(byte[] points, ref byte[] dibits) { byte state = 0; diff --git a/P25/DataBlock.cs b/P25/DataBlock.cs new file mode 100644 index 0000000..df1ee87 --- /dev/null +++ b/P25/DataBlock.cs @@ -0,0 +1,249 @@ +/** +* 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) 2022 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 fnecore.EDAC; + +namespace fnecore.P25 +{ + /// + /// Represents the data block for PDU P25 packets. + /// + public class DataBlock + { + private Trellis trellis; + + private byte headerSap; + + private byte[] data; + + /// Sets the data block serial number. + public byte SerialNo; + + /// Flag indicating this is the last block in a sequence of block. + public bool LastBlock; + + /// Logical link ID. + public uint LLId; + + /// Service access point. + public byte SAP; + + /// + /// Data format. + /// + public byte Format; + + /* + ** Methods + */ + + /// + /// Initializes a new instance of the class. + /// + public DataBlock() + { + SerialNo = 0; + LastBlock = false; + LLId = 0; + SAP = 0; + + trellis = new Trellis(); + Format = P25Defines.PDU_FMT_CONFIRMED; + headerSap = 0; + + data = new byte[P25Defines.P25_PDU_CONFIRMED_DATA_LENGTH_BYTES]; + } + + /// + /// Decodes P25 PDU data block. + /// + /// + /// Instance of the DataHeader class. + /// True, if data block was decoded, otherwise false. + public bool Decode(byte[] pduData, DataHeader header) + { + if (pduData == null) + throw new NullReferenceException("pduData"); + + byte[] buffer = new byte[P25Defines.P25_PDU_CONFIRMED_LENGTH_BYTES]; + + Format = header.Format; + headerSap = header.SAP; + + // set these to reasonable defaults + SerialNo = 0; + LastBlock = false; + LLId = 0; + + if (Format == P25Defines.PDU_FMT_CONFIRMED) { + // decode 3/4 rate Trellis + bool valid = trellis.Decode34(pduData, ref buffer); + if (!valid) { + return false; + } + + SerialNo = (byte)((buffer[0] & 0xFEU) >> 1); // Confirmed Data Serial No. + ushort crc = (ushort)(((buffer[0] & 0x01U) << 8) + buffer[1]); // CRC-9 Check Sum + + data = new byte[P25Defines.P25_PDU_CONFIRMED_DATA_LENGTH_BYTES]; + + // if this is extended addressing and the first block decode the SAP and LLId + if (headerSap == P25Defines.PDU_SAP_EXT_ADDR && SerialNo == 0U) { + SAP = (byte)(buffer[5U] & 0x3FU); // Service Access Point + LLId = (uint)((buffer[2U] << 16) + (buffer[3U] << 8) + buffer[4U]); // Logical Link ID + + for (uint i = 0; i < P25Defines.P25_PDU_CONFIRMED_DATA_LENGTH_BYTES; i++) + data[i] = buffer[i + 2]; // Payload Data + } + else { + for (uint i = 0; i < P25Defines.P25_PDU_CONFIRMED_DATA_LENGTH_BYTES; i++) + data[i] = buffer[i + 2]; // Payload Data + } + + // compute CRC-9 for the packet + ushort calculated = CRC.Crc9(buffer, 144); + if (((crc ^ calculated) != 0) && ((crc ^ calculated) != 0x1FFU)) + System.Diagnostics.Trace.WriteLine($"P25_DUID_PDU, fmt = {Format}, invalid crc = {crc} != {calculated}"); + } + else if ((Format == P25Defines.PDU_FMT_UNCONFIRMED) || (Format == P25Defines.PDU_FMT_RSP) || (Format == P25Defines.PDU_FMT_AMBT)) + { + // decode 1/2 rate Trellis + bool valid = trellis.Decode12(pduData, ref buffer); + if (!valid) + return false; + + data = new byte[P25Defines.P25_PDU_UNCONFIRMED_LENGTH_BYTES]; + for (uint i = 0; i < P25Defines.P25_PDU_UNCONFIRMED_LENGTH_BYTES; i++) + data[i] = buffer[i]; // Payload Data + } + else + return false; // unknown format value + + return true; + } + + /// + /// Encodes a P25 PDU data block. + /// + /// Buffer to encode data block to. + public void Encode(ref byte[] pduData) + { + if (pduData == null) + throw new NullReferenceException("pduData"); + + if (Format == P25Defines.PDU_FMT_CONFIRMED) + { + byte[] buffer = new byte[P25Defines.P25_PDU_CONFIRMED_LENGTH_BYTES]; + + buffer[0U] = (byte)((SerialNo << 1) & 0xFEU); // Confirmed Data Serial No. + + // if this is extended addressing and the first block decode the SAP and LLId + if (headerSap == P25Defines.PDU_SAP_EXT_ADDR && SerialNo == 0U) + { + buffer[5U] = (byte)(SAP & 0x3FU); // Service Access Point + + buffer[2U] = (byte)((LLId >> 16) & 0xFFU); // Logical Link ID + buffer[3U] = (byte)((LLId >> 8) & 0xFFU); + buffer[4U] = (byte)((LLId >> 0) & 0xFFU); + + for (uint i = 0; i < P25Defines.P25_PDU_CONFIRMED_DATA_LENGTH_BYTES - 4U; i++) + buffer[i + 6] = data[i]; + } + else + { + for (uint i = 0; i < P25Defines.P25_PDU_CONFIRMED_DATA_LENGTH_BYTES; i++) + buffer[i + 2] = data[i]; + } + + ushort crc = CRC.Crc9(buffer, 144); + buffer[0U] = (byte)(buffer[0U] + ((crc >> 8) & 0x01U)); // CRC-9 Check Sum (b8) + buffer[1U] = (byte)(crc & 0xFFU); // CRC-9 Check Sum (b0 - b7) + + trellis.Encode34(buffer, ref pduData); + } + else if (Format == P25Defines.PDU_FMT_UNCONFIRMED || Format == P25Defines.PDU_FMT_RSP || Format == P25Defines.PDU_FMT_AMBT) + { + byte[] buffer = new byte[P25Defines.P25_PDU_UNCONFIRMED_LENGTH_BYTES]; + for (uint i = 0; i < P25Defines.P25_PDU_UNCONFIRMED_LENGTH_BYTES - 4U; i++) + buffer[i] = data[i]; + + trellis.Encode12(buffer, ref pduData); + } + else + return; // unknown format value + } + + /// + /// Sets the raw data stored in the data block. + /// + /// + public void SetData(byte[] buffer) + { + if (buffer == null) + throw new NullReferenceException("buffer"); + + if (Format == P25Defines.PDU_FMT_CONFIRMED) + { + for (uint i = 0; i < P25Defines.P25_PDU_CONFIRMED_DATA_LENGTH_BYTES; i++) + data[i] = buffer[i]; + } + else if (Format == P25Defines.PDU_FMT_UNCONFIRMED || Format == P25Defines.PDU_FMT_RSP || Format == P25Defines.PDU_FMT_AMBT) + { + for (uint i = 0; i < P25Defines.P25_PDU_UNCONFIRMED_LENGTH_BYTES; i++) + data[i] = buffer[i]; + } + else + return; // unknown format value + } + + /// + /// Gets the raw data stored in the data block. + /// + /// + public uint GetData(ref byte[] buffer) + { + if (buffer == null) + throw new NullReferenceException("buffer"); + + if (Format == P25Defines.PDU_FMT_CONFIRMED) + { + for (uint i = 0; i < P25Defines.P25_PDU_CONFIRMED_DATA_LENGTH_BYTES; i++) + buffer[i] = data[i]; + return P25Defines.P25_PDU_CONFIRMED_DATA_LENGTH_BYTES; + } + else if (Format == P25Defines.PDU_FMT_UNCONFIRMED || Format == P25Defines.PDU_FMT_RSP || Format == P25Defines.PDU_FMT_AMBT) + { + for (uint i = 0; i < P25Defines.P25_PDU_UNCONFIRMED_LENGTH_BYTES; i++) + data[i] = buffer[i]; + return P25Defines.P25_PDU_UNCONFIRMED_LENGTH_BYTES; + } + else + return 0; // unknown format value + } + } // public class DataBlock +} // namespace fnecore.P25 diff --git a/P25/DataHeader.cs b/P25/DataHeader.cs new file mode 100644 index 0000000..d554f3b --- /dev/null +++ b/P25/DataHeader.cs @@ -0,0 +1,428 @@ +/** +* 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) 2022 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 fnecore.EDAC; + +namespace fnecore.P25 +{ + /// + /// Represents the data header for PDU P25 packets. + /// + public class DataHeader + { + private Trellis trellis; + private byte blocksToFollow; + private byte padCount; + private uint dataOctets; + + private byte[] data; + + /// + /// Flag indicating if acknowledgement is needed. + /// + public bool AckNeeded; + + /// + /// Flag indicating if this is an outbound data packet. + /// + public bool Outbound; + + /// + /// Data packet format. + /// + public byte Format; + + /// + /// Service access point. + /// + public byte SAP; + + /// + /// Manufacturer ID. + /// + public byte MFId; + + /// + /// Logical link ID. + /// + public uint LLId; + + /// + /// Flag indicating whether or not this data packet is a full message. + /// + /// When a response header, this represents the extended flag. + public bool FullMessage; + + /// + /// Synchronize Flag. + /// + public bool Synchronize; + + /// + /// Fragment Sequence Number. + /// + public byte FSN; + + /// + /// Send Sequence Number. + /// + public byte Ns; + + /// + /// Flag indicating whether or not this is the last fragment in a message. + /// + public bool LastFragment; + + /// + /// Offset of the header. + /// + public byte HeaderOffset; + + /** Response Data */ + /// + /// Source Logical link ID. + /// + public uint SrcLLId; + + /// + /// Response class. + /// + public byte ResponseClass; + + /// + /// Response type. + /// + public byte ResponseType; + + /// + /// Response status. + /// + public byte ResponseStatus; + + /** AMBT Data */ + /// + /// Alternate Trunking Block Opcode + /// + public byte AMBTOpcode; + + /// + /// Alternate Trunking Block Field 8 + /// + public byte AMBTField8; + + /// + /// Alternate Trunking Block Field 9 + /// + public byte AMBTField9; + + /// + /// Total number of blocks to follow this header. + /// + public byte BlocksToFollow + { + get { return blocksToFollow; } + set + { + this.blocksToFollow = value; + + // recalculate count of data octets + if (Format == P25Defines.PDU_FMT_CONFIRMED) + { + dataOctets = (uint)(16 * blocksToFollow - 4 - padCount); + } + else + { + dataOctets = (uint)(12 * blocksToFollow - 4 - padCount); + } + } + } + + /// + /// Count of block padding. + /// + public byte PadCount + { + get { return padCount; } + set + { + padCount = value; + + // recalculate count of data octets + if (Format == P25Defines.PDU_FMT_CONFIRMED) + { + dataOctets = (uint)(16 * blocksToFollow - 4 - padCount); + } + else + { + dataOctets = (uint)(12 * blocksToFollow - 4 - padCount); + } + } + } + + /* + ** Methods + */ + + /// + /// Initializes a new instance of the class. + /// + public DataHeader() + { + trellis = new Trellis(); + Reset(); + } + + /// + /// Decodes P25 PDU data header. + /// + /// + /// True, if PDU data header was decoded, otherwise false. + public bool Decode(byte[] pduData) + { + if (pduData == null) + throw new NullReferenceException("pduData"); + + // decode 1/2 rate Trellis & check CRC-CCITT 16 + bool valid = trellis.Decode12(pduData, ref data); + if (valid) + valid = CRC.CheckCCITT162(data, P25Defines.P25_PDU_HEADER_LENGTH_BYTES); + if (!valid) + { + return false; + } + + AckNeeded = (data[0U] & 0x40U) == 0x40U; // Acknowledge Needed + Outbound = (data[0U] & 0x20U) == 0x20U; // Inbound/Outbound + Format = (byte)(data[0U] & 0x1FU); // Packet Format + + SAP = (byte)(data[1U] & 0x3FU); // Service Access Point + + MFId = data[2U]; // Mfg Id. + + LLId = (uint)((data[3U] << 16) + (data[4U] << 8) + data[5U]); // Logical Link ID + + FullMessage = (data[6U] & 0x80U) == 0x80U; // Full Message Flag + blocksToFollow = (byte)(data[6U] & 0x7FU); // Block Frames to Follow + + padCount = (byte)(data[7U] & 0x1FU); // Pad Count + if (Format == P25Defines.PDU_FMT_RSP || Format == P25Defines.PDU_FMT_AMBT) { + padCount = 0; + } + + if (Format == P25Defines.PDU_FMT_CONFIRMED) { + dataOctets = (uint)(16 * blocksToFollow - 4 - padCount); + } + else { + dataOctets = (uint)(12 * blocksToFollow - 4 - padCount); + } + + switch (Format) + { + case P25Defines.PDU_FMT_CONFIRMED: + Synchronize = (data[8U] & 0x80U) == 0x80U; // Re-synchronize Flag + + Ns = (byte)((data[8U] >> 4) & 0x07U); // Packet Sequence No. + FSN = (byte)(data[8U] & 0x07U); // Fragment Sequence No. + LastFragment = (data[8U] & 0x08U) == 0x08U; // Last Fragment Flag + + HeaderOffset = (byte)(data[9U] & 0x3FU); // Data Header Offset + break; + case P25Defines.PDU_FMT_RSP: + AckNeeded = false; + SAP = P25Defines.PDU_SAP_USER_DATA; + ResponseClass = (byte)((data[1U] >> 6) & 0x03U); // Response Class + ResponseType = (byte)((data[1U] >> 3) & 0x07U); // Response Type + ResponseStatus = (byte)(data[1U] & 0x07U); // Response Status + if (!FullMessage) + { + SrcLLId = (uint)((data[7U] << 16) + (data[8U] << 8) + data[9U]); // Source Logical Link ID + } + break; + + case P25Defines.PDU_FMT_AMBT: + case P25Defines.PDU_FMT_UNCONFIRMED: + default: + if (Format == P25Defines.PDU_FMT_AMBT) + { + AMBTOpcode = (byte)(data[7U] & 0x3FU); // AMBT Opcode + AMBTField8 = data[8U]; // AMBT Field 8 + AMBTField9 = data[9U]; // AMBT Field 9 + } + + AckNeeded = false; + Synchronize = false; + + Ns = 0; + FSN = 0; + HeaderOffset = 0; + break; + } + + return true; + } + + /// + /// Encodes P25 PDU data header. + /// + /// + public void Encode(ref byte[] pduData) + { + if (pduData == null) + throw new NullReferenceException("pduData"); + + byte[] header = new byte[P25Defines.P25_PDU_HEADER_LENGTH_BYTES]; + + if (Format == P25Defines.PDU_FMT_UNCONFIRMED || Format == P25Defines.PDU_FMT_RSP) + AckNeeded = false; + + if (Format == P25Defines.PDU_FMT_CONFIRMED && !AckNeeded) + AckNeeded = true; // force set this to true + + header[0U] = (byte)((AckNeeded ? 0x40U : 0x00U) + // Acknowledge Needed + (Outbound ? 0x20U : 0x00U) + // Inbound/Outbound + (Format & 0x1FU)); // Packet Format + + header[1U] = (byte)(SAP & 0x3FU); // Service Access Point + header[1U] |= 0xC0; + + header[2U] = MFId; // Mfg Id. + + header[3U] = (byte)((LLId >> 16) & 0xFFU); // Logical Link ID + header[4U] = (byte)((LLId >> 8) & 0xFFU); + header[5U] = (byte)((LLId >> 0) & 0xFFU); + + header[6U] = (byte)((FullMessage ? 0x80U : 0x00U) + // Full Message Flag + (blocksToFollow & 0x7FU)); // Blocks Frames to Follow + + switch (Format) + { + case P25Defines.PDU_FMT_CONFIRMED: + header[7U] = (byte)(padCount & 0x1FU); // Pad Count + header[8U] = (byte)((Synchronize ? 0x80U : 0x00U) + // Re-synchronize Flag + ((Ns & 0x07U) << 4) + // Packet Sequence No. + (LastFragment ? 0x08U : 0x00U) + // Last Fragment Flag + (FSN & 0x07)); // Fragment Sequence No. + + header[9U] = (byte)(HeaderOffset & 0x3FU); // Data Header Offset + break; + case P25Defines.PDU_FMT_RSP: + header[1U] = (byte)(((ResponseClass & 0x03U) << 6) + // Response Class + ((ResponseType & 0x07U) << 3) + // Response Type + ((ResponseStatus & 0x07U))); // Response Status + if (!FullMessage) + { + header[7U] = (byte)((SrcLLId >> 16) & 0xFFU); // Source Logical Link ID + header[8U] = (byte)((SrcLLId >> 8) & 0xFFU); + header[9U] = (byte)((SrcLLId >> 0) & 0xFFU); + } + break; + case P25Defines.PDU_FMT_AMBT: + header[7U] = (byte)(AMBTOpcode & 0x3FU); // AMBT Opcode + header[8U] = AMBTField8; // AMBT Field 8 + header[9U] = AMBTField9; // AMBT Field 9 + break; + case P25Defines.PDU_FMT_UNCONFIRMED: + default: + header[7U] = (byte)(padCount & 0x1FU); // Pad Count + header[8U] = 0x00; + header[9U] = (byte)(HeaderOffset & 0x3FU); // Data Header Offset + break; + } + + // compute CRC-CCITT 16 + CRC.AddCCITT162(ref header, P25Defines.P25_PDU_HEADER_LENGTH_BYTES); + + // encode 1/2 rate Trellis + trellis.Encode12(header, ref pduData); + } + + /// + /// Helper to reset data values to defaults. + /// + public void Reset() + { + AckNeeded = false; + Outbound = false; + + Format = P25Defines.PDU_FMT_CONFIRMED; + + SAP = 0; + MFId = P25Defines.P25_MFG_STANDARD; + LLId = 0; + + FullMessage = true; + blocksToFollow = 0; + padCount = 0; + + dataOctets = 0; + + Synchronize = false; + + Ns = 0; + FSN = 0; + LastFragment = true; + + HeaderOffset = 0; + + SrcLLId = 0; + ResponseClass = P25Defines.PDU_ACK_CLASS_NACK; + ResponseType = P25Defines.PDU_ACK_TYPE_NACK_ILLEGAL; + ResponseStatus = 0; + + AMBTOpcode = 0; + AMBTField8 = 0; + AMBTField9 = 0; + + data = new byte[P25Defines.P25_PDU_HEADER_LENGTH_BYTES]; + } + + /// + /// Gets the total number of data octets. + /// + /// + public uint GetDataOctets() + { + return dataOctets; + } + + /// + /// Gets the raw header data. + /// + /// + public uint GetData(ref byte[] buffer) + { + if (buffer == null) + throw new NullReferenceException("buffer"); + + for (uint i = 0; i < P25Defines.P25_PDU_HEADER_LENGTH_BYTES; i++) + buffer[i] = data[i]; + + return P25Defines.P25_PDU_HEADER_LENGTH_BYTES; + } + } // public class DataHeader +} // namespace fnecore.P25 diff --git a/P25/P25Defines.cs b/P25/P25Defines.cs index 18e7d03..366c169 100644 --- a/P25/P25Defines.cs +++ b/P25/P25Defines.cs @@ -137,6 +137,54 @@ namespace fnecore.P25 public const byte P25_MI_LENGTH = 9; + public const byte P25_PDU_HEADER_LENGTH_BYTES = 12; + public const byte P25_PDU_CONFIRMED_LENGTH_BYTES = 18; + public const byte P25_PDU_CONFIRMED_DATA_LENGTH_BYTES = 16; + public const byte P25_PDU_UNCONFIRMED_LENGTH_BYTES = 12; + + // PDU Format Type(s) + public const byte PDU_FMT_RSP = 0x03; + public const byte PDU_FMT_UNCONFIRMED = 0x15; + public const byte PDU_FMT_CONFIRMED = 0x16; + public const byte PDU_FMT_AMBT = 0x17; + + // PDU SAP + public const byte PDU_SAP_USER_DATA = 0x00; + public const byte PDU_SAP_ENC_USER_DATA = 0x01; + + public const byte PDU_SAP_PACKET_DATA = 0x04; + + public const byte PDU_SAP_ARP = 0x05; + + public const byte PDU_SAP_SNDCP_CTRL_DATA = 0x06; + + public const byte PDU_SAP_EXT_ADDR = 0x1F; + + public const byte PDU_SAP_REG = 0x20; + + public const byte PDU_SAP_UNENC_KMM = 0x28; + public const byte PDU_SAP_ENC_KMM = 0x29; + + public const byte PDU_SAP_TRUNK_CTRL = 0x3D; + + // PDU ACK Class + public const byte PDU_ACK_CLASS_ACK = 0x00; + public const byte PDU_ACK_CLASS_NACK = 0x01; + public const byte PDU_ACK_CLASS_ACK_RETRY = 0x02; + + // PDU ACK Type(s) + public const byte PDU_ACK_TYPE_RETRY = 0x00; + + public const byte PDU_ACK_TYPE_ACK = 0x01; + + public const byte PDU_ACK_TYPE_NACK_ILLEGAL = 0x00; // Illegal Format + public const byte PDU_ACK_TYPE_NACK_PACKET_CRC = 0x01; // Packet CRC + public const byte PDU_ACK_TYPE_NACK_MEMORY_FULL = 0x02; // Memory Full + public const byte PDU_ACK_TYPE_NACK_SEQ = 0x03; // Out of logical sequence FSN + public const byte PDU_ACK_TYPE_NACK_UNDELIVERABLE = 0x04;// Undeliverable + public const byte PDU_ACK_TYPE_NACK_OUT_OF_SEQ = 0x05; // Out of sequence, N(S) != V(R) or V(R) + 1 + public const byte PDU_ACK_TYPE_NACK_INVL_USER = 0x06; // Invalid User disallowed by the system + // LDUx/TDULC Link Control Opcode(s) public const byte LC_GROUP = 0x00; // GRP VCH USER - Group Voice Channel User public const byte LC_GROUP_UPDT = 0x02; // GRP VCH UPDT - Group Voice Channel Update