Commit 4e72db49 authored by Joanne Hugé's avatar Joanne Hugé

wip

parent aa1b8a8c
#include <arpa/inet.h>
#include <assert.h>
#include <errno.h>
#include <error.h>
#include <fcntl.h>
#include <getopt.h>
#include <immintrin.h>
#include <inttypes.h>
#include <limits.h>
#include <linux/if_packet.h>
#include <math.h>
#include <netdb.h>
#include <netinet/ether.h>
#include <netinet/in.h>
#include <net/if.h>
#include <pthread.h>
#include <sched.h>
#include <semaphore.h>
#include <signal.h>
#include <stdarg.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>
#include "private/trx_driver.h"
//#define DEBUG // Enables / deactivates log_debug
//#define TRACE
#define MONITOR
#define RECV_STOP_THRESHOLD 3
#define PPS_UPDATE_PERIOD INT64_C(1000000000)
#include "utils.c"
#define EFREQ 38400
#define STAT_FRAME_INTERVAL INT64_C(EFREQ * 150)
#define START_SENDING
#define RX_N_CHANNEL 1
#define TX_N_CHANNEL 4
#define FRAME_FREQ INT64_C(3840000) // Basic frame frequency
#define TX_PACKET_SIZE 262
#define RX_MAX_PACKET_SIZE 262
#define TX_ECPRI_PACKET_SIZE (TX_PACKET_SIZE - 14)
#define N_SAMPLES (32)
#define TRX_MAX_GROUP 1500
#define TRX_BUF_MAX_SIZE 500000
#define TXRX_BUF_MAX_SIZE 500000
#define STATISTIC_REFRESH_RATE INT64_C(500 * 1000 * 1000)
#define STAT_INT_LEN "9"
typedef struct {
float re;
float im;
} Complex;
typedef struct {
const uint8_t * re_mac;
const uint8_t * rec_mac;
const uint8_t * rec_if;
const char * dpdk_options;
const uint8_t * log_directory;
int recv_affinity;
int send_affinity;
int encode_affinity;
int decode_affinity;
int statistic_affinity;
int ecpri_period;
int flow_id;
int sample_rate;
} TRXEcpriState;
typedef struct {
int64_t counter;
int64_t pps_counter;
int64_t pps_ts;
int64_t pps;
} counter_stat_t;
typedef struct {
volatile void * buffer;
char name[64];
int buf_len;
int len;
volatile int write_index;
volatile int read_index;
volatile int write_ahead;
} ring_buffer_t;
typedef struct {
int64_t count;
uint8_t wait;
uint8_t zeroes;
} sample_group_t;
/* Proprietary code:
- compression / decompression of IQ samples
- fast conversion between int16_t and float
*/
#include "private/bf1_avx2.c"
// Buffers
static ring_buffer_t rx_rbuf; // Received packets
static ring_buffer_t trxr_rbuf[RX_N_CHANNEL]; // Decoded IQ samples
static ring_buffer_t tx_rbuf; // Packets to send
static ring_buffer_t trxw_rbuf[TX_N_CHANNEL]; // Uncompressed IQ samples
static ring_buffer_t trxw_group_rbuf; // Group of IQ samples
// Counters
static volatile counter_stat_t recv_counter; // frames received from eRE
static volatile counter_stat_t decode_counter; // decoded frames
static volatile counter_stat_t read_counter; // frames passed to amarisoft stack
static volatile counter_stat_t write_counter; // samples to write from TRX
static volatile counter_stat_t encode_counter; // encoded frames
static volatile counter_stat_t sent_counter; // frames sent to eRE
static volatile counter_stat_t rx_drop_counter; // frames sent to eRE
static volatile counter_stat_t tx_drop_counter; // frames sent to eRE
static volatile int sync_complete = 0;
static volatile int sync_happened = 0;
static int first_trx_write = 1;
#ifdef TRACE
static volatile int rx_trace_ready = 0;
static volatile int tx_trace_ready = 1;
static int tx_trace_index_start = 0;
#endif
// Network
static volatile int seq_id;
static void rbuf_update_write_index(ring_buffer_t * rbuf) {
rbuf->write_index = (rbuf->write_index + 1) % rbuf->buf_len;
}
static void rbuf_update_read_index(ring_buffer_t * rbuf) {
rbuf->read_index = (rbuf->read_index + 1) % rbuf->buf_len;
}
static int rbuf_read_amount(ring_buffer_t * rbuf) {
return (rbuf->write_index + rbuf->buf_len - rbuf->read_index) % rbuf->buf_len;
}
static int rbuf_write_amount(ring_buffer_t * rbuf) {
// Don't write everything to avoid write index catching up to read index
// That we way we don't have to use locks
return ((rbuf->read_index + rbuf->buf_len - rbuf->write_index - 1) % rbuf->buf_len);
}
static int rbuf_contiguous_copy(ring_buffer_t * rbuf1, ring_buffer_t * rbuf2, int n) {
int ret = n;
if(rbuf1) {
n = rbuf1->buf_len - rbuf1->read_index;
ret = n < ret ? n : ret;
}
if(rbuf2)
n = rbuf2->buf_len - rbuf2->write_index;
return n < ret ? n : ret;
}
#define RBUF_READ0(rbuf, type) (((type *) rbuf.buffer) + (rbuf.read_index * rbuf.len))
#define RBUF_WRITE0(rbuf, type) (((type *) rbuf.buffer) + (rbuf.write_index * rbuf.len))
#define RBUF_READ(rbuf, i, type) (((type *) rbuf.buffer) + (((rbuf.read_index + i) % rbuf.buf_len) * rbuf.len))
#define RBUF_WRITE(rbuf, i, type) (((type *) rbuf.buffer) + (((rbuf.write_index + i) % rbuf.buf_len) * rbuf.len))
#define RBUF_INIT(rbuf, _name, _buf_len, _len, type) do\
{\
log_debug("TRX_ECPRI", "Allocating %s with %d bytes\n", _name, (_buf_len * _len * sizeof(type)));\
rbuf.buffer = (type *) malloc(_buf_len * _len * sizeof(type));\
strcpy(rbuf.name, _name);\
rbuf.buf_len = _buf_len;\
rbuf.len = _len;\
rbuf.write_index = 0;\
rbuf.read_index = 0;\
rbuf.write_ahead = 0;\
} while(0)
static void print_stats(FILE * f, int print_header) {
if(print_header) {
fprintf(f,
"%" STAT_INT_LEN "s "
"%" STAT_INT_LEN "s "
"%" STAT_INT_LEN "s "
"%" STAT_INT_LEN "s "
"%" STAT_INT_LEN "s "
"%" STAT_INT_LEN "s "
"%" STAT_INT_LEN "s "
"%" STAT_INT_LEN "s "
"%" STAT_INT_LEN "s "
"%" STAT_INT_LEN "s "
"%" STAT_INT_LEN "s "
"%" STAT_INT_LEN "s "
"%" STAT_INT_LEN "s "
"%" STAT_INT_LEN "s "
"\n",
"rx dropped",
"tx dropped",
"received",
"decode",
"read",
"write",
"encode",
"sent",
"received pps",
"decode pps",
"read pps",
"write pps",
"encode pps",
"sent pps");
}
fprintf(f,
"%" STAT_INT_LEN "" PRIi64 " "
"%" STAT_INT_LEN "" PRIi64 " "
"%" STAT_INT_LEN "" PRIi64 " "
"%" STAT_INT_LEN "" PRIi64 " "
"%" STAT_INT_LEN "" PRIi64 " "
"%" STAT_INT_LEN "" PRIi64 " "
"%" STAT_INT_LEN "" PRIi64 " "
"%" STAT_INT_LEN "" PRIi64 " "
"%" STAT_INT_LEN "" PRIi64 "pps "
"%" STAT_INT_LEN "" PRIi64 "pps "
"%" STAT_INT_LEN "" PRIi64 "pps "
"%" STAT_INT_LEN "" PRIi64 "pps "
"%" STAT_INT_LEN "" PRIi64 "pps "
"%" STAT_INT_LEN "" PRIi64 "pps "
"\n",
rx_drop_counter.counter,
tx_drop_counter.counter,
recv_counter.counter,
decode_counter.counter,
read_counter.counter,
write_counter.counter,
encode_counter.counter,
sent_counter.counter,
recv_counter.pps,
decode_counter.pps,
read_counter.pps,
write_counter.pps,
encode_counter.pps,
sent_counter.pps);
}
static void log_exit(const char * section, const char * msg, ...) {
time_t t;
struct tm ts;
char line[256];
va_list arglist;
time(&t);
ts = *localtime(&t);
strftime(line, 80, "%m-%d %H:%M:%S", &ts);
sprintf(line + strlen(line), " EXIT [%s] ", section);
va_start(arglist, msg);
vsprintf(line + strlen(line), msg, arglist);
va_end(arglist);
fprintf(stderr, "%s\n", line);
// Dump useful information
print_stats(stderr, 1);
fprintf(stderr, "TX RBUF: ri %d wi %d ra %d wa %d\n", tx_rbuf.read_index, tx_rbuf.write_index, rbuf_read_amount(&tx_rbuf), rbuf_write_amount(&tx_rbuf));
fprintf(stderr, "RX RBUF: ri %d wi %d ra %d wa %d\n", rx_rbuf.read_index, rx_rbuf.write_index, rbuf_read_amount(&rx_rbuf), rbuf_write_amount(&rx_rbuf));
fprintf(stderr, "TRXW RBUF: ri %d wi %d ra %d wa %d\n", trxw_rbuf[0].read_index, trxw_rbuf[0].write_index, rbuf_read_amount(&trxw_rbuf[0]), rbuf_write_amount(&trxw_rbuf[0]));
fprintf(stderr, "TRXR RBUF: ri %d wi %d ra %d wa %d\n", trxr_rbuf[0].read_index, trxr_rbuf[0].write_index, rbuf_read_amount(&trxr_rbuf[0]), rbuf_write_amount(&trxr_rbuf[0]));
fprintf(stderr, "TRXW GROUP RBUF: ri %d wi %d ra %d wa %d\n", trxw_group_rbuf.read_index, trxw_group_rbuf.write_index, rbuf_read_amount(&trxw_group_rbuf), rbuf_write_amount(&trxw_group_rbuf));
fflush(stdout);
fflush(stderr);
exit(EXIT_FAILURE);
}
#define BURST_SIZE 16
#define TX_POOL_SIZE 16
int8_t tx_data[BURST_SIZE][TX_PACKET_SIZE];
struct rte_mbuf {
int buf_addr;
int data_off;
};
static void send_packets(int port) {
struct rte_mbuf * pkt[TX_POOL_SIZE];
struct rte_ether_hdr *eth_hdr;
uint16_t nb_tx = 0;
for(int i = 0; i < TX_POOL_SIZE; i++) {
int pkt_size;
pkt[i] = rte_pktmbuf_alloc(tx_mbuf_pool);
eth_hdr = rte_pktmbuf_mtod(pkt[i], struct rte_ether_hdr*);
if(port) {
eth_hdr->d_addr = s_addr;
eth_hdr->s_addr = d_addr;
} else {
eth_hdr->d_addr = d_addr;
eth_hdr->s_addr = s_addr;
}
eth_hdr->ether_type = htons(0xaefe);
memcpy(rte_pktmbuf_mtod_offset(pkt[i], uint8_t *, sizeof(struct rte_ether_hdr)), tx_data[i], TX_ECPRI_PACKET_SIZE);
pkt_size = TX_PACKET_SIZE;
pkt[i]->data_len = pkt_size;
pkt[i]->pkt_len = pkt_size;
}
while(nb_tx < TX_POOL_SIZE) {
int64_t x = TX_POOL_SIZE - nb_tx;
nb_tx += rte_eth_tx_burst(port, 0, pkt + nb_tx, x > BURST_SIZE ? BURST_SIZE : x);
}
/* Free any unsent packets. */
if (nb_tx < BURST_SIZE) {
uint16_t buf;
for (buf = nb_tx; buf < BURST_SIZE; buf++)
rte_pktmbuf_free(pkt[buf]);
log_exit("SEND_THREAD", "Sent %d packets instead of %d", nb_tx, BURST_SIZE);
}
}
static void init_counter(volatile counter_stat_t * c) {
c->counter = 0;
c->pps_counter = 0;
c->pps_ts = 0;
c->pps = 0;
}
static void update_counter_pps(volatile counter_stat_t * c) {
struct timespec _ts;
int64_t ts;
clock_gettime(CLOCK_TAI, &_ts);
ts = ts_to_int(_ts);
if((ts - c->pps_ts) > PPS_UPDATE_PERIOD) {
if(c->pps_ts)
c->pps = ((c->counter - c->pps_counter) * NSEC_PER_SEC) / (ts - c->pps_ts);
c->pps_counter = c->counter;
c->pps_ts = ts;
}
}
static void update_counter(volatile counter_stat_t * c, int64_t v) {
c->counter += v;
}
#ifdef TRACE
static void trace_handler(struct timespec initial, TRXEcpriState * s) {
struct timespec next;
if(tx_trace_ready && rx_trace_ready) {
int64_t d;
clock_gettime(CLOCK_TAI, &next);
d = calcdiff_ns(next, initial);
log_info("TRACE", "Packets sent: %" PRIi64, sent_counter.counter);
log_info("TRACE", "Duration: %" PRIi64, d);
log_info("TRACE", "FRAME_FREQ: %" PRIi64, FRAME_FREQ);
FILE * f;
char n[256];
uint8_t ones[14];
for(int i = 0; i < 14; i++)
ones[i] = 0xff;
memset(n, '\0', 256);
sprintf(n, "%s/tx.trace", s->log_directory);
f = fopen(n, "wb+");
log_info("TRACE", "Writing %d frames to tx.trace", tx_rbuf.write_index + tx_rbuf.buf_len - tx_trace_index_start);
for(int i = tx_trace_index_start; i != tx_rbuf.write_index; i = (i + 1) % tx_rbuf.buf_len) {
fwrite(ones, 14, 1, f);
fwrite(((uint8_t*) tx_rbuf.buffer) + i * tx_rbuf.len, tx_rbuf.len, 1, f);
}
fclose(f);
memset(n, '\0', 256);
sprintf(n, "%s/rx.trace", s->log_directory);
f = fopen(n, "wb+");
log_info("TRACE", "Writing %d frames to rx.trace", rx_rbuf.write_index);
for(int i = 0; i < rx_rbuf.write_index; i++) {
fwrite(((uint8_t*) rx_rbuf.buffer) + i * rx_rbuf.len, rx_rbuf.len, 1, f);
}
fclose(f);
memset(n, '\0', 256);
sprintf(n, "%s/trxw.trace", s->log_directory);
f = fopen(n, "wb+");
log_info("TRACE", "Writing %d frames to trxw.trace", trxw_rbuf[0].write_index);
for(int i = 0; i < trxw_rbuf[0].write_index; i++) {
for(int j = 0; j < TX_N_CHANNEL; j++)
fwrite((uint8_t *) (((Complex *) trxw_rbuf[j].buffer) + i * trxw_rbuf[0].len), trxw_rbuf[0].len * sizeof(Complex), 1, f);
}
fclose(f);
memset(n, '\0', 256);
sprintf(n, "%s/trxr.trace", s->log_directory);
f = fopen(n, "wb+");
log_info("TRACE", "Writing %d frames to trxr.trace", trxr_rbuf[0].write_index);
for(int i = 0; i < trxr_rbuf[0].write_index; i++) {
for(int j = 0; j < RX_N_CHANNEL; j++)
fwrite((uint8_t *) (((Complex *) trxr_rbuf[j].buffer) + i * trxr_rbuf[0].len), trxr_rbuf[0].len * sizeof(Complex), 1, f);
}
fclose(f);
log_exit("", "Finished tracing");
}
}
#endif
static void *recv_thread(void *p) {
cpu_set_t mask;
TRXEcpriState * s = (TRXEcpriState *) p;
int first_seq_id = 1;
log_info("RECV_THREAD", "Thread init");
// Set thread CPU affinity
CPU_ZERO(&mask);
CPU_SET(s->recv_affinity, &mask);
if (sched_setaffinity(0, sizeof(mask), &mask))
error(EXIT_FAILURE, errno, "Could not set CPU affinity to CPU %d\n", s->recv_affinity);
#define RTE_MBUF_SIZE 20000
#define MIN_RX 10000
for(;;) {
struct rte_mbuf * pkt[RTE_MBUF_SIZE];
uint8_t * buf, * rtebuf;
int port = 0;
int nb_rx = 0;
int n;
int drop_packet = 0;
while(!nb_rx)
nb_rx = recv_packets(port, 0, pkt + nb_rx, 1024);
n = rbuf_write_amount(&rx_rbuf);
drop_packet = nb_rx > n;
if(drop_packet) {
for(int i = 0; i < nb_rx; i++)
rte_pktmbuf_free(pkt[i]);
if(nb_rx)
update_counter(&rx_drop_counter, nb_rx);
}
else {
int nc; int nr;
nr = nb_rx;
while((nc = rbuf_contiguous_copy(NULL, &rx_rbuf, nr))) {
#ifdef TRACE
if((rx_rbuf.write_index + nc) >= rx_rbuf.buf_len) {
log_info("RECV_THREAD", "RX Trace ready");
rx_trace_ready = 1;
pthread_exit(EXIT_SUCCESS);
}
#endif
buf = ((uint8_t *) rx_rbuf.buffer) + (rx_rbuf.write_index * rx_rbuf.len);
for(int i = 0; i < nc; i++) {
rtebuf = (uint8_t *) (pkt[i])->buf_addr + (pkt[i])->data_off;
if(first_seq_id) {
uint16_t seq_id = htons(((uint16_t *) (rtebuf + 20))[0]);
printf("seq_id = %d\n", seq_id);
first_seq_id = 0;
}
memcpy(buf + i * rx_rbuf.len, rtebuf, rx_rbuf.len);
}
rx_rbuf.write_index = (rx_rbuf.write_index + nc) % rx_rbuf.buf_len;
for(int i = 0; i < nc; i++)
rte_pktmbuf_free(pkt[i]);
nr -= nc;
}
}
update_counter(&recv_counter, nb_rx);
}
pthread_exit(EXIT_SUCCESS);
}
// Send as soon as packets are encoded
// Signal to encode thread that packets has been sent
static void *send_thread(void *p) {
cpu_set_t mask;
struct timespec initial;
TRXEcpriState * s = (TRXEcpriState *) p;
log_info("SEND_THREAD", "Thread init");
// Set thread CPU affinity
CPU_ZERO(&mask);
CPU_SET(s->send_affinity, &mask);
if (sched_setaffinity(0, sizeof(mask), &mask))
error(EXIT_FAILURE, errno, "Could not set CPU affinity to CPU %d\n", s->send_affinity);
clock_gettime(CLOCK_TAI, &initial);
for(int64_t i = 1;; i++) {
int64_t n = rbuf_read_amount(&tx_rbuf);
if(n >= BURST_SIZE) {
int nb_burst = n / BURST_SIZE;
for(int j = 0; j < nb_burst; j++) {
for(int k = 0; k < BURST_SIZE; k++) {
memcpy(tx_data[k], RBUF_READ0(tx_rbuf, uint8_t), tx_rbuf.len);
rbuf_update_read_index(&tx_rbuf);
}
send_packets(0);
}
update_counter(&sent_counter, nb_burst * BURST_SIZE);
}
}
pthread_exit(EXIT_SUCCESS);
}
/*
If sync has happenned (=we have received frames):
Prepare as soon as TRX has packet to write
Signal
Else:
Prepare as soon as there is space in tx buffer
*/
#define TX_SYNC_BURST_SIZE 512
static void *encode_thread(void *p) {
cpu_set_t mask;
TRXEcpriState * s = (TRXEcpriState *) p;
struct timespec next;
int64_t target_counter = 0;
int reset_encode_counter = 1;
// Set thread CPU affinity
CPU_ZERO(&mask);
CPU_SET(s->encode_affinity, &mask);
if (sched_setaffinity(0, sizeof(mask), &mask))
error(EXIT_FAILURE, errno, "Could not set CPU affinity to CPU %d\n", s->encode_affinity);
for(int64_t i = 0;; i++) {
int n;
if(sync_complete && reset_encode_counter) {
encode_counter.counter = 0;
reset_encode_counter = 0;
seq_id = 0;
#ifdef TRACE
tx_trace_index_start = tx_rbuf.write_index;
#endif
}
// If we have frames to encode (is there space in TX buffer)
n = rbuf_write_amount(&tx_rbuf);
if(n) {
// If there are frames from trx_write callback to encode
if(rbuf_read_amount(&trxw_rbuf[0]) && rbuf_read_amount(&trxw_group_rbuf)) {
sample_group_t * g; int nb_frames;
g = RBUF_READ0(trxw_group_rbuf, sample_group_t);
if(g->wait) {
g->wait = 0;
g->count -= encode_counter.counter;
g->zeroes = 1;
}
nb_frames = g->count > n ? n : g->count;
g->count -= nb_frames;
#ifdef TRACE
if((encode_counter.counter + nb_frames) >= tx_rbuf.buf_len) {
log_info("ENCODE_THREAD", "TX Trace ready");
tx_trace_ready = 1;
pthread_exit(EXIT_SUCCESS);
}
#endif
if(g->zeroes) {
for(int j = 0; j < nb_frames; j++) {
memset(RBUF_WRITE0(tx_rbuf, uint8_t) + 8, 0x00, 240);
*((uint16_t *) (RBUF_WRITE0(tx_rbuf, uint8_t) + 6)) = htons(seq_id++);
rbuf_update_write_index(&tx_rbuf);
}
trxw_rbuf[0].read_index = (trxw_rbuf[0].read_index + nb_frames) % trxw_rbuf[0].buf_len;
} else {
int nc;
int nf = nb_frames;
while((nc = rbuf_contiguous_copy(&trxw_rbuf[0], &tx_rbuf, nf))) {
Complex * iq_samples[4];
uint8_t * buf = RBUF_WRITE0(tx_rbuf, uint8_t) + 8;
for(int j = 0; j < TX_N_CHANNEL; j++)
iq_samples[j] = ((Complex *) trxw_rbuf[j].buffer) + (trxw_rbuf[0].read_index * trxw_rbuf[0].len);
for(int i = 0; i < nc; i++) {
for(int i = 0; i < TX_N_CHANNEL ; i++)
encode_s64_b60_2(buf + i * 60, (float *) iq_samples[i]);
*((uint16_t *)(buf - 2)) = htons(seq_id++);
for(int j = 0; j < TX_N_CHANNEL; j++)
iq_samples[j] += trxw_rbuf[0].len;
buf += tx_rbuf.len;
}
tx_rbuf.write_index = (tx_rbuf.write_index + nc) % tx_rbuf.buf_len;
trxw_rbuf[0].read_index = (trxw_rbuf[0].read_index + nc) % trxw_rbuf[0].buf_len;
nf -= nc;
}
if(nf)
exit(EXIT_FAILURE);
}
update_counter(&encode_counter, nb_frames);
if(!g->count) {
rbuf_update_read_index(&trxw_group_rbuf);
}
}
else {
// Send empty frames until we receive something
#ifdef START_SENDING
if(!sync_complete) {
if(i == 0)
clock_gettime(CLOCK_TAI, &next);
// Limit packets sent
if(encode_counter.counter > target_counter) {
int k = (encode_counter.counter - target_counter + EFREQ - 1) / EFREQ;
add_ns(&next, k * 1000 * 1000 * 10); // 10ms to send 38400 packets
clock_nanosleep(CLOCK_TAI, TIMER_ABSTIME, &next, NULL);
target_counter += k * EFREQ;
}
n = (n > TX_SYNC_BURST_SIZE) ? n : TX_SYNC_BURST_SIZE;
n = (n < EFREQ) ? n : EFREQ;
for(int j = 0; j < n; j++) {
*((uint16_t *) (RBUF_WRITE0(tx_rbuf, uint8_t) + 6)) = htons(seq_id++);
rbuf_update_write_index(&tx_rbuf);
}
update_counter(&encode_counter, n);
}
#endif
}
}
}
pthread_exit(EXIT_SUCCESS);
}
static void *decode_thread(void *p) {
cpu_set_t mask;
TRXEcpriState * s = (TRXEcpriState *) p;
log_info("DECODE_THREAD", "Thread init");
// Set thread CPU affinity
CPU_ZERO(&mask);
CPU_SET(s->decode_affinity, &mask);
if (sched_setaffinity(0, sizeof(mask), &mask))
error(EXIT_FAILURE, errno, "Could not set CPU affinity to CPU %d\n", s->decode_affinity);
for(;;) {
int n;
while(!(n = rbuf_read_amount(&rx_rbuf)));
while(rbuf_write_amount(&trxr_rbuf[0]) < n);
int nc;
while((nc = rbuf_contiguous_copy(&rx_rbuf, &trxr_rbuf[0], n))) {
uint8_t * buf = ((uint8_t *) rx_rbuf.buffer) + (rx_rbuf.read_index * rx_rbuf.len) + 22;
#ifdef TRACE
if((trxr_rbuf[0].write_index + nc) >= trxr_rbuf[0].buf_len) {
rx_trace_ready = 1;
log_info("DECODE_THREAD", "RX Trace ready");
pthread_exit(EXIT_SUCCESS);
}
#endif
Complex * iq_samples[4];
for(int i = 0; i < RX_N_CHANNEL; i++)
iq_samples[i] = (((Complex *) trxr_rbuf[i].buffer) + (trxr_rbuf[0].write_index * trxr_rbuf[0].len));
for(int i = 0; i < nc; i++) {
for(int j = 0; j < RX_N_CHANNEL ; j++) {
decode_s64_b60_2((float *) (iq_samples[j] + i * 32), buf + j * 60 + i * rx_rbuf.len);
}
}
trxr_rbuf[0].write_index = (trxr_rbuf[0].write_index + nc) % trxr_rbuf[0].buf_len;
rx_rbuf.read_index = (rx_rbuf.read_index + nc) % rx_rbuf.buf_len;
n -= nc;
update_counter(&decode_counter, nc);
}
}
pthread_exit(EXIT_SUCCESS);
}
static void *statistic_thread(void *p) {
struct timespec next, initial;
int64_t recv_stop = 0;
cpu_set_t mask;
TRXEcpriState * s = (TRXEcpriState *) p;
FILE * stats_file_desc;
log_info("STATISTIC_THREAD", "Thread init");
char stats_file_name[256];
memset(stats_file_name, '\0', 256);
sprintf(stats_file_name, "%s/ecpri.stats", s->log_directory);
stats_file_desc = fopen(stats_file_name, "w+");
if(!stats_file_desc)
error(EXIT_FAILURE, errno, "Couldn't open %s\n", stats_file_name);
// Set thread CPU affinity
CPU_ZERO(&mask);
CPU_SET(s->statistic_affinity, &mask);
if (sched_setaffinity(0, sizeof(mask), &mask))
error(EXIT_FAILURE, errno, "Could not set CPU affinity to CPU %d\n", s->statistic_affinity);
clock_gettime(CLOCK_TAI, &initial);
next = initial;
for(int64_t i = 0;; i++) {
add_ns(&next, STATISTIC_REFRESH_RATE);
#ifdef TRACE
trace_handler(initial, s);
#endif
print_stats(stats_file_desc, (i % 50) == 0);
#ifdef DEBUG
fprintf(stats_file_desc,
"%d %d %d %d %d %d %d %d\n",
rx_rbuf.write_index,
rx_rbuf.read_index,
trxr_rbuf[0].write_index,
trxr_rbuf[0].read_index,
trxw_rbuf[0].write_index,
trxw_rbuf[0].read_index,
tx_rbuf.write_index,
tx_rbuf.read_index);
fprintf(stats_file_desc, "TRXW RBUF: ri %d wi %d ra %d wa %d\n", trxw_rbuf[0].read_index, trxw_rbuf[0].write_index, rbuf_read_amount(&trxw_rbuf[0]), rbuf_write_amount(&trxw_rbuf[0]));
#endif
fflush(stats_file_desc);
update_counter_pps(&rx_drop_counter);
update_counter_pps(&tx_drop_counter);
update_counter_pps(&recv_counter);
update_counter_pps(&decode_counter);
update_counter_pps(&read_counter);
update_counter_pps(&write_counter);
update_counter_pps(&encode_counter);
update_counter_pps(&sent_counter);
#ifdef MONITOR
if(recv_counter.pps < 3000000) {
struct timespec _ts;
int64_t ts;
clock_gettime(CLOCK_MONOTONIC, &_ts);
ts = ts_to_int(_ts);
if(sync_happened && (recv_stop && ((ts - recv_stop) > RECV_STOP_THRESHOLD * INT64_C(1000000000)))) {
log_info("MONITOR", "Stopped recieving packets, sending again...");
sync_complete = 0;
recv_stop = 0;
}
if(!recv_stop)
recv_stop = ts;
}
#endif
clock_nanosleep(CLOCK_TAI, TIMER_ABSTIME, &next, NULL);
}
pthread_exit(EXIT_SUCCESS);
}
static int start_threads(TRXEcpriState * s) {
pthread_t recv_pthread;
pthread_t send_pthread;
pthread_t encode_pthread;
pthread_t decode_pthread;
pthread_t statistic_pthread;
struct sched_param recv_param;
struct sched_param send_param;
struct sched_param encode_param;
struct sched_param decode_param;
struct sched_param statistic_param;
pthread_attr_t recv_attr;
pthread_attr_t send_attr;
pthread_attr_t encode_attr;
pthread_attr_t decode_attr;
pthread_attr_t statistic_attr;
log_info("TRX_ECPRI", "Starting threads");
// Initialize pthread attributes (default values)
if (pthread_attr_init(&recv_attr))
log_error("TRX_ECPRI", "init pthread attributes failed\n");
// Set a specific stack size
if (pthread_attr_setstacksize(&recv_attr, PTHREAD_STACK_MIN))
log_error("TRX_ECPRI", "pthread setstacksize failed\n");
// Set scheduler policy and priority of pthread
if (pthread_attr_setschedpolicy(&recv_attr, SCHED_FIFO))
log_error("TRX_ECPRI", "pthread setschedpolicy failed\n");
recv_param.sched_priority = 97;
if (pthread_attr_setschedparam(&recv_attr, &recv_param))
log_error("TRX_ECPRI", "pthread setschedparam failed\n");
/* Use scheduling parameters of attr */
if (pthread_attr_setinheritsched(&recv_attr, PTHREAD_EXPLICIT_SCHED))
log_error("TRX_ECPRI", "pthread setinheritsched failed\n");
if (pthread_attr_init(&send_attr))
log_error("TRX_ECPRI", "init pthread attributes failed\n");
if (pthread_attr_setstacksize(&send_attr, PTHREAD_STACK_MIN))
log_error("TRX_ECPRI", "pthread setstacksize failed\n");
if (pthread_attr_setschedpolicy(&send_attr, SCHED_FIFO))
log_error("TRX_ECPRI", "pthread setschedpolicy failed\n");
send_param.sched_priority = 97;
if (pthread_attr_setschedparam(&send_attr, &send_param))
log_error("TRX_ECPRI", "pthread setschedparam failed\n");
if (pthread_attr_setinheritsched(&send_attr, PTHREAD_EXPLICIT_SCHED))
log_error("TRX_ECPRI", "pthread setinheritsched failed\n");
if (pthread_attr_init(&encode_attr))
log_error("TRX_ECPRI", "init pthread attributes failed\n");
if (pthread_attr_setstacksize(&encode_attr, PTHREAD_STACK_MIN))
log_error("TRX_ECPRI", "pthread setstacksize failed\n");
if (pthread_attr_setschedpolicy(&encode_attr, SCHED_FIFO))
log_error("TRX_ECPRI", "pthread setschedpolicy failed\n");
encode_param.sched_priority = 97;
if (pthread_attr_setschedparam(&encode_attr, &encode_param))
log_error("TRX_ECPRI", "pthread setschedparam failed\n");
if (pthread_attr_setinheritsched(&encode_attr, PTHREAD_EXPLICIT_SCHED))
log_error("TRX_ECPRI", "pthread setinheritsched failed\n");
if (pthread_attr_init(&decode_attr))
log_error("TRX_ECPRI", "init pthread attributes failed\n");
if (pthread_attr_setstacksize(&decode_attr, PTHREAD_STACK_MIN))
log_error("TRX_ECPRI", "pthread setstacksize failed\n");
if (pthread_attr_setschedpolicy(&decode_attr, SCHED_FIFO))
log_error("TRX_ECPRI", "pthread setschedpolicy failed\n");
decode_param.sched_priority = 97;
if (pthread_attr_setschedparam(&decode_attr, &decode_param))
log_error("TRX_ECPRI", "pthread setschedparam failed\n");
if (pthread_attr_setinheritsched(&decode_attr, PTHREAD_EXPLICIT_SCHED))
log_error("TRX_ECPRI", "pthread setinheritsched failed\n");
if (pthread_attr_init(&statistic_attr))
log_error("TRX_ECPRI", "init pthread attributes failed\n");
if (pthread_attr_setstacksize(&statistic_attr, PTHREAD_STACK_MIN))
log_error("TRX_ECPRI", "pthread setstacksize failed\n");
if (pthread_attr_setschedpolicy(&statistic_attr, SCHED_FIFO))
log_error("TRX_ECPRI", "pthread setschedpolicy failed\n");
statistic_param.sched_priority = 97;
if (pthread_attr_setschedparam(&statistic_attr, &statistic_param))
log_error("TRX_ECPRI", "pthread setschedparam failed\n");
if (pthread_attr_setinheritsched(&statistic_attr, PTHREAD_EXPLICIT_SCHED))
log_error("TRX_ECPRI", "pthread setinheritsched failed\n");
if (pthread_create(&statistic_pthread, NULL, statistic_thread, s))
error(EXIT_FAILURE, errno, "Couldn't create statistic thread");
usleep(1000 * 20);
if (pthread_create(&encode_pthread, NULL, encode_thread, s))
error(EXIT_FAILURE, errno, "Couldn't create encode thread");
usleep(1000 * 20);
if (pthread_create(&decode_pthread, NULL, decode_thread, s))
error(EXIT_FAILURE, errno, "Couldn't create decode thread");
usleep(1000 * 20);
if (pthread_create(&send_pthread, NULL, send_thread, s))
error(EXIT_FAILURE, errno, "Couldn't create send thread");
usleep(1000 * 500);
if (pthread_create(&recv_pthread, NULL, recv_thread, s))
error(EXIT_FAILURE, errno, "Couldn't create recv thread");
return 0;
}
int startdpdk(TRXEcpriState * s) {
uint8_t ecpri_message[TX_ECPRI_PACKET_SIZE];
int argc = 1;
int k = 1;
int prev_space = -1;
char ** argv;
for(int i = 0;; i++) {
if(s->dpdk_options[i] == ' ')
argc++;
else if(s->dpdk_options[i] == '\0')
break;
}
argv = (char **) malloc(sizeof(char *) * argc);
for(int i = 0;; i++) {
if(s->dpdk_options[i] == ' ') {
argv[k] = (char *) malloc(i - prev_space);
strncpy(argv[k], s->dpdk_options + prev_space + 1, i - prev_space -1);
argv[k][i - prev_space-1] = '\0';
prev_space = i;
k++;
}
else if(s->dpdk_options[i] == '\0') {
break;
}
}
argv[0] = "";
log_info("TRX_ECPRI", "Start");
//set_latency_target();
seq_id = 0;
init_counter(&rx_drop_counter);
init_counter(&tx_drop_counter);
init_counter(&recv_counter);
init_counter(&decode_counter);
init_counter(&read_counter);
init_counter(&write_counter);
init_counter(&encode_counter);
init_counter(&sent_counter);
RBUF_INIT(rx_rbuf, "RX ring buffer", TXRX_BUF_MAX_SIZE, RX_MAX_PACKET_SIZE, uint8_t);
RBUF_INIT(tx_rbuf, "TX ring buffer", TXRX_BUF_MAX_SIZE, TX_ECPRI_PACKET_SIZE, uint8_t);
for(int i = 0; i < TX_N_CHANNEL; i++) {
char s[256];
sprintf(s, "TRXWrite Ring Buffer %d", i);
RBUF_INIT(trxw_rbuf[i], s, TRX_BUF_MAX_SIZE, N_SAMPLES, Complex);
}
for(int i = 0; i < RX_N_CHANNEL; i++) {
char s[256];
sprintf(s, "TRXRead Ring Buffer %d", i);
RBUF_INIT(trxr_rbuf[i], s, TRX_BUF_MAX_SIZE, N_SAMPLES, Complex);
}
RBUF_INIT(trxw_group_rbuf, "TRXGroupWrite ring buffer", TRX_MAX_GROUP, 1, sample_group_t);
memset((uint8_t *) ecpri_message, 0, TX_ECPRI_PACKET_SIZE);
#ifdef DPDK
if(sscanf((char *) s->re_mac, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx%*c",
&d_addr.addr_bytes[0],
&d_addr.addr_bytes[1],
&d_addr.addr_bytes[2],
&d_addr.addr_bytes[3],
&d_addr.addr_bytes[4],
&d_addr.addr_bytes[5]) != 6)
fprintf(stderr, "Invalid eRE MAC address\n");
if(sscanf((char *) s->rec_mac, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx%*c",
&s_addr.addr_bytes[0],
&s_addr.addr_bytes[1],
&s_addr.addr_bytes[2],
&s_addr.addr_bytes[3],
&s_addr.addr_bytes[4],
&s_addr.addr_bytes[5]) != 6)
fprintf(stderr, "Invalid eREC MAC address\n");
#endif
/* Standard Header */
ecpri_message[0] = 0x10; // Protocol data revision 0x1, C = 0
// Message type = 0x00, IQ data
// Payload size
*((uint16_t *) (ecpri_message + 2)) = htons(244);
*((uint16_t *) (ecpri_message + 4)) = htons(s->flow_id);
for(int i = 0; i < tx_rbuf.buf_len; i++)
memcpy(((uint8_t *) tx_rbuf.buffer) + (i * tx_rbuf.len), ecpri_message, tx_rbuf.len);
start_threads(s);
return 0;
}
static void trx_ecpri_end(TRXState *s1)
{
log_info("TRX_ECPRI", "End");
TRXEcpriState *s = s1->opaque;
free(s);
}
int64_t prev_ts = 0;
int64_t prev_count = 0;
#define M 32
static void trx_ecpri_write(TRXState *s1, trx_timestamp_t timestamp, const void **__samples, int count, int tx_port_index, TRXWriteMetadata *md)
{
(void) s1;
int write_count; int64_t ts; sample_group_t * g; int nc;
float ** _samples = (float **) __samples;
write_count = count / M;
ts = timestamp / M;
log_debug("TRX_ECPRI_WRITE", "trx_ecpri_write, count = %ld", count);
if(prev_count && ((ts - prev_ts) != prev_count)) {
log_exit("TRX_ECPRI_WRITE", "Gap between timestamps: prev_ts %li ts %li prev_count %li count %li diff_ts %li", prev_ts, ts, prev_count, count, (ts - prev_ts));
}
prev_ts = ts; prev_count = write_count;
if(write_count > rbuf_write_amount(&trxw_rbuf[0])) {
//log_exit("TRX_ECPRI_WRITE", "Not enough space to write in trxw_rbuf (write count = %d)", write_count);
update_counter(&tx_drop_counter, write_count);
return;
}
#ifdef TRACE
if((trxw_rbuf[0].write_index + write_count) >= trxw_rbuf[0].buf_len) {
log_info("TRX_ECPRI_WRITE", "TX Trace ready");
tx_trace_ready = 1;
pthread_exit(EXIT_SUCCESS);
}
#endif
if(first_trx_write) {
sample_group_t * g2 = RBUF_WRITE0(trxw_group_rbuf, sample_group_t);
g2->count = ts;
g2->wait = 1;
g2->zeroes = 1;
rbuf_update_write_index(&trxw_group_rbuf);
}
g = RBUF_WRITE0(trxw_group_rbuf, sample_group_t);
g->zeroes = __samples ? 0 : 1;
g->wait = 0;
g->count = write_count;
while((nc = rbuf_contiguous_copy(NULL, &trxw_rbuf[0], write_count))) {
if(__samples)
for(int i = 0; i < TX_N_CHANNEL; i++)
memcpy(((uint8_t *) trxw_rbuf[i].buffer) + trxw_rbuf[0].write_index * trxw_rbuf[0].len * sizeof(Complex), (uint8_t*) _samples[i], nc * trxw_rbuf[0].len * sizeof(Complex));
trxw_rbuf[0].write_index = (trxw_rbuf[0].write_index + nc) % trxw_rbuf[0].buf_len;
write_count -= nc;
}
rbuf_update_write_index(&trxw_group_rbuf);
update_counter(&write_counter, count / M);
}
static int trx_ecpri_read(TRXState *s1, trx_timestamp_t *ptimestamp, void **__samples, int count, int rx_port_index, TRXReadMetadata *md)
{
(void) s1;
int nc; int n;
float ** _samples = (float **) __samples;
int read_count = (count / M);
int offset = 0;
while(rbuf_read_amount(&trxr_rbuf[0]) < read_count);
log_debug("TRX_ECPRI_READ", "count = %ld (%li)", read_count, read_counter.counter);
sync_complete = 1;
sync_happened = 1;
n = read_count;
while((nc = rbuf_contiguous_copy(&trxr_rbuf[0], NULL, n))) {
int len = nc * trxr_rbuf[0].len * sizeof(Complex);
for(int i = 0; i < RX_N_CHANNEL; i++ ) {
memcpy((uint8_t*) (_samples[i] + offset), ((uint8_t *) trxr_rbuf[i].buffer) + trxr_rbuf[0].read_index * trxr_rbuf[0].len * sizeof(Complex), len);
}
trxr_rbuf[0].read_index = (trxr_rbuf[0].read_index + nc) % trxr_rbuf[0].buf_len;
n -= nc;
offset += len;
}
*ptimestamp = recv_counter.counter * M;
update_counter(&read_counter, read_count);
return count;
}
/* This function can be used to automatically set the sample
rate. Here we don't implement it, so the user has to force a given
sample rate with the "sample_rate" configuration option */
static int trx_ecpri_get_sample_rate(TRXState *s1, TRXFraction *psample_rate,
int *psample_rate_num, int sample_rate_min)
{
return -1;
}
static int trx_ecpri_start(TRXState *s1, const TRXDriverParams *params)
{
TRXEcpriState *s = s1->opaque;
log_info("TRX_ECPRI_START", "Start");
log_info("TRX_ECPRI_START", "trx_api_version: %d", s1->trx_api_version);
log_info("TRX_ECPRI_START", "config file: %s", s1->path);
log_info("TEST-DPDK-ECPRI", "rec-mac: %s, re-mac: %s, rec-if: %s", s->rec_mac, s->re_mac, s->rec_if);
s->sample_rate = params->sample_rate[0].num / params->sample_rate[0].den;
startdpdk(s);
return 0;
}
void dummy_enb_init(TRXState *s1, TRXEcpriState *s) {
s1->trx_write_func2 = trx_ecpri_write;
s1->trx_read_func2 = trx_ecpri_read;
startdpdk(s);
}
int trx_driver_init(TRXState *s1)
{
TRXEcpriState *s;
double val;
// Lock all current and future pages from preventing of being paged to swap
if (mlockall(MCL_CURRENT | MCL_FUTURE)) {
log_error("TRX_ECPRI", "mlockall failed");
}
log_info("TRX_ECPRI", "Init");
if (s1->trx_api_version != TRX_API_VERSION) {
fprintf(stderr, "ABI compatibility mismatch between LTEENB and TRX driver (LTEENB ABI version=%d, TRX driver ABI version=%d)\n",
s1->trx_api_version, TRX_API_VERSION);
return -1;
}
s = malloc(sizeof(TRXEcpriState));
memset(s, 0, sizeof(*s));
trx_get_param_double(s1, &val, "recv_affinity");
s->recv_affinity = (int) val;
trx_get_param_double(s1, &val, "send_affinity");
s->send_affinity = (int) val;
trx_get_param_double(s1, &val, "encode_affinity");
s->encode_affinity = (int) val;
trx_get_param_double(s1, &val, "decode_affinity");
s->decode_affinity = (int) val;
trx_get_param_double(s1, &val, "statistic_affinity");
s->statistic_affinity = (int) val;
trx_get_param_double(s1, &val, "flow_id");
s->flow_id = (int) val;
trx_get_param_double(s1, &val, "ecpri_period");
if(((int) val) == 0) {
fprintf(stderr, "ecpri_period parameter can't be null\n");
return -1;
}
s->ecpri_period = (int) val;
s->re_mac = (uint8_t *) trx_get_param_string(s1, "re_mac");
s->rec_mac = (uint8_t *) trx_get_param_string(s1, "rec_mac");
s->rec_if = (uint8_t *) trx_get_param_string(s1, "rec_if");
s->dpdk_options = trx_get_param_string(s1, "dpdk_options");
s->log_directory = (uint8_t *) trx_get_param_string(s1, "log_directory");
s1->opaque = s;
s1->trx_end_func = trx_ecpri_end;
s1->trx_write_func2 = trx_ecpri_write;
s1->trx_read_func2 = trx_ecpri_read;
s1->trx_start_func = trx_ecpri_start;
s1->trx_get_sample_rate_func = trx_ecpri_get_sample_rate;
return 0;
}
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment