From 50165cc03b5e51516a64b879bd8408084d8c33bd Mon Sep 17 00:00:00 2001 From: firealarmss Date: Sun, 16 Mar 2025 23:59:35 -0500 Subject: [PATCH] Add support for KMMs; fix directory names to be uniform with the rest of the project --- Constants.cs | 2 + FneBase.cs | 48 ++++++++++++++ FnePeer.cs | 55 ++++++++++++++++ FneSystemBase.cs | 9 +++ P25/KMM/KeysetItem.cs | 127 +++++++++++++++++++++++++++++++++++++ P25/KMM/KmmFrame.cs | 76 ++++++++++++++++++++++ P25/KMM/KmmMessageType.cs | 24 +++++++ P25/KMM/KmmModifyKey.cs | 126 ++++++++++++++++++++++++++++++++++++ P25/KMM/KmmResponseKind.cs | 25 ++++++++ P25/P25Defines.cs | 8 +++ 10 files changed, 500 insertions(+) create mode 100644 P25/KMM/KeysetItem.cs create mode 100644 P25/KMM/KmmFrame.cs create mode 100644 P25/KMM/KmmMessageType.cs create mode 100644 P25/KMM/KmmModifyKey.cs create mode 100644 P25/KMM/KmmResponseKind.cs diff --git a/Constants.cs b/Constants.cs index ad0d79d..35ea341 100644 --- a/Constants.cs +++ b/Constants.cs @@ -172,6 +172,8 @@ namespace fnecore public const byte NET_FUNC_PONG = 0x75; // Pong public const byte NET_FUNC_GRANT = 0x7A; // Grant Request + public const byte NET_FUNC_KEY_REQ = 0x7C; // Encryption Key Request + public const byte NET_FUNC_KEY_RSP = 0x7D; // Encryption Key Response public const byte NET_FUNC_ACK = 0x7E; // Packet Acknowledge public const byte NET_FUNC_NAK = 0x7F; // Packet Negative Acknowledge diff --git a/FneBase.cs b/FneBase.cs index dc74d42..c94ab8f 100644 --- a/FneBase.cs +++ b/FneBase.cs @@ -19,6 +19,7 @@ using fnecore.DMR; using fnecore.P25; using fnecore.NXDN; using fnecore.EDAC; +using fnecore.P25.kmm; namespace fnecore { @@ -530,6 +531,37 @@ namespace fnecore this.Information = peer; } } // public class PeerConnectedEvent : EventArgs + + /// + /// Event called when a kmm key response is received + /// + public class KeyResponseEvent : EventArgs + { + /// + /// KMM Message Id + /// + public byte MessageId { get; set; } + + /// + /// instance + /// + public KmmModifyKey KmmKey { get; set; } + + /// + /// Raw Data + /// + public byte[] Data { get; set; } + + /* + ** Methods + */ + public KeyResponseEvent(byte messageId, KmmModifyKey kmmKey, byte[] data) : base() + { + this.MessageId = messageId; + this.KmmKey = kmmKey; + this.Data = data; + } + } /// /// This class implements some base functionality for all other FNE network classes. @@ -638,6 +670,11 @@ namespace fnecore /// public Action PeerDisconnected = null; + /// + /// Event action thats called when a key response is received + /// + public event EventHandler KeyResponse; + /// /// Callback action that handles internal logging. /// @@ -831,6 +868,7 @@ namespace fnecore Buffer.BlockCopy(message, 0, buffer, (int)(Constants.RtpHeaderLengthBytes + Constants.RtpExtensionHeaderLengthBytes + Constants.RtpFNEHeaderLengthBytes), message.Length); + return buffer; } @@ -883,5 +921,15 @@ namespace fnecore if (PeerConnected != null) PeerConnected.Invoke(this, e); } + + /// + /// Helper to fire the key response event + /// + /// + protected void FireKeyResponse(KeyResponseEvent e) + { + if (KeyResponse != null) + KeyResponse.Invoke(this, e); + } } // public abstract class FneBase } // namespace fnecore diff --git a/FnePeer.cs b/FnePeer.cs index 48864a0..3ba73fa 100644 --- a/FnePeer.cs +++ b/FnePeer.cs @@ -27,6 +27,7 @@ using fnecore.NXDN; using System.Collections.Generic; using System.Net.NetworkInformation; using System.Linq; +using fnecore.P25.kmm; namespace fnecore { @@ -343,6 +344,40 @@ namespace fnecore SendMaster(CreateOpcode(Constants.NET_FUNC_ANNOUNCE, Constants.NET_ANNC_SUBFUNC_UNIT_DEREG), res, 0, 0, true); } + /// + /// Helper to send a key request to the master + /// + /// + /// + public void SendMasterKeyRequest(byte algId, ushort kId) + { + byte[] res = new byte[32]; + + KmmModifyKey modifyKey = new KmmModifyKey + { + AlgId = algId, + KeyId = kId + }; + + KeysetItem ks = new KeysetItem + { + KeysetId = 0, + AlgId = algId, + KeyLength = 32 + }; + + modifyKey.KeysetItem = ks; + modifyKey.MessageLength = 21; + + byte[] payload = new byte[modifyKey.MessageLength]; + + modifyKey.Encode(payload); + + Array.Copy(payload, 0, res, 11, payload.Length); + + SendMaster(CreateOpcode(Constants.NET_FUNC_KEY_REQ, Constants.NET_SUBFUNC_NOP), res, Constants.RtpCallEndSeq, 0, true); + } + /// /// Helper to update the RTP packet sequence. /// @@ -673,6 +708,26 @@ namespace fnecore } } break; + case Constants.NET_FUNC_KEY_RSP: // Key Response + if (this.peerId == peerId) + { + byte[] payload = message.Skip(11).ToArray(); + + byte messageId = payload[0]; + + if (messageId == (byte)KmmMessageType.MODIFY_KEY_CMD) + { + KmmModifyKey modifyKey = new KmmModifyKey(); + modifyKey.Decode(payload); + + FireKeyResponse(new KeyResponseEvent(messageId, modifyKey, message)); + } + else + { + Log(LogLevel.WARNING, $"Unknown KEY_RSP Message ID: {messageId}"); + } + } + break; default: Log(LogLevel.ERROR, $"({systemName}) Unknown opcode {FneUtils.BytesToString(message, 0, 4)} -- {FneUtils.HexDump(message, 0)}"); diff --git a/FneSystemBase.cs b/FneSystemBase.cs index 8a8c50a..059bd00 100644 --- a/FneSystemBase.cs +++ b/FneSystemBase.cs @@ -205,6 +205,8 @@ namespace fnecore this.fne.PeerIgnored = PeerIgnored; this.fne.PeerConnected += PeerConnected; + this.fne.KeyResponse += KeyResponse; + // hook logger callback this.fne.LogLevel = fneLogLevel; } @@ -319,6 +321,13 @@ namespace fnecore /// protected abstract void PeerConnected(object sender, PeerConnectedEvent e); + /// + /// Event handler used to handle a key response + /// + /// + /// + protected abstract void KeyResponse(object sender, KeyResponseEvent e); + /// /// Creates an DMR frame message. /// diff --git a/P25/KMM/KeysetItem.cs b/P25/KMM/KeysetItem.cs new file mode 100644 index 0000000..90604e8 --- /dev/null +++ b/P25/KMM/KeysetItem.cs @@ -0,0 +1,127 @@ +// 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; +using System.Collections.Generic; +using System.Linq; + +namespace fnecore.P25.kmm +{ + /// + /// Enc. Key Item + /// + public class KeyItem + { + private const int MAX_ENC_KEY_LENGTH = 32; + private readonly byte[] _keyMaterial = new byte[MAX_ENC_KEY_LENGTH]; + + public byte KeyFormat { get; set; } = 0x80; + public ushort Sln { get; set; } + public ushort KeyId { get; set; } + public uint KeyLength { get; private set; } + + /// + /// Creates an instance of + /// + public KeyItem() { /* stub */ } + + /// + /// Creates an instance of + /// + /// + public KeyItem(KeyItem other) + { + KeyFormat = other.KeyFormat; + Sln = other.Sln; + KeyId = other.KeyId; + SetKey(other._keyMaterial, other.KeyLength); + } + + /// + /// Set Enc. Key + /// + /// + /// + /// + /// + public void SetKey(byte[] key, uint keyLength) + { + if (key == null) throw new ArgumentNullException(nameof(key)); + if (keyLength > MAX_ENC_KEY_LENGTH) throw new ArgumentOutOfRangeException(nameof(keyLength)); + + KeyLength = keyLength; + Array.Clear(_keyMaterial, 0, MAX_ENC_KEY_LENGTH); + Array.Copy(key, _keyMaterial, keyLength); + } + + /// + /// Return the Enc. Key + /// + /// Enc. Key + public byte[] GetKey() + { + return _keyMaterial.Take((int)KeyLength).ToArray(); + } + } // public class KeyItem + + /// + /// Keyset item + /// + public class KeysetItem + { + public byte KeysetId { get; set; } + public byte AlgId { get; set; } + public byte KeyLength { get; set; } + public List Keys { get; private set; } = new List(); + + /// + /// Creates an instance of + /// + public KeysetItem() { /* stub */ } + + /// + /// Creates an instance of + /// + /// + public KeysetItem(KeysetItem other) + { + KeysetId = other.KeysetId; + AlgId = other.AlgId; + KeyLength = other.KeyLength; + Keys = other.Keys.Select(k => new KeyItem(k)).ToList(); + } + + /// + /// Keyset length + /// + public uint Length => (uint)(4 + Keys.Count * (5 + KeyLength)); + + /// + /// Add to the Keys list + /// + /// + public void AddKey(KeyItem key) + { + Keys.Add(key); + } + + /// + /// Overwrite Keys list + /// + /// + public void SetKeys(IEnumerable keys) + { + Keys = keys.ToList(); + } + } // public class KeysetItem +} // namespace fnecore.P25.kmm diff --git a/P25/KMM/KmmFrame.cs b/P25/KMM/KmmFrame.cs new file mode 100644 index 0000000..fa6bab4 --- /dev/null +++ b/P25/KMM/KmmFrame.cs @@ -0,0 +1,76 @@ +// 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; + +namespace fnecore.P25.kmm +{ + /// + /// KMM Frame base class + /// + public class KmmFrame + { + public byte MessageId { get; set; } + public ushort MessageLength { get; set; } + public byte RespKind { get; private set; } = 0; + public bool Complete { get; private set; } = true; + public ushort DstLlId { get; private set; } + public ushort SrcLlId { get; private set; } + + /// + /// Creates an instance of + /// + public KmmFrame() { /* sub */ } + + /// + /// KMM Frame length + /// + public virtual ushort Length => P25Defines.KMM_FRAME_LENGTH; + + /// + /// Encode KMM Frame Header + /// + /// + /// + protected void EncodeHeader(byte[] data) + { + if (data.Length < 8) + throw new ArgumentException("Data buffer too small"); + + data[0] = MessageId; + FneUtils.WriteBytes(Length, ref data, 1); + + data[3] = (byte)((RespKind << 6) | (Complete ? 0x00 : 0x01)); + FneUtils.WriteBytes(DstLlId, ref data, 4); + FneUtils.WriteBytes(SrcLlId, ref data, 6); + } + + /// + /// Decode KMM Frame Header + /// + /// + /// + protected void DecodeHeader(byte[] data) + { + if (data.Length < 8) + throw new ArgumentException("Data buffer too small"); + + MessageId = data[0]; + MessageLength = FneUtils.ToUInt16(data, 1); + RespKind = (byte)((data[3] >> 6) & 0x03); + Complete = (data[3] & 0x01) == 0; + DstLlId = FneUtils.ToUInt16(data, 4); + SrcLlId = FneUtils.ToUInt16(data, 6); + } + } // public class KmmFrame +} // namespace fnecore.P25.kmm \ No newline at end of file diff --git a/P25/KMM/KmmMessageType.cs b/P25/KMM/KmmMessageType.cs new file mode 100644 index 0000000..2ded15b --- /dev/null +++ b/P25/KMM/KmmMessageType.cs @@ -0,0 +1,24 @@ +// 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.kmm +{ + /// + /// KMM Message Type + /// + public enum KmmMessageType + { + NULL_CMD = 0x00, + MODIFY_KEY_CMD = 0x13 + } +} // namespace fnecore.P25.kmm diff --git a/P25/KMM/KmmModifyKey.cs b/P25/KMM/KmmModifyKey.cs new file mode 100644 index 0000000..0e2a604 --- /dev/null +++ b/P25/KMM/KmmModifyKey.cs @@ -0,0 +1,126 @@ +// 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; +using System.Linq; + +namespace fnecore.P25.kmm +{ + /// + /// KMM Modify Key + /// + public class KmmModifyKey : KmmFrame + { + private byte[] _mi; + + public const int KMM_MODIFY_KEY_LENGTH = 21; + + public byte DecryptInfoFmt { get; set; } = P25Defines.KMM_DECRYPT_INSTRUCTION_NONE; + public byte AlgId { get; set; } + public ushort KeyId { get; set; } + public KeysetItem KeysetItem { get; set; } = new KeysetItem(); + + /// + /// Creates an instance + /// + public KmmModifyKey() + { + MessageId = (byte)KmmMessageType.MODIFY_KEY_CMD; + _mi = new byte[P25Defines.P25_MI_LENGTH]; + } + + /// + /// KMM Frame Length + /// + public override ushort Length => 21; + + /// + /// Encode a KMM Modify Key + /// + /// + public void Encode(byte[] data) + { + EncodeHeader(data); + + data[10] = DecryptInfoFmt; + data[11] = AlgId; + FneUtils.WriteBytes(KeyId, ref data, 12); + + int offset = 14; + if (DecryptInfoFmt == P25Defines.KMM_DECRYPT_INSTRUCTION_MI) + { + Array.Copy(_mi, 0, data, offset, P25Defines.P25_MI_LENGTH); + offset += P25Defines.P25_MI_LENGTH; + } + + data[offset] = KeysetItem.KeysetId; + data[offset + 1] = KeysetItem.AlgId; + data[offset + 2] = KeysetItem.KeyLength; + data[offset + 3] = (byte)KeysetItem.Keys.Count; + offset += 4; + + foreach (var key in KeysetItem.Keys) + { + int keyNameLen = key.KeyFormat & 0x1F; + data[offset] = (byte)(key.KeyFormat | keyNameLen); + FneUtils.WriteBytes(key.Sln, ref data, offset + 1); + FneUtils.WriteBytes(key.KeyId, ref data, offset + 3); + key.GetKey().CopyTo(data, offset + 5 + keyNameLen); + offset += 5 + keyNameLen + KeysetItem.KeyLength; + } + } + + /// + /// Decode a KMM Modify Key + /// + /// + public void Decode(byte[] data) + { + DecodeHeader(data); + + DecryptInfoFmt = data[10]; + AlgId = data[11]; + KeyId = FneUtils.ToUInt16(data, 12); + + int offset = 14; + if (DecryptInfoFmt == P25Defines.KMM_DECRYPT_INSTRUCTION_MI) + { + Array.Copy(data, offset, _mi, 0, P25Defines.P25_MI_LENGTH); + offset += P25Defines.P25_MI_LENGTH; + } + + KeysetItem.KeysetId = data[offset]; + KeysetItem.AlgId = data[offset + 1]; + KeysetItem.KeyLength = data[offset + 2]; + int keyCount = data[offset + 3]; + offset += 4; + + KeysetItem.Keys.Clear(); + for (int i = 0; i < keyCount; i++) + { + KeyItem key = new KeyItem(); + int keyNameLen = data[offset] & 0x1F; + key.KeyFormat = data[offset]; + key.Sln = FneUtils.ToUInt16(data, offset + 1); + key.KeyId = FneUtils.ToUInt16(data, offset + 3); + key.SetKey( + data.Skip(offset + 5 + keyNameLen).Take((int)KeysetItem.KeyLength).ToArray(), + KeysetItem.KeyLength + ); + + offset += 5 + keyNameLen + KeysetItem.KeyLength; + KeysetItem.Keys.Add(key); + } + } + } // public class KmmModifyKey +} // namespace fnecore.P25.kmm \ No newline at end of file diff --git a/P25/KMM/KmmResponseKind.cs b/P25/KMM/KmmResponseKind.cs new file mode 100644 index 0000000..6ca94d3 --- /dev/null +++ b/P25/KMM/KmmResponseKind.cs @@ -0,0 +1,25 @@ +// 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.kmm +{ + /// + /// KMM Response Kind + /// + public enum KmmResponseKind + { + NONE = 0x00, + DELAYED = 0x01, + IMMEDIATE = 0x02 + } +} // namespace fnecore.P25.kmm diff --git a/P25/P25Defines.cs b/P25/P25Defines.cs index bc83b0f..12963f1 100644 --- a/P25/P25Defines.cs +++ b/P25/P25Defines.cs @@ -133,9 +133,17 @@ namespace fnecore.P25 public const byte P25_MFG_STANDARD = 0x00; public const byte P25_ALGO_UNENCRYPT = 0x80; + public const byte P25_ALGO_DES = 0x81; + public const byte P25_ALGO_AES = 0x84; + public const byte P25_ALGO_ARC4 = 0xAA; + + public const byte KMM_DECRYPT_INSTRUCTION_NONE = 0x00; + public const byte KMM_DECRYPT_INSTRUCTION_MI = 0x40; public const byte P25_MI_LENGTH = 9; + public const byte KMM_FRAME_LENGTH = 9; + public const byte P25_TSDU_FRAME_LENGTH_BYTES = 45; public const byte P25_LDU_FRAME_LENGTH_BYTES = 216;