implemented dynamic symbol

- added IMarshable interface - implemented custom symbols
fbarresi · Nov 17, 2020 · cc11788 · cc11788
1 parent 36bbfa6
commit cc11788
Show file tree

Hide file tree

Showing 9 changed files with 278 additions and 18 deletions.
diff --git a/SoftBeckhoff/Extensions/TypeExtensions.cs 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");
+            }
+
+        }
+    }
+}
diff --git a/SoftBeckhoff/Interfaces/IMarshable.cs b/SoftBeckhoff/Interfaces/IMarshable.cs
@@ -0,0 +1,7 @@
+namespace SoftBeckhoff.Interfaces
+{
+    public interface IMarshable
+    {
+        public byte[] GetBytes();
+    }
+}
diff --git a/SoftBeckhoff/Models/AdsSymbol.cs 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<byte>();
+            buffer.AddRange(header.GetBytes());
+            buffer.AddRange(byteName);
+            buffer.Add(0);
+            buffer.AddRange(byteType);
+            buffer.Add(0);
+            buffer.Add(0); //comment
+
+            return buffer.ToArray();
+        }
+    }
+}
diff --git 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<string, AdsSymbol> Symbols { get; set; } = new Dictionary<string, AdsSymbol>();
+
+        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<AdsErrorCode> OnReceivedAsync(AmsCommand frame, CancellationToken cancel)
         {
-            logger.LogInformation($"{frame.Dump()}");
+            if (frame.Header.CommandId != AdsCommandId.ReadState)
+                logger.LogInformation($"{frame.Dump()}");
 
             var responseData = new List<byte>();
 
@@ -105,20 +145,46 @@ public async Task<AdsErrorCode> 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<WriteRequestData>();
+                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
@@ -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
@@ -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;
+
+        }
+    }
+}
diff --git a/SoftBeckhoff/Structs/ResponseHeader.cs 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}";
+        }
+    }
+}
diff --git 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
@@ -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}";
+        }
+    }
+}