diff --git a/SoftBeckhoff/Extensions/TypeExtensions.cs b/SoftBeckhoff/Extensions/TypeExtensions.cs new file mode 100644 index 0000000..ff1872e --- /dev/null +++ b/SoftBeckhoff/Extensions/TypeExtensions.cs @@ -0,0 +1,73 @@ +using System; +using System.Reflection; +using SoftBeckhoff.Enums; + +namespace SoftBeckhoff.Extensions +{ + public static class TypeExtensions + { + public static AdsDatatypeId ToAdsDatatypeId(this Type type) + { + switch (Type.GetTypeCode(type)) + { + case TypeCode.Int16: + return AdsDatatypeId.ADST_INT16; + case TypeCode.UInt16: + return AdsDatatypeId.ADST_UINT16; + case TypeCode.Int32: + return AdsDatatypeId.ADST_INT32; + case TypeCode.UInt32: + return AdsDatatypeId.ADST_UINT32; + case TypeCode.Int64: + return AdsDatatypeId.ADST_INT64; + case TypeCode.UInt64: + return AdsDatatypeId.ADST_UINT64; + case TypeCode.Single: + return AdsDatatypeId.ADST_REAL32; + case TypeCode.Double: + return AdsDatatypeId.ADST_REAL64; + case TypeCode.String: + return AdsDatatypeId.ADST_STRING; + case TypeCode.Boolean: + return AdsDatatypeId.ADST_UINT8; + case TypeCode.Byte: + return AdsDatatypeId.ADST_UINT8; + default: + throw new ArgumentOutOfRangeException($"Type {type} not supported"); + } + + } + + public static string ToAdsDatatypeName(this Type type) + { + switch (Type.GetTypeCode(type)) + { + case TypeCode.Int16: + return "INT"; + case TypeCode.UInt16: + return "UINT"; + case TypeCode.Int32: + return "DINT"; + case TypeCode.UInt32: + return "UDINT"; + case TypeCode.Int64: + return "LINT"; + case TypeCode.UInt64: + return "ULINT"; + case TypeCode.Single: + return "REAL"; + case TypeCode.Double: + return "LREAL"; + case TypeCode.String: + return "STRING(80)"; + case TypeCode.Boolean: + return "BOOL"; + case TypeCode.Byte: + return "BYTE"; + default: + throw new ArgumentOutOfRangeException($"Type {type} not supported"); + } + + } + } +} \ No newline at end of file diff --git a/SoftBeckhoff/Interfaces/IMarshable.cs b/SoftBeckhoff/Interfaces/IMarshable.cs new file mode 100644 index 0000000..7815de3 --- /dev/null +++ b/SoftBeckhoff/Interfaces/IMarshable.cs @@ -0,0 +1,7 @@ +namespace SoftBeckhoff.Interfaces +{ + public interface IMarshable + { + public byte[] GetBytes(); + } +} \ No newline at end of file diff --git a/SoftBeckhoff/Models/AdsSymbol.cs b/SoftBeckhoff/Models/AdsSymbol.cs new file mode 100644 index 0000000..2476880 --- /dev/null +++ b/SoftBeckhoff/Models/AdsSymbol.cs @@ -0,0 +1,48 @@ +using System; +using System.Collections.Generic; +using System.Text; +using SoftBeckhoff.Enums; +using SoftBeckhoff.Extensions; +using SoftBeckhoff.Interfaces; +using SoftBeckhoff.Structs; + +namespace SoftBeckhoff.Models +{ + public class AdsSymbol : IMarshable + { + private readonly string name; + private AdsSymbolEntryHeader header; + private string adsType; + + public AdsSymbol(string name, Type type) + { + this.name = name; + header = new AdsSymbolEntryHeader(type); + adsType = type.ToAdsDatatypeName(); + } + + public string Name => name; + public int Offset { get; set; } + public int Size => (int) header.Size; + + public byte[] GetBytes() + { + var byteName = Encoding.ASCII.GetBytes(name); + var byteType = Encoding.ASCII.GetBytes(adsType); + var headerSize = header.GetSize(); + var wholeSize = headerSize + byteName.Length + byteType.Length + 3; + header.NameLength = (ushort) byteName.Length; + header.EntryLength = (uint) wholeSize; + header.IndexOffset = (uint) Offset; + var buffer = new List(); + buffer.AddRange(header.GetBytes()); + buffer.AddRange(byteName); + buffer.Add(0); + buffer.AddRange(byteType); + buffer.Add(0); + buffer.Add(0); //comment + + return buffer.ToArray(); + } + } +} \ No newline at end of file diff --git a/SoftBeckhoff/Services/BeckhoffServer.cs b/SoftBeckhoff/Services/BeckhoffServer.cs index 5b83d78..bd90bc2 100644 --- a/SoftBeckhoff/Services/BeckhoffServer.cs +++ b/SoftBeckhoff/Services/BeckhoffServer.cs @@ -4,10 +4,12 @@ using System.Reactive; using System.Reactive.Disposables; using System.Reflection; +using System.Text; using System.Threading; using System.Threading.Tasks; using Microsoft.Extensions.Logging; using SoftBeckhoff.Extensions; +using SoftBeckhoff.Models; using SoftBeckhoff.Structs; using TwinCAT.Ads; using TwinCAT.Ads.Server; @@ -44,25 +46,62 @@ public BeckhoffServer(ILogger logger) disposables.Add(server); server.RegisterReceiver(this); + + AddSymbol(new AdsSymbol("test1", typeof(byte))); + AddSymbol(new AdsSymbol("test2", typeof(byte))); + } + + public Dictionary Symbols { get; set; } = new Dictionary(); + + public void AddSymbol(AdsSymbol symbol) + { + //Define Symbol Offset + var offset = GetCurrentDataOffset(); + symbol.Offset = offset; + var symbolBytes = symbol.GetBytes(); + //Add symbol to list + Symbols.Add(symbol.Name, symbol); + //Add symbol to data + memory.AddData(61451, symbolBytes); + memory.AddData(61449, symbolBytes); + //add symbol handlers + memory.AddData(61443, offset.GetBytes()); + memory.AddData(61446, offset.GetBytes()); + //Update symbolUploadInfo + memory.SetData(61455, new SymbolUploadInfo(Symbols.Count, GetCurrentSymbolSize()).GetBytes()); + //Add Data + memory.AddData(61445, new byte[symbol.Size]); + } + + private int GetCurrentDataOffset() + { + return memory.Count(61445); + } + + private int GetCurrentSymbolSize() + { + return memory.Count(61451); } private void InitializeServerMemory() { //Set Upload Info - memory.SetData(61455, SymbolUploadInfo.GetBytes()); + memory.SetData(61455, new byte[64]); //Set Symbols for read - memory.SetData(61451, AdsSymbolEntry.GetBytes()); + memory.SetData(61451, new byte[0]); //Set symbols for readwrite - memory.SetData(61449, AdsSymbolEntry.GetBytes()); - //Set symbols for readwrite (Handler?) - memory.SetData(61443, AdsSymbolEntry.GetBytes()); + memory.SetData(61449, new byte[0]); + //Set symbols for readwrite Handlers + memory.SetData(61443, new byte[0]); + //cleanup + memory.SetData(61446, new byte[0]); //Set Datatype memory.SetData(61454, AdsDataTypeEntry.GetBytes()); - //Set Data - memory.SetData(61472, new byte[1]); + //Set Data (access over group + offset stored into handlers) + memory.SetData(61445, new byte[0]); } @@ -74,13 +113,14 @@ public void Dispose() public byte[] RunStatus { get; set; } = {0, 0, 0, 0, 5, 0, 0, 0}; - public SymbolUploadInfo SymbolUploadInfo { get; set; } = new SymbolUploadInfo(1); + public SymbolUploadInfo SymbolUploadInfo { get; set; } = new SymbolUploadInfo(0,0); public AdsSymbolEntry AdsSymbolEntry { get; set; } = new AdsSymbolEntry(Unit.Default); public AdsDataTypeEntry AdsDataTypeEntry { get; set; } = new AdsDataTypeEntry(null); public async Task OnReceivedAsync(AmsCommand frame, CancellationToken cancel) { - logger.LogInformation($"{frame.Dump()}"); + if (frame.Header.CommandId != AdsCommandId.ReadState) + logger.LogInformation($"{frame.Dump()}"); var responseData = new List(); @@ -105,20 +145,46 @@ public async Task OnReceivedAsync(AmsCommand frame, CancellationTo logger.LogDebug($"Data: {request}"); logger.LogDebug("Data: "+string.Join(":", frame.Data.ToArray().Skip(new ReadWriteRequestData().GetSize()).Select(b => b.ToString("X2")))); //Data contains Instance path encoded - - var data = memory.GetData(request.IndexGroup, request.Offset, request.ReadLength); + var inputData = frame.Data.ToArray().Skip(new ReadWriteRequestData().GetSize()).ToArray(); + var inputString = Encoding.ASCII.GetString(inputData).Trim('\0'); + var data = new byte[0]; + switch (request.IndexGroup) + { + case 61449: + data = Symbols[inputString].GetBytes(); + break; + case 61443: + data = Symbols[inputString].Offset.GetBytes(); + break; + default: + data = memory.GetData(request.IndexGroup, request.Offset, request.ReadLength); + break; + } + var responseHeader = new ResponseHeaderData {Lenght = (uint)data.Length}; responseData.AddRange(responseHeader.GetBytes()); responseData.AddRange(data); } + else if (frame.Header.CommandId == AdsCommandId.Write) + { + var request = frame.Data.ToArray().ByteArrayToStructure(); + logger.LogDebug($"Data: {request}"); + var data = frame.Data.ToArray().Skip(new WriteRequestData().GetSize()).ToArray(); + logger.LogDebug("Data: "+string.Join(":", data.Select(b => b.ToString("X2")))); + + memory.SetData(request.IndexGroup, request.Offset, data); + var responseHeader = new ResponseHeader(); + responseData.AddRange(responseHeader.GetBytes()); + } else if (frame.Header.CommandId == AdsCommandId.AddNotification) { - + var responseHeader = new ResponseHeaderData {Lenght = (uint)new Random().Next()}; + responseData.AddRange(responseHeader.GetBytes()); } else if (frame.Header.CommandId == AdsCommandId.DeleteNotification) { - + responseData.AddRange(0.GetBytes()); } diff --git a/SoftBeckhoff/Structs/AdsSymbolEntry.cs b/SoftBeckhoff/Structs/AdsSymbolEntry.cs index 8434301..1c301db 100644 --- a/SoftBeckhoff/Structs/AdsSymbolEntry.cs +++ b/SoftBeckhoff/Structs/AdsSymbolEntry.cs @@ -33,7 +33,7 @@ namespace SoftBeckhoff.Structs public AdsSymbolEntry(Unit unit) { EntryLength = (uint) new AdsSymbolEntry().GetSize(); - IndexGroup = 61472; + IndexGroup = 61445; IndexOffset = 0; Size = 1; DataType = AdsDatatypeId.ADST_UINT8; diff --git a/SoftBeckhoff/Structs/AdsSymbolEntryHeader.cs b/SoftBeckhoff/Structs/AdsSymbolEntryHeader.cs new file mode 100644 index 0000000..0a17b94 --- /dev/null +++ b/SoftBeckhoff/Structs/AdsSymbolEntryHeader.cs @@ -0,0 +1,36 @@ +using System; +using System.Runtime.InteropServices; +using SoftBeckhoff.Enums; +using SoftBeckhoff.Extensions; + +namespace SoftBeckhoff.Structs +{ + [StructLayout(LayoutKind.Sequential, Pack = 1)]public struct AdsSymbolEntryHeader + { + [MarshalAs(UnmanagedType.U4)]public uint EntryLength; + [MarshalAs(UnmanagedType.U4)]public uint IndexGroup; + [MarshalAs(UnmanagedType.U4)]public uint IndexOffset; + [MarshalAs(UnmanagedType.U4)]public uint Size; + [MarshalAs(UnmanagedType.U4)]public AdsDatatypeId DataType; + [MarshalAs(UnmanagedType.U2)]public AdsSymbolFlags Flags; + [MarshalAs(UnmanagedType.U2)]public ushort ArrayDim; + [MarshalAs(UnmanagedType.U2)]public ushort NameLength; + [MarshalAs(UnmanagedType.U2)]public ushort TypeLength; + [MarshalAs(UnmanagedType.U2)]public ushort CommentLength; + + public AdsSymbolEntryHeader(Type type) + { + EntryLength = 0; + IndexGroup = 61445; + IndexOffset = 0; //to supply + Size = type == typeof(string) ? 81 : (uint) Marshal.SizeOf(type); + DataType = type.ToAdsDatatypeId(); + Flags = AdsSymbolFlags.None; + ArrayDim = 0; + NameLength = 0; //to supply + TypeLength = (ushort) type.ToAdsDatatypeName().Length; + CommentLength = 0; + + } + } +} \ No newline at end of file diff --git a/SoftBeckhoff/Structs/ResponseHeader.cs b/SoftBeckhoff/Structs/ResponseHeader.cs new file mode 100644 index 0000000..20691ac --- /dev/null +++ b/SoftBeckhoff/Structs/ResponseHeader.cs @@ -0,0 +1,14 @@ +using System.Runtime.InteropServices; + +namespace SoftBeckhoff.Structs +{ + [StructLayout(LayoutKind.Sequential, Pack = 1)]public struct ResponseHeader + { + [MarshalAs(UnmanagedType.U4)]public uint Result; + + public override string ToString() + { + return $"{nameof(Result)}:{Result}"; + } + } +} \ No newline at end of file diff --git a/SoftBeckhoff/Structs/SymbolUploadInfo.cs b/SoftBeckhoff/Structs/SymbolUploadInfo.cs index 0b11a29..f805323 100644 --- a/SoftBeckhoff/Structs/SymbolUploadInfo.cs +++ b/SoftBeckhoff/Structs/SymbolUploadInfo.cs @@ -12,7 +12,7 @@ namespace SoftBeckhoff.Structs [MarshalAs(UnmanagedType.U4)]public int MaxDynamicSymbolCount; [MarshalAs(UnmanagedType.U4)]public int UsedDynamicSymbolCount; [MarshalAs(UnmanagedType.U4)]public int InvalidDynamicSymbolCount; - [MarshalAs(UnmanagedType.U4)]public int EncodingCodePage; + [MarshalAs(UnmanagedType.U4)]public int EncodingCodePage; //20127 // ASCII [MarshalAs(UnmanagedType.U4)]public int Flags; //0: None, 1: Is64BitPlatform, 2: IncludesBaseTypes [MarshalAs(UnmanagedType.U4)]public int Reserve1; [MarshalAs(UnmanagedType.U4)]public int Reserve2; @@ -22,10 +22,10 @@ namespace SoftBeckhoff.Structs [MarshalAs(UnmanagedType.U4)]public int Reserve6; [MarshalAs(UnmanagedType.U4)]public int Reserve7; - public SymbolUploadInfo(int count) + public SymbolUploadInfo(int symbolCount, int symbolSize) { - SymbolCount = count; - SymbolsBlockSize = new AdsSymbolEntry().GetSize(); + SymbolCount = symbolCount; + SymbolsBlockSize = symbolSize; DataTypeCount = 0; DataTypesBlockSize = 0;//new AdsDataTypeEntry().GetSize(); MaxDynamicSymbolCount = 0; diff --git a/SoftBeckhoff/Structs/WriteRequestData.cs b/SoftBeckhoff/Structs/WriteRequestData.cs new file mode 100644 index 0000000..e264d87 --- /dev/null +++ b/SoftBeckhoff/Structs/WriteRequestData.cs @@ -0,0 +1,16 @@ +using System.Runtime.InteropServices; + +namespace SoftBeckhoff.Structs +{ + [StructLayout(LayoutKind.Sequential, Pack = 1)]public struct WriteRequestData + { + [MarshalAs(UnmanagedType.U4)]public uint IndexGroup; + [MarshalAs(UnmanagedType.U4)]public uint Offset; + [MarshalAs(UnmanagedType.U4)]public uint Length; + + public override string ToString() + { + return $"{nameof(IndexGroup)}:{IndexGroup}, {nameof(Offset)}:{Offset}, {nameof(Length)}:{Length}"; + } + } +} \ No newline at end of file