srsLTE: upgraded ZMQ for supporting frequency selection

lib/src/phy/rf/rf_zmq_imp.c

@@ -42,8 +42,8 @@ typedef struct {
   uint32_t base_srate;
   uint32_t decim_factor; // decimation factor between base_srate used on transport on radio's rate
   double   rx_gain;
-  double   tx_freq;
+  uint32_t tx_freq_mhz[SRSLTE_MAX_PORTS];
-  double   rx_freq;
+  uint32_t rx_freq_mhz[SRSLTE_MAX_PORTS];
   bool     tx_used;
 
   // Server
@@ -62,7 +62,8 @@ typedef struct {
   // Rx timestamp
   uint64_t next_rx_ts;
 
-  pthread_t thread;
+  pthread_mutex_t tx_config_mutex;
+  pthread_mutex_t rx_config_mutex;
 } rf_zmq_handler_t;
 
 void update_rates(rf_zmq_handler_t* handler, double srate);
@@ -192,6 +193,20 @@ int rf_zmq_open(char* args, void** h)
   return rf_zmq_open_multi(args, h, 1);
 }
 
+static inline int parse_double(const char* args, const char* config_arg, double* value)
+{
+  int ret = SRSLTE_ERROR;
+  if (args && config_arg && value) {
+    char* config_ptr = strstr(args, config_arg);
+
+    if (config_ptr) {
+      *value = strtod(config_ptr + strlen(config_arg), NULL);
+      ret    = SRSLTE_SUCCESS;
+    }
+  }
+  return ret;
+}
+
 int rf_zmq_open_multi(char* args, void** h, uint32_t nof_channels)
 {
   int ret = SRSLTE_ERROR;
@@ -221,6 +236,13 @@ int rf_zmq_open_multi(char* args, void** h, uint32_t nof_channels)
     tx_opts.id            = handler->id;
     rx_opts.id            = handler->id;
 
+    if (pthread_mutex_init(&handler->tx_config_mutex, NULL)) {
+      perror("Mutex init");
+    }
+    if (pthread_mutex_init(&handler->rx_config_mutex, NULL)) {
+      perror("Mutex init");
+    }
+
     // parse args
     if (args && strlen(args)) {
       // base_srate
@@ -358,6 +380,20 @@ int rf_zmq_open_multi(char* args, void** h, uint32_t nof_channels)
         }
       }
 
+      // rx_freq
+      {
+        char config_arg[PARAM_LEN] = "rx_freq=";
+        if (i > 0) {
+          snprintf(config_arg, PARAM_LEN, "rx_freq%d=", i + 1);
+        }
+
+        double freq = 0.0;
+        if (!parse_double(args, config_arg, &freq)) {
+          rx_opts.frequency_hz_mhz = (uint32_t)(freq / 1e6);
+          printf("Channel %d. Using rx_freq=%dMHz\n", i, rx_opts.frequency_hz_mhz);
+        }
+      }
+
       // txport
       {
         char config_arg[PARAM_LEN] = "tx_port=";
@@ -379,6 +415,20 @@ int rf_zmq_open_multi(char* args, void** h, uint32_t nof_channels)
         }
       }
 
+      // tx_freq
+      {
+        char config_arg[PARAM_LEN] = "tx_freq=";
+        if (i > 0) {
+          snprintf(config_arg, PARAM_LEN, "tx_freq%d=", i + 1);
+        }
+
+        double freq = 0.0;
+        if (!parse_double(args, config_arg, &freq)) {
+          tx_opts.frequency_hz_mhz = (uint32_t)(freq / 1e6);
+          printf("Channel %d. Using tx_freq=%dMHz\n", i, tx_opts.frequency_hz_mhz);
+        }
+      }
+
       // initialize transmitter
       if (strlen(handler->tx_port) != 0) {
         if (rf_zmq_tx_open(&handler->transmitter[i], tx_opts, handler->context, handler->tx_port) != SRSLTE_SUCCESS) {
@@ -438,11 +488,6 @@ int rf_zmq_close(void* h)
 
   rf_zmq_info(handler->id, "Closing ...\n");
 
-  if (handler->thread) {
-    pthread_join(handler->thread, NULL);
-    pthread_detach(handler->thread);
-  }
-
   for (int i = 0; i < handler->nof_channels; i++) {
     rf_zmq_tx_close(&handler->transmitter[i]);
     rf_zmq_rx_close(&handler->receiver[i]);
@@ -462,6 +507,9 @@ int rf_zmq_close(void* h)
     free(handler->buffer_tx);
   }
 
+  pthread_mutex_destroy(&handler->tx_config_mutex);
+  pthread_mutex_destroy(&handler->rx_config_mutex);
+
   // Free all
   free(handler);
 
@@ -556,8 +604,12 @@ double rf_zmq_set_rx_freq(void* h, uint32_t ch, double freq)
   double ret = NAN;
   if (h) {
     rf_zmq_handler_t* handler = (rf_zmq_handler_t*)h;
-    handler->rx_freq          = freq;
+    pthread_mutex_lock(&handler->rx_config_mutex);
-    ret                       = freq;
+    if (ch < handler->nof_channels && isnormal(freq) && freq > 0.0) {
+      handler->rx_freq_mhz[ch] = (uint32_t)(freq / 1e6);
+      ret                      = freq;
+    }
+    pthread_mutex_unlock(&handler->rx_config_mutex);
   }
   return ret;
 }
@@ -567,8 +619,12 @@ double rf_zmq_set_tx_freq(void* h, uint32_t ch, double freq)
   double ret = NAN;
   if (h) {
     rf_zmq_handler_t* handler = (rf_zmq_handler_t*)h;
-    handler->tx_freq          = freq;
+    pthread_mutex_lock(&handler->tx_config_mutex);
-    ret                       = freq;
+    if (ch < handler->nof_channels && isnormal(freq) && freq > 0.0) {
+      handler->tx_freq_mhz[ch] = (uint32_t)(freq / 1e6);
+      ret                      = freq;
+    }
+    pthread_mutex_unlock(&handler->tx_config_mutex);
   }
   return ret;
 }
@@ -608,6 +664,29 @@ int rf_zmq_recv_with_time_multi(void*    h,
 
     rf_zmq_info(handler->id, "Rx %d samples (%d B)\n", nsamples, nbytes);
 
+    // Map ports to data buffers according to the selected frequencies
+    pthread_mutex_lock(&handler->rx_config_mutex);
+    cf_t* buffers[SRSLTE_MAX_PORTS] = {}; // Buffer pointers, NULL if unmatched
+    for (uint32_t i = 0; i < handler->nof_channels; i++) {
+      bool mapped = false;
+
+      // Find first matching frequency
+      for (uint32_t j = 0; j < handler->nof_channels && !mapped; j++) {
+        // Traverse all channels, break if mapped
+        if (buffers[j] == NULL && rf_zmq_rx_match_freq(&handler->receiver[j], handler->rx_freq_mhz[i])) {
+          // Available buffer and matched frequency with receiver
+          buffers[j] = (cf_t*)data[i];
+          mapped     = true;
+        }
+      }
+
+      // If no matching frequency found; set data to zeros
+      if (!mapped && data[i]) {
+        memset(data[i], 0, sizeof(cf_t) * nsamples);
+      }
+    }
+    pthread_mutex_unlock(&handler->rx_config_mutex);
+
     // set timestamp for this reception
     if (secs != NULL && frac_secs != NULL) {
       srslte_timestamp_t ts = {};
@@ -657,7 +736,7 @@ int rf_zmq_recv_with_time_multi(void*    h,
 
       // Iterate channels
       for (uint32_t i = 0; i < handler->nof_channels; i++) {
-        cf_t* ptr = (handler->decim_factor != 1) ? handler->buffer_decimation[i] : data[i];
+        cf_t* ptr = (handler->decim_factor != 1 || buffers[i] == NULL) ? handler->buffer_decimation[i] : buffers[i];
 
         // Completed condition
         if (count[i] < nsamples_baserate && handler->receiver[i].running) {
@@ -689,31 +768,37 @@ int rf_zmq_recv_with_time_multi(void*    h,
 
     // decimate if needed
     if (handler->decim_factor != 1) {
-      for (int c = 0; c < handler->nof_channels; c++) {
+      for (uint32_t c = 0; c < handler->nof_channels; c++) {
-
+        // skip if buffer is not available
-        cf_t* dst = data[c];
+        if (buffers[c]) {
-        cf_t* ptr = (handler->decim_factor != 1) ? handler->buffer_decimation[c] : data[c];
+          cf_t* dst = buffers[c];
-
+          cf_t* ptr = handler->buffer_decimation[c];
-        int n;
+
-        for (uint32_t i = n = 0; i < nsamples; i++) {
+          for (uint32_t i = 0, n = 0; i < nsamples; i++) {
-          // Averaging decimation
+            // Averaging decimation
-          cf_t avg = 0.0f;
+            cf_t avg = 0.0f;
-          for (int j = 0; j < handler->decim_factor; j++, n++) {
+            for (int j = 0; j < handler->decim_factor; j++, n++) {
-            avg += ptr[n];
+              avg += ptr[n];
+            }
+            dst[i] = avg;
           }
-          dst[i] = avg;
+
+          rf_zmq_info(handler->id,
+                      "  - re-adjust bytes due to %dx decimation %d --> %d samples)\n",
+                      handler->decim_factor,
+                      nsamples_baserate,
+                      nsamples);
         }
       }
-      rf_zmq_info(handler->id,
-                  "  - re-adjust bytes due to %dx decimation %d --> %d samples)\n",
-                  handler->decim_factor,
-                  nsamples_baserate,
-                  nsamples);
     }
 
     // Set gain
     float scale = srslte_convert_dB_to_amplitude(handler->rx_gain);
-    srslte_vec_sc_prod_cfc(data[0], scale, data[0], nsamples);
+    for (uint32_t c = 0; c < handler->nof_channels; c++) {
+      if (buffers[c]) {
+        srslte_vec_sc_prod_cfc(buffers[c], scale, buffers[c], nsamples);
+      }
+    }
 
     // update rx time
     update_ts(handler, &handler->next_rx_ts, nsamples_baserate, "rx");
@@ -766,6 +851,24 @@ int rf_zmq_send_timed_multi(void*  h,
       goto clean_exit;
     }
 
+    // Map ports to data buffers according to the selected frequencies
+    pthread_mutex_lock(&handler->tx_config_mutex);
+    cf_t* buffers[SRSLTE_MAX_PORTS] = {}; // Buffer pointers, NULL if unmatched
+    for (uint32_t i = 0; i < handler->nof_channels; i++) {
+      bool mapped = false;
+
+      // Find first matching frequency
+      for (uint32_t j = 0; j < handler->nof_channels && !mapped; j++) {
+        // Traverse all channels, break if mapped
+        if (buffers[j] == NULL && rf_zmq_tx_match_freq(&handler->transmitter[j], handler->tx_freq_mhz[i])) {
+          // Available buffer and matched frequency with receiver
+          buffers[j] = (cf_t*)data[i];
+          mapped     = true;
+        }
+      }
+    }
+    pthread_mutex_unlock(&handler->tx_config_mutex);
+
     rf_zmq_info(handler->id, "Tx %d samples (%d B)\n", nsamples, nbytes);
 
     // return if transmitter is switched off
@@ -800,38 +903,45 @@ int rf_zmq_send_timed_multi(void*  h,
 
     // Send base-band samples
     for (int i = 0; i < handler->nof_channels; i++) {
-      // Select buffer pointer depending on interpolation
+      if (buffers[i] != NULL) {
-      cf_t* buf = (handler->decim_factor != 1) ? handler->buffer_tx : data[i];
+        // Select buffer pointer depending on interpolation
-
+        cf_t* buf = (handler->decim_factor != 1) ? handler->buffer_tx : buffers[i];
-      // Interpolate if required
+
-      if (handler->decim_factor != 1) {
+        // Interpolate if required
-        rf_zmq_info(handler->id,
+        if (handler->decim_factor != 1) {
-                    "  - re-adjust bytes due to %dx interpolation %d --> %d samples)\n",
+          rf_zmq_info(handler->id,
-                    handler->decim_factor,
+                      "  - re-adjust bytes due to %dx interpolation %d --> %d samples)\n",
-                    nsamples,
+                      handler->decim_factor,
-                    nsamples_baseband);
+                      nsamples,
-
+                      nsamples_baseband);
-        int   n   = 0;
+
-        cf_t* src = data[i];
+          int   n   = 0;
-        for (int k = 0; k < nsamples; k++) {
+          cf_t* src = data[i];
-          // perform zero order hold
+          for (int k = 0; k < nsamples; k++) {
-          for (int j = 0; j < handler->decim_factor; j++, n++) {
+            // perform zero order hold
-            buf[n] = src[k];
+            for (int j = 0; j < handler->decim_factor; j++, n++) {
+              buf[n] = src[k];
+            }
+          }
+
+          if (nsamples_baseband != n) {
+            fprintf(stderr,
+                    "Number of tx samples (%d) does not match with number of interpolated samples (%d)\n",
+                    nsamples_baseband,
+                    n);
+            goto clean_exit;
           }
         }
 
-        if (nsamples_baseband != n) {
+        int n = rf_zmq_tx_baseband(&handler->transmitter[i], buf, nsamples_baseband);
-          fprintf(stderr,
+        if (n == SRSLTE_ERROR) {
-                  "Number of tx samples (%d) does not match with number of interpolated samples (%d)\n",
+          goto clean_exit;
-                  nsamples_baseband,
+        }
-                  n);
+      } else {
+        int n = rf_zmq_tx_zeros(&handler->transmitter[i], nsamples_baseband);
+        if (n == SRSLTE_ERROR) {
           goto clean_exit;
         }
-      }
-
-      int n = rf_zmq_tx_baseband(&handler->transmitter[i], buf, nsamples_baseband);
-      if (n == SRSLTE_ERROR) {
-        goto clean_exit;
       }
     }
     handler->tx_used = true;

lib/src/phy/rf/rf_zmq_imp_rx.c

@@ -120,6 +120,7 @@ int rf_zmq_rx_open(rf_zmq_rx_t* q, rf_zmq_opts_t opts, void* zmq_ctx, char* sock
     }
     q->socket_type   = opts.socket_type;
     q->sample_format = opts.sample_format;
+    q->frequency_hz_mhz = opts.frequency_hz_mhz;
 
     if (opts.socket_type == ZMQ_SUB) {
       zmq_setsockopt(q->sock, ZMQ_SUBSCRIBE, "", 0);
@@ -208,6 +209,15 @@ int rf_zmq_rx_baseband(rf_zmq_rx_t* q, cf_t* buffer, uint32_t nsamples)
   return n;
 }
 
+bool rf_zmq_rx_match_freq(rf_zmq_rx_t* q, uint32_t freq_hz)
+{
+  bool ret = false;
+  if (q) {
+    ret = (q->frequency_hz_mhz == 0 || q->frequency_hz_mhz == freq_hz);
+  }
+  return ret;
+}
+
 void rf_zmq_rx_close(rf_zmq_rx_t* q)
 {
   rf_zmq_info(q->id, "Closing ...\n");

lib/src/phy/rf/rf_zmq_imp_trx.h

@@ -39,8 +39,8 @@
 #define ZMQ_TIMEOUT_MS (1000)
 #define ZMQ_BASERATE_DEFAULT_HZ (23040000)
 #define ZMQ_ID_STRLEN 16
-#define ZMQ_MAX_GAIN_DB (90.0f)
+#define ZMQ_MAX_GAIN_DB (30.0f)
-#define ZMQ_MIN_GAIN_DB (-90.0f)
+#define ZMQ_MIN_GAIN_DB (0.0f)
 
 typedef enum { ZMQ_TYPE_FC32 = 0, ZMQ_TYPE_SC16 } rf_zmq_format_t;
 
@@ -54,6 +54,7 @@ typedef struct {
   pthread_mutex_t mutex;
   cf_t*           zeros;
   void*           temp_buffer_convert;
+  uint32_t        frequency_hz_mhz;
 } rf_zmq_tx_t;
 
 typedef struct {
@@ -68,12 +69,14 @@ typedef struct {
   srslte_ringbuffer_t ringbuffer;
   cf_t*               temp_buffer;
   void*               temp_buffer_convert;
+  uint32_t            frequency_hz_mhz;
 } rf_zmq_rx_t;
 
 typedef struct {
   const char*     id;
   uint32_t        socket_type;
   rf_zmq_format_t sample_format;
+  uint32_t        frequency_hz_mhz;
 } rf_zmq_opts_t;
 
 /*
@@ -94,6 +97,10 @@ SRSLTE_API int rf_zmq_tx_align(rf_zmq_tx_t* q, uint64_t ts);
 
 SRSLTE_API int rf_zmq_tx_baseband(rf_zmq_tx_t* q, cf_t* buffer, uint32_t nsamples);
 
+SRSLTE_API int rf_zmq_tx_zeros(rf_zmq_tx_t* q, uint32_t nsamples);
+
+SRSLTE_API bool rf_zmq_tx_match_freq(rf_zmq_tx_t* q, uint32_t freq_hz);
+
 SRSLTE_API void rf_zmq_tx_close(rf_zmq_tx_t* q);
 
 /*
@@ -103,6 +110,8 @@ SRSLTE_API int rf_zmq_rx_open(rf_zmq_rx_t* q, rf_zmq_opts_t opts, void* zmq_ctx,
 
 SRSLTE_API int rf_zmq_rx_baseband(rf_zmq_rx_t* q, cf_t* buffer, uint32_t nsamples);
 
+SRSLTE_API bool rf_zmq_rx_match_freq(rf_zmq_rx_t* q, uint32_t freq_hz);
+
 SRSLTE_API void rf_zmq_rx_close(rf_zmq_rx_t* q);
 
 #endif // SRSLTE_RF_ZMQ_IMP_TRX_H

lib/src/phy/rf/rf_zmq_imp_tx.c

@@ -52,6 +52,7 @@ int rf_zmq_tx_open(rf_zmq_tx_t* q, rf_zmq_opts_t opts, void* zmq_ctx, char* sock
     }
     q->socket_type   = opts.socket_type;
     q->sample_format = opts.sample_format;
+    q->frequency_hz_mhz = opts.frequency_hz_mhz;
 
     rf_zmq_info(q->id, "Binding transmitter: %s\n", sock_args);
 
@@ -199,6 +200,27 @@ int rf_zmq_tx_baseband(rf_zmq_tx_t* q, cf_t* buffer, uint32_t nsamples)
   return n;
 }
 
+int rf_zmq_tx_zeros(rf_zmq_tx_t* q, uint32_t nsamples)
+{
+  pthread_mutex_lock(&q->mutex);
+
+  rf_zmq_info(q->id, " - Tx %d Zeros.\n", nsamples);
+  _rf_zmq_tx_baseband(q, q->zeros, (uint32_t)nsamples);
+
+  pthread_mutex_unlock(&q->mutex);
+
+  return (int)nsamples;
+}
+
+bool rf_zmq_tx_match_freq(rf_zmq_tx_t* q, uint32_t freq_hz)
+{
+  bool ret = false;
+  if (q) {
+    ret = (q->frequency_hz_mhz == 0 || q->frequency_hz_mhz == freq_hz);
+  }
+  return ret;
+}
+
 void rf_zmq_tx_close(rf_zmq_tx_t* q)
 {
   q->running = false;