sodera_lib.cpp 21.5 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33
/*******************************************************************************
    OpenAirInterface 
    Copyright(c) 1999 - 2014 Eurecom

    OpenAirInterface 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.


    OpenAirInterface 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 OpenAirInterface.The full GNU General Public License is 
    included in this distribution in the file called "COPYING". If not, 
    see <http://www.gnu.org/licenses/>.

   Contact Information
   OpenAirInterface Admin: openair_admin@eurecom.fr
   OpenAirInterface Tech : openair_tech@eurecom.fr
   OpenAirInterface Dev  : openair4g-devel@lists.eurecom.fr
  
   Address      : Eurecom, Campus SophiaTech, 450 Route des Chappes, CS 50193 - 06904 Biot Sophia Antipolis cedex, FRANCE

 *******************************************************************************/

/** sodera_lib.c
 *
 * Author: Raymond Knopp
 */
34 35 36
 
#include <vector>
#include <string>
37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52
#include <stdio.h>
#include <stdlib.h>
#include <inttypes.h>
#include <string.h>
#include <pthread.h>
#include <unistd.h>


#include <iostream>
#include <complex>
#include <fstream>
#include <cmath>

#include "common_lib.h"

#include "lmsComms.h"
53
#include "LMS7002M.h"
54
#include "Si5351C.h"
kaltenbe's avatar
kaltenbe committed
55 56
#include "LMS_StreamBoard.h"

57 58 59 60 61 62 63 64
#ifdef __SSE4_1__
#  include <smmintrin.h>
#endif
 
#ifdef __AVX2__
#  include <immintrin.h>
#endif

65 66
using namespace std;

67 68 69 70
int num_devices=0;
/*These items configure the underlying asynch stream used by the the sync interface. 
 */

Raymond Knopp's avatar
Raymond Knopp committed
71 72
#define BUFFERSIZE 4096
#define BUFFERSCOUNT 32
73 74 75 76 77 78 79
typedef struct
{

  // --------------------------------
  // variables for SoDeRa configuration
  // --------------------------------

80 81
  LMScomms Port;
  Si5351C Si;
kaltenbe's avatar
kaltenbe committed
82 83 84
  LMS7002M lmsControl;
  LMS_StreamBoard *lmsStream;

Raymond Knopp's avatar
Raymond Knopp committed
85
  char buffers_rx[BUFFERSIZE*BUFFERSCOUNT];
86
  int handles[BUFFERSCOUNT];
Raymond Knopp's avatar
Raymond Knopp committed
87
  int current_handle;
88 89
  int samples_left_buffer;

90 91 92 93
  double sample_rate;
  // time offset between transmiter timestamp and receiver timestamp;
  double tdiff;

94
  int channelscount;
95 96 97 98 99 100 101 102 103 104 105
  // --------------------------------
  // Debug and output control
  // --------------------------------
  int num_underflows;
  int num_overflows;
  int num_seq_errors;

  int64_t tx_count;
  int64_t rx_count;
  openair0_timestamp rx_timestamp;

kaltenbe's avatar
kaltenbe committed
106
} sodera_t;
107

108 109 110
typedef struct {
  uint8_t reserved[8];
  uint64_t counter;
Raymond Knopp's avatar
Raymond Knopp committed
111
  char data[4080];
112 113
} StreamPacket_t;

kaltenbe's avatar
kaltenbe committed
114
sodera_t sodera_state;
115

116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147
enum STATUS {
  SUCCESS,
  FAILURE
};

STATUS SPI_write(LMScomms* dataPort, uint16_t address, uint16_t data)
{
  assert(dataPort != nullptr);
  LMScomms::GenericPacket ctrPkt;
  ctrPkt.cmd = CMD_BRDSPI_WR;
  ctrPkt.outBuffer.push_back((address >> 8) & 0xFF);
  ctrPkt.outBuffer.push_back(address & 0xFF);
  ctrPkt.outBuffer.push_back((data >> 8) & 0xFF);
  ctrPkt.outBuffer.push_back(data & 0xFF);
  dataPort->TransferPacket(ctrPkt);
  return ctrPkt.status == 1 ? SUCCESS : FAILURE;
}

uint16_t SPI_read(LMScomms* dataPort, uint16_t address)
{
  assert(dataPort != nullptr);
  LMScomms::GenericPacket ctrPkt;
  ctrPkt.cmd = CMD_BRDSPI_RD;
  ctrPkt.outBuffer.push_back((address >> 8) & 0xFF);
  ctrPkt.outBuffer.push_back(address & 0xFF);
  dataPort->TransferPacket(ctrPkt);
  if (ctrPkt.inBuffer.size() > 4)
    return ctrPkt.inBuffer[2] * 256 + ctrPkt.inBuffer[3];
  else
    return 0;
}

148 149
static int trx_sodera_start(openair0_device *device)
{
kaltenbe's avatar
kaltenbe committed
150
  sodera_t *s = (sodera_t*)device->priv;
151

152

153 154
  // init recv and send streaming

kaltenbe's avatar
kaltenbe committed
155
  printf("Starting LMS Streaming\n");
156 157 158
  s->rx_count = 0;
  s->tx_count = 0;
  s->rx_timestamp = 0;
159 160 161
  s->current_handle = 0;

  // switch off RX
Raymond Knopp's avatar
Raymond Knopp committed
162 163
  uint16_t regVal = SPI_read(&s->Port,0x0005);
  SPI_write(&s->Port,0x0005,regVal & ~0x6);
164

kaltenbe's avatar
kaltenbe committed
165 166 167 168




169 170

  // USB FIFO reset
kaltenbe's avatar
kaltenbe committed
171
  LMScomms::GenericPacket ctrPkt; 
Raymond Knopp's avatar
Raymond Knopp committed
172
  ctrPkt.cmd = CMD_USB_FIFO_RST;
173 174 175 176
  ctrPkt.outBuffer.push_back(0x01);
  s->Port.TransferPacket(ctrPkt);
  ctrPkt.outBuffer[0]=0x00;
  s->Port.TransferPacket(ctrPkt);
kaltenbe's avatar
kaltenbe committed
177
 
Raymond Knopp's avatar
Raymond Knopp committed
178
  regVal = SPI_read(&s->Port,0x0005);
179
  // provide timestamp, set streamTXEN, set TX/RX enable 
Raymond Knopp's avatar
Raymond Knopp committed
180
  SPI_write(&s->Port,0x0005,(regVal & ~0x20) | 0x6);
181

kaltenbe's avatar
kaltenbe committed
182 183 184 185 186 187 188 189 190 191 192

  if (s->channelscount==2) {
    SPI_write(&s->Port,0x0001,0x0003);
    SPI_write(&s->Port,0x0007,0x000A);
  }
  else {
    SPI_write(&s->Port,0x0001,0x0001);
    SPI_write(&s->Port,0x0007,0x0008);
  }


193
  for (int i=0; i< BUFFERSCOUNT ; i++) 
Raymond Knopp's avatar
Raymond Knopp committed
194
    s->handles[i] = s->Port.BeginDataReading(&s->buffers_rx[i*BUFFERSIZE],BUFFERSIZE);
kaltenbe's avatar
kaltenbe committed
195
  printf("Armed %d transfers\n",BUFFERSCOUNT);
196 197 198 199 200
  return 0;
}

static void trx_sodera_end(openair0_device *device)
{
kaltenbe's avatar
kaltenbe committed
201
  sodera_t *s = (sodera_t*)device->priv;
202 203


204
  // stop TX/RX if they were active
Raymond Knopp's avatar
Raymond Knopp committed
205 206
  uint16_t regVal = SPI_read(&s->Port,0x0005);
  SPI_write(&s->Port,0x0005,regVal & ~0x6);
207

208 209 210 211
}

static int trx_sodera_write(openair0_device *device, openair0_timestamp timestamp, void **buff, int nsamps, int cc, int flags)
{
kaltenbe's avatar
kaltenbe committed
212
  sodera_t *s = (sodera_t*)device->priv;
213 214 215 216 217


  return 0;
}

kaltenbe's avatar
kaltenbe committed
218 219
#define DEBUG_READ 1

220 221
static int trx_sodera_read(openair0_device *device, openair0_timestamp *ptimestamp, void **buff, int nsamps, int cc)
{
kaltenbe's avatar
kaltenbe committed
222
   sodera_t *s = (sodera_t*)device->priv;
223 224
   int samples_received=0,i,j;
   int nsamps2;  // aligned to upper 32 or 16 byte boundary
225 226
   StreamPacket_t *p;
   int16_t sampleI,sampleQ;
Raymond Knopp's avatar
Raymond Knopp committed
227
   char *pktStart;
228 229 230
   int offset = 0;
   int num_p;
   int ind=0;
Raymond Knopp's avatar
Raymond Knopp committed
231 232
   int buffsize;
   int spp;
kaltenbe's avatar
kaltenbe committed
233
   int bufindex;
234 235

   // this assumes that each request is of size 4096 bytes (spp = 4080/4/channelscount)
Raymond Knopp's avatar
Raymond Knopp committed
236 237 238
   spp = sizeof(p->data)>>2; // spp = size of payload in samples  
   spp /= s->channelscount;

kaltenbe's avatar
kaltenbe committed
239 240 241 242 243
#ifdef DEBUG_READ
   printf("\nIn trx_read\n");
   printf("s->current_handle %d\n", s->current_handle);
   printf("s->samples_left_buffer %d\n",s->samples_left_buffer);
#endif
244 245 246
   // first get rid of remaining samples
   if (s->samples_left_buffer > 0) {
     buffsize = min(s->samples_left_buffer,nsamps);
kaltenbe's avatar
kaltenbe committed
247
     pktStart = ((StreamPacket_t*)&s->buffers_rx[s->current_handle*BUFFERSIZE])->data;
Raymond Knopp's avatar
Raymond Knopp committed
248
     pktStart += (spp-s->samples_left_buffer);
249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269
     const int stepSize = s->channelscount * 3;

     for (int b=0;b<buffsize<<2;b+=stepSize) {
       for (int ch=0;ch<s->channelscount;ch++) {
	 // I sample
	 sampleI = (pktStart[b + 1 + 3*ch]&0x0F)<<8;
	 sampleI |= (pktStart[b + 3*ch]&0xFF);
	 sampleI = (sampleI<<4)>>4;
	 // Q sample
	 sampleQ = (pktStart[b + 2 + 3*ch]&0x0F)<<8;
	 sampleQ |= (pktStart[b + 1 + 3*ch]&0xFF);
	 sampleQ = (sampleQ<<4)>>4;
	 ((uint32_t*)buff[ch])[ind] = ((uint32_t)sampleI) | (((uint32_t)sampleQ)<<16);
       }
       ind++;
     }
   }
   if (ind == nsamps) {
     s->samples_left_buffer -= nsamps;
     s->rx_count += nsamps;
     *ptimestamp = s->rx_timestamp;
kaltenbe's avatar
kaltenbe committed
270 271
     s->rx_timestamp+=nsamps;
      
272 273 274 275 276 277
     return(nsamps);
   }
   else {
     s->samples_left_buffer = 0;
     nsamps -= ind;
     samples_received = ind;
278
   }
279 280 281 282 283 284 285 286 287

   // This is for the left-over part => READ from USB



   num_p = nsamps / spp;
   if ((nsamps%spp) > 0)
     num_p++;
   s->samples_left_buffer = (num_p*spp)-nsamps;
kaltenbe's avatar
kaltenbe committed
288
  
289

kaltenbe's avatar
kaltenbe committed
290 291 292
#ifdef DEBUG_READ
   printf("num_p %d\n",num_p);
#endif
293 294 295 296
   const int stepSize = s->channelscount * 3;

   for (i=0;i<num_p;i++) {

kaltenbe's avatar
kaltenbe committed
297 298 299 300 301
     bufindex = (s->current_handle+i)&(BUFFERSCOUNT-1);
     if (s->Port.WaitForReading(s->handles[bufindex],1000) == false) {
       printf("[recv] Error: request %d samples (%d/%d) WaitForReading timed out\n",nsamps,bufindex,num_p);
       *ptimestamp = s->rx_timestamp;
       s->rx_timestamp+=samples_received;
302 303
       return(samples_received);
     }
Raymond Knopp's avatar
Raymond Knopp committed
304
     long bytesToRead=BUFFERSIZE;
kaltenbe's avatar
kaltenbe committed
305 306 307 308
     if (s->Port.FinishDataReading(&s->buffers_rx[bufindex*BUFFERSIZE],bytesToRead,s->handles[bufindex]) != BUFFERSIZE) {  
       printf("[recv] Error: request %d samples (%d/%d) WaitForReading timed out\n",nsamps,bufindex,num_p);
       *ptimestamp = s->rx_timestamp;
       s->rx_timestamp+=samples_received;
309 310
       return(samples_received);
     }
311
    
kaltenbe's avatar
kaltenbe committed
312
     p = (StreamPacket_t*)&s->buffers_rx[bufindex*BUFFERSIZE];
313 314
     // handle timestamp
     if ((i==0) & (ind==0)) { // grab the timestamp from HW
kaltenbe's avatar
kaltenbe committed
315 316 317 318 319
       *ptimestamp = p->counter;
       s->rx_timestamp = p->counter+nsamps; // for next time
#ifdef DEBUG_READ
       printf("RX timestamp %d\n",s->rx_timestamp);
#endif
320 321 322 323 324 325 326 327
     }
     else { // check the timestamp
       if (i==0) {
	 if ((s->rx_timestamp + ind) != p->counter) {
	   printf("Error, RX timestamp error, got %llu, should be %llu\n",p->counter,s->rx_timestamp+ind);
	   return(ind);
	 }
       }
kaltenbe's avatar
kaltenbe committed
328 329
       *ptimestamp = s->rx_timestamp;
       s->rx_timestamp+=nsamps;
330
     }
Raymond Knopp's avatar
Raymond Knopp committed
331 332
     pktStart = p->data;
     for (uint16_t b=0;b<sizeof(p->data);b+=stepSize) {
333 334 335 336 337 338 339 340 341 342 343 344 345 346
       for (int ch=0;ch < s->channelscount;ch++) {
	 // I sample
	 sampleI = (pktStart[b + 1 + 3*ch]&0x0F)<<8;
	 sampleI |= (pktStart[b + 3*ch]&0xFF);
	 sampleI = (sampleI<<4)>>4;
	 // Q sample
	 sampleQ = (pktStart[b + 2 + 3*ch]&0x0F)<<8;
	 sampleQ |= (pktStart[b + 1 + 3*ch]&0xFF);
	 sampleQ = (sampleQ<<4)>>4;
	 ((uint32_t*)buff[ch])[ind] = ((uint32_t)sampleI) | (((uint32_t)sampleQ)<<16);
       }
       ind++;	        
     }
     samples_received+=spp;
kaltenbe's avatar
kaltenbe committed
347 348 349
     // schedule a new transmission for this index
     s->handles[bufindex] = s->Port.BeginDataReading(&s->buffers_rx[bufindex*BUFFERSIZE],BUFFERSIZE);
     s->current_handle=(s->current_handle+1)&(BUFFERSCOUNT-1);
350
   }   
351 352 353

  //handle the error code

354
  s->rx_count += samples_received;
355 356 357 358 359 360 361 362 363 364 365 366 367 368
  //  s->rx_timestamp = s->rx_md.time_spec.to_ticks(s->sample_rate);

  return samples_received;
}



static bool is_equal(double a, double b)
{
  return fabs(a-b) < 1e-6;
}

int trx_sodera_set_freq(openair0_device* device, openair0_config_t *openair0_cfg, int dummy) {

kaltenbe's avatar
kaltenbe committed
369
  sodera_t *s = (sodera_t*)device->priv;
370 371 372 373 374 375 376 377 378 379

  //  s->usrp->set_tx_freq(openair0_cfg[0].tx_freq[0]);
  //  s->usrp->set_rx_freq(openair0_cfg[0].rx_freq[0]);

  return(0);
  
}

int openair0_set_rx_frequencies(openair0_device* device, openair0_config_t *openair0_cfg) {

kaltenbe's avatar
kaltenbe committed
380
  sodera_t *s = (sodera_t*)device->priv;
381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397
  static int first_call=1;
  static double rf_freq,diff;

  //  uhd::tune_request_t rx_tune_req(openair0_cfg[0].rx_freq[0]);

  //  rx_tune_req.rf_freq_policy = uhd::tune_request_t::POLICY_MANUAL;
  //  rx_tune_req.rf_freq = openair0_cfg[0].rx_freq[0];
  //  rf_freq=openair0_cfg[0].rx_freq[0];
  //  s->usrp->set_rx_freq(rx_tune_req);

  return(0);
  
}

int trx_sodera_set_gains(openair0_device* device, 
		       openair0_config_t *openair0_cfg) {

kaltenbe's avatar
kaltenbe committed
398
  sodera_t *s = (sodera_t*)device->priv;
399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488

  //  s->usrp->set_tx_gain(openair0_cfg[0].tx_gain[0]);
  //  ::uhd::gain_range_t gain_range = s->usrp->get_rx_gain_range(0);
  // limit to maximum gain
  /* if (openair0_cfg[0].rx_gain[0]-openair0_cfg[0].rx_gain_offset[0] > gain_range.stop()) {
    
    printf("RX Gain 0 too high, reduce by %f dB\n",
	   openair0_cfg[0].rx_gain[0]-openair0_cfg[0].rx_gain_offset[0] - gain_range.stop());	   
    exit(-1);
  }
  s->usrp->set_rx_gain(openair0_cfg[0].rx_gain[0]-openair0_cfg[0].rx_gain_offset[0]);
  printf("Setting SODERA RX gain to %f (rx_gain %f,gain_range.stop() %f)\n", openair0_cfg[0].rx_gain[0]-openair0_cfg[0].rx_gain_offset[0],openair0_cfg[0].rx_gain[0],gain_range.stop());
  */
  return(0);
}

int trx_sodera_stop(int card) {
  return(0);
}


rx_gain_calib_table_t calib_table_sodera[] = {
  {3500000000.0,44.0},
  {2660000000.0,49.0},
  {2300000000.0,50.0},
  {1880000000.0,53.0},
  {816000000.0,58.0},
  {-1,0}};

void set_rx_gain_offset(openair0_config_t *openair0_cfg, int chain_index,int bw_gain_adjust) {

  int i=0;
  // loop through calibration table to find best adjustment factor for RX frequency
  double min_diff = 6e9,diff,gain_adj=0.0;
  if (bw_gain_adjust==1) {
    switch ((int)openair0_cfg[0].sample_rate) {
    case 30720000:      
      break;
    case 23040000:
      gain_adj=1.25;
      break;
    case 15360000:
      gain_adj=3.0;
      break;
    case 7680000:
      gain_adj=6.0;
      break;
    case 3840000:
      gain_adj=9.0;
      break;
    case 1920000:
      gain_adj=12.0;
      break;
    default:
      printf("unknown sampling rate %d\n",(int)openair0_cfg[0].sample_rate);
      exit(-1);
      break;
    }
  }
  while (openair0_cfg->rx_gain_calib_table[i].freq>0) {
    diff = fabs(openair0_cfg->rx_freq[chain_index] - openair0_cfg->rx_gain_calib_table[i].freq);
    printf("cal %d: freq %f, offset %f, diff %f\n",
	   i,
	   openair0_cfg->rx_gain_calib_table[i].freq,
	   openair0_cfg->rx_gain_calib_table[i].offset,diff);
    if (min_diff > diff) {
      min_diff = diff;
      openair0_cfg->rx_gain_offset[chain_index] = openair0_cfg->rx_gain_calib_table[i].offset+gain_adj;
    }
    i++;
  }
  
}


int trx_sodera_get_stats(openair0_device* device) {

  return(0);

}
int trx_sodera_reset_stats(openair0_device* device) {

  return(0);

}


int openair0_dev_init_sodera(openair0_device* device, openair0_config_t *openair0_cfg)
{

kaltenbe's avatar
kaltenbe committed
489
  sodera_t *s=&sodera_state;
490

491 492 493
  size_t i;

  // Initialize SODERA device
494 495
  s->Port.RefreshDeviceList();
  vector<string> deviceNames=s->Port.GetDeviceList();
496

497 498 499 500 501 502 503 504 505 506 507
  if (deviceNames.size() == 1) {
    if (s->Port.Open(0) != IConnection::SUCCESS) {
      printf("Cannot open SoDeRa\n");
      exit(-1);
    }
    LMSinfo devInfo = s->Port.GetInfo();
    printf("Device %s, HW: %d, FW: %d, Protocol %d\n",
	   GetDeviceName(devInfo.device),
	   (int)devInfo.hardware,
	   (int)devInfo.firmware,
	   (int)devInfo.protocol);
kaltenbe's avatar
kaltenbe committed
508
    
509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528
    printf("Configuring Si5351C\n");
    s->Si.Initialize(&s->Port);
    s->Si.SetPLL(0, 25000000, 0);
    s->Si.SetPLL(1, 25000000, 0);
    s->Si.SetClock(0, 27000000, true, false);
    s->Si.SetClock(1, 27000000, true, false);
    for (int i = 2; i < 8; ++i)
      s->Si.SetClock(i, 27000000, false, false);
    Si5351C::Status status = s->Si.ConfigureClocks();
    if (status != Si5351C::SUCCESS)
      {
	printf("Failed to configure Si5351C");
	exit(-1);
      }
    status = s->Si.UploadConfiguration();
    if (status != Si5351C::SUCCESS)
      printf("Failed to upload Si5351C configuration");
    

    printf("Configuring LMS7002\n");
529
    
530
    int bw_gain_adjust=0;
531 532

   
533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570
    openair0_cfg[0].rx_gain_calib_table = calib_table_sodera;

    switch ((int)openair0_cfg[0].sample_rate) {
    case 30720000:
      // from usrp_time_offset
      openair0_cfg[0].samples_per_packet    = 2048;
      openair0_cfg[0].tx_sample_advance     = 15;
      openair0_cfg[0].tx_bw                 = 20e6;
      openair0_cfg[0].rx_bw                 = 20e6;
      openair0_cfg[0].tx_scheduling_advance = 8*openair0_cfg[0].samples_per_packet;
      break;
    case 15360000:
      openair0_cfg[0].samples_per_packet    = 2048;
      openair0_cfg[0].tx_sample_advance     = 45;
      openair0_cfg[0].tx_bw                 = 10e6;
      openair0_cfg[0].rx_bw                 = 10e6;
      openair0_cfg[0].tx_scheduling_advance = 5*openair0_cfg[0].samples_per_packet;
      break;
    case 7680000:
      openair0_cfg[0].samples_per_packet    = 1024;
      openair0_cfg[0].tx_sample_advance     = 50;
      openair0_cfg[0].tx_bw                 = 5e6;
      openair0_cfg[0].rx_bw                 = 5e6;
      openair0_cfg[0].tx_scheduling_advance = 5*openair0_cfg[0].samples_per_packet;
      break;
    case 1920000:
      openair0_cfg[0].samples_per_packet    = 256;
      openair0_cfg[0].tx_sample_advance     = 50;
      openair0_cfg[0].tx_bw                 = 1.25e6;
      openair0_cfg[0].rx_bw                 = 1.25e6;
      openair0_cfg[0].tx_scheduling_advance = 8*openair0_cfg[0].samples_per_packet;
      break;
    default:
      printf("Error: unknown sampling rate %f\n",openair0_cfg[0].sample_rate);
      exit(-1);
      break;

    }
kaltenbe's avatar
kaltenbe committed
571 572 573

    s->lmsControl = LMS7002M(&s->Port);

574
    liblms7_status opStatus;
kaltenbe's avatar
kaltenbe committed
575 576
    s->lmsControl.ResetChip();
    opStatus = s->lmsControl.LoadConfig(openair0_cfg[0].configFilename);
577 578 579 580 581
    
    if (opStatus != LIBLMS7_SUCCESS) {
      printf("Failed to load configuration file %s\n",openair0_cfg[0].configFilename);
      exit(-1);
    }
kaltenbe's avatar
kaltenbe committed
582
    opStatus = s->lmsControl.UploadAll();
583 584 585 586 587 588

    if (opStatus != LIBLMS7_SUCCESS) {
      printf("Failed to upload configuration file\n");
      exit(-1);
    }
    
kaltenbe's avatar
kaltenbe committed
589
    opStatus = s->lmsControl.SetFrequencySX(LMS7002M::Tx, openair0_cfg[0].tx_freq[0]/1e6,30.72);
590 591 592 593 594 595

    if (opStatus != LIBLMS7_SUCCESS) {
      printf("Cannot set TX frequency %f MHz\n",openair0_cfg[0].tx_freq[0]/1e6);
      exit(-1);
    }

kaltenbe's avatar
kaltenbe committed
596
    opStatus = s->lmsControl.SetFrequencySX(LMS7002M::Rx, openair0_cfg[0].rx_freq[0]/1e6,30.72);
597 598 599 600 601 602

    if (opStatus != LIBLMS7_SUCCESS) {
      printf("Cannot set RX frequency %f MHz\n",openair0_cfg[0].rx_freq[0]/1e6);
      exit(-1);
    }

603

604 605
    
    // this makes RX/TX sampling rates equal
kaltenbe's avatar
kaltenbe committed
606
    opStatus = s->lmsControl.Modify_SPI_Reg_bits(EN_ADCCLKH_CLKGN,0);
607 608 609 610
    if (opStatus != LIBLMS7_SUCCESS) {
      printf("Cannot modify SPI (EN_ADCCLKH_CLKGN)\n");
      exit(-1);
    }
kaltenbe's avatar
kaltenbe committed
611
    opStatus = s->lmsControl.Modify_SPI_Reg_bits(CLKH_OV_CLKL_CGEN,2);
612 613 614 615 616 617 618 619
    if (opStatus != LIBLMS7_SUCCESS) {
      printf("Cannot modify SPI (CLKH_OV_CLKL_CGEN)\n");
      exit(-1);
    }

    const float cgen_freq_MHz = 245.76;
    const int interpolation   = 0; // real interpolation = 2
    const int decimation      = 0; // real decimation = 2
kaltenbe's avatar
kaltenbe committed
620
    opStatus = s->lmsControl.SetInterfaceFrequency(cgen_freq_MHz,interpolation,decimation);
621 622 623 624
    if (opStatus != LIBLMS7_SUCCESS) {
      printf("Cannot SetInterfaceFrequency (%f,%d,%d)\n",cgen_freq_MHz,interpolation,decimation);
      exit(-1);
    }
kaltenbe's avatar
kaltenbe committed
625
    /*
626 627
    // Run calibration procedure
    float txrx_calibrationBandwidth_MHz = 5;
kaltenbe's avatar
kaltenbe committed
628
    opStatus = s->lmsControl.CalibrateTx(txrx_calibrationBandwidth_MHz);
629 630 631 632
    if (opStatus != LIBLMS7_SUCCESS){
      printf("TX Calibration failed\n");
      exit(-1);
    }
kaltenbe's avatar
kaltenbe committed
633
    opStatus = s->lmsControl.CalibrateRx(txrx_calibrationBandwidth_MHz);
634 635 636 637
    if (opStatus != LIBLMS7_SUCCESS){
      printf("RX Calibration failed\n");
      exit(-1);
    }
kaltenbe's avatar
kaltenbe committed
638
    */
kaltenbe's avatar
kaltenbe committed
639 640 641
        
    s->lmsStream = new LMS_StreamBoard(&s->Port);    
    LMS_StreamBoard::Status opStreamStatus; 
642
    // this will configure that sampling rate at output of FPGA
kaltenbe's avatar
kaltenbe committed
643 644 645 646 647 648
    opStreamStatus = s->lmsStream->ConfigurePLL(&s->Port,openair0_cfg[0].sample_rate,openair0_cfg[0].sample_rate,90);
    if (opStatus != LIBLMS7_SUCCESS){
      printf("Sample rate programming failed\n");
      exit(-1);
    }
    
649
    /*
650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700
      ::uhd::gain_range_t gain_range = s->usrp->get_rx_gain_range(i);
      // limit to maximum gain
      if (openair0_cfg[0].rx_gain[i]-openair0_cfg[0].rx_gain_offset[i] > gain_range.stop()) {
	
        printf("RX Gain %lu too high, lower by %f dB\n",i,openair0_cfg[0].rx_gain[i]-openair0_cfg[0].rx_gain_offset[i] - gain_range.stop());
	exit(-1);
      }
      s->usrp->set_rx_gain(openair0_cfg[0].rx_gain[i]-openair0_cfg[0].rx_gain_offset[i],i);
      printf("RX Gain %lu %f (%f) => %f (max %f)\n",i,
	     openair0_cfg[0].rx_gain[i],openair0_cfg[0].rx_gain_offset[i],
	     openair0_cfg[0].rx_gain[i]-openair0_cfg[0].rx_gain_offset[i],gain_range.stop());
    }
  }
  for(i=0;i<s->usrp->get_tx_num_channels();i++) {
    if (i<openair0_cfg[0].tx_num_channels) {
      s->usrp->set_tx_rate(openair0_cfg[0].sample_rate,i);
      s->usrp->set_tx_bandwidth(openair0_cfg[0].tx_bw,i);
      printf("Setting tx freq/gain on channel %lu/%lu: BW %f (readback %f)\n",i,s->usrp->get_tx_num_channels(),openair0_cfg[0].tx_bw/1e6,s->usrp->get_tx_bandwidth(i)/1e6);
      s->usrp->set_tx_freq(openair0_cfg[0].tx_freq[i],i);
      s->usrp->set_tx_gain(openair0_cfg[0].tx_gain[i],i);
    }
  }
  */

  // create tx & rx streamer

  //stream_args_rx.args["spp"] = str(boost::format("%d") % 2048);//(openair0_cfg[0].rx_num_channels*openair0_cfg[0].samples_per_packet));
  
  /*
  for (i=0;i<openair0_cfg[0].rx_num_channels;i++) {
    if (i<openair0_cfg[0].rx_num_channels) {
      printf("RX Channel %lu\n",i);
      std::cout << boost::format("Actual RX sample rate: %fMSps...") % (s->usrp->get_rx_rate(i)/1e6) << std::endl;
      std::cout << boost::format("Actual RX frequency: %fGHz...") % (s->usrp->get_rx_freq(i)/1e9) << std::endl;
      std::cout << boost::format("Actual RX gain: %f...") % (s->usrp->get_rx_gain(i)) << std::endl;
      std::cout << boost::format("Actual RX bandwidth: %fM...") % (s->usrp->get_rx_bandwidth(i)/1e6) << std::endl;
      std::cout << boost::format("Actual RX antenna: %s...") % (s->usrp->get_rx_antenna(i)) << std::endl;
    }
  }
  
  for (i=0;i<openair0_cfg[0].tx_num_channels;i++) {

    if (i<openair0_cfg[0].tx_num_channels) { 
      printf("TX Channel %lu\n",i);
      std::cout << std::endl<<boost::format("Actual TX sample rate: %fMSps...") % (s->usrp->get_tx_rate(i)/1e6) << std::endl;
      std::cout << boost::format("Actual TX frequency: %fGHz...") % (s->usrp->get_tx_freq(i)/1e9) << std::endl;
      std::cout << boost::format("Actual TX gain: %f...") % (s->usrp->get_tx_gain(i)) << std::endl;
      std::cout << boost::format("Actual TX bandwidth: %fM...") % (s->usrp->get_tx_bandwidth(i)/1e6) << std::endl;
      std::cout << boost::format("Actual TX antenna: %s...") % (s->usrp->get_tx_antenna(i)) << std::endl;
    }
  */
701 702 703 704 705
  }
  else {
    printf("Please connect SoDeRa\n");
    exit(-1);
  }
706 707 708 709 710 711 712 713 714 715 716 717

  device->priv = s;
  device->trx_start_func = trx_sodera_start;
  device->trx_write_func = trx_sodera_write;
  device->trx_read_func  = trx_sodera_read;
  device->trx_get_stats_func = trx_sodera_get_stats;
  device->trx_reset_stats_func = trx_sodera_reset_stats;
  device->trx_end_func   = trx_sodera_end;
  device->trx_stop_func  = trx_sodera_stop;
  device->trx_set_freq_func = trx_sodera_set_freq;
  device->trx_set_gains_func   = trx_sodera_set_gains;
  
718 719 720
  s->sample_rate   = openair0_cfg[0].sample_rate;
  s->channelscount = openair0_cfg[0].rx_num_channels;

721 722 723
  // TODO:
  return 0;
}