From 75ad0c55164e8e2f08b4bfc9d5361f2445a80f52 Mon Sep 17 00:00:00 2001
From: Martin Braun <martin.braun@ettus.com>
Date: Thu, 18 Jul 2019 16:23:12 -0700
Subject: convert: Add chdr converters for sc16 -> fc32 and vice versa

These differ from the item32 converters in that they don't IQ swap, and
also don't have a BE/LE version.
---
 host/lib/convert/sse2_sc16_to_fc64.cpp | 45 ++++++++++++++++++++++++++++++++++
 1 file changed, 45 insertions(+)

(limited to 'host/lib/convert/sse2_sc16_to_fc64.cpp')

diff --git a/host/lib/convert/sse2_sc16_to_fc64.cpp b/host/lib/convert/sse2_sc16_to_fc64.cpp
index 45821ac9f..4be6bd084 100644
--- a/host/lib/convert/sse2_sc16_to_fc64.cpp
+++ b/host/lib/convert/sse2_sc16_to_fc64.cpp
@@ -103,3 +103,48 @@ DECLARE_CONVERTER(sc16_item32_be, 1, fc64, 1, PRIORITY_SIMD)
     // convert remainder
     item32_sc16_to_xx<uhd::htonx>(input + i, output + i, nsamps - i, scale_factor);
 }
+
+DECLARE_CONVERTER(sc16_chdr, 1, fc64, 1, PRIORITY_SIMD)
+{
+    const sc16_t* input = reinterpret_cast<const sc16_t*>(inputs[0]);
+    fc64_t* output      = reinterpret_cast<fc64_t*>(outputs[0]);
+
+    const __m128d scalar = _mm_set1_pd(scale_factor / (1 << 16));
+    const __m128i zeroi  = _mm_setzero_si128();
+
+#define convert_chdr_1_to_fc64_1_guts(_al_)                                            \
+    for (; i + 3 < nsamps; i += 4) {                                                   \
+        /* load from input */                                                          \
+        __m128i tmpi = _mm_loadu_si128(reinterpret_cast<const __m128i*>(input + i));   \
+                                                                                       \
+        /* unpack 16-bit pairs */                                                      \
+        __m128i tmpilo = _mm_unpacklo_epi16(zeroi, tmpi); /* value in upper 16 bits */ \
+        __m128i tmpihi = _mm_unpackhi_epi16(zeroi, tmpi);                              \
+                                                                                       \
+        /* convert and scale */                                                        \
+        __m128d tmp0 = _mm_mul_pd(_mm_cvtepi32_pd(tmpilo), scalar);                    \
+        tmpilo       = _mm_unpackhi_epi64(tmpilo, zeroi);                              \
+        __m128d tmp1 = _mm_mul_pd(_mm_cvtepi32_pd(tmpilo), scalar);                    \
+        __m128d tmp2 = _mm_mul_pd(_mm_cvtepi32_pd(tmpihi), scalar);                    \
+        tmpihi       = _mm_unpackhi_epi64(tmpihi, zeroi);                              \
+        __m128d tmp3 = _mm_mul_pd(_mm_cvtepi32_pd(tmpihi), scalar);                    \
+                                                                                       \
+        /* store to output */                                                          \
+        _mm_store##_al_##pd(reinterpret_cast<double*>(output + i + 0), tmp0);          \
+        _mm_store##_al_##pd(reinterpret_cast<double*>(output + i + 1), tmp1);          \
+        _mm_store##_al_##pd(reinterpret_cast<double*>(output + i + 2), tmp2);          \
+        _mm_store##_al_##pd(reinterpret_cast<double*>(output + i + 3), tmp3);          \
+    }
+
+    size_t i = 0;
+
+    // dispatch according to alignment
+    if ((size_t(output) & 0xf) == 0) {
+        convert_chdr_1_to_fc64_1_guts(_)
+    } else {
+        convert_chdr_1_to_fc64_1_guts(u_)
+    }
+
+    // convert remainder
+    chdr_sc16_to_xx(input + i, output + i, nsamps - i, scale_factor);
+}
-- 
cgit v1.2.3