uhd: convert types corrected for little endian, created SSE2 float/short conversion for no-swap case

1 files changed, 115 insertions, 26 deletions

diff --git a/host/lib/transport/convert_types_impl.hpp b/host/lib/transport/convert_types_impl.hpp
index 5958b08cb..641029795 100644
--- a/host/lib/transport/convert_types_impl.hpp
+++ b/host/lib/transport/convert_types_impl.hpp
@@ -28,6 +28,13 @@
     #define USE_EMMINTRIN_H //use sse2 intrinsics
 #endif
 
+#if defined(USE_EMMINTRIN_H)
+    #include <emmintrin.h>
+#endif
+
+//! shortcut for a byteswap16 with casting
+#define BSWAP16_C(num) uhd::byteswap(boost::uint16_t(num))
+
 /***********************************************************************
  * Typedefs
  **********************************************************************/
@@ -47,9 +54,10 @@ static UHD_INLINE void sc16_to_item32_nswap(
 static UHD_INLINE void sc16_to_item32_bswap(
     const sc16_t *input, item32_t *output, size_t nsamps
 ){
-    const item32_t *item32_input = (const item32_t *)input;
     for (size_t i = 0; i < nsamps; i++){
-        output[i] = uhd::byteswap(item32_input[i]);
+        boost::uint16_t real = BSWAP16_C(input[i].real());
+        boost::uint16_t imag = BSWAP16_C(input[i].imag());
+        output[i] = (item32_t(real) << 0) | (item32_t(imag) << 16);
     }
 }
 
@@ -65,34 +73,71 @@ static UHD_INLINE void item32_to_sc16_nswap(
 static UHD_INLINE void item32_to_sc16_bswap(
     const item32_t *input, sc16_t *output, size_t nsamps
 ){
-    item32_t *item32_output = (item32_t *)output;
     for (size_t i = 0; i < nsamps; i++){
-        item32_output[i] = uhd::byteswap(input[i]);
+        boost::int16_t real = BSWAP16_C(input[i] >> 0);
+        boost::int16_t imag = BSWAP16_C(input[i] >> 16);
+        output[i] = sc16_t(real, imag);
     }
 }
 
 /***********************************************************************
- * Convert complex float buffer to items32
+ * Convert complex float buffer to items32 (no swap)
  **********************************************************************/
 static const float shorts_per_float = float(32767);
 
-static UHD_INLINE item32_t fc32_to_item32(fc32_t num){
-    boost::uint16_t real = boost::int16_t(num.real()*shorts_per_float);
-    boost::uint16_t imag = boost::int16_t(num.imag()*shorts_per_float);
-    return (item32_t(real) << 16) | (item32_t(imag) << 0);
+#define FC32_TO_SC16_C(num) boost::int16_t(num*shorts_per_float)
+
+////////////////////////////////////
+// none-swap
+////////////////////////////////////
+#if defined(USE_EMMINTRIN_H)
+static UHD_INLINE void fc32_to_item32_nswap(
+    const fc32_t *input, item32_t *output, size_t nsamps
+){
+    __m128 scalar = _mm_set_ps1(shorts_per_float);
+
+    //convert blocks of samples with intrinsics
+    size_t i = 0; for (; i < (nsamps & ~0x3); i+=4){
+        //load from input
+        __m128 tmplo = _mm_loadu_ps(reinterpret_cast<const float *>(input+i+0));
+        __m128 tmphi = _mm_loadu_ps(reinterpret_cast<const float *>(input+i+2));
+
+        //convert and scale
+        __m128i tmpilo = _mm_cvtps_epi32(_mm_mul_ps(tmplo, scalar));
+        __m128i tmpihi = _mm_cvtps_epi32(_mm_mul_ps(tmphi, scalar));
+
+        //pack
+        __m128i tmpi = _mm_packs_epi32(tmpilo, tmpihi);
+
+        //store to output
+        _mm_storeu_si128(reinterpret_cast<__m128i *>(output+i), tmpi);
+    }
+
+    //convert remainder
+    for (; i < nsamps; i++){
+        boost::uint16_t real = FC32_TO_SC16_C(input[i].real());
+        boost::uint16_t imag = FC32_TO_SC16_C(input[i].imag());
+        output[i] = (item32_t(real) << 0) | (item32_t(imag) << 16);
+    }
 }
 
+#else
 static UHD_INLINE void fc32_to_item32_nswap(
     const fc32_t *input, item32_t *output, size_t nsamps
 ){
     for (size_t i = 0; i < nsamps; i++){
-        output[i] = fc32_to_item32(input[i]);
+        boost::uint16_t real = FC32_TO_SC16_C(input[i].real());
+        boost::uint16_t imag = FC32_TO_SC16_C(input[i].imag());
+        output[i] = (item32_t(real) << 0) | (item32_t(imag) << 16);
     }
 }
 
-#if defined(USE_EMMINTRIN_H)
-#include <emmintrin.h>
+#endif
 
+////////////////////////////////////
+// byte-swap
+////////////////////////////////////
+#if defined(USE_EMMINTRIN_H)
 static UHD_INLINE void fc32_to_item32_bswap(
     const fc32_t *input, item32_t *output, size_t nsamps
 ){
@@ -108,7 +153,7 @@ static UHD_INLINE void fc32_to_item32_bswap(
         __m128i tmpilo = _mm_cvtps_epi32(_mm_mul_ps(tmplo, scalar));
         __m128i tmpihi = _mm_cvtps_epi32(_mm_mul_ps(tmphi, scalar));
 
-        //pack + byteswap -> byteswap 32 bit words
+        //pack + byteswap -> byteswap 16 bit words
         __m128i tmpi = _mm_packs_epi32(tmpilo, tmpihi);
         tmpi = _mm_or_si128(_mm_srli_epi16(tmpi, 8), _mm_slli_epi16(tmpi, 8));
 
@@ -118,7 +163,9 @@ static UHD_INLINE void fc32_to_item32_bswap(
 
     //convert remainder
     for (; i < nsamps; i++){
-        output[i] = uhd::byteswap(fc32_to_item32(input[i]));
+        boost::uint16_t real = BSWAP16_C(FC32_TO_SC16_C(input[i].real()));
+        boost::uint16_t imag = BSWAP16_C(FC32_TO_SC16_C(input[i].imag()));
+        output[i] = (item32_t(real) << 0) | (item32_t(imag) << 16);
     }
 }
 
@@ -127,7 +174,9 @@ static UHD_INLINE void fc32_to_item32_bswap(
     const fc32_t *input, item32_t *output, size_t nsamps
 ){
     for (size_t i = 0; i < nsamps; i++){
-        output[i] = uhd::byteswap(fc32_to_item32(input[i]));
+        boost::uint16_t real = BSWAP16_C(FC32_TO_SC16_C(input[i].real()));
+        boost::uint16_t imag = BSWAP16_C(FC32_TO_SC16_C(input[i].imag()));
+        output[i] = (item32_t(real) << 0) | (item32_t(imag) << 16);
     }
 }
 
@@ -138,24 +187,60 @@ static UHD_INLINE void fc32_to_item32_bswap(
  **********************************************************************/
 static const float floats_per_short = float(1.0/shorts_per_float);
 
-static UHD_INLINE fc32_t item32_to_fc32(item32_t item){
-    return fc32_t(
-        float(boost::int16_t(item >> 16)*floats_per_short),
-        float(boost::int16_t(item >> 0)*floats_per_short)
-    );
+#define I16_TO_FC32_C(num) (boost::int16_t(num)*floats_per_short)
+
+////////////////////////////////////
+// none-swap
+////////////////////////////////////
+#if defined(USE_EMMINTRIN_H)
+static UHD_INLINE void item32_to_fc32_nswap(
+    const item32_t *input, fc32_t *output, size_t nsamps
+){
+    __m128 scalar = _mm_set_ps1(floats_per_short/(1 << 16));
+    __m128i zeroi = _mm_setzero_si128();
+
+    //convert blocks of samples with intrinsics
+    size_t i = 0; for (; i < (nsamps & ~0x3); i+=4){
+        //load from input
+        __m128i tmpi = _mm_loadu_si128(reinterpret_cast<const __m128i *>(input+i));
+
+        //unpack
+        __m128i tmpilo = _mm_unpacklo_epi16(zeroi, tmpi); //value in upper 16 bits
+        __m128i tmpihi = _mm_unpackhi_epi16(zeroi, tmpi);
+
+        //convert and scale
+        __m128 tmplo = _mm_mul_ps(_mm_cvtepi32_ps(tmpilo), scalar);
+        __m128 tmphi = _mm_mul_ps(_mm_cvtepi32_ps(tmpihi), scalar);
+
+        //store to output
+        _mm_storeu_ps(reinterpret_cast<float *>(output+i+0), tmplo);
+        _mm_storeu_ps(reinterpret_cast<float *>(output+i+2), tmphi);
+    }
+
+    //convert remainder
+    for (; i < nsamps; i++){
+        float real = I16_TO_FC32_C(input[i] >> 0);
+        float imag = I16_TO_FC32_C(input[i] >> 16);
+        output[i] = fc32_t(real, imag);
+    }
 }
 
+#else
 static UHD_INLINE void item32_to_fc32_nswap(
     const item32_t *input, fc32_t *output, size_t nsamps
 ){
     for (size_t i = 0; i < nsamps; i++){
-        output[i] = item32_to_fc32(input[i]);
+        float real = I16_TO_FC32_C(input[i] >> 0);
+        float imag = I16_TO_FC32_C(input[i] >> 16);
+        output[i] = fc32_t(real, imag);
     }
 }
+#endif
 
+////////////////////////////////////
+// byte-swap
+////////////////////////////////////
 #if defined(USE_EMMINTRIN_H)
-#include <emmintrin.h>
-
 static UHD_INLINE void item32_to_fc32_bswap(
     const item32_t *input, fc32_t *output, size_t nsamps
 ){
@@ -167,7 +252,7 @@ static UHD_INLINE void item32_to_fc32_bswap(
         //load from input
         __m128i tmpi = _mm_loadu_si128(reinterpret_cast<const __m128i *>(input+i));
 
-        //byteswap + unpack -> byteswap 32 bit words
+        //byteswap + unpack -> byteswap 16 bit words
         tmpi = _mm_or_si128(_mm_srli_epi16(tmpi, 8), _mm_slli_epi16(tmpi, 8));
         __m128i tmpilo = _mm_unpacklo_epi16(zeroi, tmpi); //value in upper 16 bits
         __m128i tmpihi = _mm_unpackhi_epi16(zeroi, tmpi);
@@ -183,7 +268,9 @@ static UHD_INLINE void item32_to_fc32_bswap(
 
     //convert remainder
     for (; i < nsamps; i++){
-        output[i] = item32_to_fc32(uhd::byteswap(input[i]));
+        float real = I16_TO_FC32_C(BSWAP16_C(input[i] >> 0));
+        float imag = I16_TO_FC32_C(BSWAP16_C(input[i] >> 16));
+        output[i] = fc32_t(real, imag);
     }
 }
 
@@ -192,7 +279,9 @@ static UHD_INLINE void item32_to_fc32_bswap(
     const item32_t *input, fc32_t *output, size_t nsamps
 ){
     for (size_t i = 0; i < nsamps; i++){
-        output[i] = item32_to_fc32(uhd::byteswap(input[i]));
+        float real = I16_TO_FC32_C(BSWAP16_C(input[i] >> 0));
+        float imag = I16_TO_FC32_C(BSWAP16_C(input[i] >> 16));
+        output[i] = fc32_t(real, imag);
     }
 }