diff --git a/P25/P25Crypto.cs b/P25/P25Crypto.cs index f491251..ad20e8b 100644 --- a/P25/P25Crypto.cs +++ b/P25/P25Crypto.cs @@ -28,15 +28,13 @@ namespace fnecore.P25 private byte algId; private ushort keyId; + private byte[] keystream; private byte[] messageIndicator = new byte[9]; - private Dictionary keys = new Dictionary(); - - private byte[] aesKeystream = new byte[240]; // AES buffer - private byte[] adpKeystream = new byte[469]; // ADP buffer - private int ksPosition; + private KeyInfo currentKey; + /* ** Class */ @@ -91,7 +89,7 @@ namespace fnecore.P25 /// public void Reset() { - keys.Clear(); + currentKey = null; } /// @@ -100,12 +98,14 @@ namespace fnecore.P25 /// /// /// - public void AddKey(ushort keyid, byte algid, byte[] key) + public void SetKey(ushort keyid, byte algid, byte[] key) { if (keyid == 0 || algid == 0x80) return; - keys[keyid] = new KeyInfo(algid, key); + this.keyId = keyid; + this.algId = algid; + this.currentKey = new KeyInfo(algid, key); } /// @@ -113,9 +113,9 @@ namespace fnecore.P25 /// /// /// - public bool HasKey(ushort keyId) + public bool HasKey() { - return keys.ContainsKey(keyId); + return currentKey != null; } /// @@ -133,18 +133,20 @@ namespace fnecore.P25 Array.Copy(MI, this.messageIndicator, Math.Min(MI.Length, this.messageIndicator.Length)); - if (!keys.ContainsKey(keyid)) + if (currentKey == null) return false; this.ksPosition = 0; if (algid == P25Defines.P25_ALGO_AES) { + keystream = new byte[240]; GenerateAESKeystream(); return true; } else if (algid == P25Defines.P25_ALGO_ARC4) { + keystream = new byte[469]; GenerateARC4Keystream(); return true; } @@ -160,7 +162,7 @@ namespace fnecore.P25 /// public bool Process(byte[] imbe, P25DUID duid) { - if (!keys.ContainsKey(keyId)) + if (currentKey == null) return false; return algId switch @@ -193,10 +195,10 @@ namespace fnecore.P25 byte[] permutation = new byte[256]; byte[] key = new byte[256]; - if (!keys.ContainsKey(keyId)) + if (currentKey == null) return; - byte[] keyData = keys[keyId].Key; + byte[] keyData = currentKey.Key; int keySize = keyData.Length; int padding = Math.Max(5 - keySize, 0); @@ -239,7 +241,7 @@ namespace fnecore.P25 Swap(permutation, i, j); // transform byte - adpKeystream[k] = permutation[(permutation[i] + permutation[j]) & 0xFF]; + keystream[k] = permutation[(permutation[i] + permutation[j]) & 0xFF]; } } @@ -248,10 +250,10 @@ namespace fnecore.P25 /// private void GenerateAESKeystream() { - if (!keys.ContainsKey(keyId)) + if (currentKey == null) return; - byte[] key = keys[keyId].Key; + byte[] key = currentKey.Key; byte[] iv = ExpandMIToIV(messageIndicator); using (var aes = Aes.Create()) @@ -268,10 +270,10 @@ namespace fnecore.P25 Array.Copy(iv, input, 16); byte[] output = new byte[16]; - for (int i = 0; i < aesKeystream.Length / 16; i++) + for (int i = 0; i < keystream.Length / 16; i++) { encryptor.TransformBlock(input, 0, 16, output, 0); - Buffer.BlockCopy(output, 0, aesKeystream, i * 16, 16); + Buffer.BlockCopy(output, 0, keystream, i * 16, 16); Array.Copy(output, input, 16); } } @@ -294,7 +296,7 @@ namespace fnecore.P25 ksPosition = (ksPosition + 1) % 9; for (int j = 0; j < IMBE_BUF_LEN; ++j) - imbe[j] ^= aesKeystream[j + offset]; + imbe[j] ^= keystream[j + offset]; return true; } @@ -318,7 +320,7 @@ namespace fnecore.P25 ksPosition = (ksPosition + 1) % 9; for (int j = 0; j < IMBE_BUF_LEN; ++j) - imbe[j] ^= adpKeystream[j + offset]; + imbe[j] ^= keystream[j + offset]; return true; } diff --git a/P25/P25Defines.cs b/P25/P25Defines.cs index 12963f1..65abbee 100644 --- a/P25/P25Defines.cs +++ b/P25/P25Defines.cs @@ -120,6 +120,29 @@ namespace fnecore.P25 TDULC = 0x0F } // public enum P25DUID : byte + /// + /// Extended Function types for EXT_FNCT TSBK + /// + public enum ExtendedFunction : ushort + { + CHECK = 0x0000, // Radio Check + UNINHIBIT = 0x007E, // Radio Uninhibit + INHIBIT = 0x007F, // Radio Inhibit + CHECK_ACK = 0x0080, // Radio Check Ack + UNINHIBIT_ACK = 0x00FE, // Radio Uninhibit Ack + INHIBIT_ACK = 0x00FF, // Radio Inhibit Ack + + DYN_REGRP_REQ = 0x0200, // MFID $90 (Motorola) Dynamic Regroup IR + DYN_REGRP_CANCEL = 0x0201, // MFID $90 (Motorola) Dynamic Regroup IR Cancellation + DYN_REGRP_LOCK = 0x0202, // MFID $90 (Motorola) Lock Selector + DYN_REGRP_UNLOCK = 0x0203, // MFID $90 (Motorola) Unlock Selector + + DYN_REGRP_REQ_ACK = 0x0280, // MFID $90 (Motorola) Dynamic Regroup IR Ack + DYN_REGRP_CANCEL_ACK = 0x0281, // MFID $90 (Motorola) Dynamic Regroup IR Cancellation Ack + DYN_REGRP_LOCK_ACK = 0x0282, // MFID $90 (Motorola) Lock Selector Ack + DYN_REGRP_UNLOCK_ACK = 0x0283 // MFID $90 (Motorola) Unlock Selector Ack + } // public enum ExtendedFunction : ushort + /// /// P25 Constants /// diff --git a/P25/lc/tsbk/IOSP_EXT_FNCT.cs b/P25/lc/tsbk/IOSP_EXT_FNCT.cs new file mode 100644 index 0000000..88db2cd --- /dev/null +++ b/P25/lc/tsbk/IOSP_EXT_FNCT.cs @@ -0,0 +1,84 @@ +// SPDX-License-Identifier: AGPL-3.0-only +/** +* Digital Voice Modem - Fixed Network Equipment Core Library +* 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 Core Library +* @license AGPLv3 License (https://opensource.org/licenses/AGPL-3.0) +* +* Copyright (C) 2025 Caleb, K4PHP +* +*/ + +namespace fnecore.P25.LC.TSBK +{ + /// + /// IOSP_EXT_FNCT TSBK + /// + public class IOSP_EXT_FNCT : TSBKBase + { + public ushort ExtendedFunction { get; set; } + public uint SrcId { get; set; } + public uint DstId { get; set; } + + /// + /// Creates an instance of + /// + /// + /// + /// + public IOSP_EXT_FNCT(ushort extendedFunction = 0, uint srcId = 0, uint dstId = 0) + { + ExtendedFunction = extendedFunction; + SrcId = srcId; + DstId = dstId; + Lco = P25Defines.TSBK_IOSP_EXT_FNCT; + } + + /// + /// Decode EXT_FNCT 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); + + ExtendedFunction = (ushort)((tsbkValue >> 48) & 0xFFFF); // Extended Function + SrcId = (uint)((tsbkValue >> 24) & 0xFFFFFF); // Argument + DstId = (uint)(tsbkValue & 0xFFFFFF); // Target Radio Address + + return true; + } + + /// + /// Encode EXT_FNCT TSBK + /// + /// + /// + /// + /// + public override void Encode(ref byte[] data, ref byte[] payload, bool rawTSBK, bool noTrellis) + { + ulong tsbkValue = 0; + + tsbkValue = (tsbkValue << 16) + ExtendedFunction; // Extended Function + tsbkValue = (tsbkValue << 24) + SrcId; // Argument + tsbkValue = (tsbkValue << 24) + DstId; // Target 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_EXT_FNCT +} // namespace fnecore.P25.LC.TSBK