From 77ca59a2ad5881580cb04f75a1173cc05eaeca4c Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Wed, 16 Apr 2014 22:35:03 +0400
Subject: [PATCH] LPC11C24 test: Added time sync slave and logging

---
 .../test_olimex_lpc_p11c24/src/main.cpp       | 68 +++++++++++++++++++
 .../src/sys/libstubs.cpp                      |  1 +
 2 files changed, 69 insertions(+)

diff --git a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp
index cbe8b020f6..6066b3267a 100644
--- a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp
@@ -6,6 +6,8 @@
 #include <board.hpp>
 #include <chip.h>
 #include <uavcan_lpc11c24/uavcan_lpc11c24.hpp>
+#include <uavcan/protocol/global_time_sync_slave.hpp>
+#include <uavcan/protocol/logger.hpp>
 
 namespace
 {
@@ -18,6 +20,18 @@ Node& getNode()
     return node;
 }
 
+uavcan::GlobalTimeSyncSlave& getTimeSyncSlave()
+{
+    static uavcan::GlobalTimeSyncSlave tss(getNode());
+    return tss;
+}
+
+uavcan::Logger& getLogger()
+{
+    static uavcan::Logger logger(getNode());
+    return logger;
+}
+
 __attribute__((noreturn))
 void die()
 {
@@ -37,6 +51,46 @@ void init()
     while (getNode().start() < 0)
     {
     }
+
+    while (getTimeSyncSlave().start() < 0)
+    {
+    }
+
+    while (getLogger().init() < 0)
+    {
+    }
+    getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
+}
+
+void reverse(char* s)
+{
+    for (int i = 0, j = std::strlen(s) - 1; i < j; i++, j--)
+    {
+        const char c = s[i];
+        s[i] = s[j];
+        s[j] = c;
+    }
+}
+
+void lltoa(long long n, char buf[24])
+{
+    const short sign = (n < 0) ? -1 : 1;
+    if (sign < 0)
+    {
+        n = -n;
+    }
+    unsigned i = 0;
+    do
+    {
+        buf[i++] = n % 10 + '0';
+    }
+    while ((n /= 10) > 0);
+    if (sign < 0)
+    {
+        buf[i++] = '-';
+    }
+    buf[i] = '\0';
+    reverse(buf);
 }
 
 }
@@ -47,10 +101,24 @@ int main()
 
     getNode().setStatusOk();
 
+    uavcan::MonotonicTime prev_log_at;
+
     while (true)
     {
         const int res = getNode().spin(uavcan::MonotonicDuration::fromMSec(25));
         board::setErrorLed(res < 0);
         board::setStatusLed(uavcan_lpc11c24::CanDriver::instance().hadActivity());
+
+        const auto ts = uavcan_lpc11c24::clock::getMonotonic();
+        if ((ts - prev_log_at).toMSec() >= 1000)
+        {
+            prev_log_at = ts;
+
+            // We don't want to use formatting functions provided by libuavcan because they rely on std::snprintf()
+            char buf[24];
+            lltoa(uavcan_lpc11c24::clock::getPrevUtcAdjustment().toUSec(), buf);
+            buf[sizeof(buf) - 1] = '\0';
+            (void)getLogger().logInfo("app", buf);
+        }
     }
 }
diff --git a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/libstubs.cpp b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/libstubs.cpp
index 34dd31e92b..ec0744cb11 100644
--- a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/libstubs.cpp
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/libstubs.cpp
@@ -155,6 +155,7 @@ int _close_r(struct _reent*, int)
     return -1;
 }
 
+__attribute__((used))
 caddr_t _sbrk_r(struct _reent*, int)
 {
     return 0;