From 1c05c1eddffbc508828508d64708a2180e5dac7d Mon Sep 17 00:00:00 2001
From: "Samuele E. Locatelli" <samuele@steamware.net>
Date: Fri, 5 Feb 2021 14:00:07 +0100
Subject: [PATCH] fix write x IO val/force

---
 CMS_CORE_Library/S7Net/Nc_S7Net.cs | 49 +++++++++++++++++++++++++-----
 CMS_CORE_Library/Utils/Nc_Utils.cs | 13 ++++++++
 2 files changed, 54 insertions(+), 8 deletions(-)

diff --git a/CMS_CORE_Library/S7Net/Nc_S7Net.cs b/CMS_CORE_Library/S7Net/Nc_S7Net.cs
index 19ce5a3..1d52205 100644
--- a/CMS_CORE_Library/S7Net/Nc_S7Net.cs
+++ b/CMS_CORE_Library/S7Net/Nc_S7Net.cs
@@ -3910,18 +3910,36 @@ namespace CMS_CORE_Library.S7Net
         public override CmsError PLC_W_IO_DO_Val(Dictionary<int, bool> newValues)
         {
             CmsError libraryError = NO_ERROR;
-            List<ushort> readValuesDO = new List<ushort>();
+            List<ushort> rwValuesDO = new List<ushort>();
 
             // leggo da PLC valori DO...
-            libraryError = MEM_RWWordList(R, 0, IO_DO_VAL_FOR.MemType, IO_DO_VAL_FOR.Address, IO_DO_VAL_FOR.SubAddress, IO_DO_VAL_FOR.Size, ref readValuesDO);
+            libraryError = MEM_RWWordList(R, 0, IO_DO_VAL_FOR.MemType, IO_DO_VAL_FOR.Address, IO_DO_VAL_FOR.SubAddress, IO_DO_VAL_FOR.Size, ref rwValuesDO);
             if (libraryError.IsError())
                 return libraryError;
 
-            // aggiorno valori ricevuti
+            int bank = 0;
+            int pos = 0;
+            int size = 16;
+            // imposto i valori NUOVI... cerco il BANK da 16 bit...
+            foreach (var item in newValues)
+            {
+                bank = item.Key / size;
+                pos = item.Key % size;
+                // aggiorno
+                if (item.Value)
+                {
+                    rwValuesDO[bank] = (ushort)(((int)rwValuesDO[bank]).SetBitOne(pos));
+                }
+                else
+                {
+                    rwValuesDO[bank] = (ushort)(((int)rwValuesDO[bank]).SetBitZero(pos));
+                }
+            }
 
-            // scrivo valori
-
-            // scrivo forced
+            // scrivo!
+            libraryError = MEM_RWWordList(W, 0, IO_DO_VAL_FOR.MemType, IO_DO_VAL_FOR.Address, IO_DO_VAL_FOR.SubAddress, IO_DO_VAL_FOR.Size, ref rwValuesDO);
+            if (libraryError.IsError())
+                return libraryError;
 
             return libraryError;
         }
@@ -3987,13 +4005,21 @@ namespace CMS_CORE_Library.S7Net
                 bank = item.Key / size;
                 pos = item.Key % size;
                 // aggiorno
-                rwValuesDO[bank] = (ushort)(rwValuesDO[bank] & (1 << pos));
+                if (item.Value)
+                {
+                    rwValuesDO[bank] = (ushort)(((int)rwValuesDO[bank]).SetBitOne(pos));
+                }
+                else
+                {
+                    rwValuesDO[bank] = (ushort)(((int)rwValuesDO[bank]).SetBitZero(pos));
+                }
             }
 
             // scrivo!
             libraryError = MEM_RWWordList(W, 0, IO_DO_FORCE.MemType, IO_DO_FORCE.Address, IO_DO_FORCE.SubAddress, IO_DO_FORCE.Size, ref rwValuesDO);
             if (libraryError.IsError())
                 return libraryError;
+
             return libraryError;
         }
         /// <summary>
@@ -4023,7 +4049,14 @@ namespace CMS_CORE_Library.S7Net
                 bank = item.Key/size;
                 pos = item.Key % size;
                 // aggiorno
-                rwValuesAO[bank] = (ushort)(rwValuesAO[bank] & (1 << pos));
+                if (item.Value)
+                {
+                    rwValuesAO[bank] = (ushort)(((int)rwValuesAO[bank]).SetBitOne(pos));
+                }
+                else
+                {
+                    rwValuesAO[bank] = (ushort)(((int)rwValuesAO[bank]).SetBitZero(pos));
+                }
             }
 
             // scrivo!
diff --git a/CMS_CORE_Library/Utils/Nc_Utils.cs b/CMS_CORE_Library/Utils/Nc_Utils.cs
index 5bf8de1..cb90055 100644
--- a/CMS_CORE_Library/Utils/Nc_Utils.cs
+++ b/CMS_CORE_Library/Utils/Nc_Utils.cs
@@ -340,6 +340,19 @@ namespace CMS_CORE_Library.Utils
             return (b & (1 << bitNumber)) != 0;
         }
 
+        public static int SetBitOne(this int value, int position)
+        {
+            // Set a bit at position to 1.
+            return value |= (1 << position);
+        }
+
+        public static int SetBitZero(this int value, int position)
+        {
+            // Set a bit at position to 0.
+            return value & ~(1 << position);
+        }
+        
+
         public static CultureInfo GetCultureFromThreeLetter(string threeLetter)
         {
             // Get culture info from three letter iso standard