Digital_Voice_Modem_Project
/
fnecore
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add support code to assemble P25 PDU data headers and blocks;

Browse Source
pull/1/head
Bryan Biedenkapp 2 years ago
parent 4ae905cd26
commit 900a43b932
4 changed files with 763 additions and 38 deletions
Show Stats Download Patch File Download Diff File

76
EDAC/Trellis.cs
Unescape View File

@ -76,7 +76,7 @@ namespace fnecore.EDAC
/// <param name="data">Trellis symbol bytes.</param>
/// <param name="payload">Output bytes.</param>
/// <returns>True, if decoded, otherwise false.</returns>
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);
}
/// <summary>
@ -117,7 +117,7 @@ namespace fnecore.EDAC
/// </summary>
/// <param name="payload">Input bytes.</param>
/// <param name="data">Trellis symbol bytes.</param>
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);
}
/// <summary>
@ -151,7 +151,7 @@ namespace fnecore.EDAC
/// <param name="data">Trellis symbol bytes.</param>
/// <param name="payload">Output bytes.</param>
/// <returns>True, if decoded, otherwise false.</returns>
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);
}
/// <summary>
@ -193,7 +193,7 @@ namespace fnecore.EDAC
/// </summary>
/// <param name="payload">Input bytes.</param>
/// <param name="data">Trellis symbol bytes.</param>
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);
}
/// <summary>
@ -226,7 +226,7 @@ namespace fnecore.EDAC
/// </summary>
/// <param name="data">Trellis symbol bytes.</param>
/// <param name="dibits">Dibits.</param>
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
/// </summary>
/// <param name="dibits">Dibits.</param>
/// <param name="data">Trellis symbol bytes.</param>
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
/// </summary>
/// <param name="dibits">Dibits.</param>
/// <param name="points">4FSK constellation points.</param>
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
/// </summary>
/// <param name="points">4FSK Constellation points.</param>
/// <param name="dibits">Dibits.</param>
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
/// </summary>
/// <param name="payload">Byte payload.</param>
/// <param name="tribits">Tribits.</param>
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
/// </summary>
/// <param name="payload">Byte payload.</param>
/// <param name="dibits">Dibits.</param>
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
/// </summary>
/// <param name="tribits">Tribits.</param>
/// <param name="payload">Byte payload.</param>
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
/// </summary>
/// <param name="dibits">Dibits.</param>
/// <param name="payload">Byte payload.</param>
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
/// <param name="failPos"></param>
/// <param name="payload">Byte payload.</param>
/// <returns>True, if error corrected, otherwise false.</returns>
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
/// <param name="points">4FSK constellation points.</param>
/// <param name="tribits">Tribits.</param>
/// <returns></returns>
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
/// <param name="failPos"></param>
/// <param name="payload">Byte payload.</param>
/// <returns>True, if error corrected, otherwise false.</returns>
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
/// <param name="points">4FSK constellation points.</param>
/// <param name="dibits">Dibits.</param>
/// <returns></returns>
private uint checkCode12(byte[] points, ref byte[] dibits)
private uint CheckCode12(byte[] points, ref byte[] dibits)
{
byte state = 0;

249
P25/DataBlock.cs
Unescape View File

@ -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
{
/// <summary>
/// Represents the data block for PDU P25 packets.
/// </summary>
public class DataBlock
{
private Trellis trellis;
private byte headerSap;
private byte[] data;
/// <summary>Sets the data block serial number.</summary>
public byte SerialNo;
/// <summary>Flag indicating this is the last block in a sequence of block.</summary>
public bool LastBlock;
/// <summary>Logical link ID.</summary>
public uint LLId;
/// <summary>Service access point.</summary>
public byte SAP;
/// <summary>
/// Data format.
/// </summary>
public byte Format;
/*
** Methods
*/
/// <summary>
/// Initializes a new instance of the <see cref="DataBlock"/> class.
/// </summary>
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];
}
/// <summary>
/// Decodes P25 PDU data block.
/// </summary>
/// <param name="pduData"></param>
/// <param name="header">Instance of the DataHeader class.</param>
/// <returns>True, if data block was decoded, otherwise false.</returns>
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;
}
/// <summary>
/// Encodes a P25 PDU data block.
/// </summary>
/// <param name="pduData">Buffer to encode data block to.</param>
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
}
/// <summary>
/// Sets the raw data stored in the data block.
/// </summary>
/// <param name="buffer"></param>
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
}
/// <summary>
/// Gets the raw data stored in the data block.
/// </summary>
/// <returns></returns>
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

428
P25/DataHeader.cs
Unescape View File

@ -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
{
/// <summary>
/// Represents the data header for PDU P25 packets.
/// </summary>
public class DataHeader
{
private Trellis trellis;
private byte blocksToFollow;
private byte padCount;
private uint dataOctets;
private byte[] data;
/// <summary>
/// Flag indicating if acknowledgement is needed.
/// </summary>
public bool AckNeeded;
/// <summary>
/// Flag indicating if this is an outbound data packet.
/// </summary>
public bool Outbound;
/// <summary>
/// Data packet format.
/// </summary>
public byte Format;
/// <summary>
/// Service access point.
/// </summary>
public byte SAP;
/// <summary>
/// Manufacturer ID.
/// </summary>
public byte MFId;
/// <summary>
/// Logical link ID.
/// </summary>
public uint LLId;
/// <summary>
/// Flag indicating whether or not this data packet is a full message.
/// </summary>
/// <remarks>When a response header, this represents the extended flag.</summary>
public bool FullMessage;
/// <summary>
/// Synchronize Flag.
/// </summary>
public bool Synchronize;
/// <summary>
/// Fragment Sequence Number.
/// </summary>
public byte FSN;
/// <summary>
/// Send Sequence Number.
/// </summary>
public byte Ns;
/// <summary>
/// Flag indicating whether or not this is the last fragment in a message.
/// </summary>
public bool LastFragment;
/// <summary>
/// Offset of the header.
/// </summary>
public byte HeaderOffset;
/** Response Data */
/// <summary>
/// Source Logical link ID.
/// </summary>
public uint SrcLLId;
/// <summary>
/// Response class.
/// </summary>
public byte ResponseClass;
/// <summary>
/// Response type.
/// </summary>
public byte ResponseType;
/// <summary>
/// Response status.
/// </summary>
public byte ResponseStatus;
/** AMBT Data */
/// <summary>
/// Alternate Trunking Block Opcode
/// </summary>
public byte AMBTOpcode;
/// <summary>
/// Alternate Trunking Block Field 8
/// </summary>
public byte AMBTField8;
/// <summary>
/// Alternate Trunking Block Field 9
/// </summary>
public byte AMBTField9;
/// <summary>
/// Total number of blocks to follow this header.
/// </summary>
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);
}
}
}
/// <summary>
/// Count of block padding.
/// </summary>
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
*/
/// <summary>
/// Initializes a new instance of the <see cref="DataHeader"/> class.
/// </summary>
public DataHeader()
{
trellis = new Trellis();
Reset();
}
/// <summary>
/// Decodes P25 PDU data header.
/// </summary>
/// <param name="pduData"></param>
/// <returns>True, if PDU data header was decoded, otherwise false.</returns>
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;
}
/// <summary>
/// Encodes P25 PDU data header.
/// </summary>
/// <param name="pduData"></param>
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);
}
/// <summary>
/// Helper to reset data values to defaults.
/// </summary>
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];
}
/// <summary>
/// Gets the total number of data octets.
/// </summary>
/// <returns></returns>
public uint GetDataOctets()
{
return dataOctets;
}
/// <summary>
/// Gets the raw header data.
/// </summary>
/// <returns></returns>
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

48
P25/P25Defines.cs
Unescape View File

@ -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

Write Preview
Loading…
Cancel
Save

Powered by TurnKey Linux.