From da8a31c19e4c1d83d0348042903fca8535e07bfa Mon Sep 17 00:00:00 2001
From: John Wellbelove <smartgit@wellbelove.co.uk>
Date: Sat, 27 Feb 2016 16:54:51 +0000
Subject: [PATCH] Added sign_extend template

---
 binary.h             | 55 ++++++++++++++++++++++++++++++++++++++++++++
 test/test_binary.cpp | 50 ++++++++++++++++++++++++++++++++++++++++
 2 files changed, 105 insertions(+)

diff --git a/binary.h b/binary.h
index 682725cf..b609d5e2 100644
--- a/binary.h
+++ b/binary.h
@@ -35,6 +35,9 @@ SOFTWARE.
 /// Binary utilities
 ///\ingroup utilities
 
+#include <limits>
+#include <assert.h>
+
 #include "type_traits.h"
 #include "integral_limits.h"
 #include "static_assert.h"
@@ -496,6 +499,58 @@ namespace etl
     return folded_value;
   }
 
+  //***************************************************************************
+  /// Sign extend.
+  /// Converts an N bit binary number, where bit N-1 is the sign bit, to a signed integral type.
+  //***************************************************************************
+  template <typename TReturn, const size_t NBITS, typename TValue>
+  TReturn sign_extend(TValue value)
+  {
+    STATIC_ASSERT(etl::is_integral<TValue>::value,  "TValue not an integral type");
+    STATIC_ASSERT(etl::is_integral<TReturn>::value, "TReturn not an integral type");
+    STATIC_ASSERT(etl::is_signed<TReturn>::value,   "TReturn not a signed type");
+    STATIC_ASSERT(NBITS <= std::numeric_limits<TReturn>::digits, "NBITS too large for return type");
+
+    typedef etl::make_unsigned<TReturn>::type mask_t;
+
+    mask_t negative = (1 << (NBITS - 1));
+    TReturn signed_value = value;
+
+    if ((signed_value & negative) != 0)
+    {
+      mask_t sign_bits = ~((1 << NBITS) - 1);
+      signed_value = value | sign_bits;
+    }
+
+    return signed_value;
+  }
+
+  //***************************************************************************
+  /// Sign extend.
+  /// Converts an N bit binary number, where bit N-1 is the sign bit, to a signed integral type.
+  //***************************************************************************
+  template <typename TReturn, typename TValue>
+  TReturn sign_extend(TValue value, const size_t NBITS)
+  {
+    STATIC_ASSERT(etl::is_integral<TValue>::value,  "TValue not an integral type");
+    STATIC_ASSERT(etl::is_integral<TReturn>::value, "TReturn not an integral type");
+    STATIC_ASSERT(etl::is_signed<TReturn>::value,   "TReturn not a signed type");
+    assert(NBITS <= std::numeric_limits<TReturn>::digits);
+
+    typedef etl::make_unsigned<TReturn>::type mask_t;
+
+    mask_t negative = (1 << (NBITS - 1));
+    TReturn signed_value = value;
+
+    if ((signed_value & negative) != 0)
+    {
+      mask_t sign_bits = ~((1 << NBITS) - 1);
+      signed_value = value | sign_bits;
+    }
+
+    return signed_value;
+  }
+
   //***************************************************************************
   /// 8 bit binary constants.
   //***************************************************************************
diff --git a/test/test_binary.cpp b/test/test_binary.cpp
index 5eb500a1..237b307b 100644
--- a/test/test_binary.cpp
+++ b/test/test_binary.cpp
@@ -910,6 +910,56 @@ namespace
         CHECK((std::is_same<uint64_t, etl::max_value_for_nbits<63>::value_type>::value));
         CHECK((std::is_same<uint64_t, etl::max_value_for_nbits<64>::value_type>::value));
     }
+
+    //*************************************************************************
+    TEST(test_sign_extend_template1)
+    {
+      uint8_t value8 = 0x2A;
+      value8 &= 0x3F; // 6 bit number.
+
+      CHECK_EQUAL(-22, (etl::sign_extend<int8_t,  6>(value8)));
+      CHECK_EQUAL(-22, (etl::sign_extend<int16_t, 6>(value8)));
+      CHECK_EQUAL(-22, (etl::sign_extend<int32_t, 6>(value8)));
+      CHECK_EQUAL(-22, (etl::sign_extend<int64_t, 6>(value8)));
+
+      uint16_t value16 = 0x2AAA;
+      value16 &= 0x3FFF; // 14 bit number.
+
+      CHECK_EQUAL(-5462, (etl::sign_extend<int16_t, 14>(value16)));
+      CHECK_EQUAL(-5462, (etl::sign_extend<int32_t, 14>(value16)));
+      CHECK_EQUAL(-5462, (etl::sign_extend<int64_t, 14>(value16)));
+
+      uint32_t value32 = 0x2AAAAAAA;
+      value32 &= 0x3FFFFFFF; // 30 bit number.
+
+      CHECK_EQUAL(-357913942, (etl::sign_extend<int32_t, 30>(value32)));
+      CHECK_EQUAL(-357913942, (etl::sign_extend<int64_t, 30>(value32)));
+    }
+
+    //*************************************************************************
+    TEST(test_sign_extend_template2)
+    {
+      uint8_t value8 = 0x2A;
+      value8 &= 0x3F; // 6 bit number.
+
+      CHECK_EQUAL(-22, (etl::sign_extend<int8_t>(value8, 6)));
+      CHECK_EQUAL(-22, (etl::sign_extend<int16_t>(value8, 6)));
+      CHECK_EQUAL(-22, (etl::sign_extend<int32_t>(value8, 6)));
+      CHECK_EQUAL(-22, (etl::sign_extend<int64_t>(value8, 6)));
+
+      uint16_t value16 = 0x2AAA;
+      value16 &= 0x3FFF; // 14 bit number.
+
+      CHECK_EQUAL(-5462, (etl::sign_extend<int16_t>(value16, 14)));
+      CHECK_EQUAL(-5462, (etl::sign_extend<int32_t>(value16, 14)));
+      CHECK_EQUAL(-5462, (etl::sign_extend<int64_t>(value16, 14)));
+
+      uint32_t value32 = 0x2AAAAAAA;
+      value32 &= 0x3FFFFFFF; // 30 bit number.
+
+      CHECK_EQUAL(-357913942, (etl::sign_extend<int32_t>(value32, 30)));
+      CHECK_EQUAL(-357913942, (etl::sign_extend<int64_t>(value32, 30)));
+    }
   };
 }