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

refactor MainWindow.xaml.cs -- split P25 and DMR fnecore related functionality into their own partial class files;

Browse Source
pull/1/head
Bryan Biedenkapp 11 months ago
parent d2bfd46a39
commit fe5b8bc9aa
3 changed files with 1025 additions and 944 deletions
Show Stats Download Patch File Download Diff File

342
dvmconsole/MainWindow.DMR.cs
Unescape View File

@ -0,0 +1,342 @@
// SPDX-License-Identifier: AGPL-3.0-only
/**
* Digital Voice Modem - Desktop Dispatch Console
* AGPLv3 Open Source. Use is subject to license terms.
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
*
* @package DVM / Desktop Dispatch Console
* @license AGPLv3 License (https://opensource.org/licenses/AGPL-3.0)
*
* Copyright (C) 2024-2025 Caleb, K4PHP
* Copyright (C) 2025 Bryan Biedenkapp, N2PLL
*
*/
using System.Windows;
using dvmconsole.Controls;
using Constants = fnecore.Constants;
using fnecore;
using fnecore.DMR;
namespace dvmconsole
{
/// <summary>
/// Interaction logic for MainWindow.xaml.
/// </summary>
public partial class MainWindow : Window
{
/// <summary>
/// Helper to encode and transmit PCM audio as DMR AMBE frames.
/// </summary>
/// <param name="pcm"></param>
/// <param name="fne"></param>
/// <param name="channel"></param>
/// <param name="cpgChannel"></param>
/// <param name="system"></param>
private void DMREncodeAudioFrame(byte[] pcm, PeerSystem fne, ChannelBox channel, Codeplug.Channel cpgChannel, Codeplug.System system)
{
// make sure we have a valid stream ID
if (channel.TxStreamId == 0)
Log.WriteWarning($"({channel.SystemName}) DMRD: Traffic *VOICE FRAME * Stream ID not set for traffic? Shouldn't happen.");
try
{
byte slot = 1; // TODO: Support both time slots
byte[] data = null, dmrpkt = null;
channel.dmrN = (byte)(channel.dmrSeqNo % 6);
if (channel.ambeCount == FneSystemBase.AMBE_PER_SLOT)
{
// is this the intitial sequence?
if (channel.dmrSeqNo == 0)
{
channel.pktSeq = 0;
// send DMR voice header
data = new byte[FneSystemBase.DMR_FRAME_LENGTH_BYTES];
// generate DMR LC
LC dmrLC = new LC();
dmrLC.FLCO = (byte)DMRFLCO.FLCO_GROUP;
dmrLC.SrcId = uint.Parse(system.Rid);
dmrLC.DstId = uint.Parse(cpgChannel.Tgid);
channel.embeddedData.SetLC(dmrLC);
// generate the Slot Type
SlotType slotType = new SlotType();
slotType.DataType = (byte)DMRDataType.VOICE_LC_HEADER;
slotType.GetData(ref data);
FullLC.Encode(dmrLC, ref data, DMRDataType.VOICE_LC_HEADER);
// generate DMR network frame
dmrpkt = new byte[FneSystemBase.DMR_PACKET_SIZE];
fne.CreateDMRMessage(ref dmrpkt, uint.Parse(system.Rid), uint.Parse(cpgChannel.Tgid), slot, FrameType.VOICE_SYNC, (byte)channel.dmrSeqNo, 0);
Buffer.BlockCopy(data, 0, dmrpkt, 20, FneSystemBase.DMR_FRAME_LENGTH_BYTES);
fne.peer.SendMaster(new Tuple<byte, byte>(Constants.NET_FUNC_PROTOCOL, Constants.NET_PROTOCOL_SUBFUNC_DMR), dmrpkt, channel.pktSeq, channel.TxStreamId);
channel.dmrSeqNo++;
}
ushort curr = channel.pktSeq;
++channel.pktSeq;
if (channel.pktSeq > (Constants.RtpCallEndSeq - 1))
channel.pktSeq = 0;
// send DMR voice
data = new byte[FneSystemBase.DMR_FRAME_LENGTH_BYTES];
Buffer.BlockCopy(channel.ambeBuffer, 0, data, 0, 13);
data[13U] = (byte)(channel.ambeBuffer[13U] & 0xF0);
data[19U] = (byte)(channel.ambeBuffer[13U] & 0x0F);
Buffer.BlockCopy(channel.ambeBuffer, 14, data, 20, 13);
FrameType frameType = FrameType.VOICE_SYNC;
if (channel.dmrN == 0)
frameType = FrameType.VOICE_SYNC;
else
{
frameType = FrameType.VOICE;
byte lcss = channel.embeddedData.GetData(ref data, channel.dmrN);
// generated embedded signalling
EMB emb = new EMB();
emb.ColorCode = 0;
emb.LCSS = lcss;
emb.Encode(ref data);
}
// generate DMR network frame
dmrpkt = new byte[FneSystemBase.DMR_PACKET_SIZE];
fne.CreateDMRMessage(ref dmrpkt, uint.Parse(system.Rid), uint.Parse(cpgChannel.Tgid), 1, frameType, (byte)channel.dmrSeqNo, channel.dmrN);
Buffer.BlockCopy(data, 0, dmrpkt, 20, FneSystemBase.DMR_FRAME_LENGTH_BYTES);
fne.peer.SendMaster(new Tuple<byte, byte>(Constants.NET_FUNC_PROTOCOL, Constants.NET_PROTOCOL_SUBFUNC_DMR), dmrpkt, channel.pktSeq, channel.TxStreamId);
channel.dmrSeqNo++;
FneUtils.Memset(channel.ambeBuffer, 0, 27);
channel.ambeCount = 0;
}
int smpIdx = 0;
short[] samples = new short[MBE_SAMPLES_LENGTH];
for (int pcmIdx = 0; pcmIdx < pcm.Length; pcmIdx += 2)
{
samples[smpIdx] = (short)((pcm[pcmIdx + 1] << 8) + pcm[pcmIdx + 0]);
smpIdx++;
}
// encode PCM samples into AMBE codewords
byte[] ambe = null;
if (channel.ExternalVocoderEnabled)
{
if (channel.ExtHalfRateVocoder == null)
channel.ExtHalfRateVocoder = new AmbeVocoder(false);
channel.ExtHalfRateVocoder.encode(samples, out ambe, true);
}
else
{
if (channel.Encoder == null)
channel.Encoder = new MBEEncoder(MBE_MODE.DMR_AMBE);
channel.Encoder.encode(samples, ambe);
}
Buffer.BlockCopy(ambe, 0, channel.ambeBuffer, channel.ambeCount * 9, FneSystemBase.AMBE_BUF_LEN);
channel.ambeCount++;
}
catch (Exception ex)
{
Log.StackTrace(ex, false);
}
}
/// <summary>
/// Helper to decode and playback DMR AMBE frames as PCM audio.
/// </summary>
/// <param name="ambe"></param>
/// <param name="e"></param>
/// <param name="system"></param>
/// <param name="channel"></param>
private void DMRDecodeAudioFrame(byte[] ambe, DMRDataReceivedEvent e, PeerSystem system, ChannelBox channel)
{
try
{
// Log.Logger.Debug($"FULL AMBE {FneUtils.HexDump(ambe)}");
for (int n = 0; n < FneSystemBase.AMBE_PER_SLOT; n++)
{
byte[] ambePartial = new byte[FneSystemBase.AMBE_BUF_LEN];
for (int i = 0; i < FneSystemBase.AMBE_BUF_LEN; i++)
ambePartial[i] = ambe[i + (n * 9)];
short[] samples = null;
int errs = 0;
// do we have the external vocoder library?
if (channel.ExternalVocoderEnabled)
{
if (channel.ExtHalfRateVocoder == null)
channel.ExtHalfRateVocoder = new AmbeVocoder(false);
errs = channel.ExtHalfRateVocoder.decode(ambePartial, out samples);
}
else
{
samples = new short[FneSystemBase.MBE_SAMPLES_LENGTH];
errs = channel.Decoder.decode(ambePartial, samples);
}
if (samples != null)
{
//Log.WriteLine($"({system.SystemName}) DMRD: Traffic *VOICE FRAME * PEER {e.PeerId} SRC_ID {e.SrcId} TGID {e.DstId} TS {e.Slot + 1} VC{e.n}.{n} ERRS {errs} [STREAM ID {e.StreamId}]");
// Log.Logger.Debug($"PARTIAL AMBE {FneUtils.HexDump(ambePartial)}");
// Log.Logger.Debug($"SAMPLE BUFFER {FneUtils.HexDump(samples)}");
int pcmIdx = 0;
byte[] pcm = new byte[samples.Length * 2];
for (int smpIdx = 0; smpIdx < samples.Length; smpIdx++)
{
pcm[pcmIdx + 0] = (byte)(samples[smpIdx] & 0xFF);
pcm[pcmIdx + 1] = (byte)((samples[smpIdx] >> 8) & 0xFF);
pcmIdx += 2;
}
//Log.WriteLine($"PCM BYTE BUFFER {FneUtils.HexDump(pcm)}");
audioManager.AddTalkgroupStream(e.DstId.ToString(), pcm);
}
}
}
catch (Exception ex)
{
Log.WriteError($"Audio Decode Exception: {ex.Message}");
Log.StackTrace(ex, false);
}
}
/// <summary>
/// Event handler used to process incoming DMR data.
/// </summary>
/// <param name="e"></param>
/// <param name="pktTime"></param>
public void DMRDataReceived(DMRDataReceivedEvent e, DateTime pktTime)
{
Dispatcher.Invoke(() =>
{
foreach (ChannelBox channel in selectedChannelsManager.GetSelectedChannels())
{
if (channel.SystemName == PLAYBACKSYS || channel.ChannelName == PLAYBACKCHNAME || channel.DstId == PLAYBACKTG)
continue;
Codeplug.System system = Codeplug.GetSystemForChannel(channel.ChannelName);
Codeplug.Channel cpgChannel = Codeplug.GetChannelByName(channel.ChannelName);
if (cpgChannel.GetChannelMode() != Codeplug.ChannelMode.DMR)
continue;
PeerSystem handler = fneSystemManager.GetFneSystem(system.Name);
if (!channel.IsEnabled || channel.Name == PLAYBACKCHNAME)
continue;
if (cpgChannel.Tgid != e.DstId.ToString())
continue;
if (!systemStatuses.ContainsKey(cpgChannel.Name + e.Slot))
systemStatuses[cpgChannel.Name + e.Slot] = new SlotStatus();
if (channel.Decoder == null)
channel.Decoder = new MBEDecoder(MBE_MODE.DMR_AMBE);
byte[] data = new byte[FneSystemBase.DMR_FRAME_LENGTH_BYTES];
Buffer.BlockCopy(e.Data, 20, data, 0, FneSystemBase.DMR_FRAME_LENGTH_BYTES);
byte bits = e.Data[15];
// is this a new call stream?
if (e.StreamId != systemStatuses[cpgChannel.Name + e.Slot].RxStreamId)
{
channel.IsReceiving = true;
systemStatuses[cpgChannel.Name + e.Slot].RxStart = pktTime;
Log.WriteLine($"({system.Name}) DMRD: Traffic *CALL START * PEER {e.PeerId} SRC_ID {e.SrcId} TGID {e.DstId} Slot {e.Slot} [STREAM ID {e.StreamId}]");
// if we can, use the LC from the voice header as to keep all options intact
if ((e.FrameType == FrameType.DATA_SYNC) && (e.DataType == DMRDataType.VOICE_LC_HEADER))
{
LC lc = FullLC.Decode(data, DMRDataType.VOICE_LC_HEADER);
systemStatuses[cpgChannel.Name + e.Slot].DMR_RxLC = lc;
}
else // if we don't have a voice header; don't wait to decode it, just make a dummy header
systemStatuses[cpgChannel.Name + e.Slot].DMR_RxLC = new LC()
{
SrcId = e.SrcId,
DstId = e.DstId
};
systemStatuses[cpgChannel.Name + e.Slot].DMR_RxPILC = new PrivacyLC();
Log.WriteLine($"({system.Name}) TS {e.Slot + 1} [STREAM ID {e.StreamId}] RX_LC {FneUtils.HexDump(systemStatuses[cpgChannel.Name + e.Slot].DMR_RxLC.GetBytes())}");
callHistoryWindow.AddCall(cpgChannel.Name, (int)e.SrcId, (int)e.DstId);
callHistoryWindow.ChannelKeyed(cpgChannel.Name, (int)e.SrcId, false); // TODO: Encrypted state
string alias = string.Empty;
try
{
alias = AliasTools.GetAliasByRid(system.RidAlias, (int)e.SrcId);
}
catch (Exception) { }
if (string.IsNullOrEmpty(alias))
channel.LastSrcId = "Last ID: " + e.SrcId;
else
channel.LastSrcId = "Last: " + alias;
channel.Background = ChannelBox.GREEN_GRADIENT;
}
// if we can, use the PI LC from the PI voice header as to keep all options intact
if ((e.FrameType == FrameType.DATA_SYNC) && (e.DataType == DMRDataType.VOICE_PI_HEADER))
{
PrivacyLC lc = FullLC.DecodePI(data);
systemStatuses[cpgChannel.Name + e.Slot].DMR_RxPILC = lc;
//Log.WriteLine($"({SystemName}) DMRD: Traffic *CALL PI PARAMS * PEER {e.PeerId} DST_ID {e.DstId} TS {e.Slot + 1} ALGID {lc.AlgId} KID {lc.KId} [STREAM ID {e.StreamId}]");
//Log.WriteLine($"({SystemName}) TS {e.Slot + 1} [STREAM ID {e.StreamId}] RX_PI_LC {FneUtils.HexDump(systemStatuses[cpgChannel.Name + e.Slot].DMR_RxPILC.GetBytes())}");
}
if ((e.FrameType == FrameType.DATA_SYNC) && (e.DataType == DMRDataType.TERMINATOR_WITH_LC) && (systemStatuses[cpgChannel.Name + e.Slot].RxType != FrameType.TERMINATOR))
{
channel.IsReceiving = false;
TimeSpan callDuration = pktTime - systemStatuses[cpgChannel.Name + e.Slot].RxStart;
Log.WriteLine($"({system.Name}) DMRD: Traffic *CALL END * PEER {e.PeerId} SRC_ID {e.SrcId} TGID {e.DstId} Slot {e.Slot} DUR {callDuration} [STREAM ID {e.StreamId}]");
channel.Background = ChannelBox.BLUE_GRADIENT;
channel.VolumeMeterLevel = 0;
callHistoryWindow.ChannelUnkeyed(cpgChannel.Name, (int)e.SrcId);
}
if (e.FrameType == FrameType.VOICE_SYNC || e.FrameType == FrameType.VOICE)
{
byte[] ambe = new byte[FneSystemBase.DMR_AMBE_LENGTH_BYTES];
Buffer.BlockCopy(data, 0, ambe, 0, 14);
ambe[13] &= 0xF0;
ambe[13] |= (byte)(data[19] & 0x0F);
Buffer.BlockCopy(data, 20, ambe, 14, 13);
DMRDecodeAudioFrame(ambe, e, handler, channel);
}
systemStatuses[cpgChannel.Name + e.Slot].RxRFS = e.SrcId;
systemStatuses[cpgChannel.Name + e.Slot].RxType = e.FrameType;
systemStatuses[cpgChannel.Name + e.Slot].RxTGId = e.DstId;
systemStatuses[cpgChannel.Name + e.Slot].RxTime = pktTime;
systemStatuses[cpgChannel.Name + e.Slot].RxStreamId = e.StreamId;
}
});
}
} // public partial class MainWindow : Window
} // namespace dvmconsole

666
dvmconsole/MainWindow.P25.cs
Unescape View File

@ -0,0 +1,666 @@
// SPDX-License-Identifier: AGPL-3.0-only
/**
* Digital Voice Modem - Desktop Dispatch Console
* AGPLv3 Open Source. Use is subject to license terms.
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
*
* @package DVM / Desktop Dispatch Console
* @license AGPLv3 License (https://opensource.org/licenses/AGPL-3.0)
*
* Copyright (C) 2024-2025 Caleb, K4PHP
* Copyright (C) 2025 Bryan Biedenkapp, N2PLL
*
*/
using System.Windows;
using NWaves.Signals;
using dvmconsole.Controls;
using Constants = fnecore.Constants;
using fnecore;
using fnecore.P25;
namespace dvmconsole
{
/// <summary>
/// Interaction logic for MainWindow.xaml.
/// </summary>
public partial class MainWindow : Window
{
/*
** Methods
*/
/// <summary>
/// Helper to encode and transmit PCM audio as P25 IMBE frames.
/// </summary>
/// <param name="pcm"></param>
/// <param name="fne"></param>
/// <param name="channel"></param>
/// <param name="cpgChannel"></param>
/// <param name="system"></param>
private void P25EncodeAudioFrame(byte[] pcm, PeerSystem fne, ChannelBox channel, Codeplug.Channel cpgChannel, Codeplug.System system)
{
bool encryptCall = true; // TODO: make this dynamic somewhere?
if (channel.p25N > 17)
channel.p25N = 0;
if (channel.p25N == 0)
FneUtils.Memset(channel.netLDU1, 0, 9 * 25);
if (channel.p25N == 9)
FneUtils.Memset(channel.netLDU2, 0, 9 * 25);
// Log.Logger.Debug($"BYTE BUFFER {FneUtils.HexDump(pcm)}");
//// pre-process: apply gain to PCM audio frames
//if (Program.Configuration.TxAudioGain != 1.0f)
//{
// BufferedWaveProvider buffer = new BufferedWaveProvider(waveFormat);
// buffer.AddSamples(pcm, 0, pcm.Length);
// VolumeWaveProvider16 gainControl = new VolumeWaveProvider16(buffer);
// gainControl.Volume = Program.Configuration.TxAudioGain;
// gainControl.Read(pcm, 0, pcm.Length);
//}
int smpIdx = 0;
short[] samples = new short[FneSystemBase.MBE_SAMPLES_LENGTH];
for (int pcmIdx = 0; pcmIdx < pcm.Length; pcmIdx += 2)
{
samples[smpIdx] = (short)((pcm[pcmIdx + 1] << 8) + pcm[pcmIdx + 0]);
smpIdx++;
}
channel.VolumeMeterLevel = 0;
float max = 0;
for (int index = 0; index < samples.Length; index++)
{
short sample = samples[index];
// to floating point
float sample32 = sample / 32768f;
if (sample32 < 0)
sample32 = -sample32;
// is this the max value?
if (sample32 > max)
max = sample32;
}
channel.VolumeMeterLevel = max;
// Convert to floats
float[] fSamples = AudioConverter.PcmToFloat(samples);
// Convert to signal
DiscreteSignal signal = new DiscreteSignal(8000, fSamples, true);
// encode PCM samples into IMBE codewords
byte[] imbe = new byte[FneSystemBase.IMBE_BUF_LEN];
int tone = 0;
if (true) // TODO: Disable/enable detection
{
tone = channel.ToneDetector.Detect(signal);
}
if (tone > 0)
{
MBEToneGenerator.IMBEEncodeSingleTone((ushort)tone, imbe);
Log.WriteLine($"({system.Name}) P25D: {tone} HZ TONE DETECT");
}
else
{
// do we have the external vocoder library?
if (channel.ExternalVocoderEnabled)
{
if (channel.ExtFullRateVocoder == null)
channel.ExtFullRateVocoder = new AmbeVocoder(true);
channel.ExtFullRateVocoder.encode(samples, out imbe);
}
else
{
if (channel.Encoder == null)
channel.Encoder = new MBEEncoder(MBE_MODE.IMBE_88BIT);
channel.Encoder.encode(samples, imbe);
}
}
// Log.Logger.Debug($"IMBE {FneUtils.HexDump(imbe)}");
if (encryptCall && cpgChannel.GetAlgoId() != 0 && cpgChannel.GetKeyId() != 0)
{
// initial HDU MI
if (channel.p25N == 0)
{
if (channel.mi.All(b => b == 0))
{
Random random = new Random();
for (int i = 0; i < P25Defines.P25_MI_LENGTH; i++)
channel.mi[i] = (byte)random.Next(0x00, 0x100);
}
channel.Crypter.Prepare(cpgChannel.GetAlgoId(), cpgChannel.GetKeyId(), channel.mi);
}
// crypto time
channel.Crypter.Process(imbe, channel.p25N < 9U ? P25DUID.LDU1 : P25DUID.LDU2);
// last block of LDU2, prepare a new MI
if (channel.p25N == 17U)
{
P25Crypto.CycleP25Lfsr(channel.mi);
channel.Crypter.Prepare(cpgChannel.GetAlgoId(), cpgChannel.GetKeyId(), channel.mi);
}
}
// fill the LDU buffers appropriately
switch (channel.p25N)
{
// LDU1
case 0:
Buffer.BlockCopy(imbe, 0, channel.netLDU1, 10, FneSystemBase.IMBE_BUF_LEN);
break;
case 1:
Buffer.BlockCopy(imbe, 0, channel.netLDU1, 26, FneSystemBase.IMBE_BUF_LEN);
break;
case 2:
Buffer.BlockCopy(imbe, 0, channel.netLDU1, 55, FneSystemBase.IMBE_BUF_LEN);
break;
case 3:
Buffer.BlockCopy(imbe, 0, channel.netLDU1, 80, FneSystemBase.IMBE_BUF_LEN);
break;
case 4:
Buffer.BlockCopy(imbe, 0, channel.netLDU1, 105, FneSystemBase.IMBE_BUF_LEN);
break;
case 5:
Buffer.BlockCopy(imbe, 0, channel.netLDU1, 130, FneSystemBase.IMBE_BUF_LEN);
break;
case 6:
Buffer.BlockCopy(imbe, 0, channel.netLDU1, 155, FneSystemBase.IMBE_BUF_LEN);
break;
case 7:
Buffer.BlockCopy(imbe, 0, channel.netLDU1, 180, FneSystemBase.IMBE_BUF_LEN);
break;
case 8:
Buffer.BlockCopy(imbe, 0, channel.netLDU1, 204, FneSystemBase.IMBE_BUF_LEN);
break;
// LDU2
case 9:
Buffer.BlockCopy(imbe, 0, channel.netLDU2, 10, FneSystemBase.IMBE_BUF_LEN);
break;
case 10:
Buffer.BlockCopy(imbe, 0, channel.netLDU2, 26, FneSystemBase.IMBE_BUF_LEN);
break;
case 11:
Buffer.BlockCopy(imbe, 0, channel.netLDU2, 55, FneSystemBase.IMBE_BUF_LEN);
break;
case 12:
Buffer.BlockCopy(imbe, 0, channel.netLDU2, 80, FneSystemBase.IMBE_BUF_LEN);
break;
case 13:
Buffer.BlockCopy(imbe, 0, channel.netLDU2, 105, FneSystemBase.IMBE_BUF_LEN);
break;
case 14:
Buffer.BlockCopy(imbe, 0, channel.netLDU2, 130, FneSystemBase.IMBE_BUF_LEN);
break;
case 15:
Buffer.BlockCopy(imbe, 0, channel.netLDU2, 155, FneSystemBase.IMBE_BUF_LEN);
break;
case 16:
Buffer.BlockCopy(imbe, 0, channel.netLDU2, 180, FneSystemBase.IMBE_BUF_LEN);
break;
case 17:
Buffer.BlockCopy(imbe, 0, channel.netLDU2, 204, FneSystemBase.IMBE_BUF_LEN);
break;
}
uint srcId = uint.Parse(system.Rid);
uint dstId = uint.Parse(cpgChannel.Tgid);
FnePeer peer = fne.peer;
RemoteCallData callData = new RemoteCallData()
{
SrcId = srcId,
DstId = dstId,
LCO = P25Defines.LC_GROUP
};
// make sure we have a valid stream ID
if (channel.TxStreamId == 0)
Log.WriteWarning($"({channel.SystemName}) P25D: Traffic *VOICE FRAME * Stream ID not set for traffic? Shouldn't happen.");
// send P25 LDU1
if (channel.p25N == 8U)
{
// bryanb: in multi-TG architecture we cannot use the pktSeq helper singleton in the FNE peer class otherwise we won't
// maintain outgoing RTP packet sequences properly
if (channel.p25SeqNo == 0U)
channel.pktSeq = 0;
else
{
ushort curr = channel.pktSeq;
++channel.pktSeq;
if (channel.pktSeq > (Constants.RtpCallEndSeq - 1))
channel.pktSeq = 0;
}
Log.WriteLine($"({channel.SystemName}) P25D: Traffic *VOICE FRAME LDU1* PEER {fne.PeerId} SRC_ID {srcId} TGID {dstId} [STREAM ID {channel.TxStreamId} SEQ {channel.p25SeqNo}]");
byte[] payload = new byte[200];
fne.CreateNewP25MessageHdr((byte)P25DUID.LDU1, callData, ref payload, cpgChannel.GetAlgoId(), cpgChannel.GetKeyId(), channel.mi);
fne.CreateP25LDU1Message(channel.netLDU1, ref payload, srcId, dstId);
peer.SendMaster(new Tuple<byte, byte>(Constants.NET_FUNC_PROTOCOL, Constants.NET_PROTOCOL_SUBFUNC_P25), payload, channel.pktSeq, channel.TxStreamId);
}
// send P25 LDU2
if (channel.p25N == 17U)
{
// bryanb: in multi-TG architecture we cannot use the pktSeq helper singleton in the FNE peer class otherwise we won't
// maintain outgoing RTP packet sequences properly
if (channel.p25SeqNo == 0U)
channel.pktSeq = 0;
else
{
ushort curr = channel.pktSeq;
++channel.pktSeq;
if (channel.pktSeq > (Constants.RtpCallEndSeq - 1))
channel.pktSeq = 0;
}
Log.WriteLine($"({channel.SystemName}) P25D: Traffic *VOICE FRAME LDU2* PEER {fne.PeerId} SRC_ID {srcId} TGID {dstId} [STREAM ID {channel.TxStreamId} SEQ {channel.p25SeqNo}]");
byte[] payload = new byte[200];
fne.CreateNewP25MessageHdr((byte)P25DUID.LDU2, callData, ref payload, cpgChannel.GetAlgoId(), cpgChannel.GetKeyId(), channel.mi);
fne.CreateP25LDU2Message(channel.netLDU2, ref payload, new CryptoParams { AlgId = cpgChannel.GetAlgoId(), KeyId = cpgChannel.GetKeyId(), MI = channel.mi });
peer.SendMaster(new Tuple<byte, byte>(Constants.NET_FUNC_PROTOCOL, Constants.NET_PROTOCOL_SUBFUNC_P25), payload, channel.pktSeq, channel.TxStreamId);
}
channel.p25SeqNo++;
channel.p25N++;
}
/// <summary>
/// Helper to decode and playback P25 IMBE frames as PCM audio.
/// </summary>
/// <param name="ldu"></param>
/// <param name="e"></param>
/// <param name="system"></param>
/// <param name="channel"></param>
/// <param name="emergency"></param>
/// <param name="duid"></param>
private void P25DecodeAudioFrame(byte[] ldu, P25DataReceivedEvent e, PeerSystem system, ChannelBox channel, bool emergency = false, P25DUID duid = P25DUID.LDU1)
{
try
{
// decode 9 IMBE codewords into PCM samples
for (int n = 0; n < 9; n++)
{
byte[] imbe = new byte[FneSystemBase.IMBE_BUF_LEN];
switch (n)
{
case 0:
Buffer.BlockCopy(ldu, 10, imbe, 0, FneSystemBase.IMBE_BUF_LEN);
break;
case 1:
Buffer.BlockCopy(ldu, 26, imbe, 0, FneSystemBase.IMBE_BUF_LEN);
break;
case 2:
Buffer.BlockCopy(ldu, 55, imbe, 0, FneSystemBase.IMBE_BUF_LEN);
break;
case 3:
Buffer.BlockCopy(ldu, 80, imbe, 0, FneSystemBase.IMBE_BUF_LEN);
break;
case 4:
Buffer.BlockCopy(ldu, 105, imbe, 0, FneSystemBase.IMBE_BUF_LEN);
break;
case 5:
Buffer.BlockCopy(ldu, 130, imbe, 0, FneSystemBase.IMBE_BUF_LEN);
break;
case 6:
Buffer.BlockCopy(ldu, 155, imbe, 0, FneSystemBase.IMBE_BUF_LEN);
break;
case 7:
Buffer.BlockCopy(ldu, 180, imbe, 0, FneSystemBase.IMBE_BUF_LEN);
break;
case 8:
Buffer.BlockCopy(ldu, 204, imbe, 0, FneSystemBase.IMBE_BUF_LEN);
break;
}
//Log.Logger.Debug($"Decoding IMBE buffer: {FneUtils.HexDump(imbe)}");
short[] samples = new short[FneSystemBase.MBE_SAMPLES_LENGTH];
channel.Crypter.Process(imbe, duid);
// do we have the external vocoder library?
if (channel.ExternalVocoderEnabled)
{
if (channel.ExtFullRateVocoder == null)
channel.ExtFullRateVocoder = new AmbeVocoder(true);
channel.p25Errs = channel.ExtFullRateVocoder.decode(imbe, out samples);
}
else
channel.p25Errs = channel.Decoder.decode(imbe, samples);
if (emergency && !channel.Emergency)
{
Task.Run(() =>
{
HandleEmergency(e.DstId.ToString(), e.SrcId.ToString());
});
}
if (samples != null)
{
Log.WriteLine($"P25D: Traffic *VOICE FRAME * PEER {e.PeerId} SRC_ID {e.SrcId} TGID {e.DstId} VC{n} [STREAM ID {e.StreamId}]");
channel.VolumeMeterLevel = 0;
float max = 0;
for (int index = 0; index < samples.Length; index++)
{
short sample = samples[index];
// to floating point
float sample32 = sample / 32768f;
if (sample32 < 0)
sample32 = -sample32;
// is this the max value?
if (sample32 > max)
max = sample32;
}
channel.VolumeMeterLevel = max;
int pcmIdx = 0;
byte[] pcmData = new byte[samples.Length * 2];
for (int i = 0; i < samples.Length; i++)
{
pcmData[pcmIdx] = (byte)(samples[i] & 0xFF);
pcmData[pcmIdx + 1] = (byte)((samples[i] >> 8) & 0xFF);
pcmIdx += 2;
}
audioManager.AddTalkgroupStream(e.DstId.ToString(), pcmData);
}
}
}
catch (Exception ex)
{
Log.WriteLine($"Audio Decode Exception: {ex.Message}");
Log.StackTrace(ex, false);
}
}
/// <summary>
/// Event handler used to process incoming P25 data.
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
public void P25DataReceived(P25DataReceivedEvent e, DateTime pktTime)
{
uint sysId = (uint)((e.Data[11U] << 8) | (e.Data[12U] << 0));
uint netId = FneUtils.Bytes3ToUInt32(e.Data, 16);
byte control = e.Data[14U];
byte len = e.Data[23];
byte[] data = new byte[len];
for (int i = 24; i < len; i++)
data[i - 24] = e.Data[i];
Dispatcher.Invoke(() =>
{
foreach (ChannelBox channel in selectedChannelsManager.GetSelectedChannels())
{
if (channel.SystemName == PLAYBACKSYS || channel.ChannelName == PLAYBACKCHNAME || channel.DstId == PLAYBACKTG)
continue;
Codeplug.System system = Codeplug.GetSystemForChannel(channel.ChannelName);
Codeplug.Channel cpgChannel = Codeplug.GetChannelByName(channel.ChannelName);
if (cpgChannel.GetChannelMode() != Codeplug.ChannelMode.P25)
continue;
bool isEmergency = false;
bool encrypted = false;
PeerSystem handler = fneSystemManager.GetFneSystem(system.Name);
if (!channel.IsEnabled || channel.Name == PLAYBACKCHNAME)
continue;
if (cpgChannel.Tgid != e.DstId.ToString())
continue;
if (!systemStatuses.ContainsKey(cpgChannel.Name))
systemStatuses[cpgChannel.Name] = new SlotStatus();
if (channel.Decoder == null)
channel.Decoder = new MBEDecoder(MBE_MODE.IMBE_88BIT);
SlotStatus slot = systemStatuses[cpgChannel.Name];
// if this is an LDU1 see if this is the first LDU with HDU encryption data
if (e.DUID == P25DUID.LDU1)
{
byte frameType = e.Data[180];
// get the initial MI and other enc info (bug found by the screeeeeeeeech on initial tx...)
if (frameType == P25Defines.P25_FT_HDU_VALID)
{
channel.algId = e.Data[181];
channel.kId = (ushort)((e.Data[182] << 8) | e.Data[183]);
Array.Copy(e.Data, 184, channel.mi, 0, P25Defines.P25_MI_LENGTH);
channel.Crypter.Prepare(channel.algId, channel.kId, channel.mi);
if (channel.algId != P25Defines.P25_ALGO_UNENCRYPT)
encrypted = true;
}
}
// is this a new call stream?
if (e.StreamId != slot.RxStreamId && ((e.DUID != P25DUID.TDU) && (e.DUID != P25DUID.TDULC)))
{
channel.IsReceiving = true;
slot.RxStart = pktTime;
Log.WriteLine($"({system.Name}) P25D: Traffic *CALL START * PEER {e.PeerId} SRC_ID {e.SrcId} TGID {e.DstId} ALGID {channel.algId} KID {channel.kId} [STREAM ID {e.StreamId}]");
FneUtils.Memset(channel.mi, 0x00, P25Defines.P25_MI_LENGTH);
callHistoryWindow.AddCall(cpgChannel.Name, (int)e.SrcId, (int)e.DstId);
callHistoryWindow.ChannelKeyed(cpgChannel.Name, (int)e.SrcId, encrypted);
string alias = string.Empty;
try
{
alias = AliasTools.GetAliasByRid(system.RidAlias, (int)e.SrcId);
}
catch (Exception) { }
if (string.IsNullOrEmpty(alias))
channel.LastSrcId = "Last ID: " + e.SrcId;
else
channel.LastSrcId = "Last: " + alias;
if (channel.algId != P25Defines.P25_ALGO_UNENCRYPT)
{
channel.Background = ChannelBox.ORANGE_GRADIENT;
Log.WriteLine($"({system.Name}) P25D: Traffic *CALL ENC PARMS * PEER {e.PeerId} SRC_ID {e.SrcId} TGID {e.DstId} ALGID {channel.algId} KID {channel.kId} [STREAM ID {e.StreamId}]");
}
else
channel.Background = ChannelBox.GREEN_GRADIENT;
}
// is the call over?
if (((e.DUID == P25DUID.TDU) || (e.DUID == P25DUID.TDULC)) && (slot.RxType != fnecore.FrameType.TERMINATOR))
{
channel.IsReceiving = false;
TimeSpan callDuration = pktTime - slot.RxStart;
Log.WriteLine($"({system.Name}) P25D: Traffic *CALL END * PEER {e.PeerId} SRC_ID {e.SrcId} TGID {e.DstId} DUR {callDuration} [STREAM ID {e.StreamId}]");
channel.Background = ChannelBox.BLUE_GRADIENT;
channel.VolumeMeterLevel = 0;
callHistoryWindow.ChannelUnkeyed(cpgChannel.Name, (int)e.SrcId);
return;
}
if ((channel.algId != cpgChannel.GetAlgoId() || channel.kId != cpgChannel.GetKeyId()) && channel.algId != P25Defines.P25_ALGO_UNENCRYPT)
continue;
byte[] newMI = new byte[P25Defines.P25_MI_LENGTH];
int count = 0;
switch (e.DUID)
{
case P25DUID.LDU1:
{
// The '62', '63', '64', '65', '66', '67', '68', '69', '6A' records are LDU1
if ((data[0U] == 0x62U) && (data[22U] == 0x63U) &&
(data[36U] == 0x64U) && (data[53U] == 0x65U) &&
(data[70U] == 0x66U) && (data[87U] == 0x67U) &&
(data[104U] == 0x68U) && (data[121U] == 0x69U) &&
(data[138U] == 0x6AU))
{
// The '62' record - IMBE Voice 1
Buffer.BlockCopy(data, count, channel.netLDU1, 0, 22);
count += 22;
// The '63' record - IMBE Voice 2
Buffer.BlockCopy(data, count, channel.netLDU1, 25, 14);
count += 14;
// The '64' record - IMBE Voice 3 + Link Control
Buffer.BlockCopy(data, count, channel.netLDU1, 50, 17);
byte serviceOptions = data[count + 3];
isEmergency = (serviceOptions & 0x80) == 0x80;
count += 17;
// The '65' record - IMBE Voice 4 + Link Control
Buffer.BlockCopy(data, count, channel.netLDU1, 75, 17);
count += 17;
// The '66' record - IMBE Voice 5 + Link Control
Buffer.BlockCopy(data, count, channel.netLDU1, 100, 17);
count += 17;
// The '67' record - IMBE Voice 6 + Link Control
Buffer.BlockCopy(data, count, channel.netLDU1, 125, 17);
count += 17;
// The '68' record - IMBE Voice 7 + Link Control
Buffer.BlockCopy(data, count, channel.netLDU1, 150, 17);
count += 17;
// The '69' record - IMBE Voice 8 + Link Control
Buffer.BlockCopy(data, count, channel.netLDU1, 175, 17);
count += 17;
// The '6A' record - IMBE Voice 9 + Low Speed Data
Buffer.BlockCopy(data, count, channel.netLDU1, 200, 16);
count += 16;
// decode 9 IMBE codewords into PCM samples
P25DecodeAudioFrame(channel.netLDU1, e, handler, channel, isEmergency);
}
}
break;
case P25DUID.LDU2:
{
// The '6B', '6C', '6D', '6E', '6F', '70', '71', '72', '73' records are LDU2
if ((data[0U] == 0x6BU) && (data[22U] == 0x6CU) &&
(data[36U] == 0x6DU) && (data[53U] == 0x6EU) &&
(data[70U] == 0x6FU) && (data[87U] == 0x70U) &&
(data[104U] == 0x71U) && (data[121U] == 0x72U) &&
(data[138U] == 0x73U))
{
// The '6B' record - IMBE Voice 10
Buffer.BlockCopy(data, count, channel.netLDU2, 0, 22);
count += 22;
// The '6C' record - IMBE Voice 11
Buffer.BlockCopy(data, count, channel.netLDU2, 25, 14);
count += 14;
// The '6D' record - IMBE Voice 12 + Encryption Sync
Buffer.BlockCopy(data, count, channel.netLDU2, 50, 17);
newMI[0] = data[count + 1];
newMI[1] = data[count + 2];
newMI[2] = data[count + 3];
count += 17;
// The '6E' record - IMBE Voice 13 + Encryption Sync
Buffer.BlockCopy(data, count, channel.netLDU2, 75, 17);
newMI[3] = data[count + 1];
newMI[4] = data[count + 2];
newMI[5] = data[count + 3];
count += 17;
// The '6F' record - IMBE Voice 14 + Encryption Sync
Buffer.BlockCopy(data, count, channel.netLDU2, 100, 17);
newMI[6] = data[count + 1];
newMI[7] = data[count + 2];
newMI[8] = data[count + 3];
count += 17;
// The '70' record - IMBE Voice 15 + Encryption Sync
Buffer.BlockCopy(data, count, channel.netLDU2, 125, 17);
channel.algId = data[count + 1]; // Algorithm ID
channel.kId = (ushort)((data[count + 2] << 8) | data[count + 3]); // Key ID
count += 17;
// The '71' record - IMBE Voice 16 + Encryption Sync
Buffer.BlockCopy(data, count, channel.netLDU2, 150, 17);
count += 17;
// The '72' record - IMBE Voice 17 + Encryption Sync
Buffer.BlockCopy(data, count, channel.netLDU2, 175, 17);
count += 17;
// The '73' record - IMBE Voice 18 + Low Speed Data
Buffer.BlockCopy(data, count, channel.netLDU2, 200, 16);
count += 16;
if (channel.p25Errs > 0) // temp, need to actually get errors I guess
P25Crypto.CycleP25Lfsr(channel.mi);
else
Array.Copy(newMI, channel.mi, P25Defines.P25_MI_LENGTH);
// decode 9 IMBE codewords into PCM samples
P25DecodeAudioFrame(channel.netLDU2, e, handler, channel, isEmergency, P25DUID.LDU2);
}
}
break;
}
if (channel.mi != null)
channel.Crypter.Prepare(channel.algId, channel.kId, channel.mi);
slot.RxRFS = e.SrcId;
slot.RxType = e.FrameType;
slot.RxTGId = e.DstId;
slot.RxTime = pktTime;
slot.RxStreamId = e.StreamId;
}
});
}
} // public partial class MainWindow : Window
} // namespace dvmconsole

957
dvmconsole/MainWindow.xaml.cs
Unescape View File

@ -2097,575 +2097,7 @@ namespace dvmconsole
/** fnecore Hooks / Helpers */
/// <summary>
/// Helper to encode and transmit PCM audio as P25 IMBE frames.
/// </summary>
/// <param name="pcm"></param>
/// <param name="fne"></param>
/// <param name="channel"></param>
/// <param name="cpgChannel"></param>
/// <param name="system"></param>
private void P25EncodeAudioFrame(byte[] pcm, PeerSystem fne, ChannelBox channel, Codeplug.Channel cpgChannel, Codeplug.System system)
{
bool encryptCall = true; // TODO: make this dynamic somewhere?
if (channel.p25N > 17)
channel.p25N = 0;
if (channel.p25N == 0)
FneUtils.Memset(channel.netLDU1, 0, 9 * 25);
if (channel.p25N == 9)
FneUtils.Memset(channel.netLDU2, 0, 9 * 25);
// Log.Logger.Debug($"BYTE BUFFER {FneUtils.HexDump(pcm)}");
//// pre-process: apply gain to PCM audio frames
//if (Program.Configuration.TxAudioGain != 1.0f)
//{
// BufferedWaveProvider buffer = new BufferedWaveProvider(waveFormat);
// buffer.AddSamples(pcm, 0, pcm.Length);
// VolumeWaveProvider16 gainControl = new VolumeWaveProvider16(buffer);
// gainControl.Volume = Program.Configuration.TxAudioGain;
// gainControl.Read(pcm, 0, pcm.Length);
//}
int smpIdx = 0;
short[] samples = new short[FneSystemBase.MBE_SAMPLES_LENGTH];
for (int pcmIdx = 0; pcmIdx < pcm.Length; pcmIdx += 2)
{
samples[smpIdx] = (short)((pcm[pcmIdx + 1] << 8) + pcm[pcmIdx + 0]);
smpIdx++;
}
channel.VolumeMeterLevel = 0;
float max = 0;
for (int index = 0; index < samples.Length; index++)
{
short sample = samples[index];
// to floating point
float sample32 = sample / 32768f;
if (sample32 < 0)
sample32 = -sample32;
// is this the max value?
if (sample32 > max)
max = sample32;
}
channel.VolumeMeterLevel = max;
// Convert to floats
float[] fSamples = AudioConverter.PcmToFloat(samples);
// Convert to signal
DiscreteSignal signal = new DiscreteSignal(8000, fSamples, true);
// encode PCM samples into IMBE codewords
byte[] imbe = new byte[FneSystemBase.IMBE_BUF_LEN];
int tone = 0;
if (true) // TODO: Disable/enable detection
{
tone = channel.ToneDetector.Detect(signal);
}
if (tone > 0)
{
MBEToneGenerator.IMBEEncodeSingleTone((ushort)tone, imbe);
Log.WriteLine($"({system.Name}) P25D: {tone} HZ TONE DETECT");
}
else
{
// do we have the external vocoder library?
if (channel.ExternalVocoderEnabled)
{
if (channel.ExtFullRateVocoder == null)
channel.ExtFullRateVocoder = new AmbeVocoder(true);
channel.ExtFullRateVocoder.encode(samples, out imbe);
}
else
{
if (channel.Encoder == null)
channel.Encoder = new MBEEncoder(MBE_MODE.IMBE_88BIT);
channel.Encoder.encode(samples, imbe);
}
}
// Log.Logger.Debug($"IMBE {FneUtils.HexDump(imbe)}");
if (encryptCall && cpgChannel.GetAlgoId() != 0 && cpgChannel.GetKeyId() != 0)
{
// initial HDU MI
if (channel.p25N == 0)
{
if (channel.mi.All(b => b == 0))
{
Random random = new Random();
for (int i = 0; i < P25Defines.P25_MI_LENGTH; i++)
channel.mi[i] = (byte)random.Next(0x00, 0x100);
}
channel.Crypter.Prepare(cpgChannel.GetAlgoId(), cpgChannel.GetKeyId(), channel.mi);
}
// crypto time
channel.Crypter.Process(imbe, channel.p25N < 9U ? P25DUID.LDU1 : P25DUID.LDU2);
// last block of LDU2, prepare a new MI
if (channel.p25N == 17U)
{
P25Crypto.CycleP25Lfsr(channel.mi);
channel.Crypter.Prepare(cpgChannel.GetAlgoId(), cpgChannel.GetKeyId(), channel.mi);
}
}
// fill the LDU buffers appropriately
switch (channel.p25N)
{
// LDU1
case 0:
Buffer.BlockCopy(imbe, 0, channel.netLDU1, 10, FneSystemBase.IMBE_BUF_LEN);
break;
case 1:
Buffer.BlockCopy(imbe, 0, channel.netLDU1, 26, FneSystemBase.IMBE_BUF_LEN);
break;
case 2:
Buffer.BlockCopy(imbe, 0, channel.netLDU1, 55, FneSystemBase.IMBE_BUF_LEN);
break;
case 3:
Buffer.BlockCopy(imbe, 0, channel.netLDU1, 80, FneSystemBase.IMBE_BUF_LEN);
break;
case 4:
Buffer.BlockCopy(imbe, 0, channel.netLDU1, 105, FneSystemBase.IMBE_BUF_LEN);
break;
case 5:
Buffer.BlockCopy(imbe, 0, channel.netLDU1, 130, FneSystemBase.IMBE_BUF_LEN);
break;
case 6:
Buffer.BlockCopy(imbe, 0, channel.netLDU1, 155, FneSystemBase.IMBE_BUF_LEN);
break;
case 7:
Buffer.BlockCopy(imbe, 0, channel.netLDU1, 180, FneSystemBase.IMBE_BUF_LEN);
break;
case 8:
Buffer.BlockCopy(imbe, 0, channel.netLDU1, 204, FneSystemBase.IMBE_BUF_LEN);
break;
// LDU2
case 9:
Buffer.BlockCopy(imbe, 0, channel.netLDU2, 10, FneSystemBase.IMBE_BUF_LEN);
break;
case 10:
Buffer.BlockCopy(imbe, 0, channel.netLDU2, 26, FneSystemBase.IMBE_BUF_LEN);
break;
case 11:
Buffer.BlockCopy(imbe, 0, channel.netLDU2, 55, FneSystemBase.IMBE_BUF_LEN);
break;
case 12:
Buffer.BlockCopy(imbe, 0, channel.netLDU2, 80, FneSystemBase.IMBE_BUF_LEN);
break;
case 13:
Buffer.BlockCopy(imbe, 0, channel.netLDU2, 105, FneSystemBase.IMBE_BUF_LEN);
break;
case 14:
Buffer.BlockCopy(imbe, 0, channel.netLDU2, 130, FneSystemBase.IMBE_BUF_LEN);
break;
case 15:
Buffer.BlockCopy(imbe, 0, channel.netLDU2, 155, FneSystemBase.IMBE_BUF_LEN);
break;
case 16:
Buffer.BlockCopy(imbe, 0, channel.netLDU2, 180, FneSystemBase.IMBE_BUF_LEN);
break;
case 17:
Buffer.BlockCopy(imbe, 0, channel.netLDU2, 204, FneSystemBase.IMBE_BUF_LEN);
break;
}
uint srcId = uint.Parse(system.Rid);
uint dstId = uint.Parse(cpgChannel.Tgid);
FnePeer peer = fne.peer;
RemoteCallData callData = new RemoteCallData()
{
SrcId = srcId,
DstId = dstId,
LCO = P25Defines.LC_GROUP
};
// make sure we have a valid stream ID
if (channel.TxStreamId == 0)
Log.WriteWarning($"({channel.SystemName}) P25D: Traffic *VOICE FRAME * Stream ID not set for traffic? Shouldn't happen.");
// send P25 LDU1
if (channel.p25N == 8U)
{
// bryanb: in multi-TG architecture we cannot use the pktSeq helper singleton in the FNE peer class otherwise we won't
// maintain outgoing RTP packet sequences properly
if (channel.p25SeqNo == 0U)
channel.pktSeq = 0;
else
{
ushort curr = channel.pktSeq;
++channel.pktSeq;
if (channel.pktSeq > (Constants.RtpCallEndSeq - 1))
channel.pktSeq = 0;
}
Log.WriteLine($"({channel.SystemName}) P25D: Traffic *VOICE FRAME LDU1* PEER {fne.PeerId} SRC_ID {srcId} TGID {dstId} [STREAM ID {channel.TxStreamId} SEQ {channel.p25SeqNo}]");
byte[] payload = new byte[200];
fne.CreateNewP25MessageHdr((byte)P25DUID.LDU1, callData, ref payload, cpgChannel.GetAlgoId(), cpgChannel.GetKeyId(), channel.mi);
fne.CreateP25LDU1Message(channel.netLDU1, ref payload, srcId, dstId);
peer.SendMaster(new Tuple<byte, byte>(Constants.NET_FUNC_PROTOCOL, Constants.NET_PROTOCOL_SUBFUNC_P25), payload, channel.pktSeq, channel.TxStreamId);
}
// send P25 LDU2
if (channel.p25N == 17U)
{
// bryanb: in multi-TG architecture we cannot use the pktSeq helper singleton in the FNE peer class otherwise we won't
// maintain outgoing RTP packet sequences properly
if (channel.p25SeqNo == 0U)
channel.pktSeq = 0;
else
{
ushort curr = channel.pktSeq;
++channel.pktSeq;
if (channel.pktSeq > (Constants.RtpCallEndSeq - 1))
channel.pktSeq = 0;
}
Log.WriteLine($"({channel.SystemName}) P25D: Traffic *VOICE FRAME LDU2* PEER {fne.PeerId} SRC_ID {srcId} TGID {dstId} [STREAM ID {channel.TxStreamId} SEQ {channel.p25SeqNo}]");
byte[] payload = new byte[200];
fne.CreateNewP25MessageHdr((byte)P25DUID.LDU2, callData, ref payload, cpgChannel.GetAlgoId(), cpgChannel.GetKeyId(), channel.mi);
fne.CreateP25LDU2Message(channel.netLDU2, ref payload, new CryptoParams { AlgId = cpgChannel.GetAlgoId(), KeyId = cpgChannel.GetKeyId(), MI = channel.mi });
peer.SendMaster(new Tuple<byte, byte>(Constants.NET_FUNC_PROTOCOL, Constants.NET_PROTOCOL_SUBFUNC_P25), payload, channel.pktSeq, channel.TxStreamId);
}
channel.p25SeqNo++;
channel.p25N++;
}
/// <summary>
/// Helper to decode and playback P25 IMBE frames as PCM audio.
/// </summary>
/// <param name="ldu"></param>
/// <param name="e"></param>
/// <param name="system"></param>
/// <param name="channel"></param>
/// <param name="emergency"></param>
/// <param name="duid"></param>
private void P25DecodeAudioFrame(byte[] ldu, P25DataReceivedEvent e, PeerSystem system, ChannelBox channel, bool emergency = false, P25DUID duid = P25DUID.LDU1)
{
try
{
// decode 9 IMBE codewords into PCM samples
for (int n = 0; n < 9; n++)
{
byte[] imbe = new byte[FneSystemBase.IMBE_BUF_LEN];
switch (n)
{
case 0:
Buffer.BlockCopy(ldu, 10, imbe, 0, FneSystemBase.IMBE_BUF_LEN);
break;
case 1:
Buffer.BlockCopy(ldu, 26, imbe, 0, FneSystemBase.IMBE_BUF_LEN);
break;
case 2:
Buffer.BlockCopy(ldu, 55, imbe, 0, FneSystemBase.IMBE_BUF_LEN);
break;
case 3:
Buffer.BlockCopy(ldu, 80, imbe, 0, FneSystemBase.IMBE_BUF_LEN);
break;
case 4:
Buffer.BlockCopy(ldu, 105, imbe, 0, FneSystemBase.IMBE_BUF_LEN);
break;
case 5:
Buffer.BlockCopy(ldu, 130, imbe, 0, FneSystemBase.IMBE_BUF_LEN);
break;
case 6:
Buffer.BlockCopy(ldu, 155, imbe, 0, FneSystemBase.IMBE_BUF_LEN);
break;
case 7:
Buffer.BlockCopy(ldu, 180, imbe, 0, FneSystemBase.IMBE_BUF_LEN);
break;
case 8:
Buffer.BlockCopy(ldu, 204, imbe, 0, FneSystemBase.IMBE_BUF_LEN);
break;
}
//Log.Logger.Debug($"Decoding IMBE buffer: {FneUtils.HexDump(imbe)}");
short[] samples = new short[FneSystemBase.MBE_SAMPLES_LENGTH];
channel.Crypter.Process(imbe, duid);
// do we have the external vocoder library?
if (channel.ExternalVocoderEnabled)
{
if (channel.ExtFullRateVocoder == null)
channel.ExtFullRateVocoder = new AmbeVocoder(true);
channel.p25Errs = channel.ExtFullRateVocoder.decode(imbe, out samples);
}
else
channel.p25Errs = channel.Decoder.decode(imbe, samples);
if (emergency && !channel.Emergency)
{
Task.Run(() =>
{
HandleEmergency(e.DstId.ToString(), e.SrcId.ToString());
});
}
if (samples != null)
{
Log.WriteLine($"P25D: Traffic *VOICE FRAME * PEER {e.PeerId} SRC_ID {e.SrcId} TGID {e.DstId} VC{n} [STREAM ID {e.StreamId}]");
channel.VolumeMeterLevel = 0;
float max = 0;
for (int index = 0; index < samples.Length; index++)
{
short sample = samples[index];
// to floating point
float sample32 = sample / 32768f;
if (sample32 < 0)
sample32 = -sample32;
// is this the max value?
if (sample32 > max)
max = sample32;
}
channel.VolumeMeterLevel = max;
int pcmIdx = 0;
byte[] pcmData = new byte[samples.Length * 2];
for (int i = 0; i < samples.Length; i++)
{
pcmData[pcmIdx] = (byte)(samples[i] & 0xFF);
pcmData[pcmIdx + 1] = (byte)((samples[i] >> 8) & 0xFF);
pcmIdx += 2;
}
audioManager.AddTalkgroupStream(e.DstId.ToString(), pcmData);
}
}
}
catch (Exception ex)
{
Log.WriteLine($"Audio Decode Exception: {ex.Message}");
Log.StackTrace(ex, false);
}
}
/// <summary>
/// Helper to encode and transmit PCM audio as DMR AMBE frames.
/// </summary>
/// <param name="pcm"></param>
/// <param name="fne"></param>
/// <param name="channel"></param>
/// <param name="cpgChannel"></param>
/// <param name="system"></param>
private void DMREncodeAudioFrame(byte[] pcm, PeerSystem fne, ChannelBox channel, Codeplug.Channel cpgChannel, Codeplug.System system)
{
// make sure we have a valid stream ID
if (channel.TxStreamId == 0)
Log.WriteWarning($"({channel.SystemName}) DMRD: Traffic *VOICE FRAME * Stream ID not set for traffic? Shouldn't happen.");
try
{
byte slot = 1; // TODO: Support both time slots
byte[] data = null, dmrpkt = null;
channel.dmrN = (byte)(channel.dmrSeqNo % 6);
if (channel.ambeCount == FneSystemBase.AMBE_PER_SLOT)
{
// is this the intitial sequence?
if (channel.dmrSeqNo == 0)
{
channel.pktSeq = 0;
// send DMR voice header
data = new byte[FneSystemBase.DMR_FRAME_LENGTH_BYTES];
// generate DMR LC
LC dmrLC = new LC();
dmrLC.FLCO = (byte)DMRFLCO.FLCO_GROUP;
dmrLC.SrcId = uint.Parse(system.Rid);
dmrLC.DstId = uint.Parse(cpgChannel.Tgid);
channel.embeddedData.SetLC(dmrLC);
// generate the Slot Type
SlotType slotType = new SlotType();
slotType.DataType = (byte)DMRDataType.VOICE_LC_HEADER;
slotType.GetData(ref data);
FullLC.Encode(dmrLC, ref data, DMRDataType.VOICE_LC_HEADER);
// generate DMR network frame
dmrpkt = new byte[FneSystemBase.DMR_PACKET_SIZE];
fne.CreateDMRMessage(ref dmrpkt, uint.Parse(system.Rid), uint.Parse(cpgChannel.Tgid), slot, FrameType.VOICE_SYNC, (byte)channel.dmrSeqNo, 0);
Buffer.BlockCopy(data, 0, dmrpkt, 20, FneSystemBase.DMR_FRAME_LENGTH_BYTES);
fne.peer.SendMaster(new Tuple<byte, byte>(Constants.NET_FUNC_PROTOCOL, Constants.NET_PROTOCOL_SUBFUNC_DMR), dmrpkt, channel.pktSeq, channel.TxStreamId);
channel.dmrSeqNo++;
}
ushort curr = channel.pktSeq;
++channel.pktSeq;
if (channel.pktSeq > (Constants.RtpCallEndSeq - 1))
channel.pktSeq = 0;
// send DMR voice
data = new byte[FneSystemBase.DMR_FRAME_LENGTH_BYTES];
Buffer.BlockCopy(channel.ambeBuffer, 0, data, 0, 13);
data[13U] = (byte)(channel.ambeBuffer[13U] & 0xF0);
data[19U] = (byte)(channel.ambeBuffer[13U] & 0x0F);
Buffer.BlockCopy(channel.ambeBuffer, 14, data, 20, 13);
FrameType frameType = FrameType.VOICE_SYNC;
if (channel.dmrN == 0)
frameType = FrameType.VOICE_SYNC;
else
{
frameType = FrameType.VOICE;
byte lcss = channel.embeddedData.GetData(ref data, channel.dmrN);
// generated embedded signalling
EMB emb = new EMB();
emb.ColorCode = 0;
emb.LCSS = lcss;
emb.Encode(ref data);
}
// generate DMR network frame
dmrpkt = new byte[FneSystemBase.DMR_PACKET_SIZE];
fne.CreateDMRMessage(ref dmrpkt, uint.Parse(system.Rid), uint.Parse(cpgChannel.Tgid), 1, frameType, (byte)channel.dmrSeqNo, channel.dmrN);
Buffer.BlockCopy(data, 0, dmrpkt, 20, FneSystemBase.DMR_FRAME_LENGTH_BYTES);
fne.peer.SendMaster(new Tuple<byte, byte>(Constants.NET_FUNC_PROTOCOL, Constants.NET_PROTOCOL_SUBFUNC_DMR), dmrpkt, channel.pktSeq, channel.TxStreamId);
channel.dmrSeqNo++;
FneUtils.Memset(channel.ambeBuffer, 0, 27);
channel.ambeCount = 0;
}
int smpIdx = 0;
short[] samples = new short[MBE_SAMPLES_LENGTH];
for (int pcmIdx = 0; pcmIdx < pcm.Length; pcmIdx += 2)
{
samples[smpIdx] = (short)((pcm[pcmIdx + 1] << 8) + pcm[pcmIdx + 0]);
smpIdx++;
}
// encode PCM samples into AMBE codewords
byte[] ambe = null;
if (channel.ExternalVocoderEnabled)
{
if (channel.ExtHalfRateVocoder == null)
channel.ExtHalfRateVocoder = new AmbeVocoder(false);
channel.ExtHalfRateVocoder.encode(samples, out ambe, true);
}
else
{
if (channel.Encoder == null)
channel.Encoder = new MBEEncoder(MBE_MODE.DMR_AMBE);
channel.Encoder.encode(samples, ambe);
}
Buffer.BlockCopy(ambe, 0, channel.ambeBuffer, channel.ambeCount * 9, FneSystemBase.AMBE_BUF_LEN);
channel.ambeCount++;
}
catch (Exception ex)
{
Log.StackTrace(ex, false);
}
}
/// <summary>
/// Helper to decode and playback DMR AMBE frames as PCM audio.
/// </summary>
/// <param name="ambe"></param>
/// <param name="e"></param>
/// <param name="system"></param>
/// <param name="channel"></param>
private void DMRDecodeAudioFrame(byte[] ambe, DMRDataReceivedEvent e, PeerSystem system, ChannelBox channel)
{
try
{
// Log.Logger.Debug($"FULL AMBE {FneUtils.HexDump(ambe)}");
for (int n = 0; n < FneSystemBase.AMBE_PER_SLOT; n++)
{
byte[] ambePartial = new byte[FneSystemBase.AMBE_BUF_LEN];
for (int i = 0; i < FneSystemBase.AMBE_BUF_LEN; i++)
ambePartial[i] = ambe[i + (n * 9)];
short[] samples = null;
int errs = 0;
// do we have the external vocoder library?
if (channel.ExternalVocoderEnabled)
{
if (channel.ExtHalfRateVocoder == null)
channel.ExtHalfRateVocoder = new AmbeVocoder(false);
errs = channel.ExtHalfRateVocoder.decode(ambePartial, out samples);
}
else
{
samples = new short[FneSystemBase.MBE_SAMPLES_LENGTH];
errs = channel.Decoder.decode(ambePartial, samples);
}
if (samples != null)
{
//Log.WriteLine($"({system.SystemName}) DMRD: Traffic *VOICE FRAME * PEER {e.PeerId} SRC_ID {e.SrcId} TGID {e.DstId} TS {e.Slot + 1} VC{e.n}.{n} ERRS {errs} [STREAM ID {e.StreamId}]");
// Log.Logger.Debug($"PARTIAL AMBE {FneUtils.HexDump(ambePartial)}");
// Log.Logger.Debug($"SAMPLE BUFFER {FneUtils.HexDump(samples)}");
int pcmIdx = 0;
byte[] pcm = new byte[samples.Length * 2];
for (int smpIdx = 0; smpIdx < samples.Length; smpIdx++)
{
pcm[pcmIdx + 0] = (byte)(samples[smpIdx] & 0xFF);
pcm[pcmIdx + 1] = (byte)((samples[smpIdx] >> 8) & 0xFF);
pcmIdx += 2;
}
//Log.WriteLine($"PCM BYTE BUFFER {FneUtils.HexDump(pcm)}");
audioManager.AddTalkgroupStream(e.DstId.ToString(), pcm);
}
}
}
catch (Exception ex)
{
Log.WriteError($"Audio Decode Exception: {ex.Message}");
Log.StackTrace(ex, false);
}
}
/// <summary>
///
/// Handler for FNE key responses.
/// </summary>
/// <param name="e"></param>
public void KeyResponseReceived(KeyResponseEvent e)
@ -2694,391 +2126,32 @@ namespace dvmconsole
continue;
Codeplug.System system = Codeplug.GetSystemForChannel(channel.ChannelName);
Codeplug.Channel cpgChannel = Codeplug.GetChannelByName(channel.ChannelName);
PeerSystem handler = fneSystemManager.GetFneSystem(system.Name);
ushort keyId = cpgChannel.GetKeyId();
byte algoId = cpgChannel.GetAlgoId();
KeysetItem receivedKey = e.KmmKey.KeysetItem;
if (keyId != 0 && algoId != 0 && keyId == key.KeyId && algoId == receivedKey.AlgId)
channel.Crypter.SetKey(key.KeyId, receivedKey.AlgId, key.GetKey());
}
});
}
}
/// <summary>
/// Event handler used to process incoming DMR data.
/// </summary>
/// <param name="e"></param>
/// <param name="pktTime"></param>
public void DMRDataReceived(DMRDataReceivedEvent e, DateTime pktTime)
{
Dispatcher.Invoke(() =>
{
foreach (ChannelBox channel in selectedChannelsManager.GetSelectedChannels())
if (system == null)
{
if (channel.SystemName == PLAYBACKSYS || channel.ChannelName == PLAYBACKCHNAME || channel.DstId == PLAYBACKTG)
continue;
Codeplug.System system = Codeplug.GetSystemForChannel(channel.ChannelName);
Codeplug.Channel cpgChannel = Codeplug.GetChannelByName(channel.ChannelName);
if (cpgChannel.GetChannelMode() != Codeplug.ChannelMode.DMR)
continue;
PeerSystem handler = fneSystemManager.GetFneSystem(system.Name);
if (!channel.IsEnabled || channel.Name == PLAYBACKCHNAME)
continue;
if (cpgChannel.Tgid != e.DstId.ToString())
Log.WriteLine($"{channel.ChannelName} refers to an {INVALID_SYSTEM} {channel.SystemName}. {ERR_INVALID_CODEPLUG}.");
channel.IsSelected = false;
selectedChannelsManager.RemoveSelectedChannel(channel);
continue;
if (!systemStatuses.ContainsKey(cpgChannel.Name + e.Slot))
systemStatuses[cpgChannel.Name + e.Slot] = new SlotStatus();
if (channel.Decoder == null)
channel.Decoder = new MBEDecoder(MBE_MODE.DMR_AMBE);
byte[] data = new byte[FneSystemBase.DMR_FRAME_LENGTH_BYTES];
Buffer.BlockCopy(e.Data, 20, data, 0, FneSystemBase.DMR_FRAME_LENGTH_BYTES);
byte bits = e.Data[15];
// is this a new call stream?
if (e.StreamId != systemStatuses[cpgChannel.Name + e.Slot].RxStreamId)
{
channel.IsReceiving = true;
systemStatuses[cpgChannel.Name + e.Slot].RxStart = pktTime;
Log.WriteLine($"({system.Name}) DMRD: Traffic *CALL START * PEER {e.PeerId} SRC_ID {e.SrcId} TGID {e.DstId} Slot {e.Slot} [STREAM ID {e.StreamId}]");
// if we can, use the LC from the voice header as to keep all options intact
if ((e.FrameType == FrameType.DATA_SYNC) && (e.DataType == DMRDataType.VOICE_LC_HEADER))
{
LC lc = FullLC.Decode(data, DMRDataType.VOICE_LC_HEADER);
systemStatuses[cpgChannel.Name + e.Slot].DMR_RxLC = lc;
}
else // if we don't have a voice header; don't wait to decode it, just make a dummy header
systemStatuses[cpgChannel.Name + e.Slot].DMR_RxLC = new LC()
{
SrcId = e.SrcId,
DstId = e.DstId
};
systemStatuses[cpgChannel.Name + e.Slot].DMR_RxPILC = new PrivacyLC();
Log.WriteLine($"({system.Name}) TS {e.Slot + 1} [STREAM ID {e.StreamId}] RX_LC {FneUtils.HexDump(systemStatuses[cpgChannel.Name + e.Slot].DMR_RxLC.GetBytes())}");
callHistoryWindow.AddCall(cpgChannel.Name, (int)e.SrcId, (int)e.DstId);
callHistoryWindow.ChannelKeyed(cpgChannel.Name, (int)e.SrcId, false); // TODO: Encrypted state
string alias = string.Empty;
try
{
alias = AliasTools.GetAliasByRid(system.RidAlias, (int)e.SrcId);
}
catch (Exception) { }
if (string.IsNullOrEmpty(alias))
channel.LastSrcId = "Last ID: " + e.SrcId;
else
channel.LastSrcId = "Last: " + alias;
channel.Background = ChannelBox.GREEN_GRADIENT;
}
// if we can, use the PI LC from the PI voice header as to keep all options intact
if ((e.FrameType == FrameType.DATA_SYNC) && (e.DataType == DMRDataType.VOICE_PI_HEADER))
{
PrivacyLC lc = FullLC.DecodePI(data);
systemStatuses[cpgChannel.Name + e.Slot].DMR_RxPILC = lc;
//Log.WriteLine($"({SystemName}) DMRD: Traffic *CALL PI PARAMS * PEER {e.PeerId} DST_ID {e.DstId} TS {e.Slot + 1} ALGID {lc.AlgId} KID {lc.KId} [STREAM ID {e.StreamId}]");
//Log.WriteLine($"({SystemName}) TS {e.Slot + 1} [STREAM ID {e.StreamId}] RX_PI_LC {FneUtils.HexDump(systemStatuses[cpgChannel.Name + e.Slot].DMR_RxPILC.GetBytes())}");
}
if ((e.FrameType == FrameType.DATA_SYNC) && (e.DataType == DMRDataType.TERMINATOR_WITH_LC) && (systemStatuses[cpgChannel.Name + e.Slot].RxType != FrameType.TERMINATOR))
{
channel.IsReceiving = false;
TimeSpan callDuration = pktTime - systemStatuses[cpgChannel.Name + e.Slot].RxStart;
Log.WriteLine($"({system.Name}) DMRD: Traffic *CALL END * PEER {e.PeerId} SRC_ID {e.SrcId} TGID {e.DstId} Slot {e.Slot} DUR {callDuration} [STREAM ID {e.StreamId}]");
channel.Background = ChannelBox.BLUE_GRADIENT;
channel.VolumeMeterLevel = 0;
callHistoryWindow.ChannelUnkeyed(cpgChannel.Name, (int)e.SrcId);
}
if (e.FrameType == FrameType.VOICE_SYNC || e.FrameType == FrameType.VOICE)
{
byte[] ambe = new byte[FneSystemBase.DMR_AMBE_LENGTH_BYTES];
Buffer.BlockCopy(data, 0, ambe, 0, 14);
ambe[13] &= 0xF0;
ambe[13] |= (byte)(data[19] & 0x0F);
Buffer.BlockCopy(data, 20, ambe, 14, 13);
DMRDecodeAudioFrame(ambe, e, handler, channel);
}
systemStatuses[cpgChannel.Name + e.Slot].RxRFS = e.SrcId;
systemStatuses[cpgChannel.Name + e.Slot].RxType = e.FrameType;
systemStatuses[cpgChannel.Name + e.Slot].RxTGId = e.DstId;
systemStatuses[cpgChannel.Name + e.Slot].RxTime = pktTime;
systemStatuses[cpgChannel.Name + e.Slot].RxStreamId = e.StreamId;
}
});
}
/// <summary>
/// Event handler used to process incoming P25 data.
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
public void P25DataReceived(P25DataReceivedEvent e, DateTime pktTime)
{
uint sysId = (uint)((e.Data[11U] << 8) | (e.Data[12U] << 0));
uint netId = FneUtils.Bytes3ToUInt32(e.Data, 16);
byte control = e.Data[14U];
byte len = e.Data[23];
byte[] data = new byte[len];
for (int i = 24; i < len; i++)
data[i - 24] = e.Data[i];
Dispatcher.Invoke(() =>
{
foreach (ChannelBox channel in selectedChannelsManager.GetSelectedChannels())
{
if (channel.SystemName == PLAYBACKSYS || channel.ChannelName == PLAYBACKCHNAME || channel.DstId == PLAYBACKTG)
continue;
Codeplug.System system = Codeplug.GetSystemForChannel(channel.ChannelName);
Codeplug.Channel cpgChannel = Codeplug.GetChannelByName(channel.ChannelName);
if (cpgChannel.GetChannelMode() != Codeplug.ChannelMode.P25)
continue;
bool isEmergency = false;
bool encrypted = false;
PeerSystem handler = fneSystemManager.GetFneSystem(system.Name);
if (!channel.IsEnabled || channel.Name == PLAYBACKCHNAME)
continue;
if (cpgChannel.Tgid != e.DstId.ToString())
continue;
if (!systemStatuses.ContainsKey(cpgChannel.Name))
systemStatuses[cpgChannel.Name] = new SlotStatus();
if (channel.Decoder == null)
channel.Decoder = new MBEDecoder(MBE_MODE.IMBE_88BIT);
SlotStatus slot = systemStatuses[cpgChannel.Name];
// if this is an LDU1 see if this is the first LDU with HDU encryption data
if (e.DUID == P25DUID.LDU1)
{
byte frameType = e.Data[180];
// get the initial MI and other enc info (bug found by the screeeeeeeeech on initial tx...)
if (frameType == P25Defines.P25_FT_HDU_VALID)
{
channel.algId = e.Data[181];
channel.kId = (ushort)((e.Data[182] << 8) | e.Data[183]);
Array.Copy(e.Data, 184, channel.mi, 0, P25Defines.P25_MI_LENGTH);
channel.Crypter.Prepare(channel.algId, channel.kId, channel.mi);
if (channel.algId != P25Defines.P25_ALGO_UNENCRYPT)
encrypted = true;
}
}
// is this a new call stream?
if (e.StreamId != slot.RxStreamId && ((e.DUID != P25DUID.TDU) && (e.DUID != P25DUID.TDULC)))
{
channel.IsReceiving = true;
slot.RxStart = pktTime;
Log.WriteLine($"({system.Name}) P25D: Traffic *CALL START * PEER {e.PeerId} SRC_ID {e.SrcId} TGID {e.DstId} ALGID {channel.algId} KID {channel.kId} [STREAM ID {e.StreamId}]");
FneUtils.Memset(channel.mi, 0x00, P25Defines.P25_MI_LENGTH);
callHistoryWindow.AddCall(cpgChannel.Name, (int)e.SrcId, (int)e.DstId);
callHistoryWindow.ChannelKeyed(cpgChannel.Name, (int)e.SrcId, encrypted);
string alias = string.Empty;
try
{
alias = AliasTools.GetAliasByRid(system.RidAlias, (int)e.SrcId);
}
catch (Exception) { }
if (string.IsNullOrEmpty(alias))
channel.LastSrcId = "Last ID: " + e.SrcId;
else
channel.LastSrcId = "Last: " + alias;
if (channel.algId != P25Defines.P25_ALGO_UNENCRYPT)
{
channel.Background = ChannelBox.ORANGE_GRADIENT;
Log.WriteLine($"({system.Name}) P25D: Traffic *CALL ENC PARMS * PEER {e.PeerId} SRC_ID {e.SrcId} TGID {e.DstId} ALGID {channel.algId} KID {channel.kId} [STREAM ID {e.StreamId}]");
}
else
channel.Background = ChannelBox.GREEN_GRADIENT;
}
// is the call over?
if (((e.DUID == P25DUID.TDU) || (e.DUID == P25DUID.TDULC)) && (slot.RxType != fnecore.FrameType.TERMINATOR))
if (cpgChannel == null)
{
channel.IsReceiving = false;
TimeSpan callDuration = pktTime - slot.RxStart;
Log.WriteLine($"({system.Name}) P25D: Traffic *CALL END * PEER {e.PeerId} SRC_ID {e.SrcId} TGID {e.DstId} DUR {callDuration} [STREAM ID {e.StreamId}]");
channel.Background = ChannelBox.BLUE_GRADIENT;
channel.VolumeMeterLevel = 0;
callHistoryWindow.ChannelUnkeyed(cpgChannel.Name, (int)e.SrcId);
return;
}
if ((channel.algId != cpgChannel.GetAlgoId() || channel.kId != cpgChannel.GetKeyId()) && channel.algId != P25Defines.P25_ALGO_UNENCRYPT)
Log.WriteLine($"{channel.ChannelName} refers to an {INVALID_CODEPLUG_CHANNEL}. {ERR_INVALID_CODEPLUG}.");
channel.IsSelected = false;
selectedChannelsManager.RemoveSelectedChannel(channel);
continue;
byte[] newMI = new byte[P25Defines.P25_MI_LENGTH];
int count = 0;
switch (e.DUID)
{
case P25DUID.LDU1:
{
// The '62', '63', '64', '65', '66', '67', '68', '69', '6A' records are LDU1
if ((data[0U] == 0x62U) && (data[22U] == 0x63U) &&
(data[36U] == 0x64U) && (data[53U] == 0x65U) &&
(data[70U] == 0x66U) && (data[87U] == 0x67U) &&
(data[104U] == 0x68U) && (data[121U] == 0x69U) &&
(data[138U] == 0x6AU))
{
// The '62' record - IMBE Voice 1
Buffer.BlockCopy(data, count, channel.netLDU1, 0, 22);
count += 22;
// The '63' record - IMBE Voice 2
Buffer.BlockCopy(data, count, channel.netLDU1, 25, 14);
count += 14;
// The '64' record - IMBE Voice 3 + Link Control
Buffer.BlockCopy(data, count, channel.netLDU1, 50, 17);
byte serviceOptions = data[count + 3];
isEmergency = (serviceOptions & 0x80) == 0x80;
count += 17;
// The '65' record - IMBE Voice 4 + Link Control
Buffer.BlockCopy(data, count, channel.netLDU1, 75, 17);
count += 17;
// The '66' record - IMBE Voice 5 + Link Control
Buffer.BlockCopy(data, count, channel.netLDU1, 100, 17);
count += 17;
// The '67' record - IMBE Voice 6 + Link Control
Buffer.BlockCopy(data, count, channel.netLDU1, 125, 17);
count += 17;
// The '68' record - IMBE Voice 7 + Link Control
Buffer.BlockCopy(data, count, channel.netLDU1, 150, 17);
count += 17;
// The '69' record - IMBE Voice 8 + Link Control
Buffer.BlockCopy(data, count, channel.netLDU1, 175, 17);
count += 17;
// The '6A' record - IMBE Voice 9 + Low Speed Data
Buffer.BlockCopy(data, count, channel.netLDU1, 200, 16);
count += 16;
// decode 9 IMBE codewords into PCM samples
P25DecodeAudioFrame(channel.netLDU1, e, handler, channel, isEmergency);
}
}
break;
case P25DUID.LDU2:
{
// The '6B', '6C', '6D', '6E', '6F', '70', '71', '72', '73' records are LDU2
if ((data[0U] == 0x6BU) && (data[22U] == 0x6CU) &&
(data[36U] == 0x6DU) && (data[53U] == 0x6EU) &&
(data[70U] == 0x6FU) && (data[87U] == 0x70U) &&
(data[104U] == 0x71U) && (data[121U] == 0x72U) &&
(data[138U] == 0x73U))
{
// The '6B' record - IMBE Voice 10
Buffer.BlockCopy(data, count, channel.netLDU2, 0, 22);
count += 22;
// The '6C' record - IMBE Voice 11
Buffer.BlockCopy(data, count, channel.netLDU2, 25, 14);
count += 14;
// The '6D' record - IMBE Voice 12 + Encryption Sync
Buffer.BlockCopy(data, count, channel.netLDU2, 50, 17);
newMI[0] = data[count + 1];
newMI[1] = data[count + 2];
newMI[2] = data[count + 3];
count += 17;
// The '6E' record - IMBE Voice 13 + Encryption Sync
Buffer.BlockCopy(data, count, channel.netLDU2, 75, 17);
newMI[3] = data[count + 1];
newMI[4] = data[count + 2];
newMI[5] = data[count + 3];
count += 17;
// The '6F' record - IMBE Voice 14 + Encryption Sync
Buffer.BlockCopy(data, count, channel.netLDU2, 100, 17);
newMI[6] = data[count + 1];
newMI[7] = data[count + 2];
newMI[8] = data[count + 3];
count += 17;
// The '70' record - IMBE Voice 15 + Encryption Sync
Buffer.BlockCopy(data, count, channel.netLDU2, 125, 17);
channel.algId = data[count + 1]; // Algorithm ID
channel.kId = (ushort)((data[count + 2] << 8) | data[count + 3]); // Key ID
count += 17;
// The '71' record - IMBE Voice 16 + Encryption Sync
Buffer.BlockCopy(data, count, channel.netLDU2, 150, 17);
count += 17;
// The '72' record - IMBE Voice 17 + Encryption Sync
Buffer.BlockCopy(data, count, channel.netLDU2, 175, 17);
count += 17;
// The '73' record - IMBE Voice 18 + Low Speed Data
Buffer.BlockCopy(data, count, channel.netLDU2, 200, 16);
count += 16;
if (channel.p25Errs > 0) // temp, need to actually get errors I guess
P25Crypto.CycleP25Lfsr(channel.mi);
else
Array.Copy(newMI, channel.mi, P25Defines.P25_MI_LENGTH);
// decode 9 IMBE codewords into PCM samples
P25DecodeAudioFrame(channel.netLDU2, e, handler, channel, isEmergency, P25DUID.LDU2);
}
}
break;
}
if (channel.mi != null)
channel.Crypter.Prepare(channel.algId, channel.kId, channel.mi);
slot.RxRFS = e.SrcId;
slot.RxType = e.FrameType;
slot.RxTGId = e.DstId;
slot.RxTime = pktTime;
slot.RxStreamId = e.StreamId;
ushort keyId = cpgChannel.GetKeyId();
byte algoId = cpgChannel.GetAlgoId();
KeysetItem receivedKey = e.KmmKey.KeysetItem;
if (keyId != 0 && algoId != 0 && keyId == key.KeyId && algoId == receivedKey.AlgId)
channel.Crypter.SetKey(key.KeyId, receivedKey.AlgId, key.GetKey());
}
});
}
}
} // public partial class MainWindow : Window
} // namespace dvmconsole
Write Preview
Loading…
Cancel
Save

Powered by TurnKey Linux.