From caf4488eeae069bbb5dd2bf5301c68f0a1e933a9 Mon Sep 17 00:00:00 2001
From: ismagom <gomezi@tcd.ie>
Date: Tue, 22 Jul 2014 16:56:56 +0200
Subject: [PATCH] Fixed SCAN_MIB example. Fixed other issues with
 synchronization and AGC in ue_sync module

---
 cuhd/lib/cuhd_utils.c                     |   4 +-
 lte/phy/examples/iodev.c                  |   4 -
 lte/phy/examples/pdsch_ue.c               |  24 +-
 lte/phy/examples/scan_mib.c               | 532 ++++------------------
 lte/phy/include/liblte/phy/agc/agc.h      |  14 +-
 lte/phy/include/liblte/phy/phch/pbch.h    |   3 +-
 lte/phy/include/liblte/phy/phch/ue_sync.h |  27 +-
 lte/phy/lib/agc/src/agc.c                 |  37 +-
 lte/phy/lib/common/src/phy_common.c       |   2 +-
 lte/phy/lib/phch/src/pbch.c               |  20 +-
 lte/phy/lib/phch/src/ue_dl.c              |  17 +-
 lte/phy/lib/phch/src/ue_sync.c            | 144 ++++--
 lte/phy/lib/phch/test/pbch_file_test.c    |   4 +-
 lte/phy/lib/phch/test/pbch_test.c         |   2 +-
 lte/phy/lib/phch/test/ue_sync_usrp.c      |   6 -
 lte/phy/lib/sync/src/pss.c                |   2 +-
 lte/phy/lib/sync/src/sync.c               |  69 +--
 lte/phy/lib/sync/test/sync_test.c         |   4 +-
 lte/phy/lib/utils/src/vector.c            |   3 +-
 19 files changed, 345 insertions(+), 573 deletions(-)

diff --git a/cuhd/lib/cuhd_utils.c b/cuhd/lib/cuhd_utils.c
index 9c4b1e9ac..b6dfcfa24 100644
--- a/cuhd/lib/cuhd_utils.c
+++ b/cuhd/lib/cuhd_utils.c
@@ -46,7 +46,7 @@ int cuhd_rssi_scan(void *uhd, float *freqs, float *rssi, int nof_bands, double f
     goto free_and_exit;
   }
 
-  cuhd_set_rx_gain(uhd, 0.0);
+  cuhd_set_rx_gain(uhd, 20.0);
   cuhd_set_rx_srate(uhd, fs);
 
   for (i=0;i<nof_bands;i++) {
@@ -55,7 +55,7 @@ int cuhd_rssi_scan(void *uhd, float *freqs, float *rssi, int nof_bands, double f
     f = (double) freqs[i];
     cuhd_set_rx_freq(uhd, f);
     cuhd_rx_wait_lo_locked(uhd);
-
+    usleep(10000);
     cuhd_start_rx_stream(uhd);
 
     /* discard first samples */
diff --git a/lte/phy/examples/iodev.c b/lte/phy/examples/iodev.c
index 056c64f16..af6916105 100644
--- a/lte/phy/examples/iodev.c
+++ b/lte/phy/examples/iodev.c
@@ -80,10 +80,6 @@ int iodev_init(iodev_t *q, iodev_cfg_t *config) {
 
     DEBUG("Starting receiver...\n", 0);
     cuhd_start_rx_stream(q->uhd);
-    
-    if (config->find_threshold > 0.0) {
-      ue_sync_set_threshold(&q->sframe, config->find_threshold);
-    }
         
     ue_sync_init(&q->sframe, cuhd_set_rx_srate, cuhd_recv_wrapper, q->uhd);
 
diff --git a/lte/phy/examples/pdsch_ue.c b/lte/phy/examples/pdsch_ue.c
index c0d2013af..eeed8e276 100644
--- a/lte/phy/examples/pdsch_ue.c
+++ b/lte/phy/examples/pdsch_ue.c
@@ -161,7 +161,7 @@ int main(int argc, char **argv) {
   uint32_t sf_idx;
   pbch_mib_t mib; 
   bool printed_sib = false; 
-  uint32_t rlen; 
+  int rlen; 
   
   parse_args(&prog_args, argc, argv);
   
@@ -183,6 +183,12 @@ int main(int argc, char **argv) {
   /* Initialize frame and subframe counters */
   sf_cnt = 0;
   sf_idx = 0; 
+
+  /* Decodes the SSS signal during the tracking phase. Extra overhead, but makes sure we are in the correct subframe */  
+  ue_sync_decode_sss_on_track(&iodev.sframe, true);
+  
+  /* Decodes the PBCH on each frame. Around 10% more overhead, but makes sure we are in the current System Frame Number (SFN) */
+  ue_sync_pbch_always(&iodev.sframe, false);
   
   /* Main loop */
   while (!go_exit && (sf_cnt < prog_args.nof_subframes || prog_args.nof_subframes == -1)) {
@@ -194,7 +200,10 @@ int main(int argc, char **argv) {
     }
     
     /* iodev_receive returns 1 if successfully read 1 aligned subframe */
-    if (ret == 1) {
+    if (ret == 0) {
+    printf("Finding PSS... Peak: %8.1f, Output level: %+.2f dB\r", 
+             sync_get_peak_value(&iodev.sframe.s), 10*log10f(agc_get_gain(&iodev.sframe.agc)));      
+    } else if (ret == 1) {
       if (!ue_dl_initiated) {
         if (iodev_isUSRP(&iodev)) {
           cell = ue_sync_get_cell(&iodev.sframe);
@@ -228,10 +237,12 @@ int main(int argc, char **argv) {
           printf("\n");fflush(stdout);
           printed_sib = true; 
         }
-        if (!(sf_cnt % 10)) {         
-          printf("Cell ID: %3d, RSSI: %+.2f dBm, CFO: %+.4f KHz, SFO: %+.4f Khz, TimeOffset: %4d, Errors: %4d/%4d, BLER: %.1e\r",
-              cell.id, 20*log10f(agc_get_rssi(&iodev.sframe.agc)), iodev.sframe.cur_cfo * 15, iodev.sframe.mean_time_offset / 5, iodev.sframe.peak_idx,
-              (int) ue_dl.pkt_errors, (int) ue_dl.pkts_total, (float) ue_dl.pkt_errors / ue_dl.pkts_total);
+        if (!(sf_cnt % 10)) {       
+          printf("RSSI: %+.2f dBm, CFO: %+.4f KHz, SFO: %+.4f Khz, TimeOffset: %4d, Errors: %4d/%4d, BLER: %.1e\r",
+                 20*log10f(agc_get_rssi(&iodev.sframe.agc))+30, 
+                 ue_sync_get_cfo(&iodev.sframe)/1000, ue_sync_get_sfo(&iodev.sframe)/1000, iodev.sframe.peak_idx,
+                 (int) ue_dl.pkt_errors, (int) ue_dl.pkts_total, (float) ue_dl.pkt_errors / ue_dl.pkts_total);
+          
           fflush(stdout);       
           if (VERBOSE_ISINFO()) {
             printf("\n");
@@ -250,6 +261,7 @@ int main(int argc, char **argv) {
         }        
       }
     }
+
     if (prog_args.nof_subframes > 0) {
       sf_cnt++;      
     }    
diff --git a/lte/phy/examples/scan_mib.c b/lte/phy/examples/scan_mib.c
index d92b06aea..d0670df32 100644
--- a/lte/phy/examples/scan_mib.c
+++ b/lte/phy/examples/scan_mib.c
@@ -36,73 +36,45 @@
 
 #include "liblte/phy/phy.h"
 
-//#define DISABLE_UHD
-
 #ifndef DISABLE_UHD
 #include "liblte/cuhd/cuhd.h"
 #endif
 
-#define MHZ       1000000
-#define SAMP_FREQ     1920000
-#define RSSI_FS      1000000
-#define FLEN       9600
-#define FLEN_PERIOD    0.005
+#define MHZ             1000000
+#define SAMP_FREQ       1920000
+#define FLEN            9600
+#define FLEN_PERIOD     0.005
 
-#define RSSI_DECIM    20
 
-#define IS_SIGNAL(i) (10*log10f(rssi[i]) + 30 > rssi_threshold)
-
-int band, earfcn=-1;
-float find_threshold = 10.0;
+int band = -1;
 int earfcn_start=-1, earfcn_end = -1;
-float rssi_threshold = -45.0;
-int max_track_lost=9;
-int nof_frames_find=20, nof_frames_track=100, nof_samples_rssi=50000;
-int track_len=500;
+int nof_frames_find=200;
 
-cf_t *input_buffer, *fft_buffer, *ce[MAX_PORTS];
-pbch_t pbch;
-lte_fft_t fft;
-chest_t chest;
-sync_t ssync;
-cfo_t cfocorr;
 
-float *cfo_v;
-int *idx_v, *idx_valid, *t;
-float *p2a_v;
-void *uhd;
-int nof_bands;
-float uhd_gain = 20.0;
+float uhd_gain = 60.0;
+char *uhd_args=""; 
 
 #define MAX_EARFCN 1000
 lte_earfcn_t channels[MAX_EARFCN];
-float rssi[MAX_EARFCN];
-float rssi_d[MAX_EARFCN/RSSI_DECIM];
-float freqs[MAX_EARFCN];
-float cfo[MAX_EARFCN];
-float p2a[MAX_EARFCN];
-
-enum sync_state {INIT, FIND, TRACK, MIB, DONE};
 
 
 void usage(char *prog) {
-  printf("Usage: %s [seRrFfTgv] -b band\n", prog);
+  printf("Usage: %s [asefgv] -b band\n", prog);
+  printf("\t-a UHD args [Default %s]\n", uhd_args);
+  printf("\t-g UHD gain [Default %.2f dB]\n", uhd_gain);
   printf("\t-s earfcn_start [Default All]\n");
   printf("\t-e earfcn_end [Default All]\n");
-  printf("\t-R rssi_nof_samples [Default %d]\n", nof_samples_rssi);
-  printf("\t-r rssi_threshold [Default %.2f dBm]\n", rssi_threshold);
-  printf("\t-F pss_find_nof_frames [Default %d]\n", nof_frames_find);
-  printf("\t-f pss_find_threshold [Default %.2f]\n", find_threshold);
-  printf("\t-T pss_track_nof_frames [Default %d]\n", nof_frames_track);
-  printf("\t-l pss_track_len [Default %d]\n", track_len);
-  printf("\t-g gain [Default %.2f dB]\n", uhd_gain);
+  printf("\t-f nof_frames_find [Default %d]\n", nof_frames_find);
   printf("\t-v [set verbose to debug, default none]\n");
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "bseRrFfTgv")) != -1) {
+  while ((opt = getopt(argc, argv, "asefgvb")) != -1) {
     switch(opt) {
+    case 'a':
+      uhd_args = argv[optind];
+      break;
     case 'b':
       band = atoi(argv[optind]);
       break;
@@ -112,20 +84,8 @@ void parse_args(int argc, char **argv) {
     case 'e':
       earfcn_end = atoi(argv[optind]);
       break;
-    case 'R':
-      nof_samples_rssi = atoi(argv[optind]);
-      break;
-    case 'r':
-      rssi_threshold = -atof(argv[optind]);
-      break;
-    case 'F':
-      nof_frames_find = atoi(argv[optind]);
-      break;
     case 'f':
-      find_threshold = atof(argv[optind]);
-      break;
-    case 'T':
-      nof_frames_track = atoi(argv[optind]);
+      nof_frames_find = atoi(argv[optind]);
       break;
     case 'g':
       uhd_gain = atof(argv[optind]);
@@ -138,416 +98,104 @@ void parse_args(int argc, char **argv) {
       exit(-1);
     }
   }
-}
-
-int base_init(int frame_length) {
-  int i;
-
-  input_buffer = malloc(2 * frame_length * sizeof(cf_t));
-  if (!input_buffer) {
-    perror("malloc");
-    return -1;
-  }
-
-  fft_buffer = malloc(CPNORM_NSYMB * 72 * sizeof(cf_t));
-  if (!fft_buffer) {
-    perror("malloc");
-    return -1;
-  }
-
-  for (i=0;i<MAX_PORTS;i++) {
-    ce[i] = malloc(CPNORM_NSYMB * 72 * sizeof(cf_t));
-    if (!ce[i]) {
-      perror("malloc");
-      return -1;
-    }
-  }
-  if (sync_init(&ssync, FLEN, 128, 128)) {
-    fprintf(stderr, "Error initiating PSS/SSS\n");
-    return -1;
-  }
-  if (chest_init(&chest, CPNORM, 6, MAX_PORTS)) {
-    fprintf(stderr, "Error initializing equalizer\n");
-    return -1;
-  }
-
-  if (lte_fft_init(&fft, CPNORM, 6)) {
-    fprintf(stderr, "Error initializing FFT\n");
-    return -1;
-  }
-  if (cfo_init(&cfocorr, FLEN)) {
-    fprintf(stderr, "Error initiating CFO\n");
-    return -1;
-  }
-
-  idx_v = malloc(nof_frames_track * sizeof(int));
-  if (!idx_v) {
-    perror("malloc");
-    return -1;
-  }
-  idx_valid = malloc(nof_frames_track * sizeof(int));
-  if (!idx_valid) {
-    perror("malloc");
-    return -1;
-  }
-  t = malloc(nof_frames_track * sizeof(int));
-  if (!t) {
-    perror("malloc");
-    return -1;
-  }
-  cfo_v = malloc(nof_frames_track * sizeof(float));
-  if (!cfo_v) {
-    perror("malloc");
-    return -1;
-  }
-  p2a_v = malloc(nof_frames_track * sizeof(float));
-  if (!p2a_v) {
-    perror("malloc");
-    return -1;
-  }
-
-  bzero(cfo, sizeof(float) * MAX_EARFCN);
-  bzero(p2a, sizeof(float) * MAX_EARFCN);
-
-  /* open UHD device */
-#ifndef DISABLE_UHD
-  printf("Opening UHD device...\n");
-  if (cuhd_open("",&uhd)) {
-    fprintf(stderr, "Error opening uhd\n");
-    return -1;
-  }
-#endif
-  return 0;
-}
-
-void base_free() {
-  int i;
-
-#ifndef DISABLE_UHD
-  cuhd_close(uhd);
-#endif
-
-  sync_free(&ssync);
-  lte_fft_free(&fft);
-  chest_free(&chest);
-  cfo_free(&cfocorr);
-
-  free(input_buffer);
-  free(fft_buffer);
-  for (i=0;i<MAX_PORTS;i++) {
-      free(ce[i]);
-  }
-  free(idx_v);
-  free(idx_valid);
-  free(t);
-  free(cfo_v);
-  free(p2a_v);
-}
-
-float mean_valid(int *idx_v, float *x, int nof_frames) {
-  int i;
-  float mean = 0;
-  int n = 0;
-  for (i=0;i<nof_frames;i++) {
-    if (idx_v[i] != -1) {
-      mean += x[i];
-      n++;
-    }
-  }
-  if (n > 0) {
-    return mean/n;
-  } else {
-    return 0.0;
+  if (band == -1) {
+    usage(argv[0]);
+    exit(-1);
   }
 }
 
-int preprocess_idx(int *in, int *out, int *period, int len) {
-  int i, n;
-  n=0;
-  for (i=0;i<len;i++) {
-    if (in[i] != -1) {
-      out[n] = in[i];
-      period[n] = i;
-      n++;
-    }
-  }
-  return n;
+int cuhd_recv_wrapper(void *h, void *data, uint32_t nsamples) {
+  DEBUG(" ----  Receive %d samples  ---- \n", nsamples);
+  return cuhd_recv(h, data, nsamples, 1);
 }
 
-int rssi_scan() {
-  int n=0;
-  int i;
-
-  if (nof_bands > 100) {
-    /* scan every Mhz, that is 10 freqs */
-    for (i=0;i<nof_bands;i+=RSSI_DECIM) {
-      freqs[n] = channels[i].fd * MHZ;
-      n++;
-    }
-#ifndef DISABLE_UHD
-    if (cuhd_rssi_scan(uhd, freqs, rssi_d, n, (double) RSSI_FS, nof_samples_rssi)) {
-      fprintf(stderr, "Error while doing RSSI scan\n");
-      return -1;
-    }
-#endif
-    /* linearly interpolate the rssi vector */
-    interp_linear_f(rssi_d, rssi, RSSI_DECIM, n);
-  } else {
-    for (i=0;i<nof_bands;i++) {
-      freqs[i] = channels[i].fd * MHZ;
-    }
-#ifndef DISABLE_UHD
-    if (cuhd_rssi_scan(uhd, freqs, rssi, nof_bands, (double) RSSI_FS, nof_samples_rssi)) {
-      fprintf(stderr, "Error while doing RSSI scan\n");
-      return -1;
-    }
-#endif
-    n = nof_bands;
-  }
-
-  return n;
-}
-
-
-int mib_decoder_init(int cell_id) {
-
-  lte_cell_t cell;
-  cell.id = cell_id;
-  cell.nof_prb = 6;
-  cell.nof_ports = 2;
-  cell.cp = CPNORM;
-  
-  if (chest_ref_LTEDL(&chest, cell)) {
-    fprintf(stderr, "Error initializing reference signal\n");
-    return -1;
-  }
-
-  if (pbch_init(&pbch, cell)) {
-    fprintf(stderr, "Error initiating PBCH\n");
-    return -1;
-  }
-  DEBUG("PBCH initiated cell_id=%d\n", cell_id);
-  return 0;
-}
-
-int mib_decoder_run(cf_t *input, pbch_mib_t *mib) {
-  int i;
-  lte_fft_run_slot(&fft, input, fft_buffer);
-
-  /* Get channel estimates for each port */
-  for (i=0;i<MAX_PORTS;i++) {
-    chest_ce_slot_port(&chest, fft_buffer, ce[i], 1, i);
-  }
-
-  DEBUG("Decoding PBCH\n", 0);
-  return pbch_decode(&pbch, fft_buffer, ce, mib);
-}
 
 int main(int argc, char **argv) {
-  int frame_cnt, valid_frames;
-  int freq;
-  int cell_id;
-  float max_peak_to_avg;
-  float sfo;
-  uint32_t track_idx, find_idx; 
-  int last_found;
-  enum sync_state state;
-  int n;
-  int mib_attempts;
-  int nslot;
-  pbch_mib_t mib;
   int ret;
+  int frame_cnt;
+  int nof_freqs; 
+  uint32_t freq;
+  ue_sync_t uesync; 
+  void *uhd;
+  cf_t *buffer; 
   
   if (argc < 3) {
     usage(argv[0]);
     exit(-1);
   }
-
+ 
   parse_args(argc,argv);
-
-  if (base_init(FLEN)) {
-    fprintf(stderr, "Error initializing memory\n");
+ 
+  printf("Opening UHD device...\n");
+  if (cuhd_open(uhd_args, &uhd)) {
+    fprintf(stderr, "Error opening uhd\n");
     exit(-1);
   }
-
-  sync_pss_det_peak_to_avg(&ssync);
-
-  nof_bands = lte_band_get_fd_band(band, channels, earfcn_start, earfcn_end, MAX_EARFCN);
-  printf("RSSI scan: %d freqs in band %d, RSSI threshold %.2f dBm\n", nof_bands, band, rssi_threshold);
-
-  n = rssi_scan();
-  if (n == -1) {
-    exit(-1);
-  }
-
-  printf("\nDone. Starting PSS search on %d channels\n", n);
-  usleep(500000);
-  INFO("Setting sampling frequency %.2f MHz\n", (float) SAMP_FREQ/MHZ);
-#ifndef DISABLE_UHD
-  cuhd_set_rx_srate(uhd, SAMP_FREQ);
-
+ 
+  /* set uhd_gain */
   cuhd_set_rx_gain(uhd, uhd_gain);
-#endif
 
-  freq=0;
-  state = INIT;
-  nslot = 0;
-  sfo = 0;
-  find_idx = 0;
-  frame_cnt = 0;
-  max_peak_to_avg = -1;
-  last_found = 0;
-  cell_id = 0;
+  nof_freqs = lte_band_get_fd_band(band, channels, earfcn_start, earfcn_end, MAX_EARFCN);
+  if (nof_freqs < 0) {
+    fprintf(stderr, "Error getting EARFCN list\n");
+    exit(-1);
+  }
+  
 
-  while(freq<nof_bands) {
-    /* scan only bands above rssi_threshold */
-    if (!IS_SIGNAL(freq)) {
-      INFO("[%3d/%d]: Skipping EARFCN %d %.2f MHz RSSI %.2f dB\n", freq, nof_bands,
-                channels[freq].id, channels[freq].fd,10*log10f(rssi[freq]) + 30);
-      freq++;
-    } else {
-      if (state != INIT && state != DONE) {
-#ifndef DISABLE_UHD
-        DEBUG(" -----   RECEIVING %d SAMPLES ---- \n", FLEN);
-        cuhd_recv(uhd, &input_buffer[FLEN], FLEN, 1);
-#endif
-      }
-      switch(state) {
-      case INIT:
-        DEBUG("Stopping receiver...\n",0);
-#ifndef DISABLE_UHD
-        cuhd_stop_rx_stream(uhd);
+  for (freq=0;freq<nof_freqs;freq++) {
+    /* set freq */
+    cuhd_stop_rx_stream(uhd);
+    cuhd_set_rx_freq(uhd, (double) channels[freq].fd * MHZ);
+    cuhd_rx_wait_lo_locked(uhd);
+        
+    if (ue_sync_init(&uesync, cuhd_set_rx_srate, cuhd_recv_wrapper, uhd)) {
+      fprintf(stderr, "Error initiating UE sync\n");
+      exit(-1);
+    }
+    ue_sync_set_nof_pbch_decodes(&uesync, 1);
+    ue_sync_decode_sss_on_track(&uesync, true);
+    ue_sync_change_srate(&uesync, false);
 
-        /* set freq */
-        cuhd_set_rx_freq(uhd, (double) channels[freq].fd * MHZ);
-        cuhd_rx_wait_lo_locked(uhd);
-        DEBUG("Set freq to %.3f MHz\n", (double) channels[freq].fd);
+    DEBUG("Starting receiver...\n",0);
+    cuhd_start_rx_stream(uhd);
+    usleep(10000);
+    
+    
+    /* Receive up to a maximum of nof_frames_find */
+    frame_cnt = 0; 
+    ret = 0;
+    ue_sync_reset(&uesync);
+    agc_reset(&uesync.agc);
 
-        DEBUG("Starting receiver...\n",0);
-        cuhd_start_rx_stream(uhd);
-#endif
-        /* init variables */
-        frame_cnt = 0;
-        max_peak_to_avg = -1;
-        cell_id = -1;
-
-        /* receive first frame */
-#ifndef DISABLE_UHD
-        cuhd_recv(uhd, input_buffer, FLEN, 1);
-#endif
-        /* set find_threshold and go to FIND state */
-        sync_set_threshold(&ssync, find_threshold, find_threshold/2);
-        state = FIND;
-        break;
-      case FIND:
-        /* find peak in all frame */
-        ret = sync_find(&ssync, &input_buffer[FLEN], &find_idx);
-        DEBUG("[%3d/%d]: PAR=%.2f\n", freq, nof_bands, sync_get_peak_value(&ssync));
-        if (ret == 1) {
-          /* if found peak, go to track and set lower threshold */
-          frame_cnt = -1;
-          last_found = 0;
-          max_peak_to_avg = -1;
-          cell_id = sync_get_cell_id(&ssync);
-
-          state = TRACK;
-          INFO("[%3d/%d]: EARFCN %d Freq. %.2f MHz PSS found PAR %.2f dB\n", freq, nof_bands,
-                        channels[freq].id, channels[freq].fd,
-                        10*log10f(sync_get_peak_value(&ssync)));
-        } else {
-          if (frame_cnt >= nof_frames_find) {
-            state = INIT;
-            freq++;
-          }
-        }
-        break;
-      case TRACK:
-        INFO("Tracking PSS find_idx %d offset %d\n", find_idx, find_idx - track_len);
-
-        ret = sync_track(&ssync, input_buffer, FLEN + find_idx - track_len, &track_idx);
-        p2a_v[frame_cnt] = sync_get_peak_value(&ssync);
-
-        /* save cell id for the best peak-to-avg */
-        if (p2a_v[frame_cnt] > max_peak_to_avg) {
-          max_peak_to_avg = p2a_v[frame_cnt];
-          cell_id = sync_get_cell_id(&ssync);
-        }
-        if (ret == 1) {
-          cfo_v[frame_cnt] = sync_get_cfo(&ssync);
-          last_found = frame_cnt;
-          find_idx += track_idx - track_len;
-          idx_v[frame_cnt] = find_idx;
-          nslot = sync_get_slot_id(&ssync);
-        } else {
-          idx_v[frame_cnt] = -1;
-          cfo_v[frame_cnt] = 0.0;
-        }
-        /* if we missed to many PSS it is not a cell, next freq */
-        if (frame_cnt - last_found > max_track_lost) {
-          INFO("\n[%3d/%d]: EARFCN %d Freq. %.2f MHz %d frames lost\n", freq, nof_bands,
-              channels[freq].id, channels[freq].fd, frame_cnt - last_found);
-
-          state = INIT;
-          freq++;
-        } else if (frame_cnt >= nof_frames_track) {
-          mib_decoder_init(cell_id);
-
-          cfo[freq] = mean_valid(idx_v, cfo_v, frame_cnt);
-          p2a[freq] = mean_valid(idx_v, p2a_v, frame_cnt);
-          valid_frames = preprocess_idx(idx_v, idx_valid, t, frame_cnt);
-          sfo = sfo_estimate_period(idx_valid, t, valid_frames, FLEN_PERIOD);
-
-          state = MIB;
-          nslot=(nslot+10)%20;
-        }
-        break;
-      case MIB:
-        INFO("Finding MIB at freq %.2f Mhz offset=%d, cell_id=%d, slot_idx=%d\n", channels[freq].fd, find_idx, cell_id, nslot);
-
-        // TODO: Correct SFO
-
-        // Correct CFO
-        INFO("Correcting CFO=%.4f\n", cfo[freq]);
-        cfo_correct(&cfocorr, &input_buffer[FLEN], &input_buffer[FLEN], (-cfo[freq])/128);
-
-        if (nslot == 0) {
-          if (mib_decoder_run(&input_buffer[FLEN+find_idx], &mib)) {
-            INFO("MIB detected attempt=%d\n", mib_attempts);
-            state = DONE;
-          } else {
-            INFO("MIB not detected attempt=%d\n", mib_attempts);
-            if (mib_attempts == 0) {
-              freq++;
-              state = INIT;
-            }
-          }
-          mib_attempts++;
-        }
-        nslot = (nslot+10)%20;
-
-
-        break;
-      case DONE:
-        printf("\n[%3d/%d]: FOUND EARFCN %d Freq. %.2f MHz. "
-            "PAR %2.2f dB, CFO=%+.2f KHz, SFO=%+2.3f KHz, CELL_ID=%3d\n", freq, nof_bands,
-                channels[freq].id, channels[freq].fd,
-                10*log10f(p2a[freq]), cfo[freq] * 15, sfo / 1000, cell_id);
-        pbch_mib_fprint(stdout, &mib);
-        state = INIT;
-        freq++;
-        break;
-      }
-
-      /** FIXME: This is not necessary at all */
-      if (state == TRACK || state == FIND) {
-        memcpy(input_buffer, &input_buffer[FLEN], FLEN * sizeof(cf_t));
+    while(frame_cnt < nof_frames_find && ret == 0) {
+      ret = ue_sync_get_buffer(&uesync, &buffer);
+      if (ret < 0) {
+        fprintf(stderr, "Error calling ue_sync_work()\n");
+        exit(-1);
       }
       frame_cnt++;
+      printf("[%3d/%d]: EARFCN %d Freq. %.2f MHz looking for PSS. RSSI: %+2.2f dB...\r", freq, nof_freqs,
+                      channels[freq].id, channels[freq].fd, 20*log10f(agc_get_rssi(&uesync.agc)));fflush(stdout);
+      if (VERBOSE_ISINFO()) {
+        printf("\n");
+      }
     }
+    if (ret == 1) {
+      if (VERBOSE_ISINFO()) {
+        printf("[%3d/%d]: EARFCN %d Freq. %.2f MHz FOUND MIB ", freq, nof_freqs,
+                    channels[freq].id, channels[freq].fd);
+      }
+      printf("RSSI: %+.2f dBm, CFO: %+.4f KHz\n",
+             20*log10f(agc_get_rssi(&uesync.agc)), ue_sync_get_cfo(&uesync));
+      printf("\n");fflush(stdout); 
+    }
+    
+    ue_sync_free(&uesync);      
   }
-
-  base_free();
-
+  
+  cuhd_close(uhd);
+  
+  
   printf("\n\nDone\n");
   exit(0);
 }
diff --git a/lte/phy/include/liblte/phy/agc/agc.h b/lte/phy/include/liblte/phy/agc/agc.h
index f28e7a09b..90a02a512 100644
--- a/lte/phy/include/liblte/phy/agc/agc.h
+++ b/lte/phy/include/liblte/phy/agc/agc.h
@@ -41,27 +41,35 @@
  */
 typedef _Complex float cf_t;
 
-#define AGC_DEFAULT_BW  (1e-2f)
+#define AGC_DEFAULT_BW  (5e-2)
 
 typedef struct LIBLTE_API{
   float bandwidth;
   float gain; 
   float y_out;
   bool lock;
+  bool isfirst; 
 } agc_t;
 
 LIBLTE_API int agc_init (agc_t *q);
 
 LIBLTE_API void agc_free(agc_t *q);
 
+LIBLTE_API void agc_reset(agc_t *q);
+
 LIBLTE_API void agc_set_bandwidth(agc_t *q, 
                                   float bandwidth);
 
 LIBLTE_API float agc_get_rssi(agc_t *q);
 
-LIBLTE_API void agc_lock(agc_t *q, bool enable);
+LIBLTE_API float agc_get_output_level(agc_t *q); 
 
-LIBLTE_API void agc_push(agc_t *q, 
+LIBLTE_API float agc_get_gain(agc_t *q);
+
+LIBLTE_API void agc_lock(agc_t *q, 
+                         bool enable);
+
+LIBLTE_API void agc_process(agc_t *q, 
                          cf_t *input,
                          cf_t *output, 
                          uint32_t len);
diff --git a/lte/phy/include/liblte/phy/phch/pbch.h b/lte/phy/include/liblte/phy/phch/pbch.h
index 080702807..9769f66cf 100644
--- a/lte/phy/include/liblte/phy/phch/pbch.h
+++ b/lte/phy/include/liblte/phy/phch/pbch.h
@@ -100,6 +100,7 @@ LIBLTE_API int pbch_encode(pbch_t *q,
 LIBLTE_API void pbch_decode_reset(pbch_t *q);
 
 LIBLTE_API void pbch_mib_fprint(FILE *stream, 
-                                pbch_mib_t *mib);
+                                pbch_mib_t *mib,
+                                uint32_t cell_id);
 
 #endif // PBCH_
diff --git a/lte/phy/include/liblte/phy/phch/ue_sync.h b/lte/phy/include/liblte/phy/phch/ue_sync.h
index dc374e238..d26b9a8b2 100644
--- a/lte/phy/include/liblte/phy/phch/ue_sync.h
+++ b/lte/phy/include/liblte/phy/phch/ue_sync.h
@@ -61,15 +61,16 @@
  * 
  *************************************************************/
 
-typedef enum LIBLTE_API { SF_FIND, SF_TRACK} ue_sync_state_t;
+typedef enum LIBLTE_API { SF_AGC, SF_FIND, SF_TRACK} ue_sync_state_t;
 
 #define SYNC_PBCH_NOF_PRB       6
 #define SYNC_PBCH_NOF_PORTS     2
 
 #define TRACK_MAX_LOST          10
-#define PSS_THRESHOLD           1
 
-#define NOF_MIB_DECODES         10
+#define DEFAULT_NOF_MIB_DECODES 10
+
+#define AGC_NOF_FRAMES          100
 
 #define MEASURE_EXEC_TIME 
 
@@ -112,6 +113,8 @@ typedef struct LIBLTE_API {
   bool pbch_decode_always; 
   bool pbch_decoder_enabled;
   uint32_t pbch_last_trial;
+  bool change_srate; 
+  uint32_t nof_mib_decodes; 
   
   bool decode_sss_on_track; 
   
@@ -134,24 +137,28 @@ LIBLTE_API void ue_sync_free(ue_sync_t *q);
 LIBLTE_API int ue_sync_get_buffer(ue_sync_t *q, 
                                   cf_t **sf_symbols);
 
+LIBLTE_API void ue_sync_set_nof_pbch_decodes(ue_sync_t *q, 
+                                             uint32_t nof_pbch_decodes);
+
 
 LIBLTE_API void ue_sync_reset(ue_sync_t *q);
 
-LIBLTE_API void ue_sync_decode_sss_on_track(ue_sync_t *q, bool enabled);
+LIBLTE_API void ue_sync_decode_sss_on_track(ue_sync_t *q, 
+                                            bool enabled);
 
-LIBLTE_API void ue_sync_pbch_enable(ue_sync_t *q, bool enabled); 
+LIBLTE_API void ue_sync_pbch_enable(ue_sync_t *q, 
+                                    bool enabled); 
 
-LIBLTE_API void ue_sync_pbch_always(ue_sync_t *q, bool enabled); 
+LIBLTE_API void ue_sync_change_srate(ue_sync_t *q, 
+                                     bool enabled);
 
-LIBLTE_API void ue_sync_set_threshold(ue_sync_t *q, 
-                                      float threshold);
+LIBLTE_API void ue_sync_pbch_always(ue_sync_t *q, 
+                                    bool enabled); 
 
 LIBLTE_API ue_sync_state_t ue_sync_get_state(ue_sync_t *q);
 
 LIBLTE_API uint32_t ue_sync_get_sfidx(ue_sync_t *q);
 
-LIBLTE_API uint32_t ue_sync_get_peak_idx(ue_sync_t *q);
-
 LIBLTE_API lte_cell_t ue_sync_get_cell(ue_sync_t *q);
 
 LIBLTE_API pbch_mib_t ue_sync_get_mib(ue_sync_t *q);
diff --git a/lte/phy/lib/agc/src/agc.c b/lte/phy/lib/agc/src/agc.c
index e377ac1be..5d1add9ff 100644
--- a/lte/phy/lib/agc/src/agc.c
+++ b/lte/phy/lib/agc/src/agc.c
@@ -41,10 +41,7 @@
 
 int agc_init (agc_t *q) {
   bzero(q, sizeof(agc_t));
-  q->bandwidth = AGC_DEFAULT_BW;
-  q->lock = false; 
-  q->gain = 1.0;
-  q->y_out = 1.0; 
+  agc_reset(q);
   return LIBLTE_SUCCESS;
 }
 
@@ -52,6 +49,14 @@ void agc_free(agc_t *q) {
   bzero(q, sizeof(agc_t));
 }
 
+void agc_reset(agc_t *q) {
+  q->bandwidth = AGC_DEFAULT_BW;
+  q->lock = false; 
+  q->gain = 1.0;
+  q->y_out = 1.0; 
+  q->isfirst = true; 
+}
+
 void agc_set_bandwidth(agc_t *q, float bandwidth) {
   q->bandwidth = bandwidth;
 }
@@ -60,11 +65,20 @@ float agc_get_rssi(agc_t *q) {
   return 1.0/q->gain;
 }
 
+float agc_get_output_level(agc_t *q) {
+  return q->y_out;
+}
+
+float agc_get_gain(agc_t *q) {
+  return q->gain;
+}
+
+
 void agc_lock(agc_t *q, bool enable) {
   q->lock = enable;
 }
 
-void agc_push(agc_t *q, cf_t *input, cf_t *output, uint32_t len) {
+void agc_process(agc_t *q, cf_t *input, cf_t *output, uint32_t len) {
   
   // Apply current gain to input signal 
   vec_sc_prod_cfc(input, q->gain, output, len);
@@ -72,8 +86,15 @@ void agc_push(agc_t *q, cf_t *input, cf_t *output, uint32_t len) {
   // compute output energy estimate
   float y = sqrtf(crealf(vec_dot_prod_conj_ccc(output, output, len))/len);
   
-  q->y_out = (1-q->bandwidth) * q->y_out + q->bandwidth * y;
-  if (!q->lock) {
-    q->gain *= expf(-0.5*q->bandwidth*logf(q->y_out));
+  if (q->isfirst) {
+    q->y_out = y; 
+    q->gain = 1/y;
+    q->isfirst = false; 
+  } else {
+    q->y_out = (1-q->bandwidth) * q->y_out + q->bandwidth * y;
+    if (!q->lock) {
+      q->gain *= expf(-0.5*q->bandwidth*logf(q->y_out));
+    }    
   }
+  DEBUG("AGC gain: %.3f y_out=%.3f, y=%.3f\n", q->gain, q->y_out, y);
 }
diff --git a/lte/phy/lib/common/src/phy_common.c b/lte/phy/lib/common/src/phy_common.c
index 223d2e299..6b16215a6 100644
--- a/lte/phy/lib/common/src/phy_common.c
+++ b/lte/phy/lib/common/src/phy_common.c
@@ -295,7 +295,7 @@ char *lte_mimotype2str(lte_mimo_type_t type) {
 }
 
 float get_fd(struct lte_band *band, uint32_t earfcn) {
-  if (earfcn > band->earfcn_offset) {
+  if (earfcn >= band->earfcn_offset) {
     return band->fd_low_mhz + 0.1*(earfcn - band->earfcn_offset);    
   } else {
     return 0.0;
diff --git a/lte/phy/lib/phch/src/pbch.c b/lte/phy/lib/phch/src/pbch.c
index 353dc7639..1fe2c8e87 100644
--- a/lte/phy/lib/phch/src/pbch.c
+++ b/lte/phy/lib/phch/src/pbch.c
@@ -330,7 +330,8 @@ void pbch_mib_pack(pbch_mib_t *mib, char *msg) {
   bit_pack(mib->sfn >> 2, &msg, 8);
 }
 
-void pbch_mib_fprint(FILE *stream, pbch_mib_t *mib) {
+void pbch_mib_fprint(FILE *stream, pbch_mib_t *mib, uint32_t cell_id) {
+  printf(" - Cell ID:         %d\n", cell_id);
   printf(" - Nof ports:       %d\n", mib->nof_ports);
   printf(" - PRB:             %d\n", mib->nof_prb);
   printf(" - PHICH Length:    %s\n",
@@ -376,7 +377,20 @@ uint32_t pbch_crc_check(pbch_t *q, char *bits, uint32_t nof_ports) {
   char data[40];
   memcpy(data, bits, 40 * sizeof(char));
   crc_set_mask(data, nof_ports);
-  return crc_checksum(&q->crc, data, 40);
+  int ret = crc_checksum(&q->crc, data, 40);
+  if (ret == 0) {
+    uint32_t chkzeros=0;
+    for (int i=0;i<24 && !chkzeros;i++) {
+      chkzeros += data[i];
+    }
+    if (chkzeros) {
+      return 0;
+    } else {
+      return -1;
+    }
+  } else {
+    return ret; 
+  }
 }
 
 int pbch_decode_frame(pbch_t *q, pbch_mib_t *mib, uint32_t src, uint32_t dst, uint32_t n,
@@ -522,8 +536,6 @@ int pbch_decode(pbch_t *q, cf_t *sf_symbols, cf_t *ce[MAX_PORTS], pbch_mib_t *mi
       for (nb = 0; nb < q->frame_idx && !ret; nb++) {
         for (dst = 0; (dst < 4 - nb) && !ret; dst++) {
           for (src = 0; src < q->frame_idx - nb && !ret; src++) {
-            DEBUG("Trying %d blocks at offset %d as subframe mod4 number %d\n",
-                nb + 1, src, dst);
             ret = pbch_decode_frame(q, mib, src, dst, nb + 1, nof_bits, nant);
             
           }
diff --git a/lte/phy/lib/phch/src/ue_dl.c b/lte/phy/lib/phch/src/ue_dl.c
index e48b28f62..e9aad4520 100644
--- a/lte/phy/lib/phch/src/ue_dl.c
+++ b/lte/phy/lib/phch/src/ue_dl.c
@@ -145,6 +145,7 @@ int ue_dl_receive(ue_dl_t *q, cf_t *input, char *data, uint32_t sf_idx, uint32_t
   uint32_t nof_locations;
   uint16_t crc_rem; 
   dci_format_t format; 
+  int ret = LIBLTE_ERROR; 
   
   /* If we are looking for SI Blocks, search only in appropiate places */
   if ((rnti == SIRNTI && (sfn % 2) == 0 && sf_idx == 5) ||
@@ -226,14 +227,18 @@ int ue_dl_receive(ue_dl_t *q, cf_t *input, char *data, uint32_t sf_idx, uint32_t
         }
       }
       if (q->harq_process[0].mcs.mod > 0) {
-        if (pdsch_decode(&q->pdsch, q->sf_symbols, q->ce, data, sf_idx, 
-            &q->harq_process[0], rvidx)) {
+        ret = pdsch_decode(&q->pdsch, q->sf_symbols, q->ce, data, sf_idx, 
+            &q->harq_process[0], rvidx);
+        if (ret == LIBLTE_ERROR) {
           if (rnti == SIRNTI && rvidx == 1) {
             q->pkt_errors++;
-          } else {
+          } else if (rnti != SIRNTI) {
             q->pkt_errors++;                
           }            
-        } else {
+        } else if (ret == LIBLTE_ERROR_INVALID_INPUTS) {
+          fprintf(stderr, "Error calling pdsch_decode()\n");
+          return LIBLTE_ERROR; 
+        } else if (ret == LIBLTE_SUCCESS) {
           if (VERBOSE_ISINFO()) {
             INFO("Decoded Message: ", 0);
             vec_fprint_hex(stdout, data, ra_dl.mcs.tbs);
@@ -241,6 +246,8 @@ int ue_dl_receive(ue_dl_t *q, cf_t *input, char *data, uint32_t sf_idx, uint32_t
         }
         if (rnti == SIRNTI && rvidx == 1) {
           q->pkts_total++;                      
+        } else if (rnti != SIRNTI) {
+          q->pkts_total++;                                
         }
       }
     }
@@ -249,7 +256,7 @@ int ue_dl_receive(ue_dl_t *q, cf_t *input, char *data, uint32_t sf_idx, uint32_t
     }
   }
   
-  if (crc_rem == rnti) {        
+  if (crc_rem == rnti && ret == LIBLTE_SUCCESS) {        
     return ra_dl.mcs.tbs;    
   } else {
     return 0;
diff --git a/lte/phy/lib/phch/src/ue_sync.c b/lte/phy/lib/phch/src/ue_sync.c
index bf19c5516..b877cf3b8 100644
--- a/lte/phy/lib/phch/src/ue_sync.c
+++ b/lte/phy/lib/phch/src/ue_sync.c
@@ -54,6 +54,29 @@ cf_t dummy[MAX_TIME_OFFSET];
 static int mib_decoder_initialize(ue_sync_t *q);
 static void mib_decoder_free(ue_sync_t *q);
 
+
+static void update_threshold(ue_sync_t *q) {
+  int symbol_sz = lte_symbol_sz(q->cell.nof_prb);
+  if (symbol_sz > 0) {
+    switch (symbol_sz) {
+      case 128:
+        sync_set_threshold(&q->s, 20000, 2000);
+        break;
+      case 256:
+        sync_set_threshold(&q->s, 25000, 2500);
+        break;
+      case 512:
+        sync_set_threshold(&q->s, 38000, 3800);
+        break;
+      case 1024:
+        sync_set_threshold(&q->s, 50000, 5000);
+        break;
+      case 2048:
+        sync_set_threshold(&q->s, 80000, 4000);
+    }  
+  }
+}
+
 int ue_sync_init(ue_sync_t *q, 
                     double (set_rate_callback)(void*, double),
                     int (recv_callback)(void*, void*, uint32_t),
@@ -61,8 +84,10 @@ int ue_sync_init(ue_sync_t *q,
 {
   int ret = LIBLTE_ERROR_INVALID_INPUTS;
   
-  if (q                   != NULL && 
-      stream_handler      != NULL)
+  if (q                    != NULL && 
+      stream_handler       != NULL && 
+      set_rate_callback    != NULL &&
+      recv_callback        != NULL)
   {
     ret = LIBLTE_ERROR;
     
@@ -80,7 +105,8 @@ int ue_sync_init(ue_sync_t *q,
     q->pbch_decoder_enabled = true; 
     q->pbch_decode_always = false; 
     q->decode_sss_on_track = false; 
-    
+    q->change_srate = true; 
+    q->nof_mib_decodes = DEFAULT_NOF_MIB_DECODES;
     q->stream = stream_handler;
     q->recv_callback = recv_callback;
     q->set_rate_callback = set_rate_callback;
@@ -128,7 +154,7 @@ int ue_sync_init(ue_sync_t *q,
       }
     }
    
-    sync_set_threshold(&q->s, PSS_THRESHOLD, PSS_THRESHOLD);
+    update_threshold(q);
     
     ret = LIBLTE_SUCCESS;
   }
@@ -161,10 +187,6 @@ void ue_sync_free(ue_sync_t *q) {
   agc_free(&q->agc);
 }
 
-void ue_sync_set_threshold(ue_sync_t *q, float threshold) {
-  sync_set_threshold(&q->s, threshold, threshold/2);
-}
-
 lte_cell_t ue_sync_get_cell(ue_sync_t *q) {
   return q->cell;
 }
@@ -181,6 +203,10 @@ ue_sync_state_t ue_sync_get_state(ue_sync_t *q) {
   return q->state;
 }
 
+void ue_sync_change_srate(ue_sync_t *q, bool enabled) {
+  q->change_srate = enabled; 
+}
+
 static int update_srate(ue_sync_t *q) {
   struct timeval t[3];
     
@@ -192,7 +218,7 @@ static int update_srate(ue_sync_t *q) {
   gettimeofday(&t[2], NULL);
   get_time_interval(t);
   
-  if (NOF_MIB_DECODES > 1) {
+  if (q->nof_mib_decodes > 1) {
     mib_decoder_free(q);  
     if (mib_decoder_initialize(q)) {
       fprintf(stderr, "Error reinitializing MIB decoder\n");
@@ -203,10 +229,12 @@ static int update_srate(ue_sync_t *q) {
   // Finally set the new sampling rate
   q->set_rate_callback(q->stream, (float) lte_sampling_freq_hz(q->cell.nof_prb));
  
+  update_threshold(q);
+   
   ue_sync_reset(q);
-  printf("Set sampling rate %.2f MHz, fft_size=%d, sf_len=%d Texec=%d us\n", 
+  INFO("Set sampling rate %.2f MHz, fft_size=%d, sf_len=%d Threshold=%.2f/%.2f Texec=%d us\n", 
        (float) lte_sampling_freq_hz(q->cell.nof_prb)/1000000, 
-       CURRENT_FFTSIZE, CURRENT_SFLEN, (int) t[0].tv_usec);
+       CURRENT_FFTSIZE, CURRENT_SFLEN, q->s.find_threshold, q->s.track_threshold, (int) t[0].tv_usec);
   
   return LIBLTE_SUCCESS;
 }
@@ -239,6 +267,10 @@ void ue_sync_decode_sss_on_track(ue_sync_t *q, bool enabled) {
   q->decode_sss_on_track = enabled; 
 }
 
+void ue_sync_set_nof_pbch_decodes(ue_sync_t *q, uint32_t nof_pbch_decodes) {
+  q->nof_mib_decodes = nof_pbch_decodes;
+}
+
 static int mib_decoder_initialize(ue_sync_t *q) {
     
   if (lte_fft_init(&q->fft, q->cell.cp, q->cell.nof_prb)) {
@@ -289,9 +321,11 @@ static int mib_decoder_run(ue_sync_t *q) {
     q->cell.nof_prb = q->mib.nof_prb;
     
     if (!q->pbch_decoded) {
-      printf("MIB decoded:\n");
-      pbch_mib_fprint(stdout, &q->mib);
-      ret = update_srate(q);
+      printf("\n\nMIB decoded:\n");
+      pbch_mib_fprint(stdout, &q->mib, q->cell.id);
+      if (q->change_srate) {
+        ret = update_srate(q);        
+      }
     } else { 
       INFO("MIB decoded #%d SFN: %d\n", q->pbch_decoded, q->mib.sfn);
     }
@@ -300,7 +334,7 @@ static int mib_decoder_run(ue_sync_t *q) {
     pbch_decode_reset(&q->pbch);
 
   } else {
-    INFO("MIB not decoded: %d\n", q->frame_total_cnt);
+    INFO("MIB not decoded: %d\n", q->frame_total_cnt/2);
     q->pbch_last_trial = q->frame_total_cnt;
   }
   
@@ -336,8 +370,8 @@ static int find_peak_ok(ue_sync_t *q) {
     q->state = SF_TRACK;      
     ret = LIBLTE_SUCCESS;
   
-    INFO("Found peak %d, SF_idx: %d, Cell_id: %d CP: %s\n", 
-        q->peak_idx, q->sf_idx, q->cell.id, lte_cp_string(q->cell.cp));       
+    INFO("Found peak at %d, value %.3f, SF_idx: %d, Cell_id: %d CP: %s\n", 
+        q->peak_idx, sync_get_peak_value(&q->s), q->sf_idx, q->cell.id, lte_cp_string(q->cell.cp));       
     
     if (q->peak_idx < CURRENT_SFLEN) {
       q->sf_idx++;
@@ -355,7 +389,7 @@ int track_peak_ok(ue_sync_t *q, uint32_t track_idx) {
   
    /* Make sure subframe idx is what we expect */
   if ((q->sf_idx != sync_get_slot_id(&q->s)/2) && q->decode_sss_on_track) {
-    printf("\nWarning: Expected SF idx %d but got %d!\n", 
+    INFO("\nWarning: Expected SF idx %d but got %d!\n", 
           q->sf_idx, sync_get_slot_id(&q->s)/2);
     q->sf_idx = sync_get_slot_id(&q->s)/2;
   } else {
@@ -396,7 +430,7 @@ int track_peak_no(ue_sync_t *q) {
     printf("\n%d frames lost. Going back to FIND\n", (int) q->frame_no_cnt);
     q->state = SF_FIND;
   } else {
-    INFO("Tracking peak not found, %d lost\n", (int) q->frame_no_cnt);    
+    INFO("Tracking peak not found. Peak %.3f, %d lost\n", sync_get_peak_value(&q->s), (int) q->frame_no_cnt);    
   }
 
   return LIBLTE_SUCCESS;
@@ -404,24 +438,28 @@ int track_peak_no(ue_sync_t *q) {
 
 static int receive_samples(ue_sync_t *q) {
   
-  /* A negative time offset means there are samples in our buffer for the next subframe, 
-   because we are sampling too fast. 
-  */
-  if (q->time_offset < 0) {
-    q->time_offset = -q->time_offset;
-  } 
-  /* copy last part of the last subframe (use move since there could be overlapping) */
-  memcpy(q->receive_buffer, &q->input_buffer[CURRENT_SFLEN-q->time_offset], q->time_offset*sizeof(cf_t));
+  if (q->cell.nof_prb >= 6 && q->cell.nof_prb <= 100) {
+    /* A negative time offset means there are samples in our buffer for the next subframe, 
+    because we are sampling too fast. 
+    */
+    if (q->time_offset < 0) {
+      q->time_offset = -q->time_offset;
+    } 
+    /* copy last part of the last subframe (use move since there could be overlapping) */
+    memcpy(q->receive_buffer, &q->input_buffer[CURRENT_SFLEN-q->time_offset], q->time_offset*sizeof(cf_t));
 
-  /* Get 1 subframe from the USRP getting more samples and keeping the previous samples, if any */  
-  if (q->recv_callback(q->stream, &q->receive_buffer[q->time_offset], CURRENT_SFLEN - q->time_offset) < 0) {
-    return LIBLTE_ERROR;
+    /* Get 1 subframe from the USRP getting more samples and keeping the previous samples, if any */  
+    if (q->recv_callback(q->stream, &q->receive_buffer[q->time_offset], CURRENT_SFLEN - q->time_offset) < 0) {
+      return LIBLTE_ERROR;
+    }
+    
+    /* reset time offset */
+    q->time_offset = 0;
+
+    return LIBLTE_SUCCESS; 
+  } else {
+    return LIBLTE_ERROR_INVALID_INPUTS;
   }
-  
-  /* reset time offset */
-  q->time_offset = 0;
-
-  return LIBLTE_SUCCESS; 
 }
 
 int ue_sync_get_buffer(ue_sync_t *q, cf_t **sf_symbols) {
@@ -439,9 +477,17 @@ int ue_sync_get_buffer(ue_sync_t *q, cf_t **sf_symbols) {
       return -1;
     }
     
-    agc_push(&q->agc, q->receive_buffer, q->input_buffer, CURRENT_SFLEN);
+    agc_process(&q->agc, q->receive_buffer, q->input_buffer, CURRENT_SFLEN);
     
     switch (q->state) {
+      case SF_AGC: 
+        q->frame_total_cnt++;
+        if (q->frame_total_cnt >= AGC_NOF_FRAMES) {
+          q->state = SF_FIND; 
+          q->frame_total_cnt = 0; 
+        }
+        ret = 0;
+        break;
       case SF_FIND:
         q->s.sss_en = true; 
         
@@ -469,7 +515,17 @@ int ue_sync_get_buffer(ue_sync_t *q, cf_t **sf_symbols) {
               fprintf(stderr, "Error processing find peak \n");
             }
           }
-        } 
+        } else if (q->peak_idx != 0) {
+          uint32_t rlen; 
+          if (q->peak_idx < CURRENT_SFLEN/2) {
+            rlen = CURRENT_SFLEN/2-q->peak_idx;
+          } else {
+            rlen = q->peak_idx;
+          }
+          if (q->recv_callback(q->stream, q->receive_buffer, rlen) < 0) {
+            return LIBLTE_ERROR;
+          }
+        }
       break;
       case SF_TRACK:
         ret = LIBLTE_SUCCESS;
@@ -508,14 +564,14 @@ int ue_sync_get_buffer(ue_sync_t *q, cf_t **sf_symbols) {
             ret = track_peak_no(q); 
           }
           
-          INFO("TRACK %3d: SF=%d Track_idx=%d Offset=%d CFO: %f\n", 
-                (int) q->frame_total_cnt, q->sf_idx, track_idx, q->time_offset, sync_get_cfo(&q->s));
+          INFO("TRACK %3d: Value=%.3f SF=%d Track_idx=%d Offset=%d CFO: %f\n", 
+                (int) q->frame_total_cnt, sync_get_peak_value(&q->s), q->sf_idx, track_idx, q->time_offset, sync_get_cfo(&q->s));
           
           q->frame_total_cnt++; 
           
           if (ret == LIBLTE_ERROR) {
             fprintf(stderr, "Error processing tracking peak\n");
-            ue_sync_reset(q);
+            q->state = SF_FIND; 
             return LIBLTE_SUCCESS;
           } 
         }
@@ -527,7 +583,7 @@ int ue_sync_get_buffer(ue_sync_t *q, cf_t **sf_symbols) {
         /* At subframe 0, try to decode PBCH if not yet decoded */
         if (q->sf_idx == 0) {
           if(q->pbch_decoder_enabled                     &&
-            (q->pbch_decoded < NOF_MIB_DECODES || q->pbch_decode_always)) 
+            (q->pbch_decoded < q->nof_mib_decodes || q->pbch_decode_always)) 
           {
             mib_decoder_run(q);
           } else {
@@ -537,7 +593,7 @@ int ue_sync_get_buffer(ue_sync_t *q, cf_t **sf_symbols) {
 
         if (ret == LIBLTE_SUCCESS) {
           if (q->pbch_decoder_enabled) {
-            if (q->pbch_decoded >= NOF_MIB_DECODES) {
+            if (q->pbch_decoded >= q->nof_mib_decodes) {
               ret = 1; 
             } else {
               ret = 0;
@@ -554,7 +610,7 @@ int ue_sync_get_buffer(ue_sync_t *q, cf_t **sf_symbols) {
 }
 
 void ue_sync_reset(ue_sync_t *q) {
-  q->state = SF_FIND;
+  q->state = SF_AGC;
     
   q->pbch_last_trial = 0; 
   q->frame_ok_cnt = 0;
@@ -566,5 +622,7 @@ void ue_sync_reset(ue_sync_t *q) {
   #ifdef MEASURE_EXEC_TIME
   q->mean_exec_time = 0;
   #endif
+  
+  pbch_decode_reset(&q->pbch);
 }
 
diff --git a/lte/phy/lib/phch/test/pbch_file_test.c b/lte/phy/lib/phch/test/pbch_file_test.c
index 92b2d8621..5a243856f 100644
--- a/lte/phy/lib/phch/test/pbch_file_test.c
+++ b/lte/phy/lib/phch/test/pbch_file_test.c
@@ -222,11 +222,11 @@ int main(int argc, char **argv) {
   } else {
     if (mib.nof_ports == 2 && mib.nof_prb == 50 && mib.phich_length == PHICH_NORM
         && mib.phich_resources == R_1 && mib.sfn == 28) {
-      pbch_mib_fprint(stdout, &mib);
+      pbch_mib_fprint(stdout, &mib, cell.id);
       printf("This is the signal.1.92M.dat file\n");
       exit(0);
     } else {
-      pbch_mib_fprint(stdout, &mib);
+      pbch_mib_fprint(stdout, &mib, cell.id);
       printf("This is an unknown file\n");
       exit(-1);
     }
diff --git a/lte/phy/lib/phch/test/pbch_test.c b/lte/phy/lib/phch/test/pbch_test.c
index 51f6fff7b..b202f6070 100644
--- a/lte/phy/lib/phch/test/pbch_test.c
+++ b/lte/phy/lib/phch/test/pbch_test.c
@@ -138,7 +138,7 @@ int main(int argc, char **argv) {
     printf("OK\n");
     exit(0);
   } else {
-    pbch_mib_fprint(stdout, &mib_rx);
+    pbch_mib_fprint(stdout, &mib_rx, cell.id);
     exit(-1);
   }
 }
diff --git a/lte/phy/lib/phch/test/ue_sync_usrp.c b/lte/phy/lib/phch/test/ue_sync_usrp.c
index 752c4d236..115d2649b 100644
--- a/lte/phy/lib/phch/test/ue_sync_usrp.c
+++ b/lte/phy/lib/phch/test/ue_sync_usrp.c
@@ -173,13 +173,7 @@ int main(int argc, char **argv) {
     exit(-1);
   }
   
-  if (threshold > 0.0) {
-    ue_sync_set_threshold(&s, threshold);
-  }
-  
   ue_sync_pbch_enable(&s, true);
-//  ue_sync_pbch_always(&s, true);
-//  ue_sync_decode_sss_on_track(&s, true);
   
   signal_detected = true;
   frame_cnt = 0;
diff --git a/lte/phy/lib/sync/src/pss.c b/lte/phy/lib/sync/src/pss.c
index 29ca1608c..4c8da1a26 100644
--- a/lte/phy/lib/sync/src/pss.c
+++ b/lte/phy/lib/sync/src/pss.c
@@ -262,7 +262,7 @@ int pss_synch_find_pss(pss_synch_t *q, cf_t *input,
     vec_abs_cf(q->conv_output, q->conv_abs, conv_output_len);
     corr_peak_pos = vec_max_fi(q->conv_abs, conv_output_len);
     if (corr_peak_value) {
-      *corr_peak_value = q->conv_abs[corr_peak_pos] / conv_output_len;
+      *corr_peak_value = q->conv_abs[corr_peak_pos];
     }
     if (corr_mean_value) {
       *corr_mean_value = vec_acc_ff(q->conv_abs, conv_output_len)
diff --git a/lte/phy/lib/sync/src/sync.c b/lte/phy/lib/sync/src/sync.c
index bb62a806f..1370dd066 100644
--- a/lte/phy/lib/sync/src/sync.c
+++ b/lte/phy/lib/sync/src/sync.c
@@ -299,24 +299,27 @@ int sync_track(sync_t *q, cf_t *input, uint32_t offset, uint32_t *peak_position)
     DEBUG("PSS possible tracking peak pos=%d peak=%.2f threshold=%.2f\n",
           peak_pos, peak_value, q->track_threshold);
     
-    if (peak_value > q->track_threshold) {
-      q->cfo = pss_synch_cfo_compute(&q->pss_track, &input[offset+peak_pos-q->fft_size]);
-
-      if (q->sss_en) {
-        if (sync_sss(q, input, offset + peak_pos, false) < 0) {
-          fprintf(stderr, "Error synchronizing with SSS\n");
-          return LIBLTE_ERROR;
-        }
+    if (q->peak_value           > q->track_threshold) {
+      if (offset + peak_pos       > q->fft_size) {
+        q->cfo = pss_synch_cfo_compute(&q->pss_track, &input[offset+peak_pos-q->fft_size]);
+        if (q->sss_en) {
+          if (sync_sss(q, input, offset + peak_pos, false) < 0) {
+            fprintf(stderr, "Error synchronizing with SSS\n");
+            return LIBLTE_ERROR;
+          }
+        } 
+      } else {
+        printf("Warning: no space for CFO computation\n");
       }
-    
       if (peak_position) {
         *peak_position = peak_pos;
       }
-      ret = 1;      
+      ret = 1;
     } else {
       ret = LIBLTE_SUCCESS;
     }
   }
+  
   return ret; 
 }
 
@@ -351,33 +354,39 @@ int sync_find(sync_t *q, cf_t *input, uint32_t *peak_position) {
     }
   }
 
-  if (peak_pos[N_id_2] > q->fft_size) {
-    if (q->pss_mode == ABSOLUTE) {
-      q->peak_value = peak_value[N_id_2];
-    } else {
-      q->peak_value = peak_value[N_id_2] / mean_value[N_id_2];
-    }
+  if (q->pss_mode == ABSOLUTE) {
+    q->peak_value = peak_value[N_id_2];
+  } else {
+    q->peak_value = peak_value[N_id_2] / mean_value[N_id_2];
   }
-  
+ 
+  if (peak_position) {
+    *peak_position = 0;
+  }
+
   DEBUG("PSS possible peak N_id_2=%d, pos=%d peak=%.2f threshold=%.2f\n",
       N_id_2, peak_pos[N_id_2], peak_value[N_id_2], q->find_threshold);
 
   /* If peak detected */
-  if (q->peak_value > q->find_threshold) {
-    q->N_id_2 = N_id_2;
-    pss_synch_set_N_id_2(&q->pss_find, q->N_id_2);
-    q->cfo = pss_synch_cfo_compute(&q->pss_find, &input[peak_pos[N_id_2]-q->fft_size]);
-    
-    DEBUG("PSS peak detected N_id_2=%d, pos=%d peak=%.2f par=%.2f th=%.2f cfo=%.4f\n", N_id_2,
-        peak_pos[N_id_2], peak_value[N_id_2], q->peak_value, q->find_threshold, q->cfo);
+  if (q->peak_value                   > q->find_threshold) {
+    if (peak_pos[N_id_2]                > q->fft_size             && 
+        peak_pos[N_id_2] + q->fft_size  < q->find_frame_size) 
+    {
+      q->N_id_2 = N_id_2;
+      pss_synch_set_N_id_2(&q->pss_find, q->N_id_2);
+      q->cfo = pss_synch_cfo_compute(&q->pss_find, &input[peak_pos[N_id_2]-q->fft_size]);
+      
+      DEBUG("PSS peak detected N_id_2=%d, pos=%d peak=%.2f par=%.2f th=%.2f cfo=%.4f\n", N_id_2,
+          peak_pos[N_id_2], peak_value[N_id_2], q->peak_value, q->find_threshold, q->cfo);
 
-    if (q->sss_en) {     
-      if (sync_sss(q, input, peak_pos[q->N_id_2], q->detect_cp) < 0) {
-        fprintf(stderr, "Error synchronizing with SSS\n");
-        return LIBLTE_ERROR;
+      if (q->sss_en) {     
+        if (sync_sss(q, input, peak_pos[q->N_id_2], q->detect_cp) < 0) {
+          fprintf(stderr, "Error synchronizing with SSS\n");
+          return LIBLTE_ERROR;
+        }
       }
-    }
-    
+    } 
+
     if (peak_position) {
       *peak_position = peak_pos[N_id_2];
     }
diff --git a/lte/phy/lib/sync/test/sync_test.c b/lte/phy/lib/sync/test/sync_test.c
index a11f49043..b9414a304 100644
--- a/lte/phy/lib/sync/test/sync_test.c
+++ b/lte/phy/lib/sync/test/sync_test.c
@@ -107,11 +107,11 @@ int main(int argc, char **argv) {
     return -1;
   }
 
-  sync_set_threshold(&sync, 20, 10);
+  sync_set_threshold(&sync, 1, 1);
 
   if (cell_id == -1) {
     cid = 0;
-    max_cid = 149;
+    max_cid = 49;
   } else {
     cid = cell_id;
     max_cid = cell_id;
diff --git a/lte/phy/lib/utils/src/vector.c b/lte/phy/lib/utils/src/vector.c
index 1c7ad6275..cc7b556b8 100644
--- a/lte/phy/lib/utils/src/vector.c
+++ b/lte/phy/lib/utils/src/vector.c
@@ -362,8 +362,7 @@ float vec_avg_power_cf(cf_t *x, uint32_t len) {
   int j;
   float power = 0;
   for (j=0;j<len;j++) {
-    power += (__real__ x[j]) * (__real__ x[j]) +
-        (__imag__ x[j]) * (__imag__ x[j]);
+    power += crealf(x[j]*conjf(x[j]));
   }
   return power / len;
 }