--- a/C_runtime/PLCObject.cpp	Wed May 15 18:50:26 2024 +0200
+++ b/C_runtime/PLCObject.cpp	Sat May 18 23:59:32 2024 +0200
@@ -23,6 +23,13 @@
 
 PLCObject::PLCObject(void)
 {
+    m_status.PLCstatus = Empty;
+    m_handle = NULL;
+    m_debugToken = 0;
+    m_plcID.ID = "N/A"; // TODO
+    m_plcID.PSK = "N/A"; // TODO
+    m_argc = 0;
+    m_argv = NULL;
 }
 
 PLCObject::~PLCObject(void)
@@ -114,7 +121,37 @@
 uint32_t PLCObject::GetTraceVariables(
     uint32_t debugToken, TraceVariables *traces)
 {
-    // XXX TODO
+    if(debugToken != m_debugToken)
+    {
+        return EINVAL;
+    }
+
+    // Check if there are any traces
+    m_tracesMutex.lock();
+    size_t sz = m_traces.size();
+    if(sz == 0)
+    {
+        m_tracesMutex.unlock();
+        return ENOENT;
+    }
+
+    // Allocate memory for traces
+    traces->traces.elements = (trace_sample *)malloc(sz * sizeof(trace_sample));
+    if(traces->traces.elements == NULL)
+    {
+        m_tracesMutex.unlock();
+        return ENOMEM;
+    }
+    traces->traces.elementsCount = sz;
+
+    // Copy traces from vector
+    memcpy(traces->traces.elements, m_traces.data(), sz * sizeof(trace_sample));
+
+    // Clear the vector
+    // note that the data is not freed here, it is meant to be freed by eRPC server code
+    m_traces.clear();
+    m_tracesMutex.unlock();
+
     return 0;
 }
 
@@ -169,6 +206,22 @@
     const char *md5sum, const binary_t *plcObjectBlobID,
     const list_extra_file_1_t *extrafiles, bool *success)
 {
+    if(m_status.PLCstatus == Started)
+    {
+        return EBUSY;
+    }
+
+    if(m_status.PLCstatus == Broken)
+    {
+        return EINVAL;
+    }
+
+    // Unload the PLC object
+    UnLoadPLC();
+
+    // Purge the PLC object
+    PurgePLC();
+
     // Concatenate md5sum and shared object extension to obtain filename
     std::filesystem::path filename =
         std::filesystem::path(md5sum) += SHARED_OBJECT_EXT;
@@ -238,8 +291,11 @@
 {
     // Unload the shared object file
     FOR_EACH_PLC_SYMBOLS_DO(ULSYM);
-    dlclose(m_handle);
-    
+    if(m_handle != NULL)
+    {
+        dlclose(m_handle);
+        m_handle = NULL;
+    }
     return 0;
 }
 
@@ -256,9 +312,54 @@
     return 0;
 }
 
+uint32_t PLCObject::PurgePLC(void){
+    // Purge all blobs
+    PurgeBlobs();
+
+    // Open the extra files list
+    std::ifstream extra_files_log(std::string(ExtraFilesList), std::ios::binary);
+
+    // Remove extra files
+    std::string extra_file;
+    while (std::getline(extra_files_log, extra_file))
+    {
+        std::filesystem::remove(extra_file);
+    }
+
+    // Load the last transferred PLC md5 hex digest
+    std::string md5sum;
+    std::ifstream(std::string(LastTransferredPLC), std::ios::binary) >> md5sum;
+
+    // Concatenate md5sum and shared object extension to obtain filename
+    std::filesystem::path filename =
+        std::filesystem::path(md5sum) += SHARED_OBJECT_EXT;
+
+    // Remove the PLC object shared object file
+    std::filesystem::remove(filename);
+
+    // Remove the last transferred PLC md5 hex digest
+    std::filesystem::remove(std::string(LastTransferredPLC));
+
+    // Remove the extra files list
+    std::filesystem::remove(std::string(ExtraFilesList));
+
+    return 0;
+}
+
 uint32_t PLCObject::RepairPLC(void)
 {
-    // XXX TODO
+    // Repair the PLC object
+
+    if(m_status.PLCstatus == Broken)
+    {
+        // Unload the PLC object
+        UnLoadPLC();
+
+        // Purge the PLC object
+        PurgePLC();
+    }
+
+
     LogMessage(LOG_WARNING, "RepairPLC not implemented");
     return 0;
 }
@@ -301,11 +402,53 @@
 }
 
 uint32_t PLCObject::SetTraceVariablesList(
-    const list_trace_order_1_t *orders, uint32_t *debugtoken)
-{
-    // XXX TODO
-    LogMessage(LOG_WARNING, "SetTraceVariablesList not implemented");
-    return 0;
+    const list_trace_order_1_t *orders, int32_t *debugtoken)
+{
+    // increment debug token
+    m_debugToken++;
+
+    if(orders->elementsCount == 0)
+    {
+        // actually disables debug
+        m_PLCSyms.suspendDebug(1);
+        *debugtoken = -5; // DEBUG_SUSPENDED
+        return 0;
+    }
+
+    // suspend debug before any operation
+    int res = m_PLCSyms.suspendDebug(0);
+    if(res == 0)
+    {
+        // forget about all previous debug variables
+        m_PLCSyms.ResetDebugVariables();
+
+        // call RegisterTraceVariables for each trace order
+        for (int i = 0; i < orders->elementsCount; i++)
+        {
+            trace_order *order = orders->elements + i;
+            res = m_PLCSyms.RegisterDebugVariable(order->idx, order->force.data, order->force.dataLength);
+            if(res != 0)
+            {
+                // if any error, disable debug
+                // since debug is already suspended, resume it first
+                m_PLCSyms.ResumeDebug();
+                m_PLCSyms.suspendDebug(1);
+                *debugtoken = -res;
+                return EINVAL;
+            }
+        }
+
+        // Start debug thread if not already started
+        if(!m_traceThread.joinable())
+        {
+            m_traceThread = std::thread(&PLCObject::TraceThreadProc, this);
+        }
+
+        m_PLCSyms.ResumeDebug();
+        *debugtoken = m_debugToken;
+        return 0;
+    }
+    return res;
 }
 
 uint32_t PLCObject::StartPLC(void)
@@ -318,6 +461,7 @@
         return res;
     }
     m_status.PLCstatus = Started;
+
     return 0;
 }
 
@@ -325,13 +469,20 @@
 {
     LogMessage(LOG_INFO, "Stopping PLC");
     uint32_t res = m_PLCSyms.stopPLC();
-    if(res != 0)
-    {
+    if(res == 0)
+    {
+        m_status.PLCstatus = Stopped;
+    } else {
         m_status.PLCstatus = Broken;
-        return res;
-    }
-    m_status.PLCstatus = Stopped;
-    return 0;
+    }
+
+    // Stop debug thread
+    if(m_traceThread.joinable())
+    {
+        m_traceThread.join();
+    }
+
+    return res;
 }
 
 uint32_t PLCObject::LogMessage(uint8_t level, std::string message)
@@ -339,3 +490,64 @@
     // Log std::string message with given level
     return m_PLCSyms.LogMessage(level, (char *)message.c_str(), message.size());
 }
+
+void PLCObject::TraceThreadProc(void)
+{
+    uint32_t err = 0;
+
+    m_PLCSyms.ResumeDebug();
+
+    while(m_status.PLCstatus == Started)
+    {
+        unsigned int tick;
+        unsigned int size;
+        void * buff;
+
+        // Data allocated here is meant to be freed by eRPC server code
+        uint8_t* ourData = NULL;
+
+        m_PLClibMutex.lock();
+
+        int res = m_PLCSyms.GetDebugData(&tick, &size, &buff);
+
+        if(res == 0)
+        {   
+            ourData = (uint8_t *)malloc(size);
+            if(ourData != NULL)
+            {
+                memcpy(ourData, buff, size);
+            }
+            m_PLCSyms.FreeDebugData();
+        }
+
+        m_PLClibMutex.unlock();
+
+        if(ourData == NULL)
+        {
+            err = res == 0 ? ENOMEM : res;
+            break;
+
+        } else {   
+
+            m_tracesMutex.lock();
+
+            m_traces.push_back(trace_sample{tick, binary_t{ourData, size}});
+
+            m_tracesMutex.unlock();
+        }
+    }
+
+    // Free trace buffer
+    m_tracesMutex.lock();
+    for(trace_sample s : m_traces){
+        free(s.TraceBuffer.data);
+    }
+    m_traces.clear();
+    m_tracesMutex.unlock();
+
+    LogMessage(err ? LOG_CRITICAL : LOG_INFO,
+        err == ENOMEM ? "Out of memory in TraceThreadProc" : 
+        err ? "TraceThreadProc ended because of error" : 
+        "TraceThreadProc ended normally");
+}
+

--- a/C_runtime/PLCObject.hpp	Wed May 15 18:50:26 2024 +0200
+++ b/C_runtime/PLCObject.hpp	Sat May 18 23:59:32 2024 +0200
@@ -8,6 +8,9 @@
 
 #include <map>
 #include <vector>
+#include <mutex>
+#include <thread>
+ 
 #include "blob.hpp"
 
 #include "erpc_PLCObject_interface.hpp"
@@ -36,7 +39,7 @@
     void (*ResetDebugVariables)(void);
     int (*RegisterDebugVariable)(unsigned int idx, void* force, size_t force_size);
     void (*FreeDebugData)(void);
-    int (*GetDebugData)(unsigned long *tick, unsigned long *size, void **buffer);
+    int (*GetDebugData)(unsigned int *tick, unsigned int *size, void **buffer);
     int (*suspendDebug)(int disable);
     void (*ResumeDebug)(void);
     void (*ResetLogCount)(void);
@@ -64,12 +67,10 @@
         uint32_t RepairPLC(void);
         uint32_t ResetLogCount(void);
         uint32_t SeedBlob(const binary_t * seed, binary_t * blobID);
-        uint32_t SetTraceVariablesList(const list_trace_order_1_t * orders, uint32_t * debugtoken);
+        uint32_t SetTraceVariablesList(const list_trace_order_1_t * orders, int32_t * debugtoken);
         uint32_t StartPLC(void);
         uint32_t StopPLC(bool * success);
 
-        // 
-
     private:
         // A map of all the blobs
         std::map<std::vector<uint8_t>, Blob*> m_mapBlobIDToBlob;
@@ -77,6 +78,9 @@
         // PLC object library handle
         void * m_handle;
 
+        // Shared object mutex
+        std::mutex m_PLClibMutex;
+
         // Symbols resolved from the PLC object
         PLCSyms m_PLCSyms;
 
@@ -90,16 +94,24 @@
         // PLC ID
         PSKID m_plcID;
 
+        // Debug token, used for consistency check of traces
+        uint32_t m_debugToken;
+
+        // Trace thread
+        std::thread m_traceThread;
+
+        // Trace thread mutex
+        std::mutex m_tracesMutex;
+
+        // Trace double buffer
+        std::vector<trace_sample> m_traces;
+
         uint32_t BlobAsFile(const binary_t * BlobID, std::filesystem::path filename);
         uint32_t LoadPLC(void);
         uint32_t UnLoadPLC(void);
         uint32_t LogMessage(uint8_t level, std::string message);
- 
-
-
-
-
-    
+        uint32_t PurgePLC(void);
+        void TraceThreadProc(void);
 };
 
 #endif
\ No newline at end of file

--- a/C_runtime/README.md	Wed May 15 18:50:26 2024 +0200
+++ b/C_runtime/README.md	Sat May 18 23:59:32 2024 +0200
@@ -8,4 +8,8 @@
 
 ## Description
 
-Beremiz C++ runtime. Loads shared object produced by Beremiz but doesn't use python.
\ No newline at end of file
+Beremiz C++ runtime. Loads shared object produced by Beremiz but doesn't use python.
+
+## ERPC interface
+
+erpcgen ../erpc_interface/erpc_PLCObject.erpc
\ No newline at end of file

--- a/targets/plc_debug.c	Wed May 15 18:50:26 2024 +0200
+++ b/targets/plc_debug.c	Sat May 18 23:59:32 2024 +0200
@@ -492,7 +492,7 @@
 }
 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 GetDebugData(unsigned int *tick, unsigned int *size, void **buffer){
     int wait_error = WaitDebugData(tick);
     if(!wait_error){
         *size = trace_buffer_cursor - trace_buffer;