Transceiver52M: Replace resampler with SSE enabled implementation

Replace the polyphase filter and resampler with a separate
implementation using SSE enabled convolution. The USRP2 (including
derived devices N200, N210) are the only supported devices that
require sample rate conversion, so set the default resampling
parameters for the 100 MHz FPGA clock. This changes the previous
resampling ratios.

  270.833 kHz -> 400 kHz      (65 / 96)
  270.833 kHz -> 390.625 kHz  (52 / 75)

The new resampling factor uses a USRP resampling factor of 256
instead of 250. On the device, this allows two halfband filters to
be used rather than one. The end result is reduced distortial and
aliasing effecits from CIC filter rolloff.

B100 and USRP1 will no be supported at 400 ksps with these changes.

Signed-off-by: Thomas Tsou <tom@tsou.cc>

1 files changed, 146 insertions, 249 deletions

diff --git a/Transceiver52M/radioInterfaceResamp.cpp b/Transceiver52M/radioInterfaceResamp.cpp
index c7f17ea..d3dc82e 100644
--- a/Transceiver52M/radioInterfaceResamp.cpp
+++ b/Transceiver52M/radioInterfaceResamp.cpp
@@ -1,8 +1,8 @@
 /*
  * Radio device interface with sample rate conversion
- * Written by Thomas Tsou <ttsou@vt.edu>
+ * Written by Thomas Tsou <tom@tsou.cc>
  *
- * Copyright 2011 Free Software Foundation, Inc.
+ * Copyright 2011, 2012, 2013 Free Software Foundation, Inc.
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU Affero General Public License as published by
@@ -22,6 +22,12 @@
 #include <radioInterface.h>
 #include <Logger.h>
 
+#include "Resampler.h"
+
+extern "C" {
+#include "convert.h"
+}
+
 /* New chunk sizes for resampled rate */
 #ifdef INCHUNK
   #undef INCHUNK
@@ -30,303 +36,194 @@
   #undef OUTCHUNK
 #endif
 
-/* Resampling parameters */
-#define INRATE       65 * SAMPSPERSYM
-#define INHISTORY    INRATE * 2
-#define INCHUNK      INRATE * 9
-
-#define OUTRATE      96 * SAMPSPERSYM
-#define OUTHISTORY   OUTRATE * 2
-#define OUTCHUNK     OUTRATE * 9
-
-/* Resampler low pass filters */
-signalVector *tx_lpf = 0;
-signalVector *rx_lpf = 0;
+/* Resampling parameters for 100 MHz clocking */
+#define RESAMP_INRATE			52
+#define RESAMP_OUTRATE			75
+#define RESAMP_FILT_LEN			16
 
-/* Resampler history */
-signalVector *tx_hist = 0;
-signalVector *rx_hist = 0;
+#define INCHUNK				(RESAMP_INRATE * 4)
+#define OUTCHUNK			(RESAMP_OUTRATE * 4)
 
-/* Resampler input buffer */
-signalVector *tx_vec = 0;
-signalVector *rx_vec = 0;
+static Resampler *upsampler = NULL;
+static Resampler *dnsampler = NULL;
+short *convertRecvBuffer = NULL;
+short *convertSendBuffer = NULL;
 
-/*
- * High rate (device facing) buffers
- *
- * Transmit side samples are pushed after each burst so accomodate
- * a resampled burst plus up to a chunk left over from the previous
- * resampling operation.
- *
- * Receive side samples always pulled with a fixed size.
- */
-short tx_buf[INCHUNK * 2 * 4];
-short rx_buf[OUTCHUNK * 2 * 2];
-
-/* 
- * Utilities and Conversions 
- *
- * Manipulate signal vectors dynamically for two reasons. For one,
- * it's simpler. And two, it doesn't make any reasonable difference
- * relative to the high overhead generated by the resampling.
- */
-
-/* Concatenate signal vectors. Deallocate input vectors. */
-signalVector *concat(signalVector *a, signalVector *b)
+/* Complex float to short conversion */
+static void floatToShort(short *out, float *in, int num)
 {
-	signalVector *vec = new signalVector(*a, *b);
-	delete a;
-	delete b;
-
-	return vec;
+  for (int i = 0; i < num; i++)
+    out[i] = (short) in[i];
 }
 
-/* Segment a signal vector. Deallocate the input vector. */
-signalVector *segment(signalVector *a, int indx, int sz)
+/* Complex short to float conversion */
+static void shortToFloat(float *out, short *in, int num)
 {
-	signalVector *vec = new signalVector(sz);
-	a->segmentCopyTo(*vec, indx, sz);
-	delete a;
-
-	return vec;
+  for (int i = 0; i < num; i++)
+    out[i] = (float) in[i];
 }
 
-/* Create a new signal vector from a short array. */
-signalVector *short_to_sigvec(short *smpls, size_t sz)
+RadioInterfaceResamp::RadioInterfaceResamp(RadioDevice *wRadio,
+					   int wReceiveOffset,
+					   int wSPS,
+					   GSM::Time wStartTime)
+	: RadioInterface(wRadio, wReceiveOffset, wSPS, wStartTime),
+	  innerSendBuffer(NULL), outerSendBuffer(NULL),
+	  innerRecvBuffer(NULL), outerRecvBuffer(NULL)
 {
-	int i;
-	signalVector *vec = new signalVector(sz);
-	signalVector::iterator itr = vec->begin();
-
-	for (i = 0; i < sz; i++) {
-		*itr++ = Complex<float>(smpls[2 * i + 0], smpls[2 * i + 1]);
-	}
-
-	return vec;
 }
 
-/* Convert and deallocate a signal vector into a short array. */
-int sigvec_to_short(signalVector *vec, short *smpls)
+RadioInterfaceResamp::~RadioInterfaceResamp()
 {
-	int i;
-	signalVector::iterator itr = vec->begin();
-
-	for (i = 0; i < vec->size(); i++) {
-		smpls[2 * i + 0] = itr->real();
-		smpls[2 * i + 1] = itr->imag();
-		itr++;
-	}
-	delete vec;
-
-	return i;
+	close();
 }
 
-/* Create a new signal vector from a float array. */
-signalVector *float_to_sigvec(float *smpls, int sz)
+void RadioInterfaceResamp::close()
 {
-	int i;
-	signalVector *vec = new signalVector(sz);
-	signalVector::iterator itr = vec->begin();
+	RadioInterface::close();
 
-	for (i = 0; i < sz; i++) {
-		*itr++ = Complex<float>(smpls[2 * i + 0], smpls[2 * i + 1]);
-	}
-
-	return vec;
-}
+	delete innerSendBuffer;
+	delete outerSendBuffer;
+	delete innerRecvBuffer;
+	delete outerRecvBuffer;
 
-/* Convert and deallocate a signal vector into a float array. */
-int sigvec_to_float(signalVector *vec, float *smpls)
-{
-	int i;
-	signalVector::iterator itr = vec->begin();
+	delete upsampler;
+	delete dnsampler;
 
-	for (i = 0; i < vec->size(); i++) {
-		smpls[2 * i + 0] = itr->real();
-		smpls[2 * i + 1] = itr->imag();
-		itr++;
-	}
-	delete vec;
+	innerSendBuffer = NULL;
+	outerSendBuffer = NULL;
+	innerRecvBuffer = NULL;
+	outerRecvBuffer = NULL;
 
-	return i;
+	upsampler = NULL;
+	dnsampler = NULL;
 }
 
-/* Initialize resampling signal vectors */
-void init_resampler(signalVector **lpf,
-	   	    signalVector **buf,
-		    signalVector **hist,
-		    int tx)
+/* Initialize I/O specific objects */
+bool RadioInterfaceResamp::init()
 {
-	int P, Q, taps, hist_len;
-	float cutoff_freq;
-
-	if (tx) {
-		LOG(INFO) << "Initializing Tx resampler";
-		P = OUTRATE;
-		Q = INRATE;
-		taps = 651;
-		hist_len = INHISTORY;
-	} else {
-		LOG(INFO) << "Initializing Rx resampler";
-		P = INRATE;
-		Q = OUTRATE;
-		taps = 961;
-		hist_len = OUTHISTORY;
+	float cutoff = 1.0f;
+
+	close();
+
+	/*
+	 * With oversampling, restrict bandwidth to 150% of base rate. This also
+	 * provides last ditch bandwith limiting if the pulse shaping filter is
+	 * insufficient.
+	 */
+	if (sps > 1)
+		cutoff = 1.5 / sps;
+
+	dnsampler = new Resampler(RESAMP_INRATE, RESAMP_OUTRATE);
+	if (!dnsampler->init(cutoff)) {
+		LOG(ALERT) << "Rx resampler failed to initialize";
+		return false;
 	}
 
-	if (!*lpf) {
-		cutoff_freq = (P < Q) ? (1.0/(float) Q) : (1.0/(float) P);
-		*lpf = createLPF(cutoff_freq, taps, P);
+	upsampler = new Resampler(RESAMP_OUTRATE, RESAMP_INRATE);
+	if (!upsampler->init(cutoff)) {
+		LOG(ALERT) << "Tx resampler failed to initialize";
+		return false;
 	}
 
-	if (!*buf) {
-		*buf = new signalVector();
-	}
+	/*
+	 * Allocate high and low rate buffers. The high rate receive
+	 * buffer and low rate transmit vectors feed into the resampler
+	 * and requires headroom equivalent to the filter length. Low
+	 * rate buffers are allocated in the main radio interface code.
+	 */
+	innerSendBuffer = new signalVector(INCHUNK * 20, RESAMP_FILT_LEN);
+	outerSendBuffer = new signalVector(OUTCHUNK * 20);
 
-	if (!*hist);
-		*hist = new signalVector(hist_len);
-}
+	outerRecvBuffer = new signalVector(OUTCHUNK * 2, RESAMP_FILT_LEN);
+	innerRecvBuffer = new signalVector(INCHUNK * 20);
 
-/* Resample a signal vector
- *
- * The input vector is deallocated and the pointer returned with a vector
- * of any unconverted samples.
- */
-signalVector *resmpl_sigvec(signalVector *hist, signalVector **vec,
-			    signalVector *lpf, double in_rate,
-			    double out_rate, int chunk_sz)
-{
-	signalVector *resamp_vec;
-	int num_chunks = (*vec)->size() / chunk_sz;
-
-	/* Truncate to a chunk multiple */
-	signalVector trunc_vec(num_chunks * chunk_sz);
-	(*vec)->segmentCopyTo(trunc_vec, 0, num_chunks * chunk_sz);
-
-	/* Update sample buffer with remainder */
-	*vec = segment(*vec, trunc_vec.size(), (*vec)->size() - trunc_vec.size());
-
-	/* Add history and resample */
-	signalVector input_vec(*hist, trunc_vec);
-	resamp_vec = polyphaseResampleVector(input_vec, in_rate,
-					     out_rate, lpf);
-
-	/* Update history */
-	trunc_vec.segmentCopyTo(*hist, trunc_vec.size() - hist->size(),
-				hist->size());
-	return resamp_vec;
-}
-
-/* Wrapper for receive-side integer-to-float array resampling */
- int rx_resmpl_int_flt(float *smpls_out, short *smpls_in, int num_smpls)
-{
-	int num_resmpld, num_chunks;
-	signalVector *convert_vec, *resamp_vec, *trunc_vec;
-
-	if (!rx_lpf || !rx_vec || !rx_hist)
-		init_resampler(&rx_lpf, &rx_vec, &rx_hist, false);
-
-	/* Convert and add samples to the receive buffer */
-	convert_vec = short_to_sigvec(smpls_in, num_smpls);
-	rx_vec = concat(rx_vec, convert_vec);
-
-	num_chunks = rx_vec->size() / OUTCHUNK;
-	if (num_chunks < 1)
-		return 0;
-
-	/* Resample */ 
-	resamp_vec = resmpl_sigvec(rx_hist, &rx_vec, rx_lpf,
-				   INRATE, OUTRATE, OUTCHUNK);
-	/* Truncate */
-	trunc_vec = segment(resamp_vec, INHISTORY,
-                            resamp_vec->size() - INHISTORY);
-	/* Convert */
-	num_resmpld = sigvec_to_float(trunc_vec, smpls_out);
-
-	return num_resmpld; 
-}
-
-/* Wrapper for transmit-side float-to-int array resampling */
-int tx_resmpl_flt_int(short *smpls_out, float *smpls_in, int num_smpls)
-{
-	int num_resmpl, num_chunks;
-	signalVector *convert_vec, *resamp_vec;
-
-	if (!tx_lpf || !tx_vec || !tx_hist)
-		init_resampler(&tx_lpf, &tx_vec, &tx_hist, true);
-
-	/* Convert and add samples to the transmit buffer */
-	convert_vec = float_to_sigvec(smpls_in, num_smpls);
-	tx_vec = concat(tx_vec, convert_vec);
-
-	num_chunks = tx_vec->size() / INCHUNK;
-	if (num_chunks < 1)
-		return 0;
-
-	/* Resample and convert to an integer array */
-	resamp_vec = resmpl_sigvec(tx_hist, &tx_vec, tx_lpf,
-				   OUTRATE, INRATE, INCHUNK);
-	num_resmpl = sigvec_to_short(resamp_vec, smpls_out);
+	convertSendBuffer = new short[OUTCHUNK * 2 * 20];
+	convertRecvBuffer = new short[OUTCHUNK * 2 * 2];
 
-	return num_resmpl; 
-}
+	sendBuffer = innerSendBuffer;
+	recvBuffer = innerRecvBuffer;
 
-RadioInterfaceResamp::RadioInterfaceResamp(RadioDevice *wRadio,
-					   int wReceiveOffset,
-					   int wSPS,
-					   GSM::Time wStartTime)
-	: RadioInterface(wRadio, wReceiveOffset, wSPS, wStartTime)
-{
+	return true;
 }
 
-/* Receive a timestamped chunk from the device */ 
+/* Receive a timestamped chunk from the device */
 void RadioInterfaceResamp::pullBuffer()
 {
-	int num_cv, num_rd;
 	bool local_underrun;
+	int rc, num_recv;
+	int inner_len = INCHUNK;
+	int outer_len = OUTCHUNK;
+
+	/* Outer buffer access size is fixed */
+	num_recv = mRadio->readSamples(convertRecvBuffer,
+				       outer_len,
+				       &overrun,
+				       readTimestamp,
+				       &local_underrun);
+	if (num_recv != outer_len) {
+		LOG(ALERT) << "Receive error " << num_recv;
+		return;
+	}
 
-	/* Read samples. Fail if we don't get what we want. */
-	num_rd = mRadio->readSamples(rx_buf, OUTCHUNK, &overrun,
-				     readTimestamp, &local_underrun);
-
-	LOG(DEBUG) << "Rx read " << num_rd << " samples from device";
-	assert(num_rd == OUTCHUNK);
+	shortToFloat((float *) outerRecvBuffer->begin(),
+		     convertRecvBuffer, 2 * outer_len);
 
 	underrun |= local_underrun;
-	readTimestamp += (TIMESTAMP) num_rd;
-
-	/* Convert and resample */
-	num_cv = rx_resmpl_int_flt(rcvBuffer + 2 * rcvCursor,
-				   rx_buf, num_rd);
-
-	LOG(DEBUG) << "Rx read " << num_cv << " samples from resampler";
+	readTimestamp += (TIMESTAMP) num_recv;
+
+	/* Write to the end of the inner receive buffer */
+	rc = dnsampler->rotate((float *) outerRecvBuffer->begin(), outer_len,
+			       (float *) (innerRecvBuffer->begin() + recvCursor),
+			       inner_len);
+	if (rc < 0) {
+		LOG(ALERT) << "Sample rate upsampling error";
+	}
 
-	rcvCursor += num_cv;
+	recvCursor += inner_len;
 }
 
-/* Send a timestamped chunk to the device */ 
+/* Send a timestamped chunk to the device */
 void RadioInterfaceResamp::pushBuffer()
 {
-	int num_cv, num_wr;
+	int rc, chunks, num_sent;
+	int inner_len, outer_len;
 
 	if (sendCursor < INCHUNK)
 		return;
 
-	LOG(DEBUG) << "Tx wrote " << sendCursor << " samples to resampler";
+	chunks = sendCursor / INCHUNK;
+	if (chunks > 8)
+		chunks = 8;
 
-	/* Resample and convert */
-	num_cv = tx_resmpl_flt_int(tx_buf, sendBuffer, sendCursor);
-	assert(num_cv > sendCursor);
+	inner_len = chunks * INCHUNK;
+	outer_len = chunks * OUTCHUNK;
 
-	/* Write samples. Fail if we don't get what we want. */
-	num_wr = mRadio->writeSamples(tx_buf + OUTHISTORY * 2,
-				      num_cv - OUTHISTORY,
-				      &underrun,
-				      writeTimestamp);
+	/* Always send from the beginning of the buffer */
+	rc = upsampler->rotate((float *) innerSendBuffer->begin(), inner_len,
+			       (float *) outerSendBuffer->begin(), outer_len);
+	if (rc < 0) {
+		LOG(ALERT) << "Sample rate downsampling error";
+	}
+
+	floatToShort(convertSendBuffer,
+		     (float *) outerSendBuffer->begin(),
+		     2 * outer_len);
+
+	num_sent = mRadio->writeSamples(convertSendBuffer,
+					outer_len,
+					&underrun,
+					writeTimestamp);
+	if (num_sent != outer_len) {
+		LOG(ALERT) << "Transmit error " << num_sent;
+	}
 
-	LOG(DEBUG) << "Tx wrote " << num_wr << " samples to device";
-	assert(num_wr == num_wr);
+	/* Shift remaining samples to beginning of buffer */
+	memmove(innerSendBuffer->begin(),
+		innerSendBuffer->begin() + inner_len,
+		(sendCursor - inner_len) * 2 * sizeof(float));
 
-	writeTimestamp += (TIMESTAMP) num_wr;
-	sendCursor = 0;
+	writeTimestamp += outer_len;
+	sendCursor -= inner_len;
+	assert(sendCursor >= 0);
 }