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);
 }