channel_simulator: gains for channels (for X2 handover with a COTS UE)

The main goal of this commit is to be able to perform X2 handover
using a COTS UE and two 'simulated' eNB, each running on a separate
computer, but both using the same USRP device to send and receive IQ
data over the air. By changin the gains at runtime using a small script,
it is possible to trigger X2 handover based on the A3 measurement report
as sent by the UE.

The documentation on how to perform X2 handover with only one
USRP device is in channel_simulator/README.x2_handover.

channel_simulator/channel_simulator.c

@@ -8,6 +8,7 @@
 #include <inttypes.h>
 #include <unistd.h>
 #include <sys/socket.h>
+#include <immintrin.h>
 
 void init_channel_simulator(channel_simulator *c,
     uint32_t samplerate, int n_samples)
@@ -33,7 +34,7 @@ void cleanup_connections(channel_simulator *c)
     con = &c->connections[i];
     c->channels[con->rx_channel_index].connection_count--;
     c->channels[con->tx_channel_index].connection_count--;
-    free(con->tx_buffer);
+    free(con->iq_buffer);
     if (c->connections_count == 1) {
       free(c->connections);
       c->connections = NULL;
@@ -94,8 +95,8 @@ static int find_or_create_channel(channel_simulator *c, uint64_t freq,
   chan = &c->channels[i];
   chan->frequency = freq;
   chan->sample_advance = sample_advance;
-  chan->data = calloc(1, c->n_samples * 4);
-  if (chan->data == NULL) goto oom;
+  if (posix_memalign((void **)&chan->data, 32, c->n_samples * 4) != 0)
+    goto oom;
   chan->connection_count = 0;
 
   return i;
@@ -121,8 +122,8 @@ void channel_simulator_add_connection(channel_simulator *c,
   con = &c->connections[c->connections_count-1];
 
   con->socket = socket;
-  con->tx_buffer = calloc(1, c->n_samples * 4);
-  if (con->tx_buffer == NULL) goto oom;
+  if (posix_memalign((void **)&con->iq_buffer, 32, c->n_samples * 4) != 0)
+    goto oom;
   con->rx_frequency = rx_frequency;
   con->tx_frequency = tx_frequency;
   con->rx_channel_index = find_or_create_channel(c, rx_frequency,
@@ -133,6 +134,14 @@ void channel_simulator_add_connection(channel_simulator *c,
   c->channels[con->rx_channel_index].connection_count++;
   c->channels[con->tx_channel_index].connection_count++;
 
+  if (posix_memalign(&con->gain, 32, 32) != 0) {
+    printf("posix_memalign failed\n");
+    exit(1);
+  }
+
+  /* default gain = 1 (actually 1 - 1/2^15) */
+  *(__m256i *)con->gain = _mm256_set1_epi16(0x7fff);
+
   return;
 
 oom:
@@ -144,13 +153,30 @@ void connection_send_rx(connection *c, uint64_t timestamp,
     uint32_t *data, int n_samples)
 {
   unsigned char b[8+4];
+  int k;
+  int16_t *from;
+  int16_t *to;
+  __m256i *gain;
 
   if (c->socket == -1) return;
 
+  /* apply gain on data to send */
+  from = (int16_t *)data;
+  to = (int16_t *)c->iq_buffer;
+  gain = (__m256i *)c->gain;
+
+  /* does: to[] = from[] * gain[] */
+  for (k = 0; k < n_samples * 2; k+=16) {
+    __m256i *a, *b;
+    a = (__m256i *)&to[k];
+    b = (__m256i *)&from[k];
+    *a = _mm256_mulhrs_epi16(*b, *gain);
+  }
+
   pu64(b, timestamp);
   pu32(b+8, n_samples);
   if (fullwrite(c->socket, b, 8+4) != 8+4 ||
-      fullwrite(c->socket, data, n_samples * 4) != n_samples * 4) {
+      fullwrite(c->socket, c->iq_buffer, n_samples * 4) != n_samples * 4) {
     printf("ERROR: connection_send_rx failed, dropping\n");
     shutdown(c->socket, SHUT_RDWR);
     close(c->socket);
@@ -198,10 +224,32 @@ err:
   con->socket = -1;
 }
 
+void command_set_gain(channel_simulator *cs, connection *con, int n)
+{
+  unsigned char b[4];
+  int v;
+  if (n != 4) goto err;
+  if (fullread(con->socket, b, 4) != 4) goto err;
+  v = gu32(b);
+
+  *(__m256i *)con->gain = _mm256_set1_epi16(v);
+
+  printf("INFO: setting gain to %d\n", v);
+
+  return;
+
+err:
+  printf("ERROR: command_set_gain failed, dropping\n");
+  shutdown(con->socket, SHUT_RDWR);
+  close(con->socket);
+  con->socket = -1;
+}
+
 void do_command(channel_simulator *cs, connection *c, uint64_t command, int n)
 {
   switch (command) {
   case 0: return command_set_frequency(cs, c, n);
+  case 1: return command_set_gain(cs, c, n);
   default:
     printf("ERROR: bad command %"PRIu64", dropping\n", command);
     shutdown(c->socket, SHUT_RDWR);
@@ -239,7 +287,7 @@ again:
            recv_n_samples, n_samples);
     goto err;
   }
-  if (fullread(c->socket, c->tx_buffer, n_samples * 4) != n_samples * 4)
+  if (fullread(c->socket, c->iq_buffer, n_samples * 4) != n_samples * 4)
     goto err;
 
   return;
@@ -251,8 +299,6 @@ err:
   c->socket = -1;
 }
 
-#include <immintrin.h>
-
 void channel_simulate(channel_simulator *c)
 {
   int i;
@@ -269,38 +315,20 @@ void channel_simulate(channel_simulator *c)
   for (i = 0; i < c->channels_count; i++)
     memset(c->channels[i].data, 0, c->n_samples * 4);
 
-#if 0
-  /* basic mixing */
-  for (i = 0; i < c->connections_count; i++) {
-    con = &c->connections[i];
-    from = (int16_t *)con->tx_buffer;
-
-    for (k = 0; k < c->n_samples * 2; k++)
-      mix[con->tx_channel_index][k] += from[k];
-  }
-
-  for (i = 0; i < c->channels_count; i++) {
-    chan = &c->channels[i];
-    to = (int16_t *)chan->data;
-
-    for (k = 0; k < c->n_samples * 2; k++) {
-      int v = mix[i][k];
-      if (v > 32767) v = 32767;
-      if (v < -32768) v = -32768;
-      to[k] = v;
-    }
-  }
-#else
   /* basic mixing */
   for (i = 0; i < c->connections_count; i++) {
+    __m256i *gain;
     con = &c->connections[i];
-    from = (int16_t *)con->tx_buffer;
+    from = (int16_t *)con->iq_buffer;
+    gain = (__m256i *)con->gain;
 
+    /* does: mix[] = mix[] + from[] * gain[] */
     for (k = 0; k < c->n_samples * 2; k+=16) {
-      __m256i *a, *b;
+      __m256i *a, *b, v;
       a = (__m256i *)&mix[con->tx_channel_index][k];
       b = (__m256i *)&from[k];
-      *a = _mm256_adds_epi16(*a, *b);
+      v = _mm256_mulhrs_epi16(*b, *gain);
+      *a = _mm256_adds_epi16(*a, v);
     }
   }
 
@@ -309,5 +337,4 @@ void channel_simulate(channel_simulator *c)
     to = (int16_t *)chan->data;
     memcpy(to, mix[i], c->n_samples * 2 * 2);
   }
-#endif
 }

channel_simulator/channel_simulator.h

@@ -12,11 +12,14 @@ typedef struct {
 
 typedef struct {
   int      socket;
-  uint32_t *tx_buffer;
+  /* iq_buffer is used both in tx and rx */
+  uint32_t *iq_buffer;
   uint64_t rx_frequency;
   uint64_t tx_frequency;
   int      rx_channel_index;
   int      tx_channel_index;
+  /* gain to apply to rx and tx data */
+  void     *gain;           /* actually __m256i * */
 } connection;
 
 typedef struct {

channel_simulator/usrp/Makefile

 CC=gcc
 CPP=g++
-CFLAGS=-Wall -g -pthread
+CFLAGS=-Wall -g -pthread -march=native -O3 -ffast-math
 
 PROG=usrp
 OBJS=main.o usrp.o ../utils.o

channel_simulator/usrp/main.c

+#define _GNU_SOURCE
 #include "usrp.h"
 #include "../utils.h"
 
@@ -10,6 +11,7 @@
 #include <string.h>
 #include <sys/types.h>
 #include <pthread.h>
+#include <immintrin.h>
 
 #include <inttypes.h>
 
@@ -31,7 +33,7 @@ void receive_from_channel_simulator(int sock, buffer_t *buf)
   if (n_samples != buf->n_samples) {
     free(buf->data);
     buf->n_samples = n_samples;
-    buf->data = malloc(buf->n_samples * 4); if (buf->data == NULL) goto err;
+    if (posix_memalign((void **)&buf->data, 32, buf->n_samples * 4) != 0) goto err;
   }
   if (fullread(sock, buf->data, buf->n_samples * 4) != buf->n_samples * 4)
     goto err;
@@ -139,8 +141,7 @@ int main(void)
   receive_from_channel_simulator(sock, &buf);
   sim_timestamp = buf.timestamp - 2 * samples_per_subframe + buf.n_samples;
 
-  usrp_data = calloc(1, buf.n_samples * 4);
-  if (usrp_data == NULL) {
+  if (posix_memalign((void **)&usrp_data, 32, buf.n_samples * 4) != 0) {
     printf("ERROR: out of memory\n");
     exit(1);
   }
@@ -152,9 +153,18 @@ int main(void)
   usrp_timestamp = usrp_read(usrp_data, buf.n_samples);
 
   while (1) {
+    int i;
+    for (i = 0; i < buf.n_samples * 2; i += 16) {
+      __m256i *a = (__m256i *)&((int16_t *)buf.data)[i];
+      *a = _mm256_slli_epi16(*a, 4);
+    }
     usrp_write(buf.data, buf.n_samples,
                usrp_timestamp - buf.n_samples + 2 * samples_per_subframe - tx_sample_advance);
     usrp_timestamp = usrp_read(usrp_data, buf.n_samples);
+    for (i = 0; i < buf.n_samples * 2; i += 16) {
+      __m256i *a = (__m256i *)&((int16_t *)usrp_data)[i];
+      *a = _mm256_srai_epi16(*a, 4);
+    }
     send_to_channel_simulator(sock, usrp_data, buf.n_samples, sim_timestamp);
     sim_timestamp += buf.n_samples;
     receive_from_channel_simulator(sock, &buf);

channel_simulator/usrp/usrp.cc

@@ -32,11 +32,11 @@ void usrp_init_connection(uint64_t rx_freq, uint64_t tx_freq)
 
   usrp->set_rx_rate(7680000, 0);
   usrp->set_rx_freq(rx_freq, 0);
-  usrp->set_rx_gain(40, 0);
+  usrp->set_rx_gain(62.2, 0);
 
   usrp->set_tx_rate(7680000, 0);
   usrp->set_tx_freq(tx_freq, 0);
-  usrp->set_tx_gain(90, 0);
+  usrp->set_tx_gain(89.75, 0);
 
   uhd::stream_args_t stream_args_rx("sc16", "sc16");
   stream_args_rx.args["spp"] = str(boost::format("%d") % 768 );

targets/ARCH/tcp_bridge/channel_simulator.c

+#define _GNU_SOURCE
 #include "common_lib.h"
 #include "channel_simulator/utils.h"
 
@@ -9,6 +10,7 @@
 #include <netinet/tcp.h>
 #include <arpa/inet.h>
 #include <inttypes.h>
+#include <math.h>
 
 typedef struct buffer_s {
   uint64_t timestamp;
@@ -40,6 +42,97 @@ typedef struct {
   int samples_per_packet;
 } channel_simulator_state_t;
 
+/*************************************************************************/
+/* gain setting begin                                                    */
+/*************************************************************************/
+
+void channel_simulator_set_gain(channel_simulator_state_t *c, int gain_db)
+{
+  unsigned char b[8+4+4];
+  double gain;
+  int gain_lin;
+
+  gain = exp10(gain_db/20.);
+  if (gain > 1) gain = 1;
+  gain_lin = gain * 32767;
+  if (gain_lin > 0x7fff)
+    gain_lin = 0x7fff;
+
+  printf("channel_simulator: setting gain to %d dB (%d lin)\n", gain_db, gain_lin);
+
+  pu64(b, 1);
+  pu32(b+8, 4);
+  pu32(b+8+4, gain_lin);
+
+  lock(&c->tx_lock);
+  if (fullwrite(c->sock, b, 8+4+4) != 8+4+4) {
+    printf("ERROR: channel_simulator: channel_simulator_set_gain failed\n");
+    exit(1);
+  }
+  unlock(&c->tx_lock);
+}
+
+int get_line(int s, char *l, int len)
+{
+  int pos = 0;
+  while (1) {
+    if (fullread(s, &l[pos], 1) != 1)
+      return -1;
+    if (l[pos] == '\n') {
+      l[pos] = 0;
+      break;
+    }
+    pos++;
+    if (pos == len)
+      return -1;
+  }
+  return 0;
+}
+
+void channel_simulator_gain_process(channel_simulator_state_t *c, int s)
+{
+  char line[256];
+  int gain;
+
+  if (get_line(s, line, 256) == -1) return;
+  if (sscanf(line, "%d", &gain) != 1) return;
+  channel_simulator_set_gain(c, gain);
+}
+
+void *channel_simulator_gain_thread(void *_c)
+{
+  channel_simulator_state_t *c = _c;
+  int port = 4025;
+  char *env;
+  struct sockaddr_in addr;
+  socklen_t addrlen;
+  int sock;
+
+  env = getenv("CHANNEL_SIMULATOR_GAIN_PORT");
+  if (env != NULL)
+    port = atoi(env);
+  else
+    printf("channel_simulator: environment variable CHANNEL_SIMULATOR_GAIN_PORT"
+           " not found, using default port %d\n", port);
+
+  sock = create_listen_socket("0.0.0.0", port);
+
+  while (1) {
+    int t;
+    addrlen = sizeof(addr);
+    t = accept(sock, (struct sockaddr *)&addr, &addrlen);
+    if (t == -1) { perror("accept"); exit(1); }
+    channel_simulator_gain_process(c, t);
+    close(t);
+  }
+
+  return NULL;
+}
+
+/*************************************************************************/
+/* gain setting end                                                      */
+/*************************************************************************/
+
 void add_rx_buffer(channel_simulator_state_t *c, buffer_t *b)
 {
   lock(&c->lock);
@@ -460,5 +553,7 @@ int device_init(openair0_device* device, openair0_config_t *openair0_cfg)
 
   device->openair0_cfg=&openair0_cfg[0];
 
+  new_thread(channel_simulator_gain_thread, channel_simulator);
+
   return 0;
 }