From 1a0caeb723655d7b93b0faed4e6ddf57bf3ae740 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Sat, 2 Aug 2014 12:51:26 +0400
Subject: [PATCH] LPC11C24: extended hardware and software version info

---
 .../lpc11c24/test_olimex_lpc_p11c24/Makefile    | 11 ++++++++++-
 .../test_olimex_lpc_p11c24/src/main.cpp         | 17 +++++++++++++++++
 .../test_olimex_lpc_p11c24/src/sys/board.cpp    | 13 +++++++++++++
 .../test_olimex_lpc_p11c24/src/sys/board.hpp    |  6 ++++++
 4 files changed, 46 insertions(+), 1 deletion(-)

diff --git a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/Makefile b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/Makefile
index 5079d06f88..901572cf6a 100644
--- a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/Makefile
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/Makefile
@@ -8,7 +8,7 @@ CPPSRC := $(wildcard src/*.cpp)                  \
 CSRC   := $(wildcard lpc_chip_11cxx_lib/src/*.c) \
           $(wildcard src/sys/*.c)
 
-DEF =
+DEF = -DFW_VERSION_MAJOR=1 -DFW_VERSION_MINOR=0
 
 INC = -Isrc/sys                         \
       -Ilpc_chip_11cxx_lib/inc
@@ -30,6 +30,15 @@ INC += -I$(LIBUAVCAN_LPC11C24_INC)
 $(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR)))
 INC += -Idsdlc_generated
 
+#
+# Git commit hash
+#
+
+GIT_HASH := $(shell git rev-parse --short HEAD)
+ifeq ($(words $(GIT_HASH)),1)
+    DEF += -DGIT_HASH=0x$(GIT_HASH)
+endif
+
 #
 # Build configuration
 #
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 6cce7251ba..d379a5b816 100644
--- a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp
@@ -3,6 +3,7 @@
  */
 
 #include <cstdio>
+#include <algorithm>
 #include <board.hpp>
 #include <chip.h>
 #include <uavcan_lpc11c24/uavcan_lpc11c24.hpp>
@@ -40,6 +41,9 @@ void die()
     while (true) { }
 }
 
+#if __GNUC__
+__attribute__((noinline))
+#endif
 void init()
 {
     if (uavcan_lpc11c24::CanDriver::instance().init(1000000) < 0)
@@ -50,6 +54,19 @@ void init()
     getNode().setNodeID(72);
     getNode().setName("org.uavcan.lpc11c24_test");
 
+    uavcan::protocol::SoftwareVersion swver;
+    swver.major = FW_VERSION_MAJOR;
+    swver.minor = FW_VERSION_MINOR;
+    swver.vcs_commit = GIT_HASH;
+    swver.optional_field_mask = swver.OPTIONAL_FIELD_MASK_VCS_COMMIT;
+    getNode().setSoftwareVersion(swver);
+
+    uavcan::protocol::HardwareVersion hwver;
+    std::uint8_t uid[board::UniqueIDSize] = {};
+    board::readUniqueID(uid);
+    std::copy(std::begin(uid), std::end(uid), std::begin(hwver.unique_id));
+    getNode().setHardwareVersion(hwver);
+
     while (getNode().start() < 0)
     {
     }
diff --git a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.cpp b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.cpp
index 17bf099e53..223c593d7e 100644
--- a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.cpp
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.cpp
@@ -6,6 +6,8 @@
 #include "board.hpp"
 #include <chip.h>
 #include <cstdlib>
+#include <cstring>
+#include <numeric>
 
 #define PDRUNCFGUSEMASK 0x0000ED00
 #define PDRUNCFGMASKTMP 0x000000FF
@@ -114,6 +116,17 @@ void init()
 
 } // namespace
 
+#if __GNUC__
+__attribute__((optimize(0)))     // Optimization must be disabled lest it hardfaults in the IAP call
+#endif
+void readUniqueID(uint8_t out_uid[UniqueIDSize])
+{
+    unsigned aligned_array[4] = {};  // out_uid may be unaligned, so we need to use temp array
+    unsigned iap_command = 58;
+    reinterpret_cast<void(*)(void*, void*)>(0x1FFF1FF1)(&iap_command, aligned_array);
+    std::memcpy(out_uid, aligned_array, 16);
+}
+
 void setStatusLed(bool state)
 {
     LPC_GPIO[StatusLedPort].DATA[1 << StatusLedPin] = static_cast<unsigned long>(!state) << StatusLedPin;
diff --git a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.hpp b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.hpp
index 56e6cf6099..eb0f5ea04b 100644
--- a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.hpp
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.hpp
@@ -2,9 +2,15 @@
  * Pavel Kirienko, 2014 <pavel.kirienko@gmail.com>
  */
 
+#include <stdint.h>
+
 namespace board
 {
 
+static const unsigned UniqueIDSize = 16;
+
+void readUniqueID(uint8_t out_uid[UniqueIDSize]);
+
 void setStatusLed(bool state);
 void setErrorLed(bool state);