513 lines
13 KiB
C#
513 lines
13 KiB
C#
namespace Common.Hardware.SIRT
|
|
{
|
|
using Common.Hardware.SIRT.Tasks;
|
|
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Text.RegularExpressions;
|
|
using System.Threading;
|
|
|
|
public static class SIRTExtensions
|
|
{
|
|
private const int ByteSize = 8;
|
|
private const int SByteSize = 16;
|
|
private const int Int16Size = 16;
|
|
private const int UInt16Size = 16;
|
|
private const int Int32Size = 32;
|
|
private const int UInt32Size = 32;
|
|
private const int Int64Size = 64;
|
|
private const int UInt64Size = 64;
|
|
|
|
public static byte[] GetBits(this byte value)
|
|
{
|
|
var bits = new byte[ByteSize];
|
|
|
|
for (var i = 0; i < ByteSize; i++)
|
|
{
|
|
bits[i] = (byte)((value >> i) & 1);
|
|
}
|
|
|
|
return bits;
|
|
}
|
|
|
|
public static byte[] GetBits(this ushort value)
|
|
{
|
|
var bits = new byte[UInt16Size];
|
|
|
|
for (var i = 0; i < UInt16Size; i++)
|
|
{
|
|
bits[i] = (byte)((value >> i) & 1);
|
|
}
|
|
|
|
return bits;
|
|
}
|
|
|
|
public static byte GetByte(this byte[] bits)
|
|
{
|
|
if (bits is null)
|
|
{
|
|
return default(byte);
|
|
}
|
|
|
|
var value = default(byte);
|
|
|
|
for (var i = 0; i < ByteSize; i++)
|
|
{
|
|
value |= (byte)(bits[i] << i);
|
|
}
|
|
|
|
return value;
|
|
}
|
|
|
|
public static byte GetByte(this byte[] bits, int start = 0, int count = 0)
|
|
{
|
|
if (bits is null)
|
|
{
|
|
return default(byte);
|
|
}
|
|
|
|
if (count == 0)
|
|
{
|
|
count = bits.Length;
|
|
}
|
|
|
|
var length = start + count;
|
|
var _byte = default(byte);
|
|
|
|
for (int i = start, x = 0; i < length; i++, x++)
|
|
{
|
|
_byte |= (byte)(bits[i] << x);
|
|
}
|
|
|
|
return _byte;
|
|
}
|
|
|
|
public static void SetBits(this byte value, byte[] bits, int start = 0, int count = 0)
|
|
{
|
|
if (bits is null)
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (count == 0 || count > bits.Length)
|
|
{
|
|
count = bits.Length;
|
|
}
|
|
|
|
for (int i = start, x = 0; i < count; i++, x++)
|
|
{
|
|
bits[i] = (byte)((value >> x) % 2);
|
|
}
|
|
}
|
|
|
|
public static byte[] GetBytes(this int value)
|
|
{
|
|
var bytes = new byte[4];
|
|
var index = 0;
|
|
|
|
bytes[index++] = (byte)(value >> 24);
|
|
bytes[index++] = (byte)(value >> 16);
|
|
bytes[index++] = (byte)(value >> 8);
|
|
bytes[index] = (byte)value;
|
|
|
|
return bytes;
|
|
}
|
|
|
|
public static byte[] GetBytes(this ushort value)
|
|
{
|
|
var bytes = new byte[2];
|
|
var index = 0;
|
|
|
|
bytes[index++] = (byte)(value >> 8);
|
|
bytes[index] = (byte)value;
|
|
|
|
return bytes;
|
|
}
|
|
|
|
public static byte[] GetBytesBE(this uint value)
|
|
{
|
|
var bytes = new byte[4];
|
|
var index = 0;
|
|
|
|
bytes[index++] = (byte)(value >> 24);
|
|
bytes[index++] = (byte)(value >> 16);
|
|
bytes[index++] = (byte)(value >> 8);
|
|
bytes[index] = (byte)value;
|
|
|
|
return bytes;
|
|
}
|
|
|
|
public static byte[] GetBytesLE(this ulong value)
|
|
{
|
|
var bytes = new byte[8];
|
|
var index = 0;
|
|
|
|
bytes[index++] = (byte)value;
|
|
bytes[index++] = (byte)(value >> 8);
|
|
bytes[index++] = (byte)(value >> 16);
|
|
bytes[index++] = (byte)(value >> 24);
|
|
bytes[index++] = (byte)(value >> 32);
|
|
bytes[index++] = (byte)(value >> 40);
|
|
bytes[index++] = (byte)(value >> 48);
|
|
bytes[index] = (byte)(value >> 56);
|
|
|
|
return bytes;
|
|
}
|
|
|
|
public static sbyte ToSByteBE(this byte[] bytes, int start = 0)
|
|
{
|
|
var value = default(sbyte);
|
|
|
|
value |= (sbyte)(bytes[start++] << 8);
|
|
value |= (sbyte)bytes[start];
|
|
|
|
return value;
|
|
}
|
|
|
|
public static int ToInt32BE(this byte[] bytes, int start = 0)
|
|
{
|
|
var value = default(int);
|
|
|
|
value |= bytes[start++] << 24;
|
|
value |= bytes[start++] << 16;
|
|
value |= bytes[start++] << 8;
|
|
value |= bytes[start];
|
|
|
|
return value;
|
|
}
|
|
|
|
public static ushort ToUInt16BE(this byte[] bytes, int start = 0)
|
|
{
|
|
var value = default(ushort);
|
|
|
|
value |= (ushort)(bytes[start++] << 8);
|
|
value |= bytes[start];
|
|
|
|
return value;
|
|
}
|
|
|
|
public static uint ToUInt32BE(this byte[] bytes, int start = 0)
|
|
{
|
|
var value = default(uint);
|
|
|
|
value |= (uint)(bytes[start++] << 24);
|
|
value |= (uint)(bytes[start++] << 16);
|
|
value |= (uint)(bytes[start++] << 8);
|
|
value |= bytes[start];
|
|
|
|
return value;
|
|
}
|
|
|
|
public static ushort ToUInt16LE(this byte[] bytes, int start = 0)
|
|
{
|
|
var value = default(ushort);
|
|
|
|
value |= bytes[start++];
|
|
value |= (ushort)(bytes[start] << 8);
|
|
|
|
return value;
|
|
}
|
|
|
|
public static ushort ToUIint16LEFromBits(this byte[] bits)
|
|
{
|
|
if (bits is null)
|
|
{
|
|
bits = new byte[UInt16Size];
|
|
}
|
|
|
|
Array.Resize(ref bits, UInt16Size);
|
|
|
|
ushort value = 0;
|
|
|
|
for (var i = 0; i < UInt16Size; i++)
|
|
{
|
|
value |= (ushort)(bits[i] << i);
|
|
}
|
|
|
|
return value;
|
|
}
|
|
|
|
public static ushort CRCCCITT(this byte[] bytes, int start = 0, int length = 0)
|
|
{
|
|
if (bytes.Length <= 0)
|
|
{
|
|
return default(ushort);
|
|
}
|
|
|
|
if (length == 0)
|
|
{
|
|
length = bytes.Length;
|
|
}
|
|
|
|
var crc = (ushort)0xFFFF;
|
|
|
|
for (var b = start; b < length; b++)
|
|
{
|
|
var _byte = bytes[b];
|
|
var token = (ushort)((_byte << 8) & 0xFF00);
|
|
|
|
for (var i = 0; i < 8; i++)
|
|
{
|
|
if (0x8000 == ((crc ^ token) & 0x8000))
|
|
{
|
|
crc <<= 1;
|
|
crc ^= 0x1021;
|
|
}
|
|
else
|
|
{
|
|
crc <<= 1;
|
|
}
|
|
|
|
token <<= 1;
|
|
}
|
|
}
|
|
|
|
return crc;
|
|
}
|
|
|
|
|
|
public static byte[] GetBytes(this string hexString)
|
|
{
|
|
var bytes = new List<byte>();
|
|
|
|
if (!string.IsNullOrWhiteSpace(hexString))
|
|
{
|
|
hexString = Regex.Replace(hexString, "[^a-fA-F0-9]*", string.Empty);
|
|
|
|
var first = true;
|
|
var x = 0;
|
|
|
|
foreach (var hex in hexString)
|
|
{
|
|
if ('0' <= hex && hex <= '9')
|
|
{
|
|
x += hex - '0';
|
|
}
|
|
else if ('A' <= hex && hex <= 'F')
|
|
{
|
|
x += hex - 'A' + 10;
|
|
}
|
|
else if ('a' <= hex && hex <= 'f')
|
|
{
|
|
x = hex - 'a' + 10;
|
|
}
|
|
else
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if (first)
|
|
{
|
|
x *= 16;
|
|
}
|
|
else
|
|
{
|
|
bytes.Add((byte)x);
|
|
|
|
x = 0;
|
|
}
|
|
|
|
first = !first;
|
|
}
|
|
}
|
|
|
|
return bytes.ToArray();
|
|
}
|
|
|
|
public static byte[] GetEncryptionKey(this string hexString, int requiredLength = 16)
|
|
{
|
|
var bytes = hexString.GetBytes();
|
|
|
|
if (bytes?.Length != requiredLength)
|
|
{
|
|
return default(byte[]);
|
|
}
|
|
|
|
return bytes;
|
|
}
|
|
|
|
public static uint GetAddress(this byte[] bytes, int index)
|
|
{
|
|
var value = default(uint);
|
|
var _index = index;
|
|
|
|
value |= (uint)(bytes[_index++] << 24);
|
|
value |= (uint)(bytes[_index++] << 16);
|
|
value |= (uint)(bytes[_index++] << 8);
|
|
value |= bytes[_index];
|
|
|
|
return value;
|
|
}
|
|
|
|
public static ushort GetCrc(this byte[] bytes, int index = -1)
|
|
{
|
|
var value = default(ushort);
|
|
|
|
if (bytes != null)
|
|
{
|
|
var length = bytes.Length;
|
|
|
|
if (index < 0)
|
|
{
|
|
index = length - 2;
|
|
}
|
|
|
|
var _index = index;
|
|
|
|
if (_index >= 0)
|
|
{
|
|
value ^= (ushort)(bytes[_index++] << 8);
|
|
value ^= bytes[_index];
|
|
}
|
|
}
|
|
|
|
return value;
|
|
}
|
|
|
|
public static void SetAddress(this byte[] bytes, int index, uint address)
|
|
{
|
|
var _index = index;
|
|
|
|
bytes[_index++] = (byte)(address >> 24);
|
|
bytes[_index++] = (byte)(address >> 16);
|
|
bytes[_index++] = (byte)(address >> 8);
|
|
bytes[_index] = (byte)address;
|
|
}
|
|
|
|
public static void SetCrc(this byte[] bytes, int index, ushort crc)
|
|
{
|
|
var _index = index;
|
|
|
|
bytes[_index++] = (byte)(crc >> 8);
|
|
bytes[_index] = (byte)crc;
|
|
}
|
|
|
|
public static bool TryActivate(this SIRTBroadcaster broadcaster, out string id, out int frequency)
|
|
{
|
|
id = default(string);
|
|
frequency = default(int);
|
|
var awaiter = new ManualResetEventSlim();
|
|
var task = default(SIRTTask);
|
|
var done = false;
|
|
|
|
task = new ActivateSIRTTask();
|
|
|
|
for (var i = 0; i < 3 && !done; i++)
|
|
{
|
|
broadcaster.Start(task);
|
|
task.Wait();
|
|
|
|
done = task.State == SIRTTaskState.Completed;
|
|
}
|
|
|
|
if (!done)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
done = false;
|
|
task = new IdentifySIRTTask();
|
|
|
|
for (var i = 0; i < 3 && !done; i++)
|
|
{
|
|
broadcaster.Start(task);
|
|
task.Wait();
|
|
|
|
done = task.State == SIRTTaskState.Completed;
|
|
}
|
|
|
|
if (done)
|
|
{
|
|
id = task.SirtId;
|
|
frequency = task.Frequency;
|
|
}
|
|
|
|
return done;
|
|
}
|
|
|
|
public static byte[] DecryptResponse(this byte[] encryptedData, byte[] encryptionKey)
|
|
{
|
|
if (encryptionKey?.Length != 16)
|
|
{
|
|
return encryptedData;
|
|
}
|
|
|
|
var length = encryptedData?.Length ?? 0;
|
|
|
|
if (length == 0)
|
|
{
|
|
return encryptedData;
|
|
}
|
|
|
|
var decryptedData = new byte[length];
|
|
|
|
Array.Copy(encryptedData, 0, decryptedData, 0, length);
|
|
|
|
var token = decryptedData[1];
|
|
|
|
for (var x = 6; x < length; x++)
|
|
{
|
|
token += (byte)(decryptedData[x - (token & 0x3) - 1] >> ((token >> 6) & 0x1));
|
|
|
|
decryptedData[x] ^= encryptionKey[(token >> 2) & 0x0F];
|
|
|
|
if (token >= 0x80)
|
|
{
|
|
decryptedData[x] = (byte)~decryptedData[x];
|
|
}
|
|
}
|
|
|
|
return decryptedData;
|
|
}
|
|
|
|
public static byte[] EncryptRequest(this byte[] plainData, byte[] encryptionKey, uint address)
|
|
{
|
|
if (plainData?.Length <= 0 || encryptionKey?.Length != 16 || address <= 0)
|
|
{
|
|
return plainData;
|
|
}
|
|
|
|
var dataLength = plainData.Length;
|
|
var telegramLength = dataLength + 2;
|
|
var encryptedData = new byte[telegramLength];
|
|
|
|
Array.Copy(plainData, 0, encryptedData, 0, dataLength);
|
|
|
|
var telegramForCRCLength = telegramLength + 4;
|
|
var telegramForCRC = new byte[telegramForCRCLength];
|
|
|
|
telegramForCRC[0] = (byte)(address >> 24);
|
|
telegramForCRC[1] = (byte)(address >> 16);
|
|
telegramForCRC[2] = (byte)(address >> 8);
|
|
telegramForCRC[3] = (byte)(address >> 0);
|
|
|
|
Array.Copy(encryptedData, 0, telegramForCRC, 4, telegramLength);
|
|
|
|
var telegramCRC = telegramForCRC.CRCCCITT(1, telegramForCRCLength - 2);
|
|
encryptedData[telegramLength - 2] = (byte)(telegramCRC >> 8);
|
|
encryptedData[telegramLength - 1] = (byte)(telegramCRC >> 0);
|
|
|
|
var reference = address;
|
|
var token = (byte)0xFF;
|
|
|
|
for (var i = 0; i < telegramLength; i++)
|
|
{
|
|
token += (byte)(((reference >> ((token & 0x3) << 3)) & 0xFF) >> ((token >> 6) & 1));
|
|
|
|
reference <<= 8;
|
|
reference += encryptedData[i];
|
|
|
|
encryptedData[i] ^= encryptionKey[(token >> 2) & 0x0F];
|
|
|
|
if (token >= 0x80)
|
|
{
|
|
encryptedData[i] = (byte)~encryptedData[i];
|
|
}
|
|
}
|
|
|
|
return encryptedData;
|
|
}
|
|
|
|
}
|
|
}
|