--- a/targets/plc_debug.c	Wed May 18 11:22:48 2011 +0200
+++ b/targets/plc_debug.c	Wed May 18 14:46:27 2011 +0200
@@ -55,47 +55,51 @@
     }
 }
 
-#define __BufferDebugDataIterator_case_t(TYPENAME) \
+#define __Unpack_case_t(TYPENAME) \
         case TYPENAME##_ENUM :\
             *flags = ((__IEC_##TYPENAME##_t *)varp)->flags;\
-            *ptrvalue = &((__IEC_##TYPENAME##_t *)varp)->value;\
-            break;
-
-#define __BufferDebugDataIterator_case_p(TYPENAME)\
+            forced_value_p = *real_value_p = &((__IEC_##TYPENAME##_t *)varp)->value;\
+            break;
+
+#define __Unpack_case_p(TYPENAME)\
+        case TYPENAME##_O_ENUM :\
+            *flags = __IEC_OUTPUT_FLAG;\
         case TYPENAME##_P_ENUM :\
-        case TYPENAME##_O_ENUM :\
-            *flags = ((__IEC_##TYPENAME##_p *)varp)->flags;\
-            if (*flags & __IEC_FORCE_FLAG)\
-               *ptrvalue = &((__IEC_##TYPENAME##_p *)varp)->fvalue;\
-            else\
-               *ptrvalue = ((__IEC_##TYPENAME##_p *)varp)->value;\
-            break;
-
-void UnpackVar(void* varp, __IEC_types_enum vartype, void **ptrvalue, char *flags)
-{
+            *flags |= ((__IEC_##TYPENAME##_p *)varp)->flags;\
+            *real_value_p = ((__IEC_##TYPENAME##_p *)varp)->value;\
+            forced_value_p = &((__IEC_##TYPENAME##_p *)varp)->fvalue;\
+            break;
+
+void* UnpackVar(void* varp, __IEC_types_enum vartype, void **real_value_p, char *flags)
+{
+    void *forced_value_p = NULL;
+    *flags = 0;
     /* find data to copy*/
     switch(vartype){
-        ANY(__BufferDebugDataIterator_case_t)
-        ANY(__BufferDebugDataIterator_case_p)
+        ANY(__Unpack_case_t)
+        ANY(__Unpack_case_p)
     default:
         break;
     }
+    if (*flags & __IEC_FORCE_FLAG)
+        return forced_value_p;
+    return *real_value_p;
 }
 
 void Remind(unsigned int offset, unsigned int count, void * p);
 
 void RemindIterator(void* varp, __IEC_types_enum vartype)
 {
-    void *ptrvalue = NULL;
+    void *real_value_p = NULL;
     char flags = 0;
-    UnpackVar(varp, vartype, &ptrvalue, &flags);
+    UnpackVar(varp, vartype, &real_value_p, &flags);
 
     if(flags & __IEC_RETAIN_FLAG){
         USINT size = __get_type_enum_size(vartype);
         /* compute next cursor positon*/
         unsigned int next_retain_offset = retain_offset + size;
         /* if buffer not full */
-        Remind(retain_offset, size, ptrvalue);
+        Remind(retain_offset, size, real_value_p);
         /* increment cursor according size*/
         retain_offset = next_retain_offset;
     }
@@ -129,59 +133,53 @@
 {
 }
 
-void DoDebug(void *ptrvalue, char flags, USINT size)
-{
-    /* compute next cursor positon*/
-    char* next_cursor = buffer_cursor + size;
-    /* if buffer not full */
-    if(next_cursor <= debug_buffer + BUFFER_SIZE)
-    {
-        /* copy data to the buffer */
-        memcpy(buffer_cursor, ptrvalue, size);
-        /* increment cursor according size*/
-        buffer_cursor = next_cursor;
-    }else{
-        /*TODO : signal overflow*/
-    }
-}
 
 void Retain(unsigned int offset, unsigned int count, void * p);
-void DoRetain(void *ptrvalue, char flags, USINT size){
-    /* compute next cursor positon*/
-    unsigned int next_retain_offset = retain_offset + size;
-    /* if buffer not full */
-    Retain(retain_offset, size, ptrvalue);
-    /* increment cursor according size*/
-    retain_offset = next_retain_offset;
-}
-
-void BufferDebugDataIterator(void* varp, __IEC_types_enum vartype)
-{
-    void *ptrvalue = NULL;
+
+inline void BufferIterator(void* varp, __IEC_types_enum vartype, int do_debug)
+{
+    void *real_value_p = NULL;
+    void *visible_value_p = NULL;
     char flags = 0;
-    UnpackVar(varp, vartype, &ptrvalue, &flags);
-    /* For optimization purpose we do retain and debug in the same pass */
-    if(flags & (__IEC_DEBUG_FLAG | __IEC_RETAIN_FLAG)){
+
+    visible_value_p = UnpackVar(varp, vartype, &real_value_p, &flags);
+
+    if(flags & ( __IEC_DEBUG_FLAG | __IEC_RETAIN_FLAG)){
         USINT size = __get_type_enum_size(vartype);
         if(flags & __IEC_DEBUG_FLAG){
-            DoDebug(ptrvalue, flags, size);
+            /* copy visible variable to buffer */;
+            if(do_debug){
+                /* compute next cursor positon.
+                   No need to check overflow, as BUFFER_SIZE
+                   is computed large enough */
+                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 | __IEC_OUTPUT_FLAG)){
+                memcpy(real_value_p, visible_value_p, size);
+            }
         }
         if(flags & __IEC_RETAIN_FLAG){
-            DoRetain(ptrvalue, flags, size);
+            /* 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 RetainIterator(void* varp, __IEC_types_enum vartype)
-{
-    void *ptrvalue = NULL;
-    char flags = 0;
-    UnpackVar(varp, vartype, &ptrvalue, &flags);
-
-    if(flags & __IEC_RETAIN_FLAG){
-        USINT size = __get_type_enum_size(vartype);
-        DoRetain(ptrvalue, flags, size);
-    }
+void DebugIterator(void* varp, __IEC_types_enum vartype){
+    BufferIterator(varp, vartype, 1);
+}
+
+void RetainIterator(void* varp, __IEC_types_enum vartype){
+    BufferIterator(varp, vartype, 0);
 }
 
 extern int TryEnterDebugSection(void);
@@ -208,7 +206,7 @@
             /* Reset buffer cursor */
             buffer_cursor = debug_buffer;
             /* Iterate over all variables to fill debug buffer */
-            __for_each_variable_do(BufferDebugDataIterator);
+            __for_each_variable_do(DebugIterator);
             
             /* Leave debug section,
              * Trigger asynchronous transmission