From d9995b871221b3b9af5c2bda360143d022060ce3 Mon Sep 17 00:00:00 2001 From: php Date: Sun, 3 Nov 2024 04:44:33 -0600 Subject: [PATCH] Add initial support for TSBK encode/decode --- P25/lc/TSBKBase.cs | 130 ++++++++++++++++++++++++++++++++++ P25/lc/tsbk/IOSP_ACK_RSP.cs | 91 ++++++++++++++++++++++++ P25/lc/tsbk/IOSP_CALL_ALRT.cs | 63 ++++++++++++++++ 3 files changed, 284 insertions(+) create mode 100644 P25/lc/TSBKBase.cs create mode 100644 P25/lc/tsbk/IOSP_ACK_RSP.cs create mode 100644 P25/lc/tsbk/IOSP_CALL_ALRT.cs diff --git a/P25/lc/TSBKBase.cs b/P25/lc/TSBKBase.cs new file mode 100644 index 0000000..ff7a9d2 --- /dev/null +++ b/P25/lc/TSBKBase.cs @@ -0,0 +1,130 @@ +using System; + +using fnecore.EDAC; + +namespace fnecore.P25.LC +{ + /// + /// Base TSBK Encode/Decode class + /// + public abstract class TSBKBase + { + protected bool LastBlock; + protected byte Lco; + protected byte MfId; + + /// + /// Creates an instance of + /// + protected TSBKBase() + { + MfId = P25Defines.P25_MFG_STANDARD; + } + + /// + /// Decode a TSBK + /// + /// + /// + /// + /// + public virtual bool Decode(byte[] data, ref byte[] payload, bool rawTSBK) + { + byte[] tsbk = new byte[P25Defines.P25_TSBK_LENGTH_BYTES]; + FneUtils.Memset(tsbk, 0x00, tsbk.Length); + + if (rawTSBK) + { + Array.Copy(data, tsbk, P25Defines.P25_TSBK_LENGTH_BYTES); + + if (!CRC.CheckCCITT162(tsbk, P25Defines.P25_TSBK_LENGTH_BYTES)) + { + if ((tsbk[P25Defines.P25_TSBK_LENGTH_BYTES - 2U] != 0x00U) && (tsbk[P25Defines.P25_TSBK_LENGTH_BYTES - 1U] != 0x00U)) + { + Console.WriteLine("TSBK failed CRC CCITT-162 check"); + return false; + } + } + } + else + { + byte[] raw = new byte[P25Defines.P25_TSBK_FEC_LENGTH_BYTES]; + P25Interleaver.Decode(data, ref raw, 114, 318); + + EDAC.Trellis trellis = new EDAC.Trellis(); + if (!trellis.Decode12(raw, ref tsbk)) + { + Console.WriteLine("TSBK Failed Trellis decode"); + return false; + } + + if (!CRC.CheckCCITT162(tsbk, P25Defines.P25_TSBK_LENGTH_BYTES)) + { + Console.WriteLine("TSBK Failed CRC check after Trellis"); + return false; + } + } + + Lco = (byte)(tsbk[0] & 0x3F); // LCO + LastBlock = (tsbk[0] & 0x80) == 0x80; // Last Block Marker + MfId = tsbk[1]; // Manufacturer ID + + Array.Copy(tsbk, 1, payload, 0, P25Defines.P25_TSBK_LENGTH_BYTES - 4); + + return true; + } + + /// + /// Encode a TSBK + /// + /// + /// + /// + /// + public virtual void Encode(ref byte[] data, ref byte[] payload, bool rawTSBK, bool noTrellis) + { + byte[] tsbk = new byte[P25Defines.P25_TSBK_LENGTH_BYTES]; + + FneUtils.Memset(tsbk, 0x00, tsbk.Length); + + Array.Copy(payload, 0, tsbk, 2, P25Defines.P25_TSBK_LENGTH_BYTES - 4); + + tsbk[0] = Lco; // LCO + tsbk[0] |= LastBlock ? (byte)0x80 : (byte)0x00; // Last Block Marker + tsbk[1] = MfId; // Manufacturer ID + + CRC.AddCCITT162(ref tsbk, P25Defines.P25_TSBK_LENGTH_BYTES); + + byte[] raw = new byte[P25Defines.P25_TSBK_FEC_LENGTH_BYTES]; + FneUtils.Memset(raw, 0x00, raw.Length); + + EDAC.Trellis trellis = new EDAC.Trellis(); + trellis.Encode12(tsbk, ref raw); + + if (rawTSBK) + { + if (noTrellis) + { + Array.Copy(tsbk, 0, data, 0, P25Defines.P25_TSBK_LENGTH_BYTES); + } + else + { + Array.Copy(raw, 0, data, 0, P25Defines.P25_TSBK_FEC_LENGTH_BYTES); + } + } + else + { + P25Interleaver.Encode(raw, ref data, 114, 318); + } + } + + /// + /// + /// + /// + public override string ToString() + { + return "UNKNOWN TSBK"; + } + } // public abstract class TSBKBase +} // namespace fnecore.P25.LC diff --git a/P25/lc/tsbk/IOSP_ACK_RSP.cs b/P25/lc/tsbk/IOSP_ACK_RSP.cs new file mode 100644 index 0000000..a35403a --- /dev/null +++ b/P25/lc/tsbk/IOSP_ACK_RSP.cs @@ -0,0 +1,91 @@ +namespace fnecore.P25.LC.TSBK +{ + /// + /// IOSP_ACK_RSP TSBK + /// + public class IOSP_ACK_RSP : TSBKBase + { + public uint DstId; + public uint SrcId; + public uint SysId; + public uint Wacn; + public bool Aiv; + public bool ExtendedAddr; + public byte Service; + + /// + /// Creates an instance of + /// + /// + /// + /// + /// + public IOSP_ACK_RSP(uint dstId, uint srcId, bool aivFlag, byte service) + { + DstId = dstId; + SrcId = srcId; + Aiv = aivFlag; + Service = service; + Lco = P25Defines.TSBK_IOSP_ACK_RSP; + } + + /// + /// Decode ACK_RSP TSBK + /// + /// + /// + /// + public bool Decode(byte[] data, bool rawTSBK) + { + byte[] tsbk = new byte[P25Defines.P25_TSBK_LENGTH_BYTES]; + FneUtils.Memset(tsbk, 0x00, tsbk.Length); + + bool ret = base.Decode(data, ref tsbk, rawTSBK); + if (!ret) + return false; + + ulong tsbkValue = FneUtils.ToUInt64(tsbk, 0); + + Aiv = ((tsbkValue >> 56) & 0x80U) == 0x80U; // Additional Info Flag + Service = (byte)((tsbkValue >> 56) & 0x3FU); // Service Type + + DstId = FneUtils.Bytes3ToUInt32(tsbk, 3); // Target Radio Address + SrcId = FneUtils.Bytes3ToUInt32(tsbk, 0); // Source Radio Address + + return true; + } + + /// + /// Encode ACK_RSP TSBK + /// + /// + /// + /// + /// + public override void Encode(ref byte[] data, ref byte[] payload, bool rawTSBK, bool noTrellis) + { + ulong tsbkValue = 0; + + tsbkValue = Service & 0x3FU; // Service Type + tsbkValue |= Aiv ? 0x80UL : 0x00UL; // Additional Info Flag + tsbkValue |= ExtendedAddr ? 0x40UL : 0x00UL; // Extended Addressing Flag + + if (Aiv && ExtendedAddr) + { + tsbkValue = (tsbkValue << 20) + Wacn; // Network ID + tsbkValue = (tsbkValue << 12) + SysId; // System ID + } + else + { + tsbkValue = (tsbkValue << 32) + DstId; // Target Radio Address + } + + tsbkValue = (tsbkValue << 24) + SrcId; // Source Radio Address + + FneUtils.Memset(payload, 0x00, payload.Length); + FneUtils.WriteBytes(tsbkValue, ref payload, 0); + + base.Encode(ref data, ref payload, rawTSBK, noTrellis); + } + } // public class IOSP_ACK_RSP +} // namespace fnecore.P25.LC.TSBK diff --git a/P25/lc/tsbk/IOSP_CALL_ALRT.cs b/P25/lc/tsbk/IOSP_CALL_ALRT.cs new file mode 100644 index 0000000..36caa51 --- /dev/null +++ b/P25/lc/tsbk/IOSP_CALL_ALRT.cs @@ -0,0 +1,63 @@ +namespace fnecore.P25.LC.TSBK +{ + /// + /// IOSP_CALL_ALRT TSBK + /// + public class IOSP_CALL_ALRT : TSBKBase + { + public uint DstId; + public uint SrcId; + + /// + /// Creates an instance of + /// + /// + /// + public IOSP_CALL_ALRT(uint dstId, uint srcId) + { + DstId = dstId; + SrcId = srcId; + Lco = P25Defines.TSBK_IOSP_CALL_ALRT; + } + + /// + /// Decode CALL_ALRT TSBK + /// + /// + /// + /// + public bool Decode(byte[] data, bool rawTSBK) + { + byte[] tsbk = new byte[P25Defines.P25_TSBK_LENGTH_BYTES]; + FneUtils.Memset(tsbk, 0x00, tsbk.Length); + + bool ret = base.Decode(data, ref tsbk, rawTSBK); + if (!ret) + return false; + + DstId = FneUtils.Bytes3ToUInt32(tsbk, 3); // Target Radio Address + SrcId = FneUtils.Bytes3ToUInt32(tsbk, 0); // Source Radio Address + + return true; + } + + /// + /// Encode CALL_ALRT TSBK + /// + /// + /// + /// + /// + public override void Encode(ref byte[] data, ref byte[] payload, bool rawTSBK, bool noTrellis) + { + ulong tsbkValue = 0; + tsbkValue = (tsbkValue << 40) + DstId; // Target Radio Address + tsbkValue = (tsbkValue << 24) + SrcId; // Source Radio Address + + FneUtils.Memset(payload, 0x00, payload.Length); + FneUtils.WriteBytes(tsbkValue, ref payload, 0); + + base.Encode(ref data, ref payload, rawTSBK, noTrellis); + } + } // public class IOSP_CALL_ALRT +} // namespace fnecore.P25.LC.TSBK