main.c 4.39 KB
Newer Older
1
#define _GNU_SOURCE
2 3 4 5 6 7 8 9 10 11 12 13
#include "usrp.h"
#include "../utils.h"

#include <stdio.h>
#include <stdlib.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <string.h>
#include <sys/types.h>
#include <pthread.h>
14
#include <immintrin.h>
15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35

#include <inttypes.h>

typedef struct {
  char *data;
  uint32_t n_samples;
  uint64_t timestamp;
} buffer_t;

void receive_from_channel_simulator(int sock, buffer_t *buf)
{
  unsigned char b[8+4];
  uint32_t n_samples;

  if (fullread(sock, b, 8+4) != 8+4) goto err;

  buf->timestamp = gu64(b);
  n_samples = gu32(b+8);
  if (n_samples != buf->n_samples) {
    free(buf->data);
    buf->n_samples = n_samples;
36
    if (posix_memalign((void **)&buf->data, 32, buf->n_samples * 4) != 0) goto err;
37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130
  }
  if (fullread(sock, buf->data, buf->n_samples * 4) != buf->n_samples * 4)
    goto err;

  return;

err:
  printf("ERROR: receive_from_channel_simulator failed\n");
  exit(1);
}

void send_to_channel_simulator(int sock, char *usrp_data, int n_samples,
                               uint64_t timestamp)
{
  unsigned char b[8+4];

  pu64(b, timestamp);
  pu32(b+8, n_samples);
  if (fullwrite(sock, b, 8+4) != 8+4 ||
      fullwrite(sock, usrp_data, n_samples * 4) != n_samples * 4) {
    printf("ERROR: send_to_channel_simulator failed\n");
    exit(1);
  }
}

int connect_to_channel_simulator(void)
{
  int port = 4024;
  char *ip = "127.0.0.1";
  struct sockaddr_in addr;
  int sock;
  int v;

  sock = socket(AF_INET, SOCK_STREAM, 0);
  if (sock == -1) { perror("socket"); exit(1); }

  v = 1;
  if (setsockopt(sock, IPPROTO_TCP, TCP_NODELAY, &v, sizeof(v)) == -1)
    { perror("channel_simulator: setsockopt(NODELAY)"); exit(1); }

  addr.sin_family = AF_INET;
  addr.sin_port = htons(port);
  addr.sin_addr.s_addr = inet_addr(ip);

  while (1) {
    printf("trying to connect to %s:%d\n", ip, port);

    if (connect(sock, (struct sockaddr *)&addr, sizeof(addr)) == 0) {
      printf("connection established\n");
      return sock;
    }

    perror("channel_simulator");
    sleep(1);
  }
}

void init_connection(int sock, uint64_t rx_frequency, uint64_t tx_frequency,
                     int samples_per_subframe)
{
  unsigned char b[8*2+4*3];

  pu64(b, rx_frequency);
  pu64(b+8, tx_frequency);
  pu32(b+8*2, samples_per_subframe * 2);         /* RX sample advance */
  pu32(b+8*2+4, 0);                              /* TX sample advance */
  pu32(b+8*2+4*2, samples_per_subframe * 1000);

  if (fullwrite(sock, b, 8*2+4*3) != 8*2+4*3) {
    printf("ERROR: init_connection failed\n");
    exit(1);
  }
}

void go_realtime(void)
{
  struct sched_param sparam;
  int policy;
  memset(&sparam, 0, sizeof(sparam));
  sparam.sched_priority = sched_get_priority_max(SCHED_FIFO)-1;
  policy = SCHED_FIFO ;
  if (pthread_setschedparam(pthread_self(), policy, &sparam) != 0) {
    printf("ERROR: Failed to set pthread priority\n");
    exit(1);
  }
}

int main(void)
{
  int sock;
  buffer_t buf = { data:NULL, n_samples:0, timestamp:0 };
  uint64_t sim_timestamp;
  uint64_t usrp_timestamp;
  int samples_per_subframe = 7680;
131
  int tx_sample_advance = 40;
132 133 134 135 136 137 138 139 140 141 142 143
  char *usrp_data;

  go_realtime();

  usrp_init_connection(2560000000, 2680000000);

  sock = connect_to_channel_simulator();
  init_connection(sock, 1, 2, samples_per_subframe);

  receive_from_channel_simulator(sock, &buf);
  sim_timestamp = buf.timestamp - 2 * samples_per_subframe + buf.n_samples;

144
  if (posix_memalign((void **)&usrp_data, 32, buf.n_samples * 4) != 0) {
145 146 147
    printf("ERROR: out of memory\n");
    exit(1);
  }
148
  memset(usrp_data, 0, buf.n_samples * 4);
149 150 151 152 153 154 155 156

  send_to_channel_simulator(sock, usrp_data, buf.n_samples, sim_timestamp);
  sim_timestamp += buf.n_samples;

  usrp_start();
  usrp_timestamp = usrp_read(usrp_data, buf.n_samples);

  while (1) {
157 158 159 160 161
    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);
    }
162 163 164
    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);
165 166 167 168
    for (i = 0; i < buf.n_samples * 2; i += 16) {
      __m256i *a = (__m256i *)&((int16_t *)usrp_data)[i];
      *a = _mm256_srai_epi16(*a, 4);
    }
169 170 171 172 173 174 175
    send_to_channel_simulator(sock, usrp_data, buf.n_samples, sim_timestamp);
    sim_timestamp += buf.n_samples;
    receive_from_channel_simulator(sock, &buf);
  }

  return 0;
}