5G-MAG Reference Tools - MBMS Modem
SdrReader.cpp
Go to the documentation of this file.
1 // 5G-MAG Reference Tools
2 // MBMS Modem Process
3 //
4 // Copyright (C) 2021 Klaus Kühnhammer (Österreichische Rundfunksender GmbH & Co KG)
5 //
6 // This program is free software: you can redistribute it and/or modify
7 // it under the terms of the GNU Affero General Public License as published by
8 // the Free Software Foundation, either version 3 of the License, or
9 // (at your option) any later version.
10 //
11 // This program is distributed in the hope that it will be useful,
12 // but WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 // GNU General Public License for more details.
15 //
16 // You should have received a copy of the GNU General Public License
17 // along with this program. If not, see <http://www.gnu.org/licenses/>.
18 
19 #include "SdrReader.h"
20 #include <SoapySDR/Device.hpp>
21 #include <SoapySDR/Types.hpp>
22 #include <SoapySDR/Formats.hpp>
23 
24 #include <boost/algorithm/string/join.hpp>
25 
26 #include <chrono>
27 #include <cmath>
28 #include <csignal>
29 
30 #include "spdlog/spdlog.h"
31 
33  if (_sdr != nullptr) {
34  auto sdr = (SoapySDR::Device*)_sdr;
35  sdr->deactivateStream((SoapySDR::Stream*)_stream, 0, 0);
36  sdr->closeStream((SoapySDR::Stream*)_stream);
37  SoapySDR::Device::unmake( sdr );
38  }
39 
40  if (_reading_from_file) {
41  srsran_filesource_free(&file_source);
42  }
43 
44  if (_writing_to_file) {
45  srsran_filesink_free(&file_sink);
46  }
47 }
48 
50 {
51  auto results = SoapySDR::Device::enumerate();
52  SoapySDR::Kwargs::iterator it;
53 
54  for( int i = 0; i < results.size(); ++i)
55  {
56  printf("Device #%d:\n", i);
57  for( it = results[i].begin(); it != results[i].end(); ++it)
58  {
59  printf("%s = %s\n", it->first.c_str(), it->second.c_str());
60  }
61  printf("\n\n");
62  }
63 
64 }
65 
66 auto SdrReader::init(const std::string& device_args, const char* sample_file,
67  const char* write_sample_file, bool repeat_sample_file) -> bool {
68  if (sample_file != nullptr) {
69  if (0 == srsran_filesource_init(&file_source,
70  const_cast<char*>(sample_file),
71  SRSRAN_COMPLEX_FLOAT_BIN)) {
72  _reading_from_file = true;
73  _repeat_sample_file = repeat_sample_file;
74  } else {
75  spdlog::error("Could not open file {}", sample_file);
76  return false;
77  }
78  } else {
79  if (write_sample_file != nullptr) {
80  if (0 == srsran_filesink_init(&file_sink,
81  const_cast<char*>(write_sample_file),
82  SRSRAN_COMPLEX_FLOAT_BIN)) {
83  _writing_to_file = true;
84  } else {
85  spdlog::error("Could not open file {}", write_sample_file);
86  return false;
87  }
88  }
89  _device_args = SoapySDR::KwargsFromString(device_args);
90  _sdr = SoapySDR::Device::make(_device_args);
91  if (_sdr == nullptr)
92  {
93  spdlog::error("SoapySDR: failed to open device with args {}", device_args);
94  return false;
95  }
96  }
97 
98  _cfg.lookupValue("modem.sdr.ringbuffer_size_ms", _buffer_ms);
99  return true;
100 }
101 
103  auto buffer_size = (unsigned int)ceil(_sampleRate/1000.0 * _buffer_ms);
104  _buffer = std::make_unique<MultichannelRingbuffer>(sizeof(cf_t) * buffer_size, _rx_channels);
105  _buffer_write = std::make_unique<MultichannelRingbuffer>(sizeof(cf_t) * buffer_size / 2, _rx_channels); // This is the buffer where we will store the samples to write a big chunk of samples instead many little ones. 1 GB seems to be a sweet amount to write (16e6 * sizeof(cf_t) = 1GB).
106  _buffer_ready = true;
107 }
108 
110  _buffer->clear();
111  _buffer_write->clear();
112  _high_watermark_reached = false;
113 }
114 
115 auto SdrReader::set_antenna(const std::string& antenna, uint8_t idx) -> bool {
116  auto sdr = (SoapySDR::Device*)_sdr;
117  auto antenna_list = sdr->listAntennas(SOAPY_SDR_RX, idx);
118  if (std::find(antenna_list.begin(), antenna_list.end(), antenna) != antenna_list.end()) {
119  sdr->setAntenna( SOAPY_SDR_RX, idx, antenna);
120  _antenna = sdr->getAntenna( SOAPY_SDR_RX, idx);
121  return true;
122  } else {
123  spdlog::error("Unknown antenna \"{}\". Available: {}.", antenna, boost::algorithm::join(antenna_list, ", ") );
124  return false;
125  }
126 }
127 
128 auto SdrReader::set_frequency(uint32_t frequency, uint8_t idx) -> bool {
129  auto sdr = (SoapySDR::Device*)_sdr;
130  sdr->setFrequency( SOAPY_SDR_RX, idx, frequency);
131  return true;
132 }
133 
134 auto SdrReader::set_filter_bw(uint32_t bandwidth, uint8_t idx) -> bool {
135  auto sdr = (SoapySDR::Device*)_sdr;
136  sdr->setBandwidth( SOAPY_SDR_RX, idx, bandwidth);
137  return true;
138 }
139 
140 auto SdrReader::set_sample_rate(uint32_t rate, uint8_t idx) -> bool {
141  auto sdr = (SoapySDR::Device*)_sdr;
142  sdr->setSampleRate( SOAPY_SDR_RX, idx, rate);
143  return true;
144 }
145 
146 auto SdrReader::set_gain(bool use_agc, double gain, uint8_t idx) -> bool {
147  auto sdr = (SoapySDR::Device*)_sdr;
148  if (sdr->hasGainMode(SOAPY_SDR_RX, idx)) {
149 // spdlog::info("{} AGC", use_agc ? "Enabling" : "Disabling");
150  sdr->setGainMode(SOAPY_SDR_RX, idx, use_agc);
151  } else if (use_agc) {
152 // spdlog::info("AGC is not supported by this device, please set gain manually");
153  }
154  auto gain_range = sdr->getGainRange(SOAPY_SDR_RX, idx);
155  _min_gain = gain_range.minimum();
156  _max_gain = gain_range.maximum();
157  if (gain >= gain_range.minimum() && gain <= gain_range.maximum()) {
158  sdr->setGain( SOAPY_SDR_RX, idx, gain);
159  if (idx == 0) {
160  _gain = sdr->getGain( SOAPY_SDR_RX, idx);
161  }
162  return true;
163  } else {
164  spdlog::error("Invalid gain setting {}. Allowed range is: {} - {}.", gain, gain_range.minimum(), gain_range.maximum());
165  return false;
166  }
167 }
168 
169 auto SdrReader::tune(uint32_t frequency, uint32_t sample_rate,
170  uint32_t bandwidth, double gain, const std::string& antenna, bool use_agc) -> bool {
171  _frequency = frequency;
172  _filterBw = bandwidth;
173  _sampleRate = sample_rate;
174  _use_agc = use_agc;
175 
176  init_buffer();
177 
178  if (_reading_from_file) {
179  return true;
180  }
181 
182  if (_sdr == nullptr) {
183  return false;
184  }
185 
186  spdlog::info("Tuning to {} MHz, filter bandwidth {} MHz, sample rate {}, gain {}, antenna path {} with AGC set to {}",
187  frequency/1000000.0, bandwidth/1000000.0, sample_rate/1000000.0, gain, antenna, use_agc);
188 
189  auto sdr = (SoapySDR::Device*)_sdr;
190 
191  for (auto ch = 0; ch < _rx_channels; ch++) {
192  set_antenna(antenna, ch);
193  set_gain(_use_agc, gain, ch);
194  set_frequency(frequency, ch);
195  set_filter_bw(bandwidth, ch);
196  set_sample_rate(sample_rate, ch);
197  }
198 
199  _frequency = sdr->getFrequency( SOAPY_SDR_RX, 0);
200  bandwidth = sdr->getBandwidth( SOAPY_SDR_RX, 0);
201  _sampleRate = sdr->getSampleRate( SOAPY_SDR_RX, 0);
202 
203  spdlog::info("SDR tuned to {} MHz, filter bandwidth {} MHz, sample rate {}, gain {}, antenna path {}",
204  _frequency/1000000.0, bandwidth/1000000.0, _sampleRate/1000000.0, _gain, _antenna);
205 
206 
207  auto sensors = sdr->listSensors();
208  if (std::find(sensors.begin(), sensors.end(), "lms7_temp") != sensors.end()) {
209  _temp_sensor_available = true;
210  _temp_sensor_key = "lms7_temp";
211  }
212 
213  return true;
214 }
215 
217  if (_sdr != nullptr) {
218  auto sdr = (SoapySDR::Device*)_sdr;
219  std::vector<size_t> channels(_rx_channels);
220  for (auto ch = 0; ch < _rx_channels; ch++) {
221  channels[ch] = ch;
222  }
223  sdr->setHardwareTime(0); // Set SDR timestamp to zero.
224  _stream = sdr->setupStream( SOAPY_SDR_RX, SOAPY_SDR_CF32, channels, _device_args);
225  if( _stream == nullptr)
226  {
227  spdlog::error("Failed to set up RX stream");
228  SoapySDR::Device::unmake( sdr );
229  return ;
230  }
231  sdr->activateStream( (SoapySDR::Stream*)_stream, SOAPY_SDR_HAS_TIME, 100000000, 0); // Delayed start of the SDR reception, to avoid overfloas at the beggining.
232  }
233  _running = true;
234 
235  // Start the reader thread and elevate its priority to realtime
236  _readerThread = std::thread{&SdrReader::read, this};
237  struct sched_param thread_param = {};
238  thread_param.sched_priority = 50;
239  int min_prio = sched_get_priority_min(SCHED_RR);
240  int max_prio = sched_get_priority_max(SCHED_RR);
241 
242  if (min_prio == -1 or max_prio == -1)
243  spdlog::error("Something went wrong, error in sched_get_priority_min/max");
244 
245  _cfg.lookupValue("modem.sdr.reader_thread_priority_rt", thread_param.sched_priority);
246 
247  spdlog::debug("Launching sample reader thread with realtime scheduling priority {}, available priorities, max: {}, min: {}", thread_param.sched_priority, max_prio, min_prio);
248 
249  int error = pthread_setschedparam(_readerThread.native_handle(), SCHED_RR, &thread_param);
250  if (error != 0) {
251  spdlog::warn("Cannot set reader thread priority to realtime: {}. Thread will run at default priority with a high probability of dropped samples and loss of synchronisation.", strerror(error));
252  }
253 }
254 
256  _running = false;
257 
258  _readerThread.join();
259  if (_sdr != nullptr) {
260  auto sdr = (SoapySDR::Device*)_sdr;
261  sdr->deactivateStream((SoapySDR::Stream*)_stream, 0, 0);
262  sdr->closeStream((SoapySDR::Stream*)_stream);
263  }
264 
265  clear_buffer();
266 }
267 
269  std::array<void*, SRSRAN_MAX_CHANNELS> radio_buffers = { nullptr };
270  while (_running) {
271  int toRead = ceil(_sampleRate / 1000.0);
272  //int toRead = 254;
273  if (_buffer->free_size() < toRead * sizeof(cf_t)) {
274  spdlog::debug("ringbuffer overflow");
275  std::this_thread::sleep_for(std::chrono::microseconds(1000));
276  } else {
277  int read = 0;
278  size_t writeable = 0;
279  size_t writeable_write = 0;
280  auto buffers = _buffer->write_head(&writeable);
281  auto buffers_write = _buffer_write->write_head(&writeable_write);
282  int writeable_samples = (int)floor(writeable / sizeof(cf_t));
283  int writeable_write_samples = (int)floor(writeable_write / sizeof(cf_t));
284 
285  if (_reading_from_file) {
286  std::chrono::steady_clock::time_point entered = {};
287  entered = std::chrono::steady_clock::now();
288 
289  read = srsran_filesource_read_multi(&file_source, buffers.data(), std::min(writeable_samples, toRead), (int)_rx_channels);
290  if ( read == 0 ) {
291  if (_repeat_sample_file) {
292  srsran_filesource_seek(&file_source, 0);
293  } else {
294  spdlog::info("EOF, exiting...");
295  raise(SIGINT); //SIGINT to signal srsran that we want to exit.
296  }
297  }
298  read = read / _rx_channels;
299  int64_t required_time_us = (1000000.0/_sampleRate) * read;
300 
301  if (read > 0) {
302  _buffer->commit( read * sizeof(cf_t) );
303  }
304 
305  std::chrono::microseconds sleep = (std::chrono::microseconds(required_time_us) -
306  std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() - entered));
307  std::this_thread::sleep_for(sleep);
308  } else {
309  auto sdr = (SoapySDR::Device*)_sdr;
310  int flags = 0;
311  long long time_ns = 0;
312  auto rbuff = buffers.data();
313  auto wbuff = buffers_write.data();
314 
315  read = sdr->readStream( (SoapySDR::Stream*)_stream, buffers.data(), std::min(writeable_samples, toRead), flags, time_ns);
316 
317  if (read> 0 ) {
318 
319  if (_writing_to_file && _write_samples && writeable_write_samples) { // Only if we are going to write into a file.
320  for (int i = 0; i < _rx_channels; i++) {
321  memcpy(wbuff[i], rbuff[i], std::min(writeable_write_samples, read) * sizeof(cf_t)); // Copy the data in the input buffer to the toWrite buffer.
322  }
323  _buffer_write->commit( std::min(writeable_write_samples, read) * sizeof(cf_t)); // We used another ring buffer for the written of the samples, to not block a lot we only write to the disk when the ring buffer is at 90% of its capacity
324  }
325  _buffer->commit( read * sizeof(cf_t) );
326 
327  if (_writing_to_file && _write_samples && _buffer_write->used_size() >= 0.90* _buffer_write->capacity()) {
328  int toWrite_samples = _buffer_write->used_size() / sizeof(cf_t); // We are going to storage all the info
329  auto buff_to_write = _buffer_write->read_head(); // Gives the beggining of the buffer, it also puts _used and _head to 0, to start adding at the beggining again.
330  srsran_filesink_write_multi(&file_sink, buff_to_write.data(), toWrite_samples, (int)_rx_channels); // From the beggining of the buffer we write used_size data
331  }
332  spdlog::debug("buffer: commited {}, requested {}, writeable {}, writeable_write {}, flags {}", read, toRead, writeable_samples, writeable_write_samples, flags);
333  }
334  else {
335  spdlog::error("readStream returned {}", read);
336  _buffer->commit( toRead * sizeof(cf_t) );
337  _buffer_write->commit(toRead * sizeof(cf_t));
338  }
339  }
340  }
341  }
342  spdlog::debug("Sample reader thread exited");
343 }
344 
345 auto SdrReader::get_samples(cf_t* data[SRSRAN_MAX_CHANNELS], uint32_t nsamples, //NOLINT
346  srsran_timestamp_t *
347  /*rx_time*/) -> int {
348  std::chrono::steady_clock::time_point entered = {};
349  entered = std::chrono::steady_clock::now();
350 
351  int64_t required_time_us = (1000000.0/_sampleRate) * nsamples;
352  size_t cnt = nsamples * sizeof(cf_t);
353 
354  if (_high_watermark_reached && _buffer->used_size() < (_sampleRate / 1000.0) * 10 * sizeof(cf_t)) {
355  _high_watermark_reached = false;
356  }
357 
358  if (!_high_watermark_reached) {
359  while (_buffer->used_size() < (_sampleRate / 1000.0) * (_buffer_ms / 2.0) * sizeof(cf_t)) {
360  std::this_thread::sleep_for(std::chrono::microseconds(500));
361  }
362  spdlog::debug("Filled ringbuffer to half capacity");
363  _high_watermark_reached = true;
364  }
365 
366  std::vector<char*> buffers(_rx_channels);
367  for (auto ch = 0; ch < _rx_channels; ch++) {
368  buffers[ch] = (char*)data[ch];
369  }
370  _buffer->read(buffers, cnt); // Copy from the ringbuffer to the data array. This also decreases _used.
371 
372  if (_buffer->used_size() < (_sampleRate / 1000.0) * (_buffer_ms / 4.0) * sizeof(cf_t)) {
373  required_time_us += 500;
374  } else {
375  required_time_us -= 500;
376  }
377 
378  spdlog::debug("took {}, read {} samples, adjusted required {} us, delta {} us, sleep adj {}, sleeping for {} us",
379  std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() - entered).count(),
380  nsamples,
381  std::chrono::microseconds(required_time_us).count(),
382  std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() - _last_read).count(),
383  _sleep_adjustment,
384  (std::chrono::microseconds(_sleep_adjustment + required_time_us) - std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() - _last_read)).count());
385 
386  std::chrono::microseconds sleep = (std::chrono::microseconds(_sleep_adjustment + required_time_us) - std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() - _last_read));
387 
388  if (sleep.count() > 0) {
389  std::this_thread::sleep_for(sleep);
390  _sleep_adjustment = 0;
391  } else if (sleep.count() > -100000) {
392  _sleep_adjustment = sleep.count();
393  }
394 
395  _last_read = std::chrono::steady_clock::now();
396  return 0;
397 }
398 
400 {
401  if (!_buffer_ready) {
402  return 0;
403  }
404  return static_cast<double>(_buffer->used_size()) / static_cast<double>(_buffer->capacity());
405 }
bool _reading_from_file
Definition: SdrReader.h:191
void stop()
Stop reading samples from the SDR.
Definition: SdrReader.cpp:255
const libconfig::Config & _cfg
Definition: SdrReader.h:159
void start()
Start reading samples from the SDR.
Definition: SdrReader.cpp:216
double get_buffer_level()
Get current ringbuffer level (0 = empty .
Definition: SdrReader.cpp:399
bool _high_watermark_reached
Definition: SdrReader.h:186
bool init(const std::string &device_args, const char *sample_file, const char *write_sample_file, bool repeat_sample_file)
Initializes the SDR interface and creates a ring buffer according to the params from Cfg.
Definition: SdrReader.cpp:66
std::unique_ptr< MultichannelRingbuffer > _buffer_write
Definition: SdrReader.h:162
void enumerateDevices()
Prints a list of all available SDR devices.
Definition: SdrReader.cpp:49
std::thread _readerThread
Definition: SdrReader.h:164
int get_samples(cf_t *data[SRSRAN_MAX_CHANNELS], uint32_t nsamples, srsran_timestamp_t *rx_time)
Store nsamples count samples into the buffer at data.
Definition: SdrReader.cpp:345
srsran_filesource_t file_source
Definition: SdrReader.h:181
bool set_gain(bool use_agc, double gain, uint8_t idx)
Definition: SdrReader.cpp:146
srsran_filesink_t file_sink
Definition: SdrReader.h:182
void * _stream
Definition: SdrReader.h:157
unsigned _rx_channels
Definition: SdrReader.h:167
bool _writing_to_file
Definition: SdrReader.h:192
bool _running
Definition: SdrReader.h:165
void clear_buffer()
Clear all samples from the rx buffers.
Definition: SdrReader.cpp:109
std::map< std::string, std::string > _device_args
Definition: SdrReader.h:201
bool _buffer_ready
Definition: SdrReader.h:190
bool set_sample_rate(uint32_t rate, uint8_t idx)
Definition: SdrReader.cpp:140
bool set_antenna(const std::string &antenna, uint8_t idx)
Definition: SdrReader.cpp:115
void init_buffer()
Definition: SdrReader.cpp:102
bool tune(uint32_t frequency, uint32_t sample_rate, uint32_t bandwidth, double gain, const std::string &antenna, bool use_agc)
Tune the SDR to the desired frequency, and set gain, filter and antenna parameters.
Definition: SdrReader.cpp:169
std::unique_ptr< MultichannelRingbuffer > _buffer
Definition: SdrReader.h:161
bool set_filter_bw(uint32_t bandwidth, uint8_t idx)
Definition: SdrReader.cpp:134
virtual ~SdrReader()
Default destructor.
Definition: SdrReader.cpp:32
bool _repeat_sample_file
Definition: SdrReader.h:194
void * _sdr
Definition: SdrReader.h:156
bool set_frequency(uint32_t frequency, uint8_t idx)
Definition: SdrReader.cpp:128
bool _write_samples
Definition: SdrReader.h:193
double _sampleRate
Definition: SdrReader.h:168
unsigned _buffer_ms
Definition: SdrReader.h:189
void read()
Definition: SdrReader.cpp:268
static std::string antenna
Antenna input to be used.
Definition: main.cpp:179
static bool use_agc
Definition: main.cpp:180
static unsigned sample_rate
Sample rate of the SDR.
Definition: main.cpp:174
static uint32_t bandwidth
Low pass filter bandwidth for the SDR.
Definition: main.cpp:177
static unsigned frequency
Center freqeuncy the SDR is tuned to.
Definition: main.cpp:176
static double gain
Overall system gain for the SDR.
Definition: main.cpp:178