Merge
authorEdouard Tisserant <edouard.tisserant@gmail.com>
Sun, 16 Jan 2022 16:59:40 +0100
changeset 3404 6f5cd8d5dc11
parent 3403 4a048b5b9e3b (diff)
parent 3392 4353c56a53d1 (current diff)
child 3405 fdc12f7d27c8
child 3406 3aac4241dafa
child 3408 13c5cac55ac7
Merge
--- a/ProjectController.py	Tue Nov 30 18:43:10 2021 +0100
+++ b/ProjectController.py	Sun Jan 16 16:59:40 2022 +0100
@@ -276,6 +276,7 @@
         # copy StatusMethods so that it can be later customized
         self.StatusMethods = [dic.copy() for dic in self.StatusMethods]
         self.DebugToken = None
+        self.LastComplainDebugToken = None
         self.debug_status = PlcStatus.Stopped
 
         self.IECcodeDigest = None
@@ -971,7 +972,7 @@
                 # describes CSV columns
                 ProgramsListAttributeName = ["num", "C_path", "type"]
                 VariablesListAttributeName = [
-                    "num", "vartype", "IEC_path", "C_path", "type", "derived"]
+                    "num", "vartype", "IEC_path", "C_path", "type", "derived", "retain"]
                 self._ProgramList = []
                 self._VariablesList = []
                 self._DbgVariablesList = []
@@ -1052,10 +1053,9 @@
 
         # prepare debug code
         variable_decl_array = []
-        bofs = 0
-        for v in self._DbgVariablesList:
-            sz = DebugTypesSize.get(v["type"], 0)
-            variable_decl_array += [
+        retain_indexes = []
+        for i, v in enumerate(self._DbgVariablesList):
+            variable_decl_array.append(
                 "{&(%(C_path)s), " % v +
                 {
                     "EXT": "%(type)s_P_ENUM",
@@ -1064,10 +1064,12 @@
                     "OUT": "%(type)s_O_ENUM",
                     "VAR": "%(type)s_ENUM"
                 }[v["vartype"]] % v +
-                "}"]
-            bofs += sz
+                "}")
+
+            if v["retain"] == "1":
+                retain_indexes.append("/* "+v["C_path"]+" */ "+str(i))
+
         debug_code = targets.GetCode("plc_debug.c") % {
-            "buffer_size": bofs,
             "programs_declarations": "\n".join(["extern %(type)s %(C_path)s;" %
                                                 p for p in self._ProgramList]),
             "extern_variables_declarations": "\n".join([
@@ -1081,6 +1083,7 @@
                 }[v["vartype"]] % v
                 for v in self._VariablesList if v["C_path"].find('.') < 0]),
             "variable_decl_array": ",\n".join(variable_decl_array),
+            "retain_vardsc_index_array": ",\n".join(retain_indexes),
             "var_access_code": targets.GetCode("var_access.c")
         }
 
@@ -1555,6 +1558,14 @@
                                     else:
                                         values_buffer.append((value, forced))
                             self.DebugTicks.append(debug_tick)
+                        else:
+                            # complain if trace is incomplete, but only once per debug session
+                            if self.LastComplainDebugToken != self.DebugToken :
+                                self.logger.write_warning(
+                                    _("Debug: target couldn't trace all requested variables.\n"))
+                                self.LastComplainDebugToken = self.DebugToken
+
+
 
         buffers, self.DebugValuesBuffers = (self.DebugValuesBuffers,
                                             [list() for dummy in xrange(len(self.TracedIECPath))])
@@ -1563,6 +1574,15 @@
 
         return debug_status, ticks, buffers
 
+    RegisterDebugVariableErrorCodes = { 
+        # TRACE_LIST_OVERFLOW
+        1 : _("Debug: Too many variables traced. Max 1024.\n"),
+        # FORCE_LIST_OVERFLOW
+        2 : _("Debug: Too many variables forced. Max 256.\n"),
+        # FORCE_BUFFER_OVERFLOW
+        3 : _("Debug: Cumulated forced variables size too large. Max 1KB.\n")
+    }
+
     def RegisterDebugVarToConnector(self):
         Idxs = []
         self.TracedIECPath = []
@@ -1596,7 +1616,14 @@
                 IdxsT = zip(*Idxs)
                 self.TracedIECPath = IdxsT[3]
                 self.TracedIECTypes = IdxsT[1]
-                self.DebugToken = self._connector.SetTraceVariablesList(zip(*IdxsT[0:3]))
+                res = self._connector.SetTraceVariablesList(zip(*IdxsT[0:3]))
+                if res is not None and res > 0:
+                    self.DebugToken = res
+                else:
+                    self.DebugToken = None
+                    self.logger.write_warning(
+                        self.RegisterDebugVariableErrorCodes.get(
+                            -res, _("Debug: Unknown error")))
             else:
                 self.TracedIECPath = []
                 self._connector.SetTraceVariablesList([])
--- a/runtime/PLCObject.py	Tue Nov 30 18:43:10 2021 +0100
+++ b/runtime/PLCObject.py	Sun Jan 16 16:59:40 2022 +0100
@@ -223,7 +223,7 @@
             self._ResetDebugVariables.restype = None
 
             self._RegisterDebugVariable = self.PLClibraryHandle.RegisterDebugVariable
-            self._RegisterDebugVariable.restype = None
+            self._RegisterDebugVariable.restype = ctypes.c_int
             self._RegisterDebugVariable.argtypes = [ctypes.c_int, ctypes.c_void_p]
 
             self._FreeDebugData = self.PLClibraryHandle.FreeDebugData
@@ -294,7 +294,7 @@
         self._startPLC = lambda x, y: None
         self._stopPLC = lambda: None
         self._ResetDebugVariables = lambda: None
-        self._RegisterDebugVariable = lambda x, y: None
+        self._RegisterDebugVariable = lambda x, y: 0
         self._IterDebugData = lambda x, y: None
         self._FreeDebugData = lambda: None
         self._GetDebugData = lambda: -1
@@ -720,7 +720,11 @@
                             TypeTranslator.get(iectype,
                                                (None, None, None))
                         force = ctypes.byref(pack_func(c_type, force))
-                    self._RegisterDebugVariable(idx, force)
+                    res = self._RegisterDebugVariable(idx, force)
+                    if res != 0:
+                        self._resumeDebug()
+                        self._suspendDebug(True)
+                        return -res
                 self._TracesSwap()
                 self._resumeDebug()
                 return self.DebugToken
--- a/runtime/typemapping.py	Tue Nov 30 18:43:10 2021 +0100
+++ b/runtime/typemapping.py	Sun Jan 16 16:59:40 2022 +0100
@@ -85,11 +85,22 @@
     for iectype in indexes:
         c_type, unpack_func, _pack_func = \
             TypeTranslator.get(iectype, (None, None, None))
-        if c_type is not None and buffoffset < buffsize:
-            cursor = c_void_p(buffptr + buffoffset)
+
+        cursor = c_void_p(buffptr + buffoffset)
+        if iectype == "STRING":
+            # strlen is stored in c_uint8 and sizeof(c_uint8) is 1
+            # first check we can read size
+            if (buffoffset + 1) <= buffsize:
+                size = 1 + cast(cursor,POINTER(c_type)).contents.len
+            else:
+                return None
+        else:
+            size = sizeof(c_type)
+
+        if c_type is not None and (buffoffset + size) <= buffsize:
             value = unpack_func(cast(cursor,
                                      POINTER(c_type)).contents)
-            buffoffset += sizeof(c_type) if iectype != "STRING" else len(value)+1
+            buffoffset += size
             res.append(value)
         else:
             return None
--- a/svghmi/inline_svg.ysl2	Tue Nov 30 18:43:10 2021 +0100
+++ b/svghmi/inline_svg.ysl2	Sun Jan 16 16:59:40 2022 +0100
@@ -174,7 +174,7 @@
     param "seed";
     choose {
         // node recursive copy ends when finding a widget
-        when "@id = $hmi_elements/@id" {
+        when "@id = $hmi_widgets/@id" {
             // place a clone instead of copying
             use{
                 attrib "xlink:href" > «concat('#',@id)»
--- a/svghmi/svghmi.c	Tue Nov 30 18:43:10 2021 +0100
+++ b/svghmi/svghmi.c	Sun Jan 16 16:59:40 2022 +0100
@@ -10,6 +10,7 @@
 #define HMI_ITEM_COUNT %(item_count)d
 #define HMI_HASH_SIZE 8
 #define MAX_CONNECTIONS %(max_connections)d
+#define MAX_CON_INDEX MAX_CONNECTIONS - 1
 
 static uint8_t hmi_hash[HMI_HASH_SIZE] = {%(hmi_hash_ints)s};
 
@@ -19,7 +20,6 @@
 /* PLC writes to that buffer */
 static char wbuf[HMI_BUFFER_SIZE];
 
-/* TODO change that in case of multiclient... */
 /* worst biggest send buffer. FIXME : use dynamic alloc ? */
 static char sbuf[HMI_HASH_SIZE +  HMI_BUFFER_SIZE + (HMI_ITEM_COUNT * sizeof(uint32_t))];
 static unsigned int sbufidx;
@@ -42,11 +42,15 @@
 static long hmitree_rlock = 0;
 static long hmitree_wlock = 0;
 
-typedef struct {
+typedef struct hmi_tree_item_s hmi_tree_item_t;
+struct hmi_tree_item_s{
     void *ptr;
     __IEC_types_enum type;
     uint32_t buf_index;
 
+    /* retrieve/read/recv */
+    buf_state_t rstate;
+
     /* publish/write/send */
     buf_state_t wstate[MAX_CONNECTIONS];
 
@@ -54,111 +58,114 @@
     uint16_t refresh_period_ms[MAX_CONNECTIONS];
     uint16_t age_ms[MAX_CONNECTIONS];
 
-    /* retrieve/read/recv */
-    buf_state_t rstate;
-
-} hmi_tree_item_t;
-
-static hmi_tree_item_t hmi_tree_item[] = {
+    /* dual linked list for subscriptions */
+    hmi_tree_item_t *subscriptions_next;
+    hmi_tree_item_t *subscriptions_prev;
+
+    /* single linked list for changes from HMI */
+    hmi_tree_item_t *incoming_prev;
+
+};
+
+#define HMITREE_ITEM_INITIALIZER(cpath,type,buf_index) {        \
+    &(cpath),                             /*ptr*/               \
+    type,                                 /*type*/              \
+    buf_index,                            /*buf_index*/         \
+    buf_free,                             /*rstate*/            \
+    {[0 ... MAX_CON_INDEX] = buf_free},   /*wstate*/            \
+    {[0 ... MAX_CON_INDEX] = 0},          /*refresh_period_ms*/ \
+    {[0 ... MAX_CON_INDEX] = 0},          /*age_ms*/            \
+    NULL,                                 /*subscriptions_next*/\
+    NULL,                                 /*subscriptions_prev*/\
+    NULL}                                 /*incoming_next*/
+
+
+/* entry for dual linked list for HMI subscriptions */
+/* points to the end of the list */
+static hmi_tree_item_t  *subscriptions_tail = NULL;
+
+/* entry for single linked list for changes from HMI */
+/* points to the end of the list */
+static hmi_tree_item_t *incoming_tail = NULL;
+
+static hmi_tree_item_t hmi_tree_items[] = {
 %(variable_decl_array)s
 };
 
-typedef int(*hmi_tree_iterator)(uint32_t, hmi_tree_item_t*);
-static int traverse_hmi_tree(hmi_tree_iterator fp)
-{
-    unsigned int i;
-    for(i = 0; i < sizeof(hmi_tree_item)/sizeof(hmi_tree_item_t); i++){
-        hmi_tree_item_t *dsc = &hmi_tree_item[i];
-        int res = (*fp)(i, dsc);
-        if(res != 0){
-            return res;
-        }
-    }
+#define __Unpack_desc_type hmi_tree_item_t
+
+%(var_access_code)s
+
+static int write_iterator(hmi_tree_item_t *dsc)
+{
+    uint32_t session_index = 0;
+    int value_changed = 0;
+    void *dest_p = NULL;
+    void *value_p = NULL;
+    size_t sz = 0;
+    while(session_index < MAX_CONNECTIONS) {
+        if(dsc->wstate[session_index] == buf_set){
+            /* if being subscribed */
+            if(dsc->refresh_period_ms[session_index]){
+                if(dsc->age_ms[session_index] + ticktime_ms < dsc->refresh_period_ms[session_index]){
+                    dsc->age_ms[session_index] += ticktime_ms;
+                }else{
+                    dsc->wstate[session_index] = buf_tosend;
+                    global_write_dirty = 1;
+                }
+            }
+        }
+
+        /* variable is sample only if just subscribed
+           or already subscribed and having value change */
+        int do_sample = 0;
+        int just_subscribed = dsc->wstate[session_index] == buf_new;
+        if(!just_subscribed){
+            int already_subscribed = dsc->refresh_period_ms[session_index] > 0;
+            if(already_subscribed){
+                if(!value_changed){
+                    if(!value_p){
+                        UnpackVar(dsc, &value_p, NULL, &sz);
+                        if(__Is_a_string(dsc)){
+                            sz = ((STRING*)value_p)->len + 1;
+                        }
+                        dest_p = &wbuf[dsc->buf_index];
+                    }
+                    value_changed = memcmp(dest_p, value_p, sz) != 0;
+                    do_sample = value_changed;
+                }else{
+                    do_sample = 1;
+                }
+            }
+        } else {
+            do_sample = 1;
+        }
+
+
+        if(do_sample){
+            if(dsc->wstate[session_index] != buf_set && dsc->wstate[session_index] != buf_tosend) {
+                if(dsc->wstate[session_index] == buf_new \
+                   || ticktime_ms > dsc->refresh_period_ms[session_index]){
+                    dsc->wstate[session_index] = buf_tosend;
+                    global_write_dirty = 1;
+                } else {
+                    dsc->wstate[session_index] = buf_set;
+                }
+                dsc->age_ms[session_index] = 0;
+            }
+        }
+
+        session_index++;
+    }
+    /* copy value if changed (and subscribed) */
+    if(value_changed)
+        memcpy(dest_p, value_p, sz);
     return 0;
 }
 
-#define __Unpack_desc_type hmi_tree_item_t
-
-%(var_access_code)s
-
-static int write_iterator(uint32_t index, hmi_tree_item_t *dsc)
-{
-    {
-        uint32_t session_index = 0;
-        int value_changed = 0;
-        void *dest_p = NULL;
-        void *real_value_p = NULL;
-        void *visible_value_p = NULL;
-        USINT sz = 0;
-        while(session_index < MAX_CONNECTIONS) {
-            if(dsc->wstate[session_index] == buf_set){
-                /* if being subscribed */
-                if(dsc->refresh_period_ms[session_index]){
-                    if(dsc->age_ms[session_index] + ticktime_ms < dsc->refresh_period_ms[session_index]){
-                        dsc->age_ms[session_index] += ticktime_ms;
-                    }else{
-                        dsc->wstate[session_index] = buf_tosend;
-                        global_write_dirty = 1;
-                    }
-                }
-            }
-
-            /* variable is sample only if just subscribed
-               or already subscribed and having value change */
-            int do_sample = 0;
-            int just_subscribed = dsc->wstate[session_index] == buf_new;
-            if(!just_subscribed){
-                int already_subscribed = dsc->refresh_period_ms[session_index] > 0;
-                if(already_subscribed){
-                    if(!value_changed){
-                        if(!visible_value_p){
-                            char flags = 0;
-                            visible_value_p = UnpackVar(dsc, &real_value_p, &flags);
-                            if(__Is_a_string(dsc)){
-                                sz = ((STRING*)visible_value_p)->len + 1;
-                            } else {
-                                sz = __get_type_enum_size(dsc->type);
-                            }
-                            dest_p = &wbuf[dsc->buf_index];
-                        }
-                        value_changed = memcmp(dest_p, visible_value_p, sz) != 0;
-                        do_sample = value_changed;
-                    }else{
-                        do_sample = 1;
-                    }
-                }
-            } else {
-                do_sample = 1;
-            }
-
-
-            if(do_sample){
-                if(dsc->wstate[session_index] != buf_set && dsc->wstate[session_index] != buf_tosend) {
-                    if(dsc->wstate[session_index] == buf_new \
-                       || ticktime_ms > dsc->refresh_period_ms[session_index]){
-                        dsc->wstate[session_index] = buf_tosend;
-                        global_write_dirty = 1;
-                    } else {
-                        dsc->wstate[session_index] = buf_set;
-                    }
-                    dsc->age_ms[session_index] = 0;
-                }
-            }
-
-            session_index++;
-        }
-        /* copy value if changed (and subscribed) */
-        if(value_changed)
-            memcpy(dest_p, visible_value_p, sz);
-    }
-    // else ... : PLC can't wait, variable will be updated next turn
-    return 0;
-}
-
-static uint32_t send_session_index;
-static int send_iterator(uint32_t index, hmi_tree_item_t *dsc)
-{
-    if(dsc->wstate[send_session_index] == buf_tosend)
+static int send_iterator(uint32_t index, hmi_tree_item_t *dsc, uint32_t session_index)
+{
+    if(dsc->wstate[session_index] == buf_tosend)
     {
         uint32_t sz = __get_type_enum_size(dsc->type);
         if(sbufidx + sizeof(uint32_t) + sz <=  sizeof(sbuf))
@@ -171,7 +178,7 @@
             /* TODO : force into little endian */
             memcpy(dst_p, &index, sizeof(uint32_t));
             memcpy(dst_p + sizeof(uint32_t), src_p, sz);
-            dsc->wstate[send_session_index] = buf_free;
+            dsc->wstate[session_index] = buf_free;
             sbufidx += sizeof(uint32_t) /* index */ + sz;
         }
         else
@@ -184,15 +191,15 @@
     return 0;
 }
 
-static int read_iterator(uint32_t index, hmi_tree_item_t *dsc)
+static int read_iterator(hmi_tree_item_t *dsc)
 {
     if(dsc->rstate == buf_set)
     {
         void *src_p = &rbuf[dsc->buf_index];
-        void *real_value_p = NULL;
-        char flags = 0;
-        void *visible_value_p = UnpackVar(dsc, &real_value_p, &flags);
-        memcpy(real_value_p, src_p, __get_type_enum_size(dsc->type));
+        void *value_p = NULL;
+        size_t sz = 0;
+        UnpackVar(dsc, &value_p, NULL, &sz);
+        memcpy(value_p, src_p, sz);
         dsc->rstate = buf_free;
     }
     return 0;
@@ -200,24 +207,64 @@
 
 void update_refresh_period(hmi_tree_item_t *dsc, uint32_t session_index, uint16_t refresh_period_ms)
 {
-    if(refresh_period_ms) {
-        if(!dsc->refresh_period_ms[session_index])
+    uint32_t other_session_index = 0;
+    int previously_subscribed = 0;
+    int session_only_subscriber = 0;
+    int session_already_subscriber = 0;
+    int needs_subscription_for_session = (refresh_period_ms != 0);
+
+    while(other_session_index < session_index) {
+        previously_subscribed |= (dsc->refresh_period_ms[other_session_index++] != 0);
+    }
+    session_already_subscriber = (dsc->refresh_period_ms[other_session_index++] != 0);
+    while(other_session_index < MAX_CONNECTIONS) {
+        previously_subscribed |= (dsc->refresh_period_ms[other_session_index++] != 0);
+    }
+    session_only_subscriber = session_already_subscriber && !previously_subscribed;
+    previously_subscribed |= session_already_subscriber;
+
+    if(needs_subscription_for_session) {
+        if(!session_already_subscriber)
         {
             dsc->wstate[session_index] = buf_new;
         }
+        /* item is appended to list only when no session was previously subscribed */
+        if(!previously_subscribed){
+            /* append subsciption to list */
+            if(subscriptions_tail != NULL){ 
+                /* if list wasn't empty, link with previous tail*/
+                subscriptions_tail->subscriptions_next = dsc;
+            }
+            dsc->subscriptions_prev = subscriptions_tail;
+            subscriptions_tail = dsc;
+            dsc->subscriptions_next = NULL;
+        }
     } else {
         dsc->wstate[session_index] = buf_free;
+        /* item is removed from list only when session was the only one remaining */
+        if (session_only_subscriber) {
+            if(dsc->subscriptions_next == NULL){ /* remove tail  */
+                /* re-link tail to previous */
+                subscriptions_tail = dsc->subscriptions_prev;
+                if(subscriptions_tail != NULL){
+                    subscriptions_tail->subscriptions_next = NULL;
+                }
+            } else if(dsc->subscriptions_prev == NULL){ /* remove head  */
+                dsc->subscriptions_next->subscriptions_prev = NULL;
+            } else { /* remove entry in between other entries */
+                /* re-link previous and next node */
+                dsc->subscriptions_next->subscriptions_prev = dsc->subscriptions_prev;
+                dsc->subscriptions_prev->subscriptions_next = dsc->subscriptions_next;
+            }
+            /* unnecessary
+            dsc->subscriptions_next = NULL;
+            dsc->subscriptions_prev = NULL;
+            */
+        }
     }
     dsc->refresh_period_ms[session_index] = refresh_period_ms;
 }
 
-static uint32_t reset_session_index;
-static int reset_iterator(uint32_t index, hmi_tree_item_t *dsc)
-{
-    update_refresh_period(dsc, reset_session_index, 0);
-    return 0;
-}
-
 static void *svghmi_handle;
 
 void SVGHMI_SuspendFromPythonThread(void)
@@ -242,7 +289,7 @@
     /* create svghmi_pipe */
     svghmi_handle = create_RT_to_nRT_signal("SVGHMI_pipe");
 
-    if(!svghmi_handle) 
+    if(!svghmi_handle)
         return 1;
 
     return 0;
@@ -258,7 +305,18 @@
 void __retrieve_svghmi()
 {
     if(AtomicCompareExchange(&hmitree_rlock, 0, 1) == 0) {
-        traverse_hmi_tree(read_iterator);
+        hmi_tree_item_t *dsc = incoming_tail;
+        /* iterate through read list (changes from HMI) */
+        while(dsc){
+            hmi_tree_item_t *_dsc = dsc->incoming_prev;
+            read_iterator(dsc);
+            /* unnecessary
+            dsc->incoming_prev = NULL;
+            */
+            dsc = _dsc;
+        }
+        /* flush read list */
+        incoming_tail = NULL;
         AtomicCompareExchange(&hmitree_rlock, 1, 0);
     }
 }
@@ -266,10 +324,16 @@
 void __publish_svghmi()
 {
     global_write_dirty = 0;
+
     if(AtomicCompareExchange(&hmitree_wlock, 0, 1) == 0) {
-        traverse_hmi_tree(write_iterator);
+        hmi_tree_item_t *dsc = subscriptions_tail;
+        while(dsc){
+            write_iterator(dsc);
+            dsc = dsc->subscriptions_prev;
+        }
         AtomicCompareExchange(&hmitree_wlock, 1, 0);
     }
+
     if(global_write_dirty) {
         SVGHMI_WakeupFromRTThread();
     }
@@ -283,16 +347,25 @@
 
 int svghmi_send_collect(uint32_t session_index, uint32_t *size, char **ptr){
 
+
     if(svghmi_continue_collect) {
         int res;
         sbufidx = HMI_HASH_SIZE;
-        send_session_index = session_index;
 
         while(AtomicCompareExchange(&hmitree_wlock, 0, 1)){
             nRT_reschedule();
         }
 
-        if((res = traverse_hmi_tree(send_iterator)) == 0)
+        hmi_tree_item_t *dsc = subscriptions_tail;
+        while(dsc){
+            uint32_t index = dsc - hmi_tree_items;
+            res = send_iterator(index, dsc, session_index);
+            if(res != 0){
+                break;
+            }
+            dsc = dsc->subscriptions_prev;
+        }
+        if(res == 0)
         {
             if(sbufidx > HMI_HASH_SIZE){
                 memcpy(&sbuf[0], &hmi_hash[0], HMI_HASH_SIZE);
@@ -304,7 +377,6 @@
             AtomicCompareExchange(&hmitree_wlock, 1, 0);
             return ENODATA;
         }
-        // printf("collected BAD result %%d\n", res);
         AtomicCompareExchange(&hmitree_wlock, 1, 0);
         return res;
     }
@@ -322,8 +394,16 @@
 } cmd_from_JS;
 
 int svghmi_reset(uint32_t session_index){
-    reset_session_index = session_index;
-    traverse_hmi_tree(reset_iterator);
+    hmi_tree_item_t *dsc = subscriptions_tail;
+    while(AtomicCompareExchange(&hmitree_wlock, 0, 1)){
+        nRT_reschedule();
+    }
+    while(dsc){
+        hmi_tree_item_t *_dsc = dsc->subscriptions_prev;
+        update_refresh_period(dsc, session_index, 0);
+        dsc = _dsc;
+    }
+    AtomicCompareExchange(&hmitree_wlock, 1, 0);
     return 1;
 }
 
@@ -355,6 +435,7 @@
         cmd_old = cmd;
         cmd = *(cursor++);
 
+
         if(cmd_old != cmd){
             if(got_wlock){
                 AtomicCompareExchange(&hmitree_wlock, 1, 0);
@@ -372,20 +453,20 @@
                 uint32_t index = *(uint32_t*)(cursor);
                 uint8_t const *valptr = cursor + sizeof(uint32_t);
 
+
                 if(index == heartbeat_index)
                     was_hearbeat = 1;
 
                 if(index < HMI_ITEM_COUNT)
                 {
-                    hmi_tree_item_t *dsc = &hmi_tree_item[index];
-                    void *real_value_p = NULL;
-                    char flags = 0;
-                    void *visible_value_p = UnpackVar(dsc, &real_value_p, &flags);
+                    hmi_tree_item_t *dsc = &hmi_tree_items[index];
+                    size_t sz = 0;
                     void *dst_p = &rbuf[dsc->buf_index];
-                    uint32_t sz = __get_type_enum_size(dsc->type);
 
                     if(__Is_a_string(dsc)){
                         sz = ((STRING*)valptr)->len + 1;
+                    } else {
+                        UnpackVar(dsc, NULL, NULL, &sz);
                     }
 
                     if((valptr + sz) <= end)
@@ -399,7 +480,14 @@
                         }
 
                         memcpy(dst_p, valptr, sz);
-                        dsc->rstate = buf_set;
+
+                        /* check that rstate is not already buf_set */
+                        if(dsc->rstate != buf_set){
+                            dsc->rstate = buf_set;
+                            /* append entry to read list (changes from HMI) */
+                            dsc->incoming_prev = incoming_tail;
+                            incoming_tail = dsc;
+                        }
 
                         progress = sz + sizeof(uint32_t) /* index */;
                     }
@@ -420,14 +508,20 @@
             case reset:
             {
                 progress = 0;
-                reset_session_index = session_index;
                 if(!got_wlock){
                     while(AtomicCompareExchange(&hmitree_wlock, 0, 1)){
                         nRT_reschedule();
                     }
                     got_wlock = 1;
                 }
-                traverse_hmi_tree(reset_iterator);
+                {
+                    hmi_tree_item_t *dsc = subscriptions_tail;
+                    while(dsc){
+                        hmi_tree_item_t *_dsc = dsc->subscriptions_prev;
+                        update_refresh_period(dsc, session_index, 0);
+                        dsc = _dsc;
+                    }
+                }
             }
             break;
 
@@ -444,7 +538,7 @@
                         }
                         got_wlock = 1;
                     }
-                    hmi_tree_item_t *dsc = &hmi_tree_item[index];
+                    hmi_tree_item_t *dsc = &hmi_tree_items[index];
                     update_refresh_period(dsc, session_index, refresh_period_ms);
                 }
                 else
--- a/svghmi/svghmi.py	Tue Nov 30 18:43:10 2021 +0100
+++ b/svghmi/svghmi.py	Sun Jan 16 16:59:40 2022 +0100
@@ -191,14 +191,14 @@
             if hasattr(node, "iectype"):
                 sz = DebugTypesSize.get(node.iectype, 0)
                 variable_decl_array += [
-                    "{&(" + node.cpath + "), " + node.iectype + {
+                    "HMITREE_ITEM_INITIALIZER(" + node.cpath + ", " + node.iectype + {
                         "EXT": "_P_ENUM",
                         "IN":  "_P_ENUM",
                         "MEM": "_O_ENUM",
                         "OUT": "_O_ENUM",
                         "VAR": "_ENUM"
                     }[node.vartype] + ", " +
-                    str(buf_index) + ", 0, }"]
+                    str(buf_index) + ")"]
                 buf_index += sz
                 item_count += 1
                 if len(node.path) == 1:
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/svghmi/widget_listswitch.ysl2	Sun Jan 16 16:59:40 2022 +0100
@@ -0,0 +1,38 @@
+// widget_switch.ysl2
+
+widget_desc("ListSwitch") {
+    longdesc
+    ||
+    ListSwitch widget displays one item of an HMI:List depending on value of
+    given variable. Main element of the widget must be a clone of the list or
+    of an item of that list.  
+
+    Given variable's current value is compared to list items
+    label. For exemple if given variable type
+    is HMI_INT and value is 1, then item with label '1' will be displayed.
+    If matching variable of type HMI_STRING, then no quotes are needed. 
+    For exemple, 'hello' match HMI_STRING 'hello'.
+    ||
+
+    shortdesc > Displays item of an HMI:List whose label matches value.
+
+    path name="value" accepts="HMI_INT,HMI_STRING" > value to compare to labels
+    
+}
+
+widget_class("ListSwitch"){
+    ||
+        frequency = 5;
+    ||
+}
+
+widget_defs("ListSwitch") {
+    // cloned element must be part of a HMI:List or a HMI:List
+    const "targetid", "substring-after($hmi_element/@xlink:href,'#')";
+    const "from_list", "$hmi_lists[(@id | */@id) = $targetid]";
+    ||
+        dispatch: function(value) {
+            this.element.href.baseVal = "#"+hmi_widgets["«$from_list/@id»"].items[value];
+        },
+    ||
+}
--- a/targets/Xenomai/plc_Xenomai_main.c	Tue Nov 30 18:43:10 2021 +0100
+++ b/targets/Xenomai/plc_Xenomai_main.c	Sun Jan 16 16:59:40 2022 +0100
@@ -384,6 +384,7 @@
 
 void resumeDebug(void)
 {
+    __DEBUG = 1;
     AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);
 }
 
--- a/targets/plc_debug.c	Tue Nov 30 18:43:10 2021 +0100
+++ b/targets/plc_debug.c	Sun Jan 16 16:59:40 2022 +0100
@@ -25,22 +25,59 @@
 #include <string.h>
 #include <stdio.h>
 
+typedef unsigned int dbgvardsc_index_t;
+typedef unsigned short trace_buf_offset_t;
+
+#define BUFFER_EMPTY 0
+#define BUFFER_FULL 1
+
 #ifndef TARGET_ONLINE_DEBUG_DISABLE
-#define BUFFER_SIZE %(buffer_size)d
+
+#define TRACE_BUFFER_SIZE 4096
+#define TRACE_LIST_SIZE 1024
 
 /* Atomically accessed variable for buffer state */
-#define BUFFER_FREE 0
-#define BUFFER_BUSY 1
-static long buffer_state = BUFFER_FREE;
-
-/* The buffer itself */
-char debug_buffer[BUFFER_SIZE];
-
-/* Buffer's cursor*/
-static char* buffer_cursor = debug_buffer;
+static long trace_buffer_state = BUFFER_EMPTY;
+
+typedef struct trace_item_s {
+    dbgvardsc_index_t dbgvardsc_index;
+} trace_item_t;
+
+trace_item_t trace_list[TRACE_LIST_SIZE];
+char trace_buffer[TRACE_BUFFER_SIZE];
+
+/* Trace's cursor*/
+static trace_item_t *trace_list_collect_cursor = trace_list;
+static trace_item_t *trace_list_addvar_cursor = trace_list;
+static const trace_item_t *trace_list_end = 
+    &trace_list[TRACE_LIST_SIZE-1];
+static char *trace_buffer_cursor = trace_buffer;
+static const char *trace_buffer_end = trace_buffer + TRACE_BUFFER_SIZE;
+
+
+
+#define FORCE_BUFFER_SIZE 1024
+#define FORCE_LIST_SIZE 256
+
+typedef struct force_item_s {
+    dbgvardsc_index_t dbgvardsc_index;
+    void *value_pointer_backup;
+} force_item_t;
+
+force_item_t force_list[FORCE_LIST_SIZE];
+char force_buffer[FORCE_BUFFER_SIZE];
+
+/* Force's cursor*/
+static force_item_t *force_list_apply_cursor = force_list;
+static force_item_t *force_list_addvar_cursor = force_list;
+static const force_item_t *force_list_end = 
+    &force_list[FORCE_LIST_SIZE-1];
+static char *force_buffer_cursor = force_buffer;
+static const char *force_buffer_end = force_buffer + FORCE_BUFFER_SIZE;
+
+
 #endif
 
-static unsigned int retain_offset = 0;
 /***
  * Declare programs 
  **/
@@ -56,10 +93,16 @@
     __IEC_types_enum type;
 } dbgvardsc_t;
 
-static dbgvardsc_t dbgvardsc[] = {
+static const dbgvardsc_t dbgvardsc[] = {
 %(variable_decl_array)s
 };
 
+static const dbgvardsc_index_t retain_list[] = {
+%(retain_vardsc_index_array)s
+};
+static unsigned int retain_list_collect_cursor = 0;
+static const unsigned int retain_list_size = sizeof(retain_list)/sizeof(dbgvardsc_index_t);
+
 typedef void(*__for_each_variable_do_fp)(dbgvardsc_t*);
 void __for_each_variable_do(__for_each_variable_do_fp fp)
 {
@@ -77,23 +120,6 @@
 
 void Remind(unsigned int offset, unsigned int count, void * p);
 
-void RemindIterator(dbgvardsc_t *dsc)
-{
-    void *real_value_p = NULL;
-    char flags = 0;
-    UnpackVar(dsc, &real_value_p, &flags);
-
-    if(flags & __IEC_RETAIN_FLAG){
-        USINT size = __get_type_enum_size(dsc->type);
-        /* compute next cursor positon*/
-        unsigned int next_retain_offset = retain_offset + size;
-        /* if buffer not full */
-        Remind(retain_offset, size, real_value_p);
-        /* increment cursor according size*/
-        retain_offset = next_retain_offset;
-    }
-}
-
 extern int CheckRetainBuffer(void);
 extern void InitRetain(void);
 
@@ -101,20 +127,46 @@
 {
     /* init local static vars */
 #ifndef TARGET_ONLINE_DEBUG_DISABLE
-    buffer_cursor = debug_buffer;
-    buffer_state = BUFFER_FREE;
+    trace_buffer_cursor = trace_buffer;
+    trace_list_addvar_cursor = trace_list;
+    trace_list_collect_cursor = trace_list;
+    trace_buffer_state = BUFFER_EMPTY;
+
+    force_buffer_cursor = force_buffer;
+    force_list_addvar_cursor = force_list;
+    force_list_apply_cursor = force_list;
 #endif
 
-    retain_offset = 0;
     InitRetain();
     /* Iterate over all variables to fill debug buffer */
     if(CheckRetainBuffer()){
-        __for_each_variable_do(RemindIterator);
+        static unsigned int retain_offset = 0;
+        retain_list_collect_cursor = 0;
+
+        /* iterate over retain list */
+        while(retain_list_collect_cursor < retain_list_size){
+            void *value_p = NULL;
+            size_t size;
+            char* next_cursor;
+
+            dbgvardsc_t *dsc = &dbgvardsc[
+                retain_list[retain_list_collect_cursor]];
+
+            UnpackVar(dsc, &value_p, NULL, &size);
+
+            printf("Reminding %%d %%ld \n", retain_list_collect_cursor, size);
+
+            /* if buffer not full */
+            Remind(retain_offset, size, value_p);
+            /* increment cursor according size*/
+            retain_offset += size;
+
+            retain_list_collect_cursor++;
+        }
     }else{
         char mstr[] = "RETAIN memory invalid - defaults used";
         LogMessage(LOG_WARNING, mstr, sizeof(mstr));
     }
-    retain_offset = 0;
 }
 
 extern void InitiateDebugTransfer(void);
@@ -125,7 +177,7 @@
 void __cleanup_debug(void)
 {
 #ifndef TARGET_ONLINE_DEBUG_DISABLE
-    buffer_cursor = debug_buffer;
+    trace_buffer_cursor = trace_buffer;
     InitiateDebugTransfer();
 #endif    
 
@@ -136,84 +188,31 @@
 {
 }
 
-
 void Retain(unsigned int offset, unsigned int count, void * p);
 
-static inline void BufferIterator(dbgvardsc_t *dsc, int do_debug)
-{
-    void *real_value_p = NULL;
-    void *visible_value_p = NULL;
-    char flags = 0;
-
-    visible_value_p = UnpackVar(dsc, &real_value_p, &flags);
-
-    if(flags & ( __IEC_DEBUG_FLAG | __IEC_RETAIN_FLAG)){
-        USINT size = __get_type_enum_size(dsc->type);
-
-#ifndef TARGET_ONLINE_DEBUG_DISABLE
-        if(flags & __IEC_DEBUG_FLAG){
-            /* copy visible variable to buffer */;
-            if(do_debug){
-                /* compute next cursor positon.
-                   No need to check overflow, as BUFFER_SIZE
-                   is computed large enough */
-                if(__Is_a_string(dsc)){
-                    /* optimization for strings */
-                    size = ((STRING*)visible_value_p)->len + 1;
-                }
-                char* next_cursor = buffer_cursor + size;
-                /* copy data to the buffer */
-                memcpy(buffer_cursor, visible_value_p, size);
-                /* increment cursor according size*/
-                buffer_cursor = next_cursor;
-            }
-            /* re-force real value of outputs (M and Q)*/
-            if((flags & __IEC_FORCE_FLAG) && (flags & __IEC_OUTPUT_FLAG)){
-                memcpy(real_value_p, visible_value_p, size);
-            }
-        }
-#endif
-
-        if(flags & __IEC_RETAIN_FLAG){
-            /* compute next cursor positon*/
-            unsigned int next_retain_offset = retain_offset + size;
-            /* if buffer not full */
-            Retain(retain_offset, size, real_value_p);
-            /* increment cursor according size*/
-            retain_offset = next_retain_offset;
-        }
-    }
-}
-
-void DebugIterator(dbgvardsc_t *dsc){
-    BufferIterator(dsc, 1);
-}
-
-void RetainIterator(dbgvardsc_t *dsc){
-    BufferIterator(dsc, 0);
-}
-
-
-unsigned int retain_size = 0;
-
-/* GetRetainSizeIterator */
-void GetRetainSizeIterator(dbgvardsc_t *dsc)
-{
-    void *real_value_p = NULL;
-    char flags = 0;
-    UnpackVar(dsc, &real_value_p, &flags);
-
-    if(flags & __IEC_RETAIN_FLAG){
-        USINT size = __get_type_enum_size(dsc->type);
-        /* Calc retain buffer size */
-        retain_size += size;
-    }
-}
-
 /* Return size of all retain variables */
 unsigned int GetRetainSize(void)
 {
-    __for_each_variable_do(GetRetainSizeIterator);
+    unsigned int retain_size = 0;
+    retain_list_collect_cursor = 0;
+
+    /* iterate over retain list */
+    while(retain_list_collect_cursor < retain_list_size){
+        void *value_p = NULL;
+        size_t size;
+        char* next_cursor;
+
+        dbgvardsc_t *dsc = &dbgvardsc[
+            retain_list[retain_list_collect_cursor]];
+
+        UnpackVar(dsc, &value_p, NULL, &size);
+
+        retain_size += size;
+        retain_list_collect_cursor++;
+    }
+
+    printf("Retain size %%d \n", retain_size);
+            
     return retain_size;
 }
 
@@ -226,9 +225,26 @@
 extern void ValidateRetainBuffer(void);
 extern void InValidateRetainBuffer(void);
 
+#define __ReForceOutput_case_p(TYPENAME)                                                            \
+        case TYPENAME##_P_ENUM :                                                                    \
+        case TYPENAME##_O_ENUM :                                                                    \
+            {                                                                                       \
+                char *next_cursor = force_buffer_cursor + sizeof(TYPENAME);                         \
+                if(next_cursor <= force_buffer_end ){                                               \
+                    /* outputs real value must be systematically forced */                          \
+                    if(vartype == TYPENAME##_O_ENUM)                                                \
+                        /* overwrite value pointed by backup */                                     \
+                        *((TYPENAME *)force_list_apply_cursor->value_pointer_backup) =  \
+                            *((TYPENAME *)force_buffer_cursor);                                     \
+                    /* inc force_buffer cursor */                                                   \
+                    force_buffer_cursor = next_cursor;                                              \
+                }else{                                                                              \
+                    stop = 1;                                                                       \
+                }                                                                                   \
+            }                                                                                       \
+            break;
 void __publish_debug(void)
 {
-    retain_offset = 0;
     InValidateRetainBuffer();
     
 #ifndef TARGET_ONLINE_DEBUG_DISABLE 
@@ -236,116 +252,248 @@
     if(TryEnterDebugSection()){
         /* Lock buffer */
         long latest_state = AtomicCompareExchange(
-            &buffer_state,
-            BUFFER_FREE,
-            BUFFER_BUSY);
+            &trace_buffer_state,
+            BUFFER_EMPTY,
+            BUFFER_FULL);
             
         /* If buffer was free */
-        if(latest_state == BUFFER_FREE)
+        if(latest_state == BUFFER_EMPTY)
         {
+            int stop = 0;
+            /* Reset force list cursor */
+            force_list_apply_cursor = force_list;
+
+            /* iterate over force list */
+            while(!stop && force_list_apply_cursor < force_list_addvar_cursor){
+                dbgvardsc_t *dsc = &dbgvardsc[
+                    force_list_apply_cursor->dbgvardsc_index];
+                void *varp = dsc->ptr;
+                __IEC_types_enum vartype = dsc->type;
+                switch(vartype){
+                    __ANY(__ReForceOutput_case_p)
+                default:
+                    break;
+                }
+                force_list_apply_cursor++;                                                      \
+            }
+
             /* Reset buffer cursor */
-            buffer_cursor = debug_buffer;
-            /* Iterate over all variables to fill debug buffer */
-            __for_each_variable_do(DebugIterator);
+            trace_buffer_cursor = trace_buffer;
+            /* Reset trace list cursor */
+            trace_list_collect_cursor = trace_list;
+
+            /* iterate over trace list */
+            while(trace_list_collect_cursor < trace_list_addvar_cursor){
+                void *value_p = NULL;
+                size_t size;
+                char* next_cursor;
+
+                dbgvardsc_t *dsc = &dbgvardsc[
+                    trace_list_collect_cursor->dbgvardsc_index];
+
+                UnpackVar(dsc, &value_p, NULL, &size);
+
+                /* copy visible variable to buffer */;
+                if(__Is_a_string(dsc)){
+                    /* optimization for strings */
+                    /* assume NULL terminated strings */
+                    size = ((STRING*)value_p)->len + 1;
+                }
+
+                /* compute next cursor positon.*/
+                next_cursor = trace_buffer_cursor + size;
+                /* check for buffer overflow */
+                if(next_cursor < trace_buffer_end)
+                    /* copy data to the buffer */
+                    memcpy(trace_buffer_cursor, value_p, size);
+                else
+                    /* stop looping in case of overflow */
+                    break;
+                /* increment cursor according size*/
+                trace_buffer_cursor = next_cursor;
+                trace_list_collect_cursor++;
+            }
             
             /* Leave debug section,
              * Trigger asynchronous transmission 
              * (returns immediately) */
             InitiateDebugTransfer(); /* size */
-        }else{
-            /* when not debugging, do only retain */
-            __for_each_variable_do(RetainIterator);
         }
         LeaveDebugSection();
-    }else
+    }
 #endif
-    {
-        /* when not debugging, do only retain */
-        __for_each_variable_do(RetainIterator);
+    static unsigned int retain_offset = 0;
+    /* when not debugging, do only retain */
+    retain_list_collect_cursor = 0;
+
+    /* iterate over retain list */
+    while(retain_list_collect_cursor < retain_list_size){
+        void *value_p = NULL;
+        size_t size;
+        char* next_cursor;
+
+        dbgvardsc_t *dsc = &dbgvardsc[
+            retain_list[retain_list_collect_cursor]];
+
+        UnpackVar(dsc, &value_p, NULL, &size);
+
+        printf("Retaining %%d %%ld \n", retain_list_collect_cursor, size);
+
+        /* if buffer not full */
+        Retain(retain_offset, size, value_p);
+        /* increment cursor according size*/
+        retain_offset += size;
+
+        retain_list_collect_cursor++;
     }
     ValidateRetainBuffer();
 }
 
 #ifndef TARGET_ONLINE_DEBUG_DISABLE
-#define __RegisterDebugVariable_case_t(TYPENAME) \
-        case TYPENAME##_ENUM :\
-            ((__IEC_##TYPENAME##_t *)varp)->flags |= flags;\
-            if(force)\
-             ((__IEC_##TYPENAME##_t *)varp)->value = *((TYPENAME *)force);\
+
+#define TRACE_LIST_OVERFLOW    1
+#define FORCE_LIST_OVERFLOW    2
+#define FORCE_BUFFER_OVERFLOW  3
+
+#define __ForceVariable_case_t(TYPENAME)                                                \
+        case TYPENAME##_ENUM :                                                          \
+            /* add to force_list*/                                                      \
+            force_list_addvar_cursor->dbgvardsc_index = idx;                            \
+            ((__IEC_##TYPENAME##_t *)varp)->flags |= __IEC_FORCE_FLAG;                  \
+            ((__IEC_##TYPENAME##_t *)varp)->value = *((TYPENAME *)force);               \
             break;
-#define __RegisterDebugVariable_case_p(TYPENAME)\
-        case TYPENAME##_P_ENUM :\
-            ((__IEC_##TYPENAME##_p *)varp)->flags |= flags;\
-            if(force)\
-             ((__IEC_##TYPENAME##_p *)varp)->fvalue = *((TYPENAME *)force);\
-            break;\
-        case TYPENAME##_O_ENUM :\
-            ((__IEC_##TYPENAME##_p *)varp)->flags |= flags;\
-            if(force){\
-             ((__IEC_##TYPENAME##_p *)varp)->fvalue = *((TYPENAME *)force);\
-             *(((__IEC_##TYPENAME##_p *)varp)->value) = *((TYPENAME *)force);\
-            }\
+#define __ForceVariable_case_p(TYPENAME)                                                \
+        case TYPENAME##_P_ENUM :                                                        \
+        case TYPENAME##_O_ENUM :                                                        \
+            {                                                                           \
+                char *next_cursor = force_buffer_cursor + sizeof(TYPENAME);             \
+                if(next_cursor <= force_buffer_end ){                                   \
+                    /* add to force_list*/                                              \
+                    force_list_addvar_cursor->dbgvardsc_index = idx;                    \
+                    /* save pointer to backup */                                        \
+                    force_list_addvar_cursor->value_pointer_backup =                    \
+                        ((__IEC_##TYPENAME##_p *)varp)->value;                          \
+                    /* store forced value in force_buffer */                            \
+                    *((TYPENAME *)force_buffer_cursor) = *((TYPENAME *)force);          \
+                    /* replace pointer with pointer to force_buffer */                  \
+                    ((__IEC_##TYPENAME##_p *)varp)->value =                             \
+                        (TYPENAME *)force_buffer_cursor;                                \
+                    /* mark variable as forced */                                       \
+                    ((__IEC_##TYPENAME##_p *)varp)->flags |= __IEC_FORCE_FLAG;          \
+                    /* inc force_buffer cursor */                                       \
+                    force_buffer_cursor = next_cursor;                                  \
+                    /* outputs real value must be systematically forced */              \
+                    if(vartype == TYPENAME##_O_ENUM)                                    \
+                        *(((__IEC_##TYPENAME##_p *)varp)->value) = *((TYPENAME *)force);\
+                } else {                                                                \
+                    error_code = FORCE_BUFFER_OVERFLOW;                                 \
+                    goto error_cleanup;                                                 \
+                }                                                                       \
+            }                                                                           \
             break;
-void RegisterDebugVariable(unsigned int idx, void* force)
-{
-    if(idx  < sizeof(dbgvardsc)/sizeof(dbgvardsc_t)){
-        unsigned char flags = force ?
-            __IEC_DEBUG_FLAG | __IEC_FORCE_FLAG :
-            __IEC_DEBUG_FLAG;
-        dbgvardsc_t *dsc = &dbgvardsc[idx];
+
+
+void ResetDebugVariables(void);
+
+int RegisterDebugVariable(dbgvardsc_index_t idx, void* force)
+{
+    int error_code = 0;
+    if(idx < sizeof(dbgvardsc)/sizeof(dbgvardsc_t)){
+        /* add to trace_list, inc trace_list_addvar_cursor*/
+        if(trace_list_addvar_cursor <= trace_list_end){
+            trace_list_addvar_cursor->dbgvardsc_index = idx;
+            trace_list_addvar_cursor++;
+        } else {
+            error_code = TRACE_LIST_OVERFLOW;
+            goto error_cleanup;
+        }
+        if(force){
+            if(force_list_addvar_cursor <= force_list_end){
+                dbgvardsc_t *dsc = &dbgvardsc[idx];
+                void *varp = dsc->ptr;
+                __IEC_types_enum vartype = dsc->type;
+
+                switch(vartype){
+                    __ANY(__ForceVariable_case_t)
+                    __ANY(__ForceVariable_case_p)
+                default:
+                    break;
+                }
+                /* inc force_list cursor */
+                force_list_addvar_cursor++;
+            } else {
+                error_code = FORCE_LIST_OVERFLOW;
+                goto error_cleanup;
+            }
+        }
+    }
+    return 0;
+
+error_cleanup:
+    ResetDebugVariables();
+    trace_buffer_state = BUFFER_EMPTY;
+    return error_code;
+    
+}
+
+#define ResetForcedVariable_case_t(TYPENAME)                                            \
+        case TYPENAME##_ENUM :                                                          \
+            ((__IEC_##TYPENAME##_t *)varp)->flags &= ~__IEC_FORCE_FLAG;                 \
+            /* for local variable we don't restore original value */                    \
+            /* that can be added if needed, but it was like that since ever */          \
+            break;
+
+#define ResetForcedVariable_case_p(TYPENAME)                                            \
+        case TYPENAME##_P_ENUM :                                                        \
+        case TYPENAME##_O_ENUM :                                                        \
+            ((__IEC_##TYPENAME##_p *)varp)->flags &= ~__IEC_FORCE_FLAG;                 \
+            /* restore backup to pointer */                                             \
+            ((__IEC_##TYPENAME##_p *)varp)->value =                                     \
+                force_list_apply_cursor->value_pointer_backup;                          \
+            break;
+
+void ResetDebugVariables(void)
+{
+    /* Reset trace list */
+    trace_list_addvar_cursor = trace_list;
+
+    force_list_apply_cursor = force_list;
+    /* Restore forced variables */
+    while(force_list_apply_cursor < force_list_addvar_cursor){
+        dbgvardsc_t *dsc = &dbgvardsc[
+            force_list_apply_cursor->dbgvardsc_index];
         void *varp = dsc->ptr;
         switch(dsc->type){
-            __ANY(__RegisterDebugVariable_case_t)
-            __ANY(__RegisterDebugVariable_case_p)
+            __ANY(ResetForcedVariable_case_t)
+            __ANY(ResetForcedVariable_case_p)
         default:
             break;
         }
-    }
-}
-
-#define __ResetDebugVariablesIterator_case_t(TYPENAME) \
-        case TYPENAME##_ENUM :\
-            ((__IEC_##TYPENAME##_t *)varp)->flags &= ~(__IEC_DEBUG_FLAG|__IEC_FORCE_FLAG);\
-            break;
-
-#define __ResetDebugVariablesIterator_case_p(TYPENAME)\
-        case TYPENAME##_P_ENUM :\
-        case TYPENAME##_O_ENUM :\
-            ((__IEC_##TYPENAME##_p *)varp)->flags &= ~(__IEC_DEBUG_FLAG|__IEC_FORCE_FLAG);\
-            break;
-
-void ResetDebugVariablesIterator(dbgvardsc_t *dsc)
-{
-    /* force debug flag to 0*/
-    void *varp = dsc->ptr;
-    switch(dsc->type){
-        __ANY(__ResetDebugVariablesIterator_case_t)
-        __ANY(__ResetDebugVariablesIterator_case_p)
-    default:
-        break;
-    }
-}
-
-void ResetDebugVariables(void)
-{
-    __for_each_variable_do(ResetDebugVariablesIterator);
+        /* inc force_list cursor */
+        force_list_apply_cursor++;
+    } /* else TODO: warn user about failure to force */ 
+
+    /* Reset force list */
+    force_list_addvar_cursor = force_list;
+    /* Reset force buffer */
+    force_buffer_cursor = force_buffer;
 }
 
 void FreeDebugData(void)
 {
     /* atomically mark buffer as free */
     AtomicCompareExchange(
-        &buffer_state,
-        BUFFER_BUSY,
-        BUFFER_FREE);
+        &trace_buffer_state,
+        BUFFER_FULL,
+        BUFFER_EMPTY);
 }
 int WaitDebugData(unsigned long *tick);
 /* Wait until debug data ready and return pointer to it */
 int GetDebugData(unsigned long *tick, unsigned long *size, void **buffer){
     int wait_error = WaitDebugData(tick);
     if(!wait_error){
-        *size = buffer_cursor - debug_buffer;
-        *buffer = debug_buffer;
+        *size = trace_buffer_cursor - trace_buffer;
+        *buffer = trace_buffer;
     }
     return wait_error;
 }
--- a/targets/var_access.c	Tue Nov 30 18:43:10 2021 +0100
+++ b/targets/var_access.c	Sun Jan 16 16:59:40 2022 +0100
@@ -1,37 +1,33 @@
 
-#define __Unpack_case_t(TYPENAME) \
-        case TYPENAME##_ENUM :\
-            *flags = ((__IEC_##TYPENAME##_t *)varp)->flags;\
-            forced_value_p = *real_value_p = &((__IEC_##TYPENAME##_t *)varp)->value;\
+#define __Unpack_case_t(TYPENAME)                                           \
+        case TYPENAME##_ENUM :                                              \
+            if(flags) *flags = ((__IEC_##TYPENAME##_t *)varp)->flags;       \
+            if(value_p) *value_p = &((__IEC_##TYPENAME##_t *)varp)->value;  \
+		    if(size) *size = sizeof(TYPENAME);                              \
             break;
 
-#define __Unpack_case_p(TYPENAME)\
-        case TYPENAME##_O_ENUM :\
-            *flags = __IEC_OUTPUT_FLAG;\
-        case TYPENAME##_P_ENUM :\
-            *flags |= ((__IEC_##TYPENAME##_p *)varp)->flags;\
-            *real_value_p = ((__IEC_##TYPENAME##_p *)varp)->value;\
-            forced_value_p = &((__IEC_##TYPENAME##_p *)varp)->fvalue;\
+#define __Unpack_case_p(TYPENAME)                                           \
+        case TYPENAME##_O_ENUM :                                            \
+        case TYPENAME##_P_ENUM :                                            \
+            if(flags) *flags = ((__IEC_##TYPENAME##_p *)varp)->flags;       \
+            if(value_p) *value_p = ((__IEC_##TYPENAME##_p *)varp)->value;   \
+		    if(size) *size = sizeof(TYPENAME);                              \
             break;
 
 #define __Is_a_string(dsc) (dsc->type == STRING_ENUM)   ||\
                            (dsc->type == STRING_P_ENUM) ||\
                            (dsc->type == STRING_O_ENUM)
 
-static void* UnpackVar(__Unpack_desc_type *dsc, void **real_value_p, char *flags)
+static int UnpackVar(__Unpack_desc_type *dsc, void **value_p, char *flags, size_t *size)
 {
     void *varp = dsc->ptr;
-    void *forced_value_p = NULL;
-    *flags = 0;
     /* find data to copy*/
     switch(dsc->type){
         __ANY(__Unpack_case_t)
         __ANY(__Unpack_case_p)
     default:
-        break;
+        return 0; /* should never happen */
     }
-    if (*flags & __IEC_FORCE_FLAG)
-        return forced_value_p;
-    return *real_value_p;
+    return 1;
 }