SoapySDR uses time stamps to prevent gaps/overflows in transmit stream

A common option for both UHD and SoapySDR allows to turn off time stamps.

7 files changed, 128 insertions, 46 deletions

diff --git a/src/libsdr/sdr.c b/src/libsdr/sdr.c
index 73107c0..e2d3958 100644
--- a/src/libsdr/sdr.c
+++ b/src/libsdr/sdr.c
@@ -471,7 +471,7 @@ void *sdr_open(const char __attribute__((__unused__)) *audiodev, double *tx_freq
 
 #ifdef HAVE_UHD
 	if (sdr_config->uhd) {
-		rc = uhd_open(sdr_config->channel, sdr_config->device_args, sdr_config->stream_args, sdr_config->tune_args, sdr_config->tx_antenna, sdr_config->rx_antenna, sdr_config->clock_source, tx_center_frequency, rx_center_frequency, sdr_config->lo_offset, sdr_config->samplerate, sdr_config->tx_gain, sdr_config->rx_gain, sdr_config->bandwidth, sdr_config->uhd_tx_timestamps);
+		rc = uhd_open(sdr_config->channel, sdr_config->device_args, sdr_config->stream_args, sdr_config->tune_args, sdr_config->tx_antenna, sdr_config->rx_antenna, sdr_config->clock_source, tx_center_frequency, rx_center_frequency, sdr_config->lo_offset, sdr_config->samplerate, sdr_config->tx_gain, sdr_config->rx_gain, sdr_config->bandwidth, sdr_config->timestamps);
 		if (rc)
 			goto error;
 	}
@@ -479,7 +479,7 @@ void *sdr_open(const char __attribute__((__unused__)) *audiodev, double *tx_freq
 
 #ifdef HAVE_SOAPY
 	if (sdr_config->soapy) {
-		rc = soapy_open(sdr_config->channel, sdr_config->device_args, sdr_config->stream_args, sdr_config->tune_args, sdr_config->tx_antenna, sdr_config->rx_antenna, sdr_config->clock_source, tx_center_frequency, rx_center_frequency, sdr_config->lo_offset, sdr_config->samplerate, sdr_config->tx_gain, sdr_config->rx_gain, sdr_config->bandwidth);
+		rc = soapy_open(sdr_config->channel, sdr_config->device_args, sdr_config->stream_args, sdr_config->tune_args, sdr_config->tx_antenna, sdr_config->rx_antenna, sdr_config->clock_source, tx_center_frequency, rx_center_frequency, sdr_config->lo_offset, sdr_config->samplerate, sdr_config->tx_gain, sdr_config->rx_gain, sdr_config->bandwidth, sdr_config->timestamps);
 		if (rc)
 			goto error;
 	}
@@ -997,10 +997,11 @@ int sdr_get_tosend(void *inst, int latspl)
 #endif
 	if (count < 0)
 		return count;
+	/* rounding down, so we never overfill */
 	count /= sdr->oversample;
 
 	if (sdr->threads) {
-		/* subtract what we have in write buffer, because this is not jent sent to the SDR */
+		/* subtract what we have in write buffer, because this is not jet sent to the SDR */
 		int fill;
 
 		fill = (sdr->thread_write.in - sdr->thread_write.out + sdr->thread_write.buffer_size) % sdr->thread_write.buffer_size;
diff --git a/src/libsdr/sdr_config.c b/src/libsdr/sdr_config.c
index 61cf87c..6486641 100644
--- a/src/libsdr/sdr_config.c
+++ b/src/libsdr/sdr_config.c
@@ -41,6 +41,7 @@ void sdr_config_init(double lo_offset)
 	sdr_config->stream_args = "";
 	sdr_config->tune_args = "";
 	sdr_config->lo_offset = lo_offset;
+	sdr_config->timestamps = 1;
 
 	got_init = 1;
 }
@@ -92,10 +93,8 @@ void sdr_config_print_help(void)
 	printf("        Replace transmitted IQ data by given wave file.\n");
 	printf("    --sdr-swap-links\n");
 	printf("        Swap RX and TX frequencies for loopback tests over the air.\n");
-#ifdef HAVE_UHD
-	printf("    --sdr-uhd-tx-timestamps\n");
-	printf("        Use TX timestamps on UHD device. (May not work with some devices!)\n");
-#endif
+	printf("    --sdr-timestamps 1 | 0\n");
+	printf("        Use TX timestamps on UHD device. (default = %d)\n", sdr_config->timestamps);
 }
 
 void sdr_config_print_hotkeys(void)
@@ -124,7 +123,7 @@ void sdr_config_print_hotkeys(void)
 #define	OPT_READ_IQ_RX_WAVE	1516
 #define	OPT_READ_IQ_TX_WAVE	1517
 #define	OPT_SDR_SWAP_LINKS	1518
-#define	OPT_SDR_UHD_TX_TS	1519
+#define	OPT_SDR_TIMESTAMPS	1519
 
 void sdr_config_add_options(void)
 {
@@ -147,7 +146,7 @@ void sdr_config_add_options(void)
 	option_add(OPT_READ_IQ_RX_WAVE, "read-iq-rx-wave", 1);
 	option_add(OPT_READ_IQ_TX_WAVE, "read-iq-tx-wave", 1);
 	option_add(OPT_SDR_SWAP_LINKS, "sdr-swap-links", 0);
-	option_add(OPT_SDR_UHD_TX_TS, "sdr-uhd-tx-timestamps", 0);
+	option_add(OPT_SDR_TIMESTAMPS, "sdr-timestamps", 1);
 }
 
 int sdr_config_handle_options(int short_option, int argi, char **argv)
@@ -222,8 +221,8 @@ int sdr_config_handle_options(int short_option, int argi, char **argv)
 	case OPT_SDR_SWAP_LINKS:
 		sdr_config->swap_links = 1;
 		break;
-	case OPT_SDR_UHD_TX_TS:
-		sdr_config->uhd_tx_timestamps = 1;
+	case OPT_SDR_TIMESTAMPS:
+		sdr_config->timestamps = atoi(argv[argi]);
 		break;
 	default:
 		return -EINVAL;
@@ -257,4 +256,3 @@ int sdr_configure(int samplerate)
 	return 1;
 }
 
-
diff --git a/src/libsdr/sdr_config.h b/src/libsdr/sdr_config.h
index 37f3643..a37b9ed 100644
--- a/src/libsdr/sdr_config.h
+++ b/src/libsdr/sdr_config.h
@@ -19,7 +19,7 @@ typedef struct sdr_config {
 	const char	*read_iq_tx_wave;
 	const char	*read_iq_rx_wave;
 	int		swap_links;		/* swap DL and UL frequency */
-	int		uhd_tx_timestamps;	/* use UHD time stamps */
+	int		timestamps;		/* use time stamps when transmitting */
 } sdr_config_t;
 
 extern sdr_config_t *sdr_config;
diff --git a/src/libsdr/soapy.c b/src/libsdr/soapy.c
index 48f75b1..33dc436 100644
--- a/src/libsdr/soapy.c
+++ b/src/libsdr/soapy.c
@@ -17,12 +17,31 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+/* how time stamp process works:
+ *
+ * TX and RX time stamps are not valid in the beginning.
+ *
+ * If a first chunk is received from SDR, RX time becomes valid. The duration
+ * of the received chunk is added to the RX time stamp, so it becomes the time
+ * of the next expected chunk.
+ *
+ * If a RX time stamp is valid and first chunk is to be transmitted (tosend()
+ * is called), TX time stamp becomes valid and is set to RX time stamp, but
+ * advanced by the duration of the latency (latspl). tosend() always returns
+ * the number of samples that are needed, to make TX time stamp advance RX time
+ * stamp by given latency.
+ *
+ * If chunk is transmitted to SDR, the TX time stamp is advanced by the
+ * duration of the transmitted chunk.
+ */
+
 #include <stdio.h>
 #include <stdlib.h>
 #include <stdint.h>
 #include <string.h>
 #include <errno.h>
 #include <math.h>
+#include <pthread.h>
 #include <SoapySDR/Device.h>
 #include <SoapySDR/Formats.h>
 #include "soapy.h"
@@ -36,8 +55,13 @@ SoapySDRStream *rxStream = NULL;
 SoapySDRStream *txStream = NULL;
 static int			tx_samps_per_buff, rx_samps_per_buff;
 static double			samplerate;
-static uint64_t			rx_count = 0;
-static uint64_t			tx_count = 0;
+static pthread_mutex_t		timestamp_mutex;
+static int			use_time_stamps;
+static int			rx_valid = 0;
+static long long		rx_timeNs = 0;
+static int			tx_valid = 0;
+static long long		tx_timeNs = 0;
+static long long		Ns_per_sample;
 
 static int parse_args(SoapySDRKwargs *args, const char *_args_string)
 {
@@ -63,7 +87,7 @@ static int parse_args(SoapySDRKwargs *args, const char *_args_string)
 	return 0;
 }
 
-int soapy_open(size_t channel, const char *_device_args, const char *_stream_args, const char *_tune_args, const char *tx_antenna, const char *rx_antenna, const char *clock_source, double tx_frequency, double rx_frequency, double lo_offset, double rate, double tx_gain, double rx_gain, double bandwidth)
+int soapy_open(size_t channel, const char *_device_args, const char *_stream_args, const char *_tune_args, const char *tx_antenna, const char *rx_antenna, const char *clock_source, double tx_frequency, double rx_frequency, double lo_offset, double rate, double tx_gain, double rx_gain, double bandwidth, int timestamps)
 {
 	double got_frequency, got_rate, got_gain, got_bandwidth;
 	const char *got_antenna, *got_clock;
@@ -73,6 +97,12 @@ int soapy_open(size_t channel, const char *_device_args, const char *_stream_arg
 	SoapySDRKwargs tune_args;
 	int rc;
 
+	use_time_stamps = timestamps;
+	if (use_time_stamps && (1000000000LL % (long long)rate)) {
+		PDEBUG(DSOAPY, DEBUG_ERROR, "The given sample duration is not a multiple of a nano second. I.e. we can't divide 1000,000,000 by sample rate of %.0f. Please choose a different sample rate for time stamp support!\n", rate);
+		use_time_stamps = 0;
+	}
+	Ns_per_sample = 1000000000LL / (long long)rate;
 	samplerate = rate;
 
 	/* parsing ARGS */
@@ -394,6 +424,13 @@ int soapy_open(size_t channel, const char *_device_args, const char *_stream_arg
 		}
 	}
 
+	/* create mutex for time stamp protection */
+	rc = pthread_mutex_init(&timestamp_mutex, NULL);
+	if (rc < 0) {
+		PDEBUG(DSOAPY, DEBUG_ERROR, "Mutex init failed!\n");
+		return rc;
+	}
+
 	return 0;
 }
 
@@ -430,6 +467,7 @@ void soapy_close(void)
 	if (sdr) {
 		SoapySDRDevice_unmake(sdr);
 		sdr = NULL;
+		pthread_mutex_destroy(&timestamp_mutex);
 	}
 }
 
@@ -444,20 +482,28 @@ int soapy_send(float *buff, int num)
 		chunk = num;
 		if (chunk > tx_samps_per_buff)
 			chunk = tx_samps_per_buff;
-		/* create tx metadata */
+		/* write TX stream */
 		buffs_ptr[0] = buff;
-		count = SoapySDRDevice_writeStream(sdr, txStream, buffs_ptr, chunk, &flags, 0, 1000000);
+		if (use_time_stamps)
+			flags |= SOAPY_SDR_HAS_TIME;
+		count = SoapySDRDevice_writeStream(sdr, txStream, buffs_ptr, chunk, &flags, tx_timeNs, 1000000);
 		if (count <= 0) {
 			PDEBUG(DUHD, DEBUG_ERROR, "Failed to write to TX streamer (error=%d)\n", count);
 			break;
 		}
-
+		/* process TX time stamp */
+		if (!tx_valid)
+			PDEBUG(DSOAPY, DEBUG_ERROR, "SDR TX: tosend() was not called before, prease fix!\n");
+		else {
+			pthread_mutex_lock(&timestamp_mutex);
+			tx_timeNs += count * Ns_per_sample;
+			pthread_mutex_unlock(&timestamp_mutex);
+		}
+		/* increment transmit counters */
 		sent += count;
 		buff += count * 2;
 		num -= count;
 	}
-	/* increment tx counter */
-	tx_count += sent;
 
 	return sent;
 }
@@ -480,6 +526,23 @@ int soapy_receive(float *buff, int max)
 		buffs_ptr[0] = buff;
 		count = SoapySDRDevice_readStream(sdr, rxStream, buffs_ptr, rx_samps_per_buff, &flags, &timeNs, 0);
 		if (count > 0) {
+			if (!use_time_stamps || !(flags & SOAPY_SDR_HAS_TIME)) {
+				if (use_time_stamps) {
+					PDEBUG(DSOAPY, DEBUG_ERROR, "SDR RX: No time stamps available. This may cuse little gaps and problems with time slot based networks, like C-Netz.\n");
+					use_time_stamps = 0;
+				}
+				timeNs = rx_timeNs;
+			}
+			/* process RX time stamp */
+			if (!rx_valid) {
+				rx_timeNs = timeNs;
+				rx_valid = 1;
+			}
+			pthread_mutex_lock(&timestamp_mutex);
+			if (rx_timeNs != timeNs)
+				PDEBUG(DSOAPY, DEBUG_ERROR, "SDR RX overflow, seems we are too slow. Use lower SDR sample rate.\n");
+			rx_timeNs = timeNs + count * Ns_per_sample;
+			pthread_mutex_unlock(&timestamp_mutex);
 			/* commit received data to buffer */
 			got += count;
 			buff += count * 2;
@@ -489,8 +552,6 @@ int soapy_receive(float *buff, int max)
 			break;
 		}
 	}
-	/* update current rx time */
-	rx_count += got;
 
 	return got;
 }
@@ -500,24 +561,29 @@ int soapy_get_tosend(int latspl)
 {
 	int tosend;
 
-	/* we need the rx time stamp to determine how much data is already sent in advance */
-	if (rx_count == 0)
+	/* if no RX time stamp is set, we must wait until we receive a valid time stamp */
+	if (!rx_valid)
 		return 0;
 
-	/* if we have not yet sent any data, we set initial tx time stamp */
-	if (tx_count == 0)
-		tx_count = rx_count;
+	/* RX time stamp is valid the first time, set the TX time stamp in advance */
+	if (!tx_valid) {
+		tx_timeNs = rx_timeNs + latspl * Ns_per_sample;
+		tx_valid = 1;
+		return 0;
+	}
 
 	/* we check how advance our transmitted time stamp is */
-	tosend = latspl - (tx_count - rx_count);
-	/* in case of underrun: */
+	pthread_mutex_lock(&timestamp_mutex);
+	tosend = latspl - (tx_timeNs - rx_timeNs) / Ns_per_sample;
+	pthread_mutex_unlock(&timestamp_mutex);
+
+	/* in case of underrun */
 	if (tosend > latspl) {
-// It is normal that we have underruns, prior initial filling of buffer.
-// FIXME: better solution to detect underrun
-//		PDEBUG(DSOAPY, DEBUG_ERROR, "SDR TX underrun!\n");
-		tosend = 0;
-		tx_count = rx_count;
+		PDEBUG(DSOAPY, DEBUG_ERROR, "SDR TX underrun, seems we are too slow. Use lower SDR sample rate.\n");
+		tosend = latspl;
 	}
+
+	/* race condition and routing errors may cause TX time stamps to be in advance of slightly more than latspl */
 	if (tosend < 0)
 		tosend = 0;
 
diff --git a/src/libsdr/soapy.h b/src/libsdr/soapy.h
index c411b5b..875de3f 100644
--- a/src/libsdr/soapy.h
+++ b/src/libsdr/soapy.h
@@ -1,5 +1,5 @@
 
-int soapy_open(size_t channel, const char *_device_args, const char *_stream_args, const char *_tune_args, const char *tx_antenna, const char *rx_antenna, const char *clock_source, double tx_frequency, double rx_frequency, double lo_offset, double rate, double tx_gain, double rx_gain, double bandwidth);
+int soapy_open(size_t channel, const char *_device_args, const char *_stream_args, const char *_tune_args, const char *tx_antenna, const char *rx_antenna, const char *clock_source, double tx_frequency, double rx_frequency, double lo_offset, double rate, double tx_gain, double rx_gain, double bandwidth, int timestamps);
 int soapy_start(void);
 void soapy_close(void);
 int soapy_send(float *buff, int num);
diff --git a/src/libsdr/uhd.c b/src/libsdr/uhd.c
index 0e07fd8..e8c1d45 100644
--- a/src/libsdr/uhd.c
+++ b/src/libsdr/uhd.c
@@ -28,9 +28,6 @@
 #include "../libdebug/debug.h"
 #include "../liboptions/options.h"
 
-/* use to TX time stamp */
-//#define TX_TIMESTAMP
-
 extern int sdr_rx_overflow;
 
 static uhd_usrp_handle		usrp = NULL;
@@ -50,14 +47,14 @@ static time_t			tx_time_secs = 0;
 static double			tx_time_fract_sec = 0.0;
 static int			tx_timestamps;
 
-int uhd_open(size_t channel, const char *_device_args, const char *_stream_args, const char *_tune_args, const char *tx_antenna, const char *rx_antenna, const char *clock_source, double tx_frequency, double rx_frequency, double lo_offset, double rate, double tx_gain, double rx_gain, double bandwidth, int _tx_timestamps)
+int uhd_open(size_t channel, const char *_device_args, const char *_stream_args, const char *_tune_args, const char *tx_antenna, const char *rx_antenna, const char *clock_source, double tx_frequency, double rx_frequency, double lo_offset, double rate, double tx_gain, double rx_gain, double bandwidth, int timestamps)
 {
 	uhd_error error;
 	double got_frequency, got_rate, got_gain, got_bandwidth;
 	char got_antenna[64], got_clock[64];
 
 	samplerate = rate;
-	tx_timestamps = _tx_timestamps;
+	tx_timestamps = timestamps;
 
 	PDEBUG(DUHD, DEBUG_INFO, "Using device args \"%s\"\n", _device_args);
 	PDEBUG(DUHD, DEBUG_INFO, "Using stream args \"%s\"\n", _stream_args);
@@ -564,7 +561,7 @@ int uhd_send(float *buff, int num)
 
 		/* increment time stamp */
 		tx_time_fract_sec += (double)count / samplerate;
-		while (tx_time_fract_sec >= 1.0) {
+		if (tx_time_fract_sec >= 1.0) {
 			tx_time_secs++;
 			tx_time_fract_sec -= 1.0;
 		}
@@ -584,6 +581,8 @@ int uhd_receive(float *buff, int max)
     	void *buffs_ptr[1];
 	size_t got = 0, count;
 	uhd_error error;
+	bool has_time_spec;
+	int rc;
 
 	while (1) {
 		if (max < (int)rx_samps_per_buff) {
@@ -600,8 +599,24 @@ int uhd_receive(float *buff, int max)
 			break;
 		}
 		if (count) {
-			/* get time stamp of received RX packet */
-			uhd_rx_metadata_time_spec(rx_metadata, &rx_time_secs, &rx_time_fract_sec);
+			if (tx_timestamps) {
+				/* get time stamp of received RX packet */
+				rc = uhd_rx_metadata_has_time_spec(rx_metadata, &has_time_spec);
+				if (rc == 0 && has_time_spec)
+					rc = uhd_rx_metadata_time_spec(rx_metadata, &rx_time_secs, &rx_time_fract_sec);
+				if (rc < 0 || !has_time_spec) {
+					PDEBUG(DSOAPY, DEBUG_ERROR, "SDR RX: No time stamps available. This may cuse little gaps and problems with time slot based networks, like C-Netz.\n");
+					tx_timestamps = 0;
+				}
+			}
+			if (!tx_timestamps) {
+				/* increment time stamp */
+				rx_time_fract_sec += (double)count / samplerate;
+				if (rx_time_fract_sec >= 1.0) {
+					rx_time_secs++;
+					rx_time_fract_sec -= 1.0;
+				}
+			}
 			/* commit received data to buffer */
 			got += count;
 			buff += count * 2;
@@ -641,8 +656,10 @@ int uhd_get_tosend(int latspl)
 	/* we check how advance our transmitted time stamp is */
 	advance = ((double)tx_time_secs + tx_time_fract_sec) - ((double)rx_time_secs + rx_time_fract_sec);
 	/* in case of underrun: */
-	if (advance < 0)
+	if (advance < 0) {
+		PDEBUG(DSOAPY, DEBUG_ERROR, "SDR TX underrun, seems we are too slow. Use lower SDR sample rate.\n");
 		advance = 0;
+	}
 	tosend = latspl - (int)(advance * samplerate);
 	if (tosend < 0)
 		tosend = 0;
diff --git a/src/libsdr/uhd.h b/src/libsdr/uhd.h
index ad80e57..b2bd619 100644
--- a/src/libsdr/uhd.h
+++ b/src/libsdr/uhd.h
@@ -1,5 +1,5 @@
 
-int uhd_open(size_t channel, const char *_device_args, const char *_stream_args, const char *_tune_args, const char *tx_antenna, const char *rx_antenna, const char *clock_source, double tx_frequency, double rx_frequency, double lo_offset, double rate, double tx_gain, double rx_gain, double bandwidth, int _tx_timestamps);
+int uhd_open(size_t channel, const char *_device_args, const char *_stream_args, const char *_tune_args, const char *tx_antenna, const char *rx_antenna, const char *clock_source, double tx_frequency, double rx_frequency, double lo_offset, double rate, double tx_gain, double rx_gain, double bandwidth, int timestamps);
 int uhd_start(void);
 void uhd_close(void);
 int uhd_send(float *buff, int num);