forked from cellular-infrastructure/osmocom-analog
287 lines
7.9 KiB
C
287 lines
7.9 KiB
C
/* SoapySDR device access
|
|
*
|
|
* (C) 2017 by Andreas Eversberg <jolly@eversberg.eu>
|
|
* All Rights Reserved
|
|
*
|
|
* 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 <stdlib.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
#include <errno.h>
|
|
#include <math.h>
|
|
#include <SoapySDR/Device.h>
|
|
#include <SoapySDR/Formats.h>
|
|
#include "soapy.h"
|
|
#include "debug.h"
|
|
|
|
static SoapySDRDevice *sdr = NULL;
|
|
SoapySDRStream *rxStream = NULL;
|
|
SoapySDRStream *txStream = NULL;
|
|
static int tx_samps_per_buff, rx_samps_per_buff;
|
|
static double samplerate;
|
|
static long long rx_count = 0;
|
|
static long long tx_count = 0;
|
|
|
|
int soapy_open(const char *device_args, double tx_frequency, double rx_frequency, double rate, double rx_gain, double tx_gain)
|
|
{
|
|
double got_frequency, got_rate, got_gain;
|
|
size_t channel = 0;
|
|
char *arg_string = strdup(device_args), *key, *val;
|
|
SoapySDRKwargs args;
|
|
|
|
samplerate = rate;
|
|
|
|
/* create SoapySDR device */
|
|
PDEBUG(DUHD, DEBUG_INFO, "Creating SoapySDR with args \"%s\"...\n", arg_string);
|
|
memset(&args, 0, sizeof(args));
|
|
while (arg_string && *arg_string) {
|
|
key = arg_string;
|
|
val = strchr(key, '=');
|
|
if (!val) {
|
|
PDEBUG(DUHD, DEBUG_ERROR, "Error parsing SDR args: No '=' after key\n");
|
|
soapy_close();
|
|
return -EIO;
|
|
}
|
|
val++;
|
|
arg_string = strchr(val, ',');
|
|
if (arg_string)
|
|
*arg_string++ = '\0';
|
|
SoapySDRKwargs_set(&args, key, val);
|
|
}
|
|
sdr = SoapySDRDevice_make(&args);
|
|
if (!sdr) {
|
|
PDEBUG(DUHD, DEBUG_ERROR, "Failed to create SoapySDR\n");
|
|
soapy_close();
|
|
return -EIO;
|
|
}
|
|
|
|
/* set rate */
|
|
if (SoapySDRDevice_setSampleRate(sdr, SOAPY_SDR_TX, channel, rate) != 0) {
|
|
PDEBUG(DUHD, DEBUG_ERROR, "Failed to set TX rate to %.0f Hz\n", rate);
|
|
soapy_close();
|
|
return -EIO;
|
|
}
|
|
if (SoapySDRDevice_setSampleRate(sdr, SOAPY_SDR_RX, channel, rate) != 0) {
|
|
PDEBUG(DUHD, DEBUG_ERROR, "Failed to set RX rate to %.0f Hz\n", rate);
|
|
soapy_close();
|
|
return -EIO;
|
|
}
|
|
|
|
/* see what rate actually is */
|
|
got_rate = SoapySDRDevice_getSampleRate(sdr, SOAPY_SDR_TX, channel);
|
|
if (got_rate != rate) {
|
|
PDEBUG(DUHD, DEBUG_ERROR, "Given TX rate %.0f Hz is not supported, try %0.f Hz\n", rate, got_rate);
|
|
soapy_close();
|
|
return -EINVAL;
|
|
}
|
|
got_rate = SoapySDRDevice_getSampleRate(sdr, SOAPY_SDR_RX, channel);
|
|
if (got_rate != rate) {
|
|
PDEBUG(DUHD, DEBUG_ERROR, "Given RX rate %.0f Hz is not supported, try %0.f Hz\n", rate, got_rate);
|
|
soapy_close();
|
|
return -EINVAL;
|
|
}
|
|
|
|
/* set gain */
|
|
if (SoapySDRDevice_setGain(sdr, SOAPY_SDR_TX, channel, tx_gain) != 0) {
|
|
PDEBUG(DUHD, DEBUG_ERROR, "Failed to set TX gain to %.0f\n", tx_gain);
|
|
soapy_close();
|
|
return -EIO;
|
|
}
|
|
if (SoapySDRDevice_setGain(sdr, SOAPY_SDR_RX, channel, rx_gain) != 0) {
|
|
PDEBUG(DUHD, DEBUG_ERROR, "Failed to set RX gain to %.0f\n", rx_gain);
|
|
soapy_close();
|
|
return -EIO;
|
|
}
|
|
|
|
/* see what gain actually is */
|
|
got_gain = SoapySDRDevice_getGain(sdr, SOAPY_SDR_TX, channel);
|
|
if (got_gain != tx_gain) {
|
|
PDEBUG(DUHD, DEBUG_NOTICE, "Given TX gain %.0f is not supported, we use %0.f\n", tx_gain, got_gain);
|
|
tx_gain = got_gain;
|
|
}
|
|
got_gain = SoapySDRDevice_getGain(sdr, SOAPY_SDR_RX, channel);
|
|
if (got_gain != rx_gain) {
|
|
PDEBUG(DUHD, DEBUG_NOTICE, "Given RX gain %.3f is not supported, we use %.3f\n", rx_gain, got_gain);
|
|
rx_gain = got_gain;
|
|
}
|
|
|
|
/* set frequency */
|
|
if (SoapySDRDevice_setFrequency(sdr, SOAPY_SDR_TX, channel, tx_frequency, NULL) != 0) {
|
|
PDEBUG(DUHD, DEBUG_ERROR, "Failed to set TX frequency to %.0f Hz\n", tx_frequency);
|
|
soapy_close();
|
|
return -EIO;
|
|
}
|
|
if (SoapySDRDevice_setFrequency(sdr, SOAPY_SDR_RX, channel, rx_frequency, NULL) != 0) {
|
|
PDEBUG(DUHD, DEBUG_ERROR, "Failed to set RX frequency to %.0f Hz\n", rx_frequency);
|
|
soapy_close();
|
|
return -EIO;
|
|
}
|
|
|
|
/* see what frequency actually is */
|
|
got_frequency = SoapySDRDevice_getFrequency(sdr, SOAPY_SDR_TX, channel);
|
|
if (got_frequency != tx_frequency) {
|
|
PDEBUG(DUHD, DEBUG_ERROR, "Given TX frequency %.0f Hz is not supported, try %0.f Hz\n", tx_frequency, got_frequency);
|
|
soapy_close();
|
|
return -EINVAL;
|
|
}
|
|
got_frequency = SoapySDRDevice_getFrequency(sdr, SOAPY_SDR_RX, channel);
|
|
if (got_frequency != rx_frequency) {
|
|
PDEBUG(DUHD, DEBUG_ERROR, "Given RX frequency %.0f Hz is not supported, try %0.f Hz\n", rx_frequency, got_frequency);
|
|
soapy_close();
|
|
return -EINVAL;
|
|
}
|
|
|
|
/* set up streamer */
|
|
if (SoapySDRDevice_setupStream(sdr, &txStream, SOAPY_SDR_TX, SOAPY_SDR_CF32, &channel, 1, NULL) != 0) {
|
|
PDEBUG(DUHD, DEBUG_ERROR, "Failed to set TX streamer args\n");
|
|
soapy_close();
|
|
return -EIO;
|
|
}
|
|
if (SoapySDRDevice_setupStream(sdr, &rxStream, SOAPY_SDR_RX, SOAPY_SDR_CF32, &channel, 1, NULL) != 0) {
|
|
PDEBUG(DUHD, DEBUG_ERROR, "Failed to set RX streamer args\n");
|
|
soapy_close();
|
|
return -EIO;
|
|
}
|
|
|
|
/* get buffer sizes */
|
|
tx_samps_per_buff = SoapySDRDevice_getStreamMTU(sdr, txStream);
|
|
if (tx_samps_per_buff == 0) {
|
|
PDEBUG(DUHD, DEBUG_ERROR, "Failed to get TX streamer sample buffer\n");
|
|
soapy_close();
|
|
return -EIO;
|
|
}
|
|
rx_samps_per_buff = SoapySDRDevice_getStreamMTU(sdr, rxStream);
|
|
if (rx_samps_per_buff == 0) {
|
|
PDEBUG(DUHD, DEBUG_ERROR, "Failed to get RX streamer sample buffer\n");
|
|
soapy_close();
|
|
return -EIO;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/* start streaming */
|
|
int soapy_start(void)
|
|
{
|
|
/* enable rx stream */
|
|
if (SoapySDRDevice_activateStream(sdr, rxStream, 0, 0, 0) != 0) {
|
|
PDEBUG(DUHD, DEBUG_ERROR, "Failed to issue RX stream command\n");
|
|
return -EIO;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void soapy_close(void)
|
|
{
|
|
PDEBUG(DUHD, DEBUG_DEBUG, "Clean up UHD\n");
|
|
if (txStream) {
|
|
SoapySDRDevice_closeStream(sdr, txStream);
|
|
txStream = NULL;
|
|
}
|
|
if (rxStream) {
|
|
SoapySDRDevice_deactivateStream(sdr, rxStream, 0, 0);
|
|
SoapySDRDevice_closeStream(sdr, rxStream);
|
|
rxStream = NULL;
|
|
}
|
|
if (sdr) {
|
|
SoapySDRDevice_unmake(sdr);
|
|
sdr = NULL;
|
|
}
|
|
}
|
|
|
|
int soapy_send(float *buff, int num)
|
|
{
|
|
const void *buffs_ptr[1];
|
|
int chunk;
|
|
int sent = 0, count;
|
|
int flags = 0;
|
|
|
|
while (num) {
|
|
chunk = num;
|
|
if (chunk > tx_samps_per_buff)
|
|
chunk = tx_samps_per_buff;
|
|
/* create tx metadata */
|
|
buffs_ptr[0] = buff;
|
|
count = SoapySDRDevice_writeStream(sdr, txStream, buffs_ptr, chunk, &flags, 0, 0);
|
|
if (count <= 0)
|
|
break;
|
|
|
|
/* increment tx counter */
|
|
tx_count += count;
|
|
|
|
sent += count;
|
|
buff += count * 2;
|
|
num -= count;
|
|
}
|
|
|
|
return sent;
|
|
}
|
|
|
|
/* read what we got, return 0, if buffer is empty, otherwise return the number of samples */
|
|
int soapy_receive(float *buff, int max)
|
|
{
|
|
void *buffs_ptr[1];
|
|
int got = 0, count;
|
|
long long timeNs;
|
|
int flags = 0;
|
|
|
|
while (1) {
|
|
if (max < rx_samps_per_buff) {
|
|
PDEBUG(DUHD, DEBUG_ERROR, "SDR rx buffer overflow!\n");
|
|
break;
|
|
}
|
|
/* read RX stream */
|
|
buffs_ptr[0] = buff;
|
|
count = SoapySDRDevice_readStream(sdr, rxStream, buffs_ptr, rx_samps_per_buff, &flags, &timeNs, 0);
|
|
if (count > 0) {
|
|
/* update current rx time */
|
|
rx_count += count;
|
|
/* commit received data to buffer */
|
|
got += count;
|
|
buff += count * 2;
|
|
max -= count;
|
|
} else {
|
|
/* got nothing this time */
|
|
break;
|
|
}
|
|
}
|
|
|
|
return got;
|
|
}
|
|
|
|
/* estimate current unsent number of samples */
|
|
int soapy_get_inbuffer(void)
|
|
{
|
|
long long advance;
|
|
|
|
/* we need the rx time stamp to determine how much data is already sent in advance */
|
|
if (rx_count == 0)
|
|
return -EAGAIN;
|
|
|
|
/* if we have not yet sent any data, we set initial tx time stamp */
|
|
if (tx_count == 0)
|
|
tx_count = rx_count;
|
|
|
|
/* we check how advance our transmitted time stamp is */
|
|
advance = tx_count - rx_count;
|
|
/* in case of underrun: */
|
|
if (advance < 0)
|
|
advance = 0;
|
|
|
|
return advance;
|
|
}
|
|
|