From 1e1f6379f62286412281d61dec08641e61c34949 Mon Sep 17 00:00:00 2001 From: firealarmss Date: Thu, 20 Mar 2025 00:05:38 -0500 Subject: [PATCH] move P25Crypto to fnecore --- P25/P25Crypto.cs | 422 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 422 insertions(+) create mode 100644 P25/P25Crypto.cs diff --git a/P25/P25Crypto.cs b/P25/P25Crypto.cs new file mode 100644 index 0000000..f491251 --- /dev/null +++ b/P25/P25Crypto.cs @@ -0,0 +1,422 @@ +// 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 +* +*/ + +using System.Security.Cryptography; +using System.Collections.Generic; +using System.Linq; +using System; + +namespace fnecore.P25 +{ + /// + /// + /// + public class P25Crypto + { + public const int IMBE_BUF_LEN = 11; + + private byte algId; + private ushort keyId; + + 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; + + /* + ** Class + */ + + /// + /// + /// + private class KeyInfo + { + /* + ** Properties + */ + + /// + /// + /// + public byte AlgId { get; } + /// + /// + /// + public byte[] Key { get; } + + /// + /// Initializes a new instance of the class. + /// + /// + /// + public KeyInfo(byte algid, byte[] key) + { + AlgId = algid; + Key = key; + } + } // private class KeyInfo + + /* + ** Methods + */ + + /// + /// Initializes a new instance of the class. + /// + public P25Crypto() + { + this.algId = P25Defines.P25_ALGO_UNENCRYPT; + this.keyId = 0; + + this.ksPosition = 0; + } + + /// + /// + /// + public void Reset() + { + keys.Clear(); + } + + /// + /// + /// + /// + /// + /// + public void AddKey(ushort keyid, byte algid, byte[] key) + { + if (keyid == 0 || algid == 0x80) + return; + + keys[keyid] = new KeyInfo(algid, key); + } + + /// + /// + /// + /// + /// + public bool HasKey(ushort keyId) + { + return keys.ContainsKey(keyId); + } + + /// + /// + /// + /// + /// + /// + /// + /// + public bool Prepare(byte algid, ushort keyid, byte[] MI) + { + this.algId = algid; + this.keyId = keyid; + + Array.Copy(MI, this.messageIndicator, Math.Min(MI.Length, this.messageIndicator.Length)); + + if (!keys.ContainsKey(keyid)) + return false; + + this.ksPosition = 0; + + if (algid == P25Defines.P25_ALGO_AES) + { + GenerateAESKeystream(); + return true; + } + else if (algid == P25Defines.P25_ALGO_ARC4) + { + GenerateARC4Keystream(); + return true; + } + + return false; + } + + /// + /// + /// + /// + /// + /// + public bool Process(byte[] imbe, P25DUID duid) + { + if (!keys.ContainsKey(keyId)) + return false; + + return algId switch + { + P25Defines.P25_ALGO_AES => AESProcess(imbe, duid), + P25Defines.P25_ALGO_ARC4 => ARC4Process(imbe, duid), + _ => false + }; + } + + /// + /// + /// + /// + /// + /// + private void Swap(byte[] a, int i1, int i2) + { + byte temp = a[i1]; + a[i1] = a[i2]; + a[i2] = temp; + } + + /// + /// Create ARC4 keystream. + /// + private void GenerateARC4Keystream() + { + byte[] adpKey = new byte[13]; + byte[] permutation = new byte[256]; + byte[] key = new byte[256]; + + if (!keys.ContainsKey(keyId)) + return; + + byte[] keyData = keys[keyId].Key; + + int keySize = keyData.Length; + int padding = Math.Max(5 - keySize, 0); + int i, j = 0, k; + + for (i = 0; i < padding; i++) + adpKey[i] = 0; + + for (; i < 5; i++) + adpKey[i] = keySize > 0 ? keyData[i - padding] : (byte)0; + + for (i = 5; i < 13; ++i) + { + adpKey[i] = messageIndicator[i - 5]; + } + + // generate ARC4 keystream + // initialize state variable + for (i = 0; i < 256; ++i) + { + key[i] = adpKey[i % 13]; + permutation[i] = (byte)i; + } + + // randomize, using key + for (i = 0; i < 256; ++i) + { + j = (j + permutation[i] + key[i]) & 0xFF; + Swap(permutation, i, j); + } + + // perform RC4 transformation + i = j = 0; + for (k = 0; k < 469; ++k) + { + i = (i + 1) & 0xFF; + j = (j + permutation[i]) & 0xFF; + + // swap permutation[i] and permutation[j] + Swap(permutation, i, j); + + // transform byte + adpKeystream[k] = permutation[(permutation[i] + permutation[j]) & 0xFF]; + } + } + + /// + /// Create AES keystream. + /// + private void GenerateAESKeystream() + { + if (!keys.ContainsKey(keyId)) + return; + + byte[] key = keys[keyId].Key; + byte[] iv = ExpandMIToIV(messageIndicator); + + using (var aes = Aes.Create()) + { + aes.KeySize = 256; + aes.BlockSize = 128; + aes.Key = key.Length == 32 ? key : key.Concat(new byte[32 - key.Length]).ToArray(); + aes.Mode = CipherMode.ECB; + aes.Padding = PaddingMode.None; + + using (var encryptor = aes.CreateEncryptor()) + { + byte[] input = new byte[16]; + Array.Copy(iv, input, 16); + byte[] output = new byte[16]; + + for (int i = 0; i < aesKeystream.Length / 16; i++) + { + encryptor.TransformBlock(input, 0, 16, output, 0); + Buffer.BlockCopy(output, 0, aesKeystream, i * 16, 16); + Array.Copy(output, input, 16); + } + } + } + } + + /// + /// Helper to process IMBE audio using AES-256. + /// + /// + /// + /// + private bool AESProcess(byte[] imbe, P25DUID duid) + { + int offset = 16; + if (duid == P25DUID.LDU2) + offset += 101; + + offset += (ksPosition * IMBE_BUF_LEN) + IMBE_BUF_LEN + (ksPosition < 8 ? 0 : 2); + ksPosition = (ksPosition + 1) % 9; + + for (int j = 0; j < IMBE_BUF_LEN; ++j) + imbe[j] ^= aesKeystream[j + offset]; + + return true; + } + + /// + /// Helper to process IMBE audio using ARC4. + /// + /// + /// + /// + private bool ARC4Process(byte[] imbe, P25DUID duid) + { + int offset = 256; + + if (duid == P25DUID.LDU1) + offset = 0; + else if (duid == P25DUID.LDU2) + offset = 101; + + offset += (ksPosition * IMBE_BUF_LEN) + 267 + (ksPosition < 8 ? 0 : 2); + ksPosition = (ksPosition + 1) % 9; + + for (int j = 0; j < IMBE_BUF_LEN; ++j) + imbe[j] ^= adpKeystream[j + offset]; + + return true; + } + + /// + /// Cycle P25 LFSR + /// + /// + /// + public static void CycleP25Lfsr(byte[] MI) + { + // TODO: use step LFSR + if (MI == null || MI.Length < 9) + throw new ArgumentException("MI must be at least 9 bytes long."); + + ulong lfsr = 0; + + // Load the first 8 bytes into the LFSR + for (int i = 0; i < 8; i++) + { + lfsr = (lfsr << 8) | MI[i]; + } + + // Perform 64-bit LFSR cycling using the polynomial: + // C(x) = x^64 + x^62 + x^46 + x^38 + x^27 + x^15 + 1 + for (int cnt = 0; cnt < 64; cnt++) + { + ulong bit = ((lfsr >> 63) ^ (lfsr >> 61) ^ (lfsr >> 45) ^ (lfsr >> 37) ^ (lfsr >> 26) ^ (lfsr >> 14)) & 0x1; + lfsr = (lfsr << 1) | bit; + } + + // Store the result back into MI + for (int i = 7; i >= 0; i--) + { + MI[i] = (byte)(lfsr & 0xFF); + lfsr >>= 8; + } + + MI[8] = 0; // Last byte is always set to zero + } + + /// + /// Step LFSR + /// + /// + /// + private static ulong StepP25Lfsr(ref ulong lfsr) + { + // Extract overflow bit (bit 63) + ulong ovBit = (lfsr >> 63) & 0x1; + + // Compute feedback bit using polynomial: x^64 + x^62 + x^46 + x^38 + x^27 + x^15 + 1 + ulong fbBit = ((lfsr >> 63) ^ (lfsr >> 61) ^ (lfsr >> 45) ^ (lfsr >> 37) ^ + (lfsr >> 26) ^ (lfsr >> 14)) & 0x1; + + // Shift LFSR left and insert feedback bit + lfsr = (lfsr << 1) | fbBit; + + return ovBit; + } + + /// + /// Expland MI to 128 IV + /// + /// + /// + /// + private static byte[] ExpandMIToIV(byte[] mi) + { + if (mi == null || mi.Length < 8) + throw new ArgumentException("MI must be at least 8 bytes long."); + + byte[] iv = new byte[16]; + + // Copy first 64 bits of MI into LFSR + ulong lfsr = 0; + for (int i = 0; i < 8; i++) + lfsr = (lfsr << 8) | mi[i]; + + // Use LFSR routine to compute the expansion + ulong overflow = 0; + for (int i = 0; i < 64; i++) + overflow = (overflow << 1) | StepP25Lfsr(ref lfsr); + + // Copy expansion and LFSR to IV + for (int i = 7; i >= 0; i--) + { + iv[i] = (byte)(overflow & 0xFF); + overflow >>= 8; + } + + for (int i = 15; i >= 8; i--) + { + iv[i] = (byte)(lfsr & 0xFF); + lfsr >>= 8; + } + + return iv; + } + } // public class P25Crypto +} // namespace dvmconsole