input-soapysdr.cpp

/*
 *  input-soapysdr.cpp
 *  SoapySDR-specific routines
 *
 *  Copyright (c) 2015-2020 Tomasz Lemiech <szpajder@gmail.com>
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <iostream>
#include <assert.h>
#include <limits.h>		// SCHAR_MAX, SHRT_MAX
#include <math.h>		// round
#include <stdlib.h>		// calloc
#include <string.h>		// memcpy, strcmp
#include <syslog.h>		// LOG_* macros
#include <libconfig.h++>	// Setting
#include <SoapySDR/Device.h>	// SoapySDRDevice, SoapySDRDevice_makeStrArgs
#include <SoapySDR/Formats.h>	// SOAPY_SDR_CS constants
#include "input-common.h"	// input_t, sample_format_t, input_state_t, MODULE_EXPORT
#include "input-helpers.h"	// circbuffer_append
#include "input-soapysdr.h"	// soapysdr_dev_data_t
#include "rtl_airband.h"	// do_exit, fft_size, debug_print, XCALLOC, error()

using namespace std;

// Map SoapySDR sample format string to our internal sample format
// and set bytes_per_sample and fullscale values appropriately.
// We prefer U8 and S8 over S16 to minimize CPU load.
// If fullscale is > 0, it means it has been read by
// SoapySDRDevice_getNativeStreamFormat, so we treat this value as valid.
// Otherwise, guess a suitable default value.
static bool soapysdr_match_sfmt(input_t * const input, char const * const fmt, double const fullscale) {
	if(strcmp(fmt, SOAPY_SDR_CU8) == 0) {
		input->sfmt = SFMT_U8;
		input->bytes_per_sample = sizeof(unsigned char);
		input->fullscale = (fullscale > 0 ? fullscale : (float)SCHAR_MAX - 0.5f);
		goto matched;
	} else if(strcmp(fmt, SOAPY_SDR_CS8) == 0) {
		input->sfmt = SFMT_S8;
		input->bytes_per_sample = sizeof(char);
		input->fullscale = (fullscale > 0 ? fullscale : (float)SCHAR_MAX - 0.5f);
		goto matched;
	} else if(strcmp(fmt, SOAPY_SDR_CS16) == 0) {
		input->sfmt = SFMT_S16;
		input->bytes_per_sample = sizeof(short);
		input->fullscale = (fullscale > 0 ? fullscale : (float)SHRT_MAX - 0.5f);
		goto matched;
	} else if(strcmp(fmt, SOAPY_SDR_CF32) == 0) {
		input->sfmt = SFMT_F32;
		input->bytes_per_sample = sizeof(float);
		input->fullscale = (fullscale > 0 ? fullscale : 1.0f);
		goto matched;
	}
	return false;
matched:
	soapysdr_dev_data_t *dev_data = (soapysdr_dev_data_t *)input->dev_data;
	dev_data->sample_format = strdup(fmt);
	return true;
}

// Choose a suitable sample format.
// Bail out if no supported sample format is found.
static bool soapysdr_choose_sample_format(SoapySDRDevice * const sdr, input_t * const input) {
	bool ret = false;
	size_t len = 0;
	char **formats = NULL;
	soapysdr_dev_data_t *dev_data = (soapysdr_dev_data_t *)input->dev_data;
	input->sfmt = SFMT_UNDEF;
// First try device's native format to avoid extra conversion
	double fullscale = 0.0;
	char *fmt = SoapySDRDevice_getNativeStreamFormat(sdr, SOAPY_SDR_RX, dev_data->channel, &fullscale);

	if(soapysdr_match_sfmt(input, fmt, fullscale) == true) {
		log(LOG_NOTICE, "SoapySDR: device '%s': using native sample format '%s' (fullScale=%.1f)\n",
			dev_data->device_string, fmt, input->fullscale);
		ret = true;
		goto end;
	}
// Native format is not supported by rtl_airband; find out if there is anything else.
	formats = SoapySDRDevice_getStreamFormats(sdr, SOAPY_SDR_RX, dev_data->channel, &len);
	if(formats == NULL || len == 0) {
		log(LOG_ERR, "SoapySDR: device '%s': failed to read supported sample formats\n",
			dev_data->device_string);
		ret = false;
		goto end;
	}
	for(size_t i = 0; i < len; i++) {
		if(soapysdr_match_sfmt(input, formats[i], -1.0) == true) {
			log(LOG_NOTICE, "SoapySDR: device '%s': using non-native sample format '%s' (assuming fullScale=%.1f)\n",
				dev_data->device_string, formats[i], input->fullscale);
			ret = true;
			goto end;
		}
	}
// Nothing found; we can't use this device.
	log(LOG_ERR, "SoapySDR: device '%s': no supported sample format found\n", dev_data->device_string);
end:
	return ret;
}

static int sdrplay_get_nearest_sample_rate(SoapySDRDevice *sdr, int channel, int sample_rate) {
	size_t len = 0;
	double sr = (double)sample_rate;
	SoapySDRRange *range = SoapySDRDevice_getSampleRateRange(sdr, SOAPY_SDR_RX, channel, &len);
	if(range == NULL) {
		log(LOG_ERR, "SoapySDR: failed to read supported sampling rate ranges from the device\n");
		return -1;
	}
	debug_print("Got %zu ranges\n", len);
	double nearest_rate = range[0].minimum;
	double offset1, offset2;
	for(size_t i = 0; i < len; i++) {
		debug_print("sr=%.1f min=%.1f max=%.1f step=%.1f\n", sr, range[i].minimum, range[i].maximum, range[i].step);
		if(sr >= range[i].minimum && sr <= range[i].maximum) {
			debug_print("Found suitable range: min=%.0f max=%0.f step=%0.f\n",
				range[i].minimum, range[i].maximum, range[i].step);
			if(range[i].step == 0.0 || range[i].step >= (range[i].maximum - range[i].minimum)) {
				return (int)(range[i].maximum - sr > sr - range[i].minimum ?
					range[i].minimum : range[i].maximum);
			}
			sr = (int)(range[i].minimum + range[i].step * round((sr - range[i].minimum) / range[i].step));
			if(sr > range[i].maximum) {
				sr = (int)range[i].maximum;
			}
			return (int)sr;
		} else {
			offset1 = abs(sr - nearest_rate);
			offset2 = abs(sr - range[i].minimum);
			if(offset2 < offset1)
				nearest_rate = range[i].minimum;
			offset1 = abs(sr - nearest_rate);
			offset2 = abs(sr - range[i].maximum);
			if(offset2 < offset1)
				nearest_rate = range[i].maximum;
		}
	}
	return (int)nearest_rate;
}

int soapysdr_parse_config(input_t * const input, libconfig::Setting &cfg) {
	soapysdr_dev_data_t *dev_data = (soapysdr_dev_data_t *)input->dev_data;

	if(cfg.exists("device_string")) {
		dev_data->device_string = strdup(cfg["device_string"]);
	} else {
		cerr<<"SoapySDR configuration error: mandatory parameter missing: device_string\n";
		error();
	}
	if(cfg.exists("gain")) {
		if(cfg["gain"].getType() == libconfig::Setting::TypeInt) {
			dev_data->gain = (double)((int)cfg["gain"]);
		} else if(cfg["gain"].getType() == libconfig::Setting::TypeFloat) {
			dev_data->gain = (double)cfg["gain"];
		} else {
// Either it's a string or an unsupported type which will cause an exception - this is fine
			dev_data->gains = SoapySDRKwargs_fromString((const char *)cfg["gain"]);
			if(dev_data->gains.size < 1) {
				cerr<<"SoapySDR configuration error: device '"<<dev_data->device_string<<
					"': gain: syntax error (must be a sequence of 'name1=value1,name2=value2,...')\n";
				error();
			}
		}
		dev_data->agc = false;
	} else {
		dev_data->agc = true;
	}
	if(cfg.exists("correction")) {
		if(cfg["correction"].getType() == libconfig::Setting::TypeInt) {
			dev_data->correction = (double)((int)cfg["correction"]);
		} else if(cfg["correction"].getType() == libconfig::Setting::TypeFloat) {
			dev_data->correction = (float)cfg["correction"];
		} else {
			cerr<<"SoapySDR configuration error: device '"<<dev_data->device_string<<
				"': correction value must be numeric\n";
			error();
		}
	}
	if(cfg.exists("channel")) {
		dev_data->channel = (int)cfg["channel"];
		if(dev_data->channel < 0) {
			cerr<<"SoapySDR configuration error: device '"<<dev_data->device_string<<
				"': channel number must be positive\n";
			error();
		}
	}
	if(cfg.exists("antenna")) {
		dev_data->antenna = strdup(cfg["antenna"]);
	}
// Find a suitable sample format and sample rate (unless set in the config)
// based on device capabilities.
// We have to do this here and not in soapysdr_init, because parse_devices()
// requires sample_rate and bytes_per_sample to be set correctly in order to
// calculate the size of the sample buffer, which has to be done before
// soapysdr_init() is run.
	SoapySDRDevice *sdr = SoapySDRDevice_makeStrArgs(dev_data->device_string);
	if (sdr == NULL) {
		log(LOG_ERR, "Failed to open SoapySDR device '%s': %s\n", dev_data->device_string,
			SoapySDRDevice_lastError());
		error();
	}
	if(soapysdr_choose_sample_format(sdr, input) == false) {
		cerr<<"SoapySDR configuration error: device '"<<dev_data->device_string<<
			"': no suitable sample format found\n";
		error();
	}
	if(input->sample_rate < 0) {
		input->sample_rate = sdrplay_get_nearest_sample_rate(sdr, dev_data->channel, SOAPYSDR_DEFAULT_SAMPLE_RATE);
		if(input->sample_rate < 0) {
			log(LOG_ERR, "Failed to find a suitable sample rate for SoapySDR device '%s'\n",
				dev_data->device_string);
			log(LOG_ERR, "Specify a supported value using \"sample_rate\" option in the device configuration\n");
			error();
		}
	}
	SoapySDRDevice_unmake(sdr);
	return 0;
}

int soapysdr_init(input_t * const input) {
	soapysdr_dev_data_t *dev_data = (soapysdr_dev_data_t *)input->dev_data;

	dev_data->dev = SoapySDRDevice_makeStrArgs(dev_data->device_string);
	if (dev_data->dev == NULL) {
		log(LOG_ERR, "Failed to open SoapySDR device '%s': %s\n", dev_data->device_string,
			SoapySDRDevice_lastError());
		error();
	}
	SoapySDRDevice *sdr = dev_data->dev;

	if(SoapySDRDevice_setSampleRate(sdr, SOAPY_SDR_RX, dev_data->channel, input->sample_rate) != 0) {
		log(LOG_ERR, "Failed to set sample rate for SoapySDR device '%s': %s\n",
			dev_data->device_string, SoapySDRDevice_lastError());
		error();
	}
	log(LOG_INFO, "SoapySDR: device '%s': sample rate set to %.0f sps\n",
		dev_data->device_string, SoapySDRDevice_getSampleRate(sdr, SOAPY_SDR_RX, dev_data->channel));
	if(SoapySDRDevice_setFrequency(sdr, SOAPY_SDR_RX, dev_data->channel, input->centerfreq, NULL) != 0) {
		log(LOG_ERR, "Failed to set frequency for SoapySDR device '%s': %s\n",
			dev_data->device_string, SoapySDRDevice_lastError());
		error();
	}
	if(SoapySDRDevice_setFrequencyCorrection(sdr, SOAPY_SDR_RX, dev_data->channel, dev_data->correction) != 0) {
		log(LOG_ERR, "Failed to set frequency correction for SoapySDR device '%s': %s\n",
			dev_data->device_string, SoapySDRDevice_lastError());
		error();
	}
	if(dev_data->antenna != NULL) {
		if(SoapySDRDevice_setAntenna(sdr, SOAPY_SDR_RX, dev_data->channel, dev_data->antenna) != 0) {
			log(LOG_ERR, "Failed to set antenna to '%s' for SoapySDR device '%s': %s\n",
				dev_data->device_string, dev_data->antenna, SoapySDRDevice_lastError());
			error();
		}
		log(LOG_INFO, "SoapySDR: device '%s': antenna set to '%s'\n",
		dev_data->device_string, SoapySDRDevice_getAntenna(sdr, SOAPY_SDR_RX, dev_data->channel));
	}
	if(SoapySDRDevice_setGainMode(sdr, SOAPY_SDR_RX, dev_data->channel, dev_data->agc) != 0) {
		log(LOG_ERR, "Failed to %s AGC for SoapySDR device '%s': %s\n",
			dev_data->agc ? "enable" : "disable", dev_data->device_string, SoapySDRDevice_lastError());
		error();
	}
	log(LOG_INFO, "SoapySDR: device '%s': AGC %s (requested: %s)\n", dev_data->device_string,
		SoapySDRDevice_getGainMode(sdr, SOAPY_SDR_RX, dev_data->channel) ? "on" : "off",
		dev_data->agc ? "on" : "off");
	if(!dev_data->agc) {
		if(dev_data->gains.size > 0) {
			for(size_t i = 0; i < dev_data->gains.size; i++) {
				char * const key = dev_data->gains.keys[i];
				double val = atof(dev_data->gains.vals[i]);
				if(SoapySDRDevice_setGainElement(sdr, SOAPY_SDR_RX, dev_data->channel, key, val) != 0) {
					log(LOG_ERR, "Failed to set gain element '%s' for SoapySDR device '%s': %s\n",
					    key, dev_data->device_string, SoapySDRDevice_lastError());
					error();
				}
				log(LOG_INFO, "SoapySDR: device '%s': gain '%s' set to %.1f dB\n",
				    dev_data->device_string, key,
				    SoapySDRDevice_getGainElement(sdr, SOAPY_SDR_RX, dev_data->channel, key));
			}
		} else {
			if(SoapySDRDevice_setGain(sdr, SOAPY_SDR_RX, dev_data->channel, dev_data->gain) != 0) {
				log(LOG_ERR, "Failed to set gain for SoapySDR device '%s': %s\n",
					dev_data->device_string, SoapySDRDevice_lastError());
				error();
			}
			log(LOG_INFO, "SoapySDR: device '%s': gain set to %.1f dB\n",
				dev_data->device_string, SoapySDRDevice_getGain(sdr, SOAPY_SDR_RX, dev_data->channel));
		}
	}
	log(LOG_INFO, "SoapySDR: device '%s' initialized\n", dev_data->device_string);
	return 0;
}

void *soapysdr_rx_thread(void *ctx) {
	input_t *input = (input_t *)ctx;
	soapysdr_dev_data_t *dev_data = (soapysdr_dev_data_t *)input->dev_data;
	SoapySDRDevice *sdr = dev_data->dev;
	assert(sdr != NULL);

	unsigned char buf[SOAPYSDR_BUFSIZE];
// size of the buffer in number of I/Q sample pairs
	size_t num_elems = SOAPYSDR_BUFSIZE / (2 * input->bytes_per_sample);

	SoapySDRStream *rxStream = NULL;
	if(SoapySDRDevice_setupStream(sdr, &rxStream, SOAPY_SDR_RX, dev_data->sample_format,
	(size_t * const)&dev_data->channel, 1, NULL) != 0) {
		log(LOG_ERR, "Failed to set up stream for SoapySDR device '%s': %s\n",
			dev_data->device_string, SoapySDRDevice_lastError());
		input->state = INPUT_FAILED;
		goto cleanup;
	}
	if(SoapySDRDevice_activateStream(sdr, rxStream, 0, 0, 0)) { //start streaming
		log(LOG_ERR, "Failed to activate stream for SoapySDR device '%s': %s\n",
			dev_data->device_string, SoapySDRDevice_lastError());
		input->state = INPUT_FAILED;
		goto cleanup;
	}
	input->state = INPUT_RUNNING;
	log(LOG_NOTICE, "SoapySDR: device '%s' started\n", dev_data->device_string);

	while(!do_exit) {
		void *bufs[] = { buf };		// array of buffers
		int flags;			// flags set by receive operation
		long long timeNs;		// timestamp for receive buffer
		int samples_read = SoapySDRDevice_readStream(sdr, rxStream, bufs,
			num_elems, &flags, &timeNs, SOAPYSDR_READSTREAM_TIMEOUT_US);
		if(samples_read < 0) {	// when it's negative, it's the error code
			log(LOG_ERR, "SoapySDR device '%s': readStream failed: %s, disabling\n",
				dev_data->device_string, SoapySDR_errToStr(samples_read));
			input->state = INPUT_FAILED;
			goto cleanup;
		}
		circbuffer_append(input, buf, (size_t)(samples_read * 2 * input->bytes_per_sample));
	}
cleanup:
	SoapySDRDevice_deactivateStream(sdr, rxStream, 0, 0);
	SoapySDRDevice_closeStream(sdr, rxStream);
	SoapySDRDevice_unmake(sdr);
	return 0;
}

int soapysdr_set_centerfreq(input_t * const input, int const centerfreq) {
	soapysdr_dev_data_t *dev_data = (soapysdr_dev_data_t *)input->dev_data;
	assert(dev_data->dev != NULL);

	if(SoapySDRDevice_setFrequency(dev_data->dev, SOAPY_SDR_RX, dev_data->channel, centerfreq, NULL) != 0) {
		log(LOG_ERR, "Failed to set frequency for SoapySDR device '%s': %s\n",
			dev_data->device_string, SoapySDRDevice_lastError());
		return -1;
	}
	return 0;
}

MODULE_EXPORT input_t *soapysdr_input_new() {
	soapysdr_dev_data_t *dev_data = (soapysdr_dev_data_t *)XCALLOC(1, sizeof(soapysdr_dev_data_t));
	dev_data->gain = -1.0;	// invalid default gain value
	dev_data->agc = false;
	memset(&dev_data->gains, 0, sizeof(dev_data->gains));
	dev_data->channel = 0;
	dev_data->antenna = NULL;
/*	return &( input_t ){
		.dev_data = dev_data,
		.state = INPUT_UNKNOWN,
		.sfmt = SFMT_U8,
		.sample_rate = -1,
		.parse_config = &soapysdr_parse_config,
		.init = &soapysdr_init,
		.run_rx_thread = &soapysdr_rx_thread,
		.set_centerfreq = &soapysdr_set_centerfreq,
		.stop = &soapysdr_stop
	}; */
	input_t *input = (input_t *)XCALLOC(1, sizeof(input_t));
	input->dev_data = dev_data;
// invalid values as defaults
	input->state = INPUT_UNKNOWN;
	input->sfmt = SFMT_UNDEF;
	input->fullscale = 0.0f;
	input->bytes_per_sample = 0;
	input->sample_rate = -1;

	input->parse_config = &soapysdr_parse_config;
	input->init = &soapysdr_init;
	input->run_rx_thread = &soapysdr_rx_thread;
	input->set_centerfreq = &soapysdr_set_centerfreq;
	input->stop = NULL;
	return input;
}