From 48d508aeba681f57796acea2ad631261686e0c43 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 17 Aug 2017 10:19:19 +0200
Subject: [PATCH] Added srs_lte_cpy for aligned copy which improves a bit
 performance for aligned data

---
 lib/include/srslte/phy/utils/vector.h |  3 +++
 lib/src/phy/mimo/layermap.c           |  8 +++++++-
 lib/src/phy/mimo/test/layermap_test.c |  6 +++---
 lib/src/phy/utils/vector.c            | 21 +++++++++++++++++++++
 4 files changed, 34 insertions(+), 4 deletions(-)

diff --git a/lib/include/srslte/phy/utils/vector.h b/lib/include/srslte/phy/utils/vector.h
index 74cd60172..c8d8d1942 100644
--- a/lib/include/srslte/phy/utils/vector.h
+++ b/lib/include/srslte/phy/utils/vector.h
@@ -172,6 +172,9 @@ SRSLTE_API void srslte_vec_abs_square_cf(cf_t *x, float *abs_square, uint32_t le
 /* argument of each vector element */
 SRSLTE_API void srslte_vec_arg_cf(cf_t *x, float *arg, uint32_t len);
 
+/* Copy 256 bit aligned vector */
+SRSLTE_API void srs_vec_cf_cpy(cf_t *src, cf_t *dst, int len);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/lib/src/phy/mimo/layermap.c b/lib/src/phy/mimo/layermap.c
index 868bf2f6a..a5bfc8b92 100644
--- a/lib/src/phy/mimo/layermap.c
+++ b/lib/src/phy/mimo/layermap.c
@@ -28,6 +28,7 @@
 #include <stdio.h>
 #include <assert.h>
 #include <string.h>
+#include <srslte/phy/utils/vector.h>
 
 #include "srslte/phy/common/phy_common.h"
 #include "srslte/phy/mimo/layermap.h"
@@ -51,7 +52,12 @@ int srslte_layermap_diversity(cf_t *d, cf_t *x[SRSLTE_MAX_LAYERS], int nof_layer
 
 int srslte_layermap_multiplex(cf_t *d[SRSLTE_MAX_CODEWORDS], cf_t *x[SRSLTE_MAX_LAYERS], int nof_cw, int nof_layers,
     int nof_symbols[SRSLTE_MAX_CODEWORDS]) {
-  if (nof_cw == 1) {
+  if (nof_cw == nof_layers) {
+    for (int i = 0; i < nof_cw; i++) {
+      srs_vec_cf_cpy(x[i], d[i], (uint32_t) nof_symbols[0]);
+    }
+    return nof_symbols[0];
+  } else if (nof_cw == 1) {
     return srslte_layermap_diversity(d[0], x, nof_layers, nof_symbols[0]);
   } else {
     int n[2];
diff --git a/lib/src/phy/mimo/test/layermap_test.c b/lib/src/phy/mimo/test/layermap_test.c
index 6fcc72682..6f026f9d0 100644
--- a/lib/src/phy/mimo/test/layermap_test.c
+++ b/lib/src/phy/mimo/test/layermap_test.c
@@ -96,19 +96,19 @@ int main(int argc, char **argv) {
   }
 
   for (i=0;i<nof_cw;i++) {
-    d[i] = malloc(sizeof(cf_t) * nof_symb_cw[i]);
+    d[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symb_cw[i]);
     if (!d[i]) {
       perror("malloc");
       exit(-1);
     }
-    dp[i] = malloc(sizeof(cf_t) * nof_symb_cw[i]);
+    dp[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symb_cw[i]);
     if (!dp[i]) {
       perror("malloc");
       exit(-1);
     }
   }
   for (i=0;i<nof_layers;i++) {
-    x[i] = malloc(sizeof(cf_t) * nof_symbols);
+    x[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols);
     if (!x[i]) {
       perror("malloc");
       exit(-1);
diff --git a/lib/src/phy/utils/vector.c b/lib/src/phy/utils/vector.c
index 30527510d..ff676d6f4 100644
--- a/lib/src/phy/utils/vector.c
+++ b/lib/src/phy/utils/vector.c
@@ -843,3 +843,24 @@ void srslte_vec_quant_suc(int16_t *in, uint8_t *out, float gain, int16_t offset,
   }
 }
 
+void srs_vec_cf_cpy(cf_t *dst, cf_t *src, int len) {
+  int i = 0;
+
+#ifdef LV_HAVE_AVX
+    for (; i < len - 3; i += 4) {
+      _mm256_store_ps((float *) &dst[i], _mm256_load_ps((float *) &src[i]));
+    }
+#endif /* LV_HAVE_AVX */
+#ifdef LV_HAVE_SSE
+    for (; i < len - 1; i += 2) {
+      _mm_store_ps((float *) &dst[i], _mm_load_ps((float *) &src[i]));
+    }
+  for (; i < len; i++) {
+    ((__m64*) dst)[i] = ((__m64*) src)[i];
+  }
+#else
+  for (; i < len; i++) {
+    dst[i] = src[i];
+  }
+#endif /* LV_HAVE_SSE */
+}