--- a/targets/plc_debug.c	Wed Aug 29 21:14:23 2012 +0200
+++ b/targets/plc_debug.c	Thu Mar 07 11:47:43 2013 +0900
@@ -184,6 +184,7 @@
 
 extern int TryEnterDebugSection(void);
 extern long AtomicCompareExchange(long*, long, long);
+extern long long AtomicCompareExchange64(long long* , long long , long long);
 extern void LeaveDebugSection(void);
 extern void ValidateRetainBuffer(void);
 extern void InValidateRetainBuffer(void);
@@ -302,3 +303,128 @@
     return wait_error;
 }
 
+
+/* LOGGING
+*/
+
+#define LOG_LEVELS 4
+#define LOG_CRITICAL 0
+#define LOG_WARNING 1
+#define LOG_INFO 2
+#define LOG_DEBUG 3
+
+#ifndef LOG_BUFFER_SIZE
+#define LOG_BUFFER_SIZE (1<<14) /*16Ko*/
+#endif
+#define LOG_BUFFER_MASK (LOG_BUFFER_SIZE-1)
+static char LogBuff[LOG_LEVELS][LOG_BUFFER_SIZE];
+void inline copy_to_log(uint8_t level, uint32_t buffpos, void* buf, uint32_t size){
+    if(buffpos + size < LOG_BUFFER_SIZE){
+        memcpy(&LogBuff[level][buffpos], buf, size);
+    }else{
+        uint32_t remaining = LOG_BUFFER_SIZE - buffpos - 1; 
+        memcpy(&LogBuff[level][buffpos], buf, remaining);
+        memcpy(LogBuff[level], buf + remaining, size - remaining);
+    }
+}
+void inline copy_from_log(uint8_t level, uint32_t buffpos, void* buf, uint32_t size){
+    if(buffpos + size < LOG_BUFFER_SIZE){
+        memcpy(buf, &LogBuff[level][buffpos], size);
+    }else{
+        uint32_t remaining = LOG_BUFFER_SIZE - buffpos; 
+        memcpy(buf, &LogBuff[level][buffpos], remaining);
+        memcpy(buf + remaining, LogBuff[level], size - remaining);
+    }
+}
+
+/* Log buffer structure
+
+ |<-Tail1.msgsize->|<-sizeof(mTail)->|<--Tail2.msgsize-->|<-sizeof(mTail)->|...
+ |  Message1 Body  |      Tail1      |   Message2 Body   |      Tail2      |
+
+*/
+typedef struct {
+    uint32_t msgidx;
+    uint32_t msgsize;
+    unsigned long tick;
+    IEC_TIME time;
+} mTail;
+
+/* Log cursor : 64b
+   |63 ... 32|31 ... 0|
+   | Message | Buffer |
+   | counter | Index  | */
+static uint64_t LogCursor[LOG_LEVELS] = {0x0,0x0,0x0,0x0};
+
+/* Store one log message of give size */
+int LogMessage(uint8_t level, char* buf, uint32_t size){
+    if(size < LOG_BUFFER_SIZE - sizeof(mTail)){
+        uint32_t buffpos;
+        uint64_t new_cursor, old_cursor;
+
+        mTail tail;
+        tail.msgsize = size;
+        tail.tick = __tick;
+        PLC_GetTime(&tail.time);
+
+        /* We cannot increment both msg index and string pointer 
+           in a single atomic operation but we can detect having been interrupted.
+           So we can try with atomic compare and swap in a loop until operation
+           succeeds non interrupted */
+        do{
+            old_cursor = LogCursor[level];
+            buffpos = (uint32_t)old_cursor;
+            tail.msgidx = (old_cursor >> 32); 
+            new_cursor = ((uint64_t)(tail.msgidx + 1)<<32) 
+                         | (uint64_t)((buffpos + size + sizeof(mTail)) & LOG_BUFFER_MASK);
+        }while(AtomicCompareExchange64(
+            (long long*)&LogCursor[level],
+            (long long)old_cursor,
+            (long long)new_cursor)!=old_cursor);
+
+        copy_to_log(level, buffpos, buf, size);
+        copy_to_log(level, (buffpos + size) & LOG_BUFFER_MASK, &tail, sizeof(mTail));
+
+        return 1; /* Success */
+    }else{
+        char mstr[] = "Logging error : message too big";
+        LogMessage(LOG_CRITICAL, mstr, sizeof(mstr));
+    }
+    return 0;
+}
+
+uint32_t GetLogCount(uint8_t level){
+    return (uint64_t)LogCursor[level] >> 32;
+}
+
+/* Return message size and content */
+uint32_t GetLogMessage(uint8_t level, uint32_t msgidx, char* buf, uint32_t max_size, uint32_t* tick, uint32_t* tv_sec, uint32_t* tv_nsec){
+    uint64_t cursor = LogCursor[level];
+    if(cursor){
+        /* seach cursor */
+        uint32_t stailpos = (uint32_t)cursor; 
+        uint32_t smsgidx;
+        mTail tail;
+        tail.msgidx = cursor >> 32;
+        tail.msgsize = 0;
+
+        /* Message search loop */
+        do {
+            smsgidx = tail.msgidx;
+            stailpos = (stailpos - sizeof(mTail) - tail.msgsize ) & LOG_BUFFER_MASK;
+            copy_from_log(level, stailpos, &tail, sizeof(mTail));
+        }while((tail.msgidx == smsgidx - 1) && (tail.msgidx > msgidx));
+
+        if(tail.msgidx == msgidx){
+            uint32_t sbuffpos = (stailpos - tail.msgsize ) & LOG_BUFFER_MASK; 
+            uint32_t totalsize = tail.msgsize;
+            *tick = tail.tick; 
+            *tv_sec = tail.time.tv_sec; 
+            *tv_nsec = tail.time.tv_nsec; 
+            copy_from_log(level, sbuffpos, buf, 
+                          totalsize > max_size ? max_size : totalsize);
+            return totalsize;
+        }
+    }
+    return 0;
+}