From a44a61d7814fe457f69f5c261cd0f25798ef8259 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Wed, 25 Mar 2020 19:25:07 +0100
Subject: [PATCH] Fix HST channel emulator for keeping coherent phase between
 frames

---
 lib/include/srslte/phy/channel/channel.h | 15 +++++-----
 lib/src/phy/channel/channel.cc           | 35 ++++++++++++++++++++----
 2 files changed, 38 insertions(+), 12 deletions(-)

diff --git a/lib/include/srslte/phy/channel/channel.h b/lib/include/srslte/phy/channel/channel.h
index 198b21cf0..ae79f9812 100644
--- a/lib/include/srslte/phy/channel/channel.h
+++ b/lib/include/srslte/phy/channel/channel.h
@@ -73,17 +73,18 @@ public:
   void run(cf_t* in[SRSLTE_MAX_CHANNELS], cf_t* out[SRSLTE_MAX_CHANNELS], uint32_t len, const srslte_timestamp_t& t);
 
 private:
+  float                    hst_init_phase              = 0.0f;
   srslte_channel_fading_t* fading[SRSLTE_MAX_CHANNELS] = {};
   srslte_channel_delay_t*  delay[SRSLTE_MAX_CHANNELS]  = {};
   srslte_channel_awgn_t*   awgn                        = nullptr;
-  srslte_channel_hst_t*    hst                      = nullptr; // HST has no buffers / no multiple instance is required
-  srslte_channel_rlf_t*    rlf                      = nullptr; // RLF has no buffers / no multiple instance is required
-  cf_t*                    buffer_in                = nullptr;
-  cf_t*                    buffer_out               = nullptr;
-  log_filter*              log_h                    = nullptr;
+  srslte_channel_hst_t*    hst                         = nullptr;
+  srslte_channel_rlf_t*    rlf                         = nullptr;
+  cf_t*                    buffer_in                   = nullptr;
+  cf_t*                    buffer_out                  = nullptr;
+  log_filter*              log_h                       = nullptr;
   uint32_t                 nof_channels                = 0;
-  uint32_t                 current_srate            = 0;
-  args_t                   args                     = {};
+  uint32_t                 current_srate               = 0;
+  args_t                   args                        = {};
 };
 
 typedef std::unique_ptr<channel> channel_ptr;
diff --git a/lib/src/phy/channel/channel.cc b/lib/src/phy/channel/channel.cc
index 76c1a5493..81183bf2c 100644
--- a/lib/src/phy/channel/channel.cc
+++ b/lib/src/phy/channel/channel.cc
@@ -136,6 +136,16 @@ channel::~channel()
   }
 }
 
+extern "C" {
+static inline cf_t local_cexpf(float phase)
+{
+  cf_t ret;
+  __real__ ret = cosf(phase);
+  __imag__ ret = sinf(phase);
+  return ret;
+}
+}
+
 void channel::set_logger(log_filter* _log_h)
 {
   log_h = _log_h;
@@ -155,6 +165,11 @@ void channel::run(cf_t*                     in[SRSLTE_MAX_CHANNELS],
           // Copy input buffer
           memcpy(buffer_in, in[i], sizeof(cf_t) * len);
 
+          if (hst) {
+            srslte_channel_hst_execute(hst, buffer_in, buffer_out, len, &t);
+            srslte_vec_sc_prod_ccc(buffer_out, local_cexpf(hst_init_phase), buffer_in, len);
+          }
+
           if (awgn) {
             srslte_channel_awgn_run_c(awgn, buffer_in, buffer_out, len);
             memcpy(buffer_in, buffer_out, sizeof(cf_t) * len);
@@ -170,11 +185,6 @@ void channel::run(cf_t*                     in[SRSLTE_MAX_CHANNELS],
             memcpy(buffer_in, buffer_out, sizeof(cf_t) * len);
           }
 
-          if (hst) {
-            srslte_channel_hst_execute(hst, buffer_in, buffer_out, len, &t);
-            memcpy(buffer_in, buffer_out, sizeof(cf_t) * len);
-          }
-
           if (rlf) {
             srslte_channel_rlf_execute(rlf, buffer_in, buffer_out, len, &t);
             memcpy(buffer_in, buffer_out, sizeof(cf_t) * len);
@@ -185,6 +195,21 @@ void channel::run(cf_t*                     in[SRSLTE_MAX_CHANNELS],
         }
       }
 
+      if (hst) {
+        // Increment phase to keep it coherent between frames
+        hst_init_phase += (2 * M_PI * len * hst->fs_hz / hst->srate_hz);
+
+        // Positive Remainder
+        while (hst_init_phase > 2 * M_PI) {
+          hst_init_phase -= 2 * M_PI;
+        }
+
+        // Negative Remainder
+        while (hst_init_phase < -2 * M_PI) {
+          hst_init_phase += 2 * M_PI;
+        }
+      }
+
       if (log_h) {
         // Logging
         std::stringstream str;