From baac179d95182a323568953d8d424fb2f4a7da29 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Wed, 24 Apr 2019 20:04:47 +0200
Subject: [PATCH] Added vector complex sine generator

---
 lib/include/srslte/phy/utils/vector.h      |  2 +
 lib/include/srslte/phy/utils/vector_simd.h |  2 +
 lib/src/phy/utils/test/vector_test.c       | 20 ++++++++++
 lib/src/phy/utils/vector.c                 |  5 +++
 lib/src/phy/utils/vector_simd.c            | 46 ++++++++++++++++++++++
 5 files changed, 75 insertions(+)

diff --git a/lib/include/srslte/phy/utils/vector.h b/lib/include/srslte/phy/utils/vector.h
index ebd60e06a..5964b9246 100644
--- a/lib/include/srslte/phy/utils/vector.h
+++ b/lib/include/srslte/phy/utils/vector.h
@@ -160,6 +160,8 @@ SRSLTE_API void srslte_vec_interleave(const cf_t *x, const cf_t *y, cf_t *z, con
 
 SRSLTE_API void srslte_vec_interleave_add(const cf_t *x, const cf_t *y, cf_t *z, const int len);
 
+SRSLTE_API void srslte_vec_gen_sine(cf_t amplitude, float freq, cf_t* z, int len);
+
 SRSLTE_API void srslte_vec_apply_cfo(const cf_t *x, float cfo, cf_t *z, int len);
 
 SRSLTE_API float srslte_vec_estimate_frequency(const cf_t* x, int len);
diff --git a/lib/include/srslte/phy/utils/vector_simd.h b/lib/include/srslte/phy/utils/vector_simd.h
index 22646b729..44d276591 100644
--- a/lib/include/srslte/phy/utils/vector_simd.h
+++ b/lib/include/srslte/phy/utils/vector_simd.h
@@ -135,6 +135,8 @@ SRSLTE_API void srslte_vec_interleave_simd(const cf_t *x, const cf_t *y, cf_t *z
 
 SRSLTE_API void srslte_vec_interleave_add_simd(const cf_t *x, const cf_t *y, cf_t *z, const int len);
 
+SRSLTE_API void srslte_vec_gen_sine_simd(cf_t amplitude, float freq, cf_t* z, int len);
+
 SRSLTE_API void srslte_vec_apply_cfo_simd(const cf_t *x, float cfo, cf_t *z, int len);
 
 SRSLTE_API float srslte_vec_estimate_frequency_simd(const cf_t* x, int len);
diff --git a/lib/src/phy/utils/test/vector_test.c b/lib/src/phy/utils/test/vector_test.c
index 1bec5b503..4c5e56fc1 100644
--- a/lib/src/phy/utils/test/vector_test.c
+++ b/lib/src/phy/utils/test/vector_test.c
@@ -800,6 +800,22 @@ TEST(srslte_vec_apply_cfo,
          free(z);
 )
 
+TEST(
+    srslte_vec_gen_sine, MALLOC(cf_t, z);
+
+    const float freq = 0.1f;
+    cf_t        gold;
+    cf_t        x = RANDOM_CF();
+
+    TEST_CALL(srslte_vec_gen_sine(x, freq, z, block_size))
+
+        for (int i = 0; i < block_size; i++) {
+          gold = x * cexpf(_Complex_I * 2.0f * (float)M_PI * i * freq);
+          mse += cabsf(gold - z[i]) / cabsf(gold);
+        } mse /= block_size;
+
+    free(z);)
+
 TEST(srslte_vec_estimate_frequency, MALLOC(cf_t, x); float freq_gold = 0.1f; float freq = 0.1f;
 
      for (int i = 0; i < block_size; i++) { x[i] = cexpf(-I * 2.0f * M_PI * (float)i * freq_gold); }
@@ -971,6 +987,10 @@ int main(int argc, char **argv) {
     passed[func_count][size_count] = test_srslte_vec_apply_cfo(func_names[func_count], &timmings[func_count][size_count], block_size);
     func_count++;
 
+    passed[func_count][size_count] =
+        test_srslte_vec_gen_sine(func_names[func_count], &timmings[func_count][size_count], block_size);
+    func_count++;
+
     passed[func_count][size_count] =
         test_srslte_vec_estimate_frequency(func_names[func_count], &timmings[func_count][size_count], block_size);
     func_count++;
diff --git a/lib/src/phy/utils/vector.c b/lib/src/phy/utils/vector.c
index e75ee6e91..026bd2658 100644
--- a/lib/src/phy/utils/vector.c
+++ b/lib/src/phy/utils/vector.c
@@ -464,6 +464,11 @@ void srslte_vec_interleave_add(const cf_t *x, const cf_t *y, cf_t *z, const int
   srslte_vec_interleave_add_simd(x, y, z, len);
 }
 
+void srslte_vec_gen_sine(cf_t amplitude, float freq, cf_t* z, int len)
+{
+  srslte_vec_gen_sine_simd(amplitude, freq, z, len);
+}
+
 void srslte_vec_apply_cfo(const cf_t *x, float cfo, cf_t *z, int len) {
   srslte_vec_apply_cfo_simd(x, cfo, z, len);
 }
diff --git a/lib/src/phy/utils/vector_simd.c b/lib/src/phy/utils/vector_simd.c
index a80cf96cb..1cdb8785b 100644
--- a/lib/src/phy/utils/vector_simd.c
+++ b/lib/src/phy/utils/vector_simd.c
@@ -1507,6 +1507,52 @@ void srslte_vec_interleave_add_simd(const cf_t *x, const cf_t *y, cf_t *z, const
   }
 }
 
+void srslte_vec_gen_sine_simd(cf_t amplitude, float freq, cf_t* z, int len)
+{
+  const float TWOPI = 2.0f * (float)M_PI;
+  cf_t        osc   = cexpf(_Complex_I * TWOPI * freq);
+  cf_t        phase = 1.0f;
+  int         i     = 0;
+
+#if SRSLTE_SIMD_CF_SIZE
+  __attribute__((aligned(64))) cf_t _phase[SRSLTE_SIMD_CF_SIZE];
+  _phase[0] = phase;
+
+  if (i < len - SRSLTE_SIMD_CF_SIZE + 1) {
+    for (int k = 1; k < SRSLTE_SIMD_CF_SIZE; k++) {
+      _phase[k] = _phase[k - 1] * osc;
+    }
+  }
+  simd_cf_t _simd_osc   = srslte_simd_cf_set1(cexpf(_Complex_I * TWOPI * freq * SRSLTE_SIMD_CF_SIZE));
+  simd_cf_t _simd_phase = srslte_simd_cfi_load(_phase);
+  simd_cf_t a           = srslte_simd_cf_set1(amplitude);
+
+  if (SRSLTE_IS_ALIGNED(z)) {
+    for (; i < len - SRSLTE_SIMD_CF_SIZE + 1; i += SRSLTE_SIMD_CF_SIZE) {
+      simd_cf_t r = srslte_simd_cf_prod(a, _simd_phase);
+      srslte_simd_cfi_store(&z[i], r);
+      _simd_phase = srslte_simd_cf_prod(_simd_phase, _simd_osc);
+    }
+  } else {
+    for (; i < len - SRSLTE_SIMD_CF_SIZE + 1; i += SRSLTE_SIMD_CF_SIZE) {
+      simd_cf_t r = srslte_simd_cf_prod(a, _simd_phase);
+      srslte_simd_cfi_storeu(&z[i], r);
+      _simd_phase = srslte_simd_cf_prod(_simd_phase, _simd_osc);
+    }
+  }
+
+  // Store pahse and get last phase
+  srslte_simd_cfi_store(_phase, _simd_phase);
+  phase = _phase[0];
+#endif /* SRSLTE_SIMD_CF_SIZE */
+
+  for (; i < len; i++) {
+    z[i] = amplitude * phase;
+
+    phase *= osc;
+  }
+}
+
 void srslte_vec_apply_cfo_simd(const cf_t *x, float cfo, cf_t *z, int len) {
   const float TWOPI = 2.0f * (float) M_PI;
   int i = 0;