diff --git a/lte/phy/examples/CMakeLists.txt b/lte/phy/examples/CMakeLists.txt
index a5fe5d474..573a9772b 100644
--- a/lte/phy/examples/CMakeLists.txt
+++ b/lte/phy/examples/CMakeLists.txt
@@ -84,6 +84,9 @@ IF(${CUHD_FIND} GREATER -1)
   add_executable(cell_search cell_search.c cell_search_utils.c)
   target_link_libraries(cell_search lte_phy cuhd )
 
+  add_executable(cell_measurement cell_measurement.c cell_search_utils.c)
+  target_link_libraries(cell_measurement cuhd lte_phy)
+
   MESSAGE(STATUS "   UHD examples will be installed.")
 
 ELSE(${CUHD_FIND} GREATER -1)
diff --git a/lte/phy/examples/cell_search.c b/lte/phy/examples/cell_search.c
index 5f3306bd8..bf3d83112 100644
--- a/lte/phy/examples/cell_search.c
+++ b/lte/phy/examples/cell_search.c
@@ -121,7 +121,6 @@ int main(int argc, char **argv) {
   void *uhd;
   ue_celldetect_t s;
   ue_celldetect_result_t found_cells[3]; 
-  cf_t *buffer; 
   int nof_freqs; 
   lte_earfcn_t channels[MAX_EARFCN];
   uint32_t freq;
@@ -142,12 +141,6 @@ int main(int argc, char **argv) {
     exit(-1);
   }
     
-  buffer = vec_malloc(sizeof(cf_t) * 96000);
-  if (!buffer) {
-    perror("malloc");
-    return LIBLTE_ERROR;
-  }
-  
   if (ue_celldetect_init(&s)) {
     fprintf(stderr, "Error initiating UE sync module\n");
     exit(-1);
@@ -178,7 +171,7 @@ int main(int argc, char **argv) {
       printf("\n");
     }
     
-    n = find_cell(uhd, &s, buffer, found_cells);
+    n = find_all_cells(uhd, found_cells);
     if (n < 0) {
       fprintf(stderr, "Error searching cell\n");
       exit(-1);
@@ -186,7 +179,7 @@ int main(int argc, char **argv) {
     if (n == CS_CELL_DETECTED) {
       for (int i=0;i<3;i++) {
         if (found_cells[i].peak > threshold/2) {
-          if (decode_pbch(uhd, buffer, &found_cells[i], nof_frames_total, &mib)) {
+          if (decode_pbch(uhd, &found_cells[i], nof_frames_total, &mib)) {
             fprintf(stderr, "Error decoding PBCH\n");
             exit(-1);
           }          
@@ -194,6 +187,8 @@ int main(int argc, char **argv) {
       }
     }    
   }
+  
+  printf("\nBye\n");
     
   ue_celldetect_free(&s);
   cuhd_close(uhd);
diff --git a/lte/phy/examples/cell_search_utils.c b/lte/phy/examples/cell_search_utils.c
index a5bb30adc..f7a3d1d30 100644
--- a/lte/phy/examples/cell_search_utils.c
+++ b/lte/phy/examples/cell_search_utils.c
@@ -40,19 +40,26 @@
 #ifndef DISABLE_UHD
 #include "liblte/cuhd/cuhd.h"
 
-int decode_pbch(void *uhd, cf_t *buffer, ue_celldetect_result_t *found_cell, uint32_t nof_frames_total, pbch_mib_t *mib) 
+int decode_pbch(void *uhd, ue_celldetect_result_t *found_cell, uint32_t nof_frames_total, pbch_mib_t *mib) 
 {
   ue_mib_t uemib;
   int n; 
+  int ret = LIBLTE_ERROR; 
+
+  uint32_t nof_frames = 0;
+  uint32_t flen = MIB_FRAME_SIZE;
+
+  cf_t *buffer = vec_malloc(sizeof(cf_t) * flen);
+  if (!buffer) {
+    perror("malloc");
+    goto free_and_exit;
+  }
   
   bzero(mib, sizeof(pbch_mib_t));
   
-  uint32_t nof_frames = 0;
-  uint32_t flen = MIB_FRAME_SIZE;
-  
   if (ue_mib_init(&uemib, found_cell->cell_id, found_cell->cp)) {
     fprintf(stderr, "Error initiating PBCH decoder\n");
-    return LIBLTE_ERROR;
+    goto free_and_exit;
   }
   
   INFO("Setting sampling frequency 1.92 MHz for PBCH decoding\n", 0);
@@ -63,95 +70,203 @@ int decode_pbch(void *uhd, cf_t *buffer, ue_celldetect_result_t *found_cell, uin
   do {
     if (cuhd_recv(uhd, buffer, flen, 1)<0) {
       fprintf(stderr, "Error receiving from USRP\n");
-      return LIBLTE_ERROR;
+      goto free_and_exit;
     }      
     
-    INFO("Calling ue_mib_decode() %d/%d\n", nof_frames, nof_frames_total);
+    DEBUG("Calling ue_mib_decode() %d/%d\n", nof_frames, nof_frames_total);
     
     n = ue_mib_decode(&uemib, buffer, flen, mib);
     if (n == LIBLTE_ERROR || n == LIBLTE_ERROR_INVALID_INPUTS) {
       fprintf(stderr, "Error calling ue_mib_decode()\n");
-      return LIBLTE_ERROR;
+      goto free_and_exit;
     }
     if (n == MIB_FRAME_UNALIGNED) {
       printf("Realigning frame\n");
-      if (cuhd_recv(uhd, buffer, flen/2, 1)<0) {
+      if (cuhd_recv(uhd, buffer, 1500, 1)<0) {
         fprintf(stderr, "Error receiving from USRP\n");
-        return LIBLTE_ERROR;
+        goto free_and_exit;
       }
+      bzero(buffer, flen * sizeof(cf_t));
     }
     nof_frames++;
   } while (n != MIB_FOUND && nof_frames < 2*nof_frames_total);
+  
   if (n == MIB_FOUND) {
     printf("\n\nMIB decoded in %d ms (%d half frames)\n", nof_frames*5, nof_frames);
     pbch_mib_fprint(stdout, mib, found_cell->cell_id);
+    ret = LIBLTE_SUCCESS;
   } else {
-    printf("\nCould not decode MIB\n");
+    ret = LIBLTE_ERROR;
   }
 
+free_and_exit:
+  free(buffer);
+
   cuhd_stop_rx_stream(uhd); 
   cuhd_flush_buffer(uhd);
 
   ue_mib_free(&uemib);
   
-  return LIBLTE_SUCCESS; 
+  return ret; 
 }
 
-int find_cell(void *uhd, ue_celldetect_t *s, cf_t *buffer, ue_celldetect_result_t found_cell[3]) 
+int find_cell(void *uhd, ue_celldetect_result_t *found_cell, uint32_t N_id_2) 
 {  
-  int n; 
+  int ret = LIBLTE_ERROR; 
+  ue_celldetect_t cd;
+  
+  cf_t *buffer = vec_malloc(sizeof(cf_t) * 96000);
+  if (!buffer) {
+    perror("malloc");
+    goto free_and_exit;
+  }
+  
+  if (ue_celldetect_init(&cd)) {
+    fprintf(stderr, "Error initiating UE cell detect\n");
+    goto free_and_exit;
+  }
+  
+  ue_celldetect_set_nof_frames_detected(&cd, 50);
 
+  ue_celldetect_set_nof_frames_total(&cd, 500);
+  
   INFO("Setting sampling frequency 960 KHz for PSS search\n", 0);
   cuhd_set_rx_srate(uhd, 960000.0);
   INFO("Starting receiver...\n", 0);
   cuhd_start_rx_stream(uhd);
 
-  uint32_t nof_scanned_cells = 0; 
   uint32_t flen = 4800; 
-  int nof_detected_cells = 0; 
+  int n; 
   
   do {
-    
     if (cuhd_recv(uhd, buffer, flen, 1)<0) {
       fprintf(stderr, "Error receiving from USRP\n");
-      return LIBLTE_ERROR;
+      goto free_and_exit;
     }
+
+    DEBUG("Scanning cell at N_id_2=%d\n",N_id_2);
     
-    n = ue_celldetect_scan(s, buffer, flen, &found_cell[nof_scanned_cells]);
+    n = ue_celldetect_scan(&cd, buffer, flen, found_cell, N_id_2);
     switch(n) {
       case CS_FRAME_UNALIGNED:
         printf("Realigning frame\n");
         if (cuhd_recv(uhd, buffer, flen/2, 1)<0) {
           fprintf(stderr, "Error receiving from USRP\n");
-          return LIBLTE_ERROR;
+          goto free_and_exit;
         }
-        return LIBLTE_ERROR; 
+        /* FIXME: What should we do here?? */
+        ret = -1;
+        goto free_and_exit;
       case CS_CELL_DETECTED:
-        nof_detected_cells++;
-        if (found_cell[nof_scanned_cells].peak > 0) {
+        if (found_cell->peak > 0) {
           printf("\n\tCELL ID: %d, CP: %s, Peak: %.2f, Mode: %d/%d\n", 
-                found_cell[nof_scanned_cells].cell_id, 
-                lte_cp_string(found_cell[nof_scanned_cells].cp), 
-                found_cell[nof_scanned_cells].peak, found_cell[nof_scanned_cells].mode, 
-                s->nof_frames_detected);                      
+                found_cell->cell_id, 
+                lte_cp_string(found_cell->cp), 
+                found_cell->peak, found_cell->mode, 
+                cd.nof_frames_detected);                      
         }
         
-        nof_scanned_cells++;
+        ret = 1;
+        INFO("Cell found at N_id_2=%d\n",N_id_2);
         break;
       case CS_CELL_NOT_DETECTED:
-        nof_scanned_cells++;
+        ret = 0; 
+        DEBUG("No cell found at N_id_2=%d\n",N_id_2);
         break;
       case LIBLTE_ERROR:
       case LIBLTE_ERROR_INVALID_INPUTS: 
+        ret = LIBLTE_ERROR;
         fprintf(stderr, "Error calling cellsearch_scan()\n");
-        return LIBLTE_ERROR;         
+        goto free_and_exit;       
     }
-  } while(nof_scanned_cells < 3);
-
+    
+  } while(n == 0);
+  
+free_and_exit:
+  free(buffer);
+  ue_celldetect_free(&cd);
   INFO("Stopping receiver...\n", 0);
   cuhd_stop_rx_stream(uhd); 
   cuhd_flush_buffer(uhd);
-  
-  return nof_detected_cells; 
+
+  return ret; 
 }
+
+
+int find_all_cells(void *uhd, ue_celldetect_result_t found_cell[3]) 
+{
+
+  uint32_t N_id_2; 
+  int ret;
+  int nof_detected_cells = 0;
+  
+  for (N_id_2=0;N_id_2<3;N_id_2++) {
+    ret = find_cell(uhd, &found_cell[N_id_2], N_id_2);    
+    if (ret == 1) {
+      nof_detected_cells++;
+    } else if (ret == LIBLTE_ERROR) {
+      return LIBLTE_ERROR; 
+    }
+  }
+  return nof_detected_cells;
+}
+
+int cell_search(void *uhd, int force_N_id_2, lte_cell_t *cell, pbch_mib_t *mib) 
+{
+  int ret; 
+  
+  ue_celldetect_result_t found_cells[3];
+  bzero(found_cells, 3*sizeof(ue_celldetect_result_t));
+
+  if (force_N_id_2 >= 0) {
+    ret = find_cell(uhd, &found_cells[force_N_id_2], force_N_id_2);
+  } else {
+    ret = find_all_cells(uhd, found_cells);      
+  }
+  if (ret < 0) {
+    fprintf(stderr, "Error searching cell\n");
+    exit(-1);
+  }
+
+  int max_peak_cell = 0;
+  float max_peak_value = -1.0;
+  if (ret > 0) {
+    if (force_N_id_2 >= 0) {
+      max_peak_cell = force_N_id_2;
+    } else {
+      for (int i=0;i<3;i++) {
+        if (found_cells[i].peak > max_peak_value) {
+          max_peak_value = found_cells[i].peak;
+          max_peak_cell = i;
+        }
+      }        
+    }
+    
+    printf("Decoding PBCH for cell %d (N_id_2=%d)\n", found_cells[max_peak_cell].cell_id, max_peak_cell);
+    if (decode_pbch(uhd, &found_cells[max_peak_cell], 400, mib)) {
+      fprintf(stderr, "Could not decode PBCH from CELL ID %d\n", found_cells[max_peak_cell].cell_id);
+      return LIBLTE_ERROR;
+    }
+  } else {
+    fprintf(stderr, "Could not find any cell in this frequency\n");
+    return LIBLTE_ERROR;
+  }
+  
+  cell->cp = found_cells[max_peak_cell].cp;
+  cell->id = found_cells[max_peak_cell].cell_id;
+  cell->nof_prb = mib->nof_prb;
+  cell->nof_ports = mib->nof_ports; 
+  
+  /* set sampling frequency */
+  int srate = lte_sampling_freq_hz(cell->nof_prb);
+  if (srate != -1) {  
+    cuhd_set_rx_srate(uhd, (double) srate);      
+  } else {
+    fprintf(stderr, "Invalid number of PRB %d\n", cell->nof_prb);
+    return LIBLTE_ERROR;
+  }
+  return LIBLTE_SUCCESS;
+}
+
+
 #endif
diff --git a/lte/phy/examples/cell_search_utils.h b/lte/phy/examples/cell_search_utils.h
index 2fefed105..8d5d4f5dd 100644
--- a/lte/phy/examples/cell_search_utils.h
+++ b/lte/phy/examples/cell_search_utils.h
@@ -29,12 +29,18 @@
 #include "liblte/phy/phy.h"
 
 int decode_pbch(void *uhd, 
-                cf_t *buffer, 
                 ue_celldetect_result_t *found_cell, 
                 uint32_t nof_frames_total, 
                 pbch_mib_t *mib);
 
+int find_all_cells(void *uhd, 
+                   ue_celldetect_result_t found_cell[3]);
+
 int find_cell(void *uhd, 
-              ue_celldetect_t *s, 
-              cf_t *buffer, 
-              ue_celldetect_result_t found_cell[3]);
\ No newline at end of file
+               ue_celldetect_result_t *found_cell, 
+               uint32_t N_id_2);
+
+int cell_search(void *uhd, 
+                int force_N_id_2, 
+                lte_cell_t *cell, 
+                pbch_mib_t *mib);
\ No newline at end of file
diff --git a/lte/phy/examples/iodev.c b/lte/phy/examples/iodev.c
index ce55b3a3a..d617221b3 100644
--- a/lte/phy/examples/iodev.c
+++ b/lte/phy/examples/iodev.c
@@ -83,74 +83,27 @@ int iodev_init(iodev_t *q, iodev_cfg_t *config, lte_cell_t *cell, pbch_mib_t *mi
     q->sf_idx = 9; 
     
   } else {
-#ifndef DISABLE_UHD
+    
+    #ifndef DISABLE_UHD
+    
     printf("Opening UHD device...\n");
     if (cuhd_open(config->uhd_args, &q->uhd)) {
       fprintf(stderr, "Error opening uhd\n");
       return LIBLTE_ERROR;
     }
-    
+    /* Set receiver gain */
     cuhd_set_rx_gain(q->uhd, config->uhd_gain);
 
     /* set receiver frequency */
     cuhd_set_rx_freq(q->uhd, (double) config->uhd_freq);
-
     cuhd_rx_wait_lo_locked(q->uhd);
     DEBUG("Set uhd_freq to %.3f MHz\n", (double ) config->uhd_freq);
 
-    int n; 
-    ue_celldetect_t cd;
-    ue_celldetect_result_t found_cells[3];
-
-    cf_t *buffer = vec_malloc(sizeof(cf_t) * 96000);
-    if (!buffer) {
-      perror("malloc");
-      return LIBLTE_ERROR;
-    }
-    if (ue_celldetect_init(&cd)) {
-      fprintf(stderr, "Error initiating UE cell detect\n");
-      exit(-1);
-    }
-    n = find_cell(q->uhd, &cd, buffer, found_cells);
-    if (n < 0) {
-      fprintf(stderr, "Error searching cell\n");
-      exit(-1);
-    }
-
-    int max_peak_cell = 0;
-    float max_peak_value = -1.0;
-    if (n > 0) {
-      for (int i=0;i<3;i++) {
-        if (found_cells[i].peak > max_peak_value) {
-          max_peak_value = found_cells[i].peak;
-          max_peak_cell = i;
-        }
-      }
-      if (decode_pbch(q->uhd, buffer, &found_cells[max_peak_cell], 400, mib)) {
-        fprintf(stderr, "Could not decode PBCH from CELL ID %d\n", found_cells[max_peak_cell].cell_id);
-        return LIBLTE_ERROR;
-      }
-    } else {
-      fprintf(stderr, "Could not find any cell in this frequency\n");
-      return LIBLTE_ERROR;
+    if (cell_search(q->uhd, config->force_N_id_2, cell, mib)) {
+      fprintf(stderr, "Cell not found\n");
+      return LIBLTE_ERROR; 
     }
     
-    free(buffer);
-    cell->cp = found_cells[max_peak_cell].cp;
-    cell->id = found_cells[max_peak_cell].cell_id;
-    cell->nof_prb = mib->nof_prb;
-    cell->nof_ports = mib->nof_ports; 
-    
-    /* set sampling frequency */
-    int srate = lte_sampling_freq_hz(cell->nof_prb);
-    if (srate != -1) {  
-      cuhd_set_rx_srate(q->uhd, (double) srate);      
-    } else {
-      fprintf(stderr, "Invalid number of PRB %d\n", cell->nof_prb);
-      return LIBLTE_ERROR;
-    }
-
-    DEBUG("Starting receiver...\n", 0);
     cuhd_start_rx_stream(q->uhd);
     
     if (ue_sync_init(&q->sframe, *cell, cuhd_recv_wrapper, q->uhd)) {
@@ -158,9 +111,6 @@ int iodev_init(iodev_t *q, iodev_cfg_t *config, lte_cell_t *cell, pbch_mib_t *mi
       return LIBLTE_ERROR; 
     }
 
-    /* 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(&q->sframe, true);
-
     // Here, the subframe length and input buffer is managed by ue_sync
     q->mode = UHD; 
   
diff --git a/lte/phy/examples/iodev.h b/lte/phy/examples/iodev.h
index bc020222f..b6c36adc5 100644
--- a/lte/phy/examples/iodev.h
+++ b/lte/phy/examples/iodev.h
@@ -57,6 +57,7 @@ typedef struct LIBLTE_API {
   uint32_t cell_id_file;
   uint32_t nof_prb_file;
   uint32_t nof_ports_file; 
+  int force_N_id_2;
 
   float uhd_freq;
   float uhd_gain;
diff --git a/lte/phy/examples/pdsch_ue.c b/lte/phy/examples/pdsch_ue.c
index adf3c8f4b..07d3dae32 100644
--- a/lte/phy/examples/pdsch_ue.c
+++ b/lte/phy/examples/pdsch_ue.c
@@ -68,13 +68,14 @@ void args_default(prog_args_t *args) {
   args->disable_plots = false; 
   args->io_config.find_threshold = -1.0; 
   args->io_config.input_file_name = NULL; 
+  args->io_config.force_N_id_2 = -1; // Pick the best
   args->io_config.uhd_args = "";
   args->io_config.uhd_freq = -1.0;
   args->io_config.uhd_gain = 60.0; 
 }
 
 void usage(prog_args_t *args, char *prog) {
-  printf("Usage: %s [cargfndvtb] [-i input_file | -f rx_frequency (in Hz)]\n", prog);
+  printf("Usage: %s [cargndvtbl] [-i input_file | -f rx_frequency (in Hz)]\n", prog);
   printf("\t-c cell_id if reading from file [Default %d]\n", args->io_config.cell_id_file);
   printf("\t-p nof_prb if reading from file [Default %d]\n", args->io_config.nof_prb_file);
   printf("\t-o nof_ports if reading from file [Default %d]\n", args->io_config.nof_ports_file);
@@ -85,6 +86,7 @@ void usage(prog_args_t *args, char *prog) {
 #else
   printf("\t   UHD is disabled. CUHD library not available\n");
 #endif
+  printf("\t-l Force N_id_2 [Default best]\n");
   printf("\t-b Decode PBCH only [Default All channels]\n");
   printf("\t-n nof_subframes [Default %d]\n", args->nof_subframes);
   printf("\t-t PSS threshold [Default %f]\n", args->io_config.find_threshold);
@@ -99,7 +101,7 @@ void usage(prog_args_t *args, char *prog) {
 void parse_args(prog_args_t *args, int argc, char **argv) {
   int opt;
   args_default(args);
-  while ((opt = getopt(argc, argv, "icagfndvtbpro")) != -1) {
+  while ((opt = getopt(argc, argv, "icagfndvtbprol")) != -1) {
     switch (opt) {
     case 'i':
       args->io_config.input_file_name = argv[optind];
@@ -128,6 +130,9 @@ void parse_args(prog_args_t *args, int argc, char **argv) {
     case 'n':
       args->nof_subframes = atoi(argv[optind]);
       break;
+    case 'l':
+      args->io_config.force_N_id_2 = atoi(argv[optind]);
+      break;
     case 'r':
       args->rnti= atoi(argv[optind]);
       break;
@@ -144,6 +149,7 @@ void parse_args(prog_args_t *args, int argc, char **argv) {
   }
   if (args->io_config.uhd_freq < 0 && args->io_config.input_file_name == NULL) {
     usage(args, argv[0]);
+    exit(-1);
   }
 }
 /**********************************************************************/
@@ -177,10 +183,6 @@ int main(int argc, char **argv) {
   }
 #endif
   
-  /* Setup SIGINT handler */
-  printf("\n --- Press Ctrl+C to exit --- \n");
-  signal(SIGINT, sigintHandler);
-
   /* Initialize subframe counter */
   sf_cnt = 0;
 
@@ -225,6 +227,7 @@ int main(int argc, char **argv) {
                 (int) ue_dl.pkt_errors, (int) ue_dl.pkts_total, (int) ue_dl.nof_trials, 
                (float) ue_dl.pkt_errors / ue_dl.pkts_total, 
                mean_exec_time);                
+        
       }      
       #ifndef DISABLE_GRAPHICS
       if (!prog_args.disable_plots && iodev_get_sfidx(&iodev) == 5) {
diff --git a/lte/phy/include/liblte/phy/ch_estimation/chest.h b/lte/phy/include/liblte/phy/ch_estimation/chest.h
index e182e684b..0eb7a4401 100644
--- a/lte/phy/include/liblte/phy/ch_estimation/chest.h
+++ b/lte/phy/include/liblte/phy/ch_estimation/chest.h
@@ -76,12 +76,43 @@ LIBLTE_API void chest_free(chest_t *q);
 LIBLTE_API int chest_set_nof_ports(chest_t *q, 
                                     uint32_t nof_ports);
 
-LIBLTE_API int chest_ce_ref(chest_t *q, 
+
+LIBLTE_API float chest_rsrp(chest_t *q, 
+                            uint32_t nslot, 
+                            uint32_t port_id);
+
+LIBLTE_API float chest_rsrp_sf(chest_t *q, 
+                               uint32_t sf_idx);
+
+LIBLTE_API float chest_rssi(chest_t *q, 
+                            cf_t *input);
+
+LIBLTE_API float chest_rssi_sf(chest_t *q, 
+                               cf_t *input);
+
+LIBLTE_API float chest_rsrq(chest_t *q, 
+                            cf_t *input, 
+                            uint32_t nslot, 
+                            uint32_t port_id);
+
+LIBLTE_API float chest_rsrq_sf(chest_t *q, 
+                               cf_t *input, 
+                               uint32_t sf_idx);
+
+LIBLTE_API int chest_measure_ref(chest_t *q, 
                              cf_t *input, 
                              uint32_t nslot, 
                              uint32_t port_id, 
                              uint32_t nref);
 
+LIBLTE_API void chest_measure_slot(chest_t *q, 
+                                   cf_t *input, 
+                                   uint32_t nslot);
+
+LIBLTE_API void chest_measure_sf(chest_t *q, 
+                                 cf_t *input, 
+                                 uint32_t sf_idx);
+
 LIBLTE_API int chest_ce_slot_port(chest_t *q, 
                                    cf_t *input, 
                                    cf_t *ce, 
diff --git a/lte/phy/include/liblte/phy/ch_estimation/refsignal.h b/lte/phy/include/liblte/phy/ch_estimation/refsignal.h
index 86342cc79..e237c021a 100644
--- a/lte/phy/include/liblte/phy/ch_estimation/refsignal.h
+++ b/lte/phy/include/liblte/phy/ch_estimation/refsignal.h
@@ -45,8 +45,7 @@ typedef _Complex float cf_t;
 typedef struct LIBLTE_API{
   uint32_t time_idx;
   uint32_t freq_idx;
-  cf_t simbol;
-  cf_t recv_simbol;
+  cf_t symbol;
 }ref_t;
 
 typedef struct LIBLTE_API{
@@ -57,6 +56,7 @@ typedef struct LIBLTE_API{
   uint32_t nof_prb;
   ref_t *refs;
   cf_t *ch_est;
+  cf_t *recv_symbol;
 } refsignal_t;
 
 
diff --git a/lte/phy/include/liblte/phy/common/phy_common.h b/lte/phy/include/liblte/phy/common/phy_common.h
index 4f7efc742..52dc3a3b6 100644
--- a/lte/phy/include/liblte/phy/common/phy_common.h
+++ b/lte/phy/include/liblte/phy/common/phy_common.h
@@ -92,7 +92,7 @@ typedef enum {CPNORM, CPEXT} lte_cp_t;
 #define SLOT_IDX_CPNORM(idx, symbol_sz) (idx==0?(CP(symbol_sz, CPNORM_0_LEN)):(CP(symbol_sz, CPNORM_0_LEN)+idx*(symbol_sz+CP(symbol_sz, CPNORM_LEN))))
 #define SLOT_IDX_CPEXT(idx, symbol_sz) (idx*(symbol_sz+CP(symbol_sz, CPEXT_LEN)))
 
-#define SAMPLE_IDX(nof_prb, symbol_idx, sample_idx) (symbol_idx*nof_prb*RE_X_RB + sample_idx)
+#define SAMPLE_IDX(nof_prb, symbol_idx, sample_idx) ((symbol_idx)*(nof_prb)*(RE_X_RB) + sample_idx)
 
 #define RS_VSHIFT(cell_id) (cell_id%6)
 
diff --git a/lte/phy/include/liblte/phy/sync/sync.h b/lte/phy/include/liblte/phy/sync/sync.h
index 0cf86b7c9..917a031b1 100644
--- a/lte/phy/include/liblte/phy/sync/sync.h
+++ b/lte/phy/include/liblte/phy/sync/sync.h
@@ -69,6 +69,10 @@ typedef struct LIBLTE_API {
   bool sss_en;
   bool normalize_en; 
   lte_cp_t cp;
+  uint32_t m0;
+  uint32_t m1;
+  float m0_value;
+  float m1_value;
 }sync_t;
 
 
@@ -115,6 +119,9 @@ LIBLTE_API float sync_get_cfo(sync_t *q);
 /* Gets the CP length estimation from the last call to synch_run() */
 LIBLTE_API lte_cp_t sync_get_cp(sync_t *q);
 
+/* Sets the CP length estimation (must do it if disabled) */
+LIBLTE_API void sync_set_cp(sync_t *q, lte_cp_t cp);
+
 /* Enables/Disables energy normalization every frame. If disabled, uses the mean */
 LIBLTE_API void sync_normalize_en(sync_t *q, 
                                   bool enable);
diff --git a/lte/phy/include/liblte/phy/ue/ue_celldetect.h b/lte/phy/include/liblte/phy/ue/ue_celldetect.h
index 778e17508..3478ee02f 100644
--- a/lte/phy/include/liblte/phy/ue/ue_celldetect.h
+++ b/lte/phy/include/liblte/phy/ue/ue_celldetect.h
@@ -57,8 +57,8 @@
  * TODO: Check also peak offset 
  */
 
-#define CS_DEFAULT_MAXFRAMES_TOTAL      300
-#define CS_DEFAULT_MAXFRAMES_DETECTED   30
+#define CS_DEFAULT_MAXFRAMES_TOTAL      500
+#define CS_DEFAULT_MAXFRAMES_DETECTED   50
 
 #define CS_DEFAULT_NOFFRAMES_TOTAL      100
 #define CS_DEFAULT_NOFFRAMES_DETECTED   10
@@ -87,8 +87,6 @@ typedef struct LIBLTE_API {
   uint32_t current_nof_detected; 
   uint32_t current_nof_total; 
   
-  uint32_t current_N_id_2;
-
   uint32_t *mode_ntimes;
   char *mode_counted; 
   
@@ -109,7 +107,8 @@ LIBLTE_API void ue_celldetect_reset(ue_celldetect_t *q);
 LIBLTE_API int ue_celldetect_scan(ue_celldetect_t *q,
                                   cf_t *signal, 
                                   uint32_t nsamples,
-                                  ue_celldetect_result_t *found_cell);
+                                  ue_celldetect_result_t *found_cell,
+                                  uint32_t N_id_2);
 
 LIBLTE_API int ue_celldetect_set_nof_frames_total(ue_celldetect_t *q, 
                                                    uint32_t nof_frames);
diff --git a/lte/phy/include/liblte/phy/ue/ue_mib.h b/lte/phy/include/liblte/phy/ue/ue_mib.h
index 31624d6f9..75f8c69bb 100644
--- a/lte/phy/include/liblte/phy/ue/ue_mib.h
+++ b/lte/phy/include/liblte/phy/ue/ue_mib.h
@@ -55,10 +55,8 @@
 #include "liblte/phy/phch/pbch.h"
 #include "liblte/phy/common/fft.h"
 
-#define MIB_FIND_THRESHOLD          0.6
 
 #define MIB_NOF_PORTS           2
-
 #define MIB_FRAME_SIZE          9600
 
 #define MIB_FRAME_UNALIGNED     -3
diff --git a/lte/phy/include/liblte/phy/utils/vector.h b/lte/phy/include/liblte/phy/utils/vector.h
index 3989dd69d..a0e89471a 100644
--- a/lte/phy/include/liblte/phy/utils/vector.h
+++ b/lte/phy/include/liblte/phy/utils/vector.h
@@ -35,8 +35,11 @@
 
 typedef _Complex float cf_t;
 
-#define EXPAVERAGE(data, average, nframes) (((data) + (average) * (nframes)) / ((nframes) + 1))  
+// Cumulative moving average
+#define VEC_CMA(data, average, n) ((data) + ((data) - (average)) / ((n)+1)) 
 
+// Exponential moving average
+#define VEC_EMA(data, average, alpha) ((factor)*(data)+(1-alpha)*(average))
 
 /** Return the sum of all the elements */
 LIBLTE_API int vec_acc_ii(int *x, uint32_t len);
diff --git a/lte/phy/lib/ch_estimation/src/chest.c b/lte/phy/lib/ch_estimation/src/chest.c
index 37dabf82a..b34471122 100644
--- a/lte/phy/lib/ch_estimation/src/chest.c
+++ b/lte/phy/lib/ch_estimation/src/chest.c
@@ -38,7 +38,7 @@
 #define SLOT_SZ(q) (q->nof_symbols * q->symbol_sz)
 #define SF_SZ(q) (2 * SLOT_SZ(q))
 
-#define VOLK_INTERP
+//#define VOLK_INTERP
 
 void chest_fprint(chest_t *q, FILE *stream, uint32_t nslot, uint32_t port_id) {
   chest_ref_fprint(q, stream, nslot, port_id);
@@ -62,8 +62,8 @@ void chest_ref_fprint(chest_t *q, FILE *stream, uint32_t nslot, uint32_t port_id
   int i;
   fprintf(stream, "refs%d=[",port_id);
   for (i=0;i<q->refsignal[port_id][nslot].nof_refs;i++) {
-    fprintf(stream, "%3.3f%+3.3fi, ", __real__ q->refsignal[port_id][nslot].refs[i].simbol,
-        __imag__ q->refsignal[port_id][nslot].refs[i].simbol);
+    fprintf(stream, "%3.3f%+3.3fi, ", __real__ q->refsignal[port_id][nslot].refs[i].symbol,
+        __imag__ q->refsignal[port_id][nslot].refs[i].symbol);
   }
   fprintf(stream, "];\n");
 }
@@ -72,8 +72,8 @@ void chest_recvsig_fprint(chest_t *q, FILE *stream, uint32_t nslot, uint32_t por
   int i;
   fprintf(stream, "recvsig%d=[",port_id);
   for (i=0;i<q->refsignal[port_id][nslot].nof_refs;i++) {
-    fprintf(stream, "%3.3f%+3.3fi, ", __real__ q->refsignal[port_id][nslot].refs[i].recv_simbol,
-        __imag__ q->refsignal[port_id][nslot].refs[i].recv_simbol);
+    fprintf(stream, "%3.3f%+3.3fi, ", __real__ q->refsignal[port_id][nslot].recv_symbol[i],
+        __imag__ q->refsignal[port_id][nslot].recv_symbol[i]);
   }
   fprintf(stream, "];\n");
 }
@@ -92,7 +92,59 @@ void chest_ce_fprint(chest_t *q, FILE *stream, uint32_t nslot, uint32_t port_id)
   fprintf(stream, "];\n");
 }
 
-int chest_ce_ref(chest_t *q, cf_t *input, uint32_t nslot, uint32_t port_id, uint32_t nref) {
+float chest_rsrp(chest_t *q, uint32_t nslot, uint32_t port_id) {
+  int nof_refs = q->refsignal[port_id][nslot].nof_refs;
+  cf_t *ch_est = q->refsignal[port_id][nslot].ch_est;
+  return crealf(vec_dot_prod_conj_ccc(ch_est, ch_est, nof_refs))/nof_refs;
+}
+
+float chest_rsrp_sf(chest_t *q, uint32_t sf_idx) {
+  int n,p;
+  float rsrp=0;
+  for (p=0;p<q->nof_ports;p++) {
+    for (n=0;n<2;n++) {
+      rsrp+=chest_rsrp(q, 2*sf_idx+n, p)/(2*q->nof_ports);
+    }    
+  }
+  return rsrp;
+}
+
+float chest_rssi(chest_t *q, cf_t *input) {
+  float rssi = 0;
+  int i;
+  int l[2];
+  if (q->nof_symbols == CPNORM_NSYMB) {
+    l[0] = 0; l[1] = 4;
+  } else {
+    l[0] = 0; l[1] = 3;
+  }
+
+  for (i=0;i<2;i++) {
+    cf_t *tmp = &input[l[i]*q->nof_re];
+    rssi += crealf(vec_dot_prod_conj_ccc(tmp, tmp, q->nof_re));
+  }
+  return rssi; 
+}
+
+float chest_rssi_sf(chest_t *q, cf_t *input) {
+  int n;
+  int slotsz = q->nof_symbols*q->nof_re;
+  float rssi=0;
+  for (n=0;n<2;n++) {
+    rssi += chest_rssi(q, &input[n*slotsz]);
+  }
+  return rssi;
+}
+
+float chest_rsrq(chest_t *q, cf_t *input, uint32_t nslot, uint32_t port_id) {
+  return (q->nof_re/RE_X_RB) * chest_rsrp(q, nslot, port_id) / chest_rssi(q, input);
+}
+
+float chest_rsrq_sf(chest_t *q, cf_t *input, uint32_t sf_idx) {
+  return (4*q->nof_ports*q->nof_re/RE_X_RB) * chest_rsrp_sf(q, sf_idx) / chest_rssi_sf(q, input);
+}
+
+int chest_measure_ref(chest_t *q, cf_t *input, uint32_t nslot, uint32_t port_id, uint32_t nref) {
   int fidx, tidx;
   cf_t known_ref, channel_ref;
   int ret = LIBLTE_ERROR_INVALID_INPUTS;
@@ -107,10 +159,9 @@ int chest_ce_ref(chest_t *q, cf_t *input, uint32_t nslot, uint32_t port_id, uint
       fidx = q->refsignal[port_id][nslot].refs[nref].freq_idx; // reference frequency index
       tidx = q->refsignal[port_id][nslot].refs[nref].time_idx; // reference time index
 
-      known_ref = q->refsignal[port_id][nslot].refs[nref].simbol;
+      known_ref = q->refsignal[port_id][nslot].refs[nref].symbol;
       channel_ref = input[tidx * q->nof_re + fidx];
-      q->refsignal[port_id][nslot].refs[nref].recv_simbol = channel_ref;
-
+      q->refsignal[port_id][nslot].recv_symbol[nref] = channel_ref;
       
       DEBUG("Reference %2d pos (%2d,%2d)=%3d %.2f dB %.2f/%.2f=%.2f\n", nref, tidx, fidx, tidx * q->nof_re + fidx,          
             10*log10f(cabsf(channel_ref/known_ref)),          
@@ -130,10 +181,42 @@ int chest_ce_ref(chest_t *q, cf_t *input, uint32_t nslot, uint32_t port_id, uint
   return ret;
 }
 
+void chest_measure_slot_port(chest_t *q, cf_t *input, uint32_t nslot, uint32_t port_id) 
+{
+  int i;
+  refsignal_t *r = &q->refsignal[port_id][nslot];
+  
+  DEBUG("Estimating channel slot=%d port=%d using %d reference signals\n",
+          nslot, port_id, r->nof_refs);
+
+  for (i=0;i<r->nof_refs;i++) {
+    chest_measure_ref(q, input, nslot, port_id, i);
+  }
+}
+
+void chest_measure_slot(chest_t *q, cf_t *input, uint32_t nslot) {
+  int p;
+  for (p=0;p<q->nof_ports;p++) {
+    chest_measure_slot_port(q, input, nslot, p);
+  }
+}
+
+void chest_measure_sf(chest_t *q, cf_t *input, uint32_t sf_idx) {
+  int p, n, slotsz;
+  slotsz = q->nof_symbols*q->nof_re;
+  for (p=0;p<q->nof_ports;p++) {
+    for (n=0;n<2;n++) {
+      chest_measure_slot_port(q, &input[n*slotsz], 2*sf_idx+n, p);
+    }
+  }
+}
+
+
 /* Computes channel estimates for each reference in a slot and port.
  * Saves the nof_prb * 12 * nof_symbols channel estimates in the array ce
  */
-int chest_ce_slot_port(chest_t *q, cf_t *input, cf_t *ce, uint32_t nslot, uint32_t port_id) {
+int chest_ce_slot_port(chest_t *q, cf_t *input, cf_t *ce, uint32_t nslot, uint32_t port_id) 
+{
   uint32_t i, j;
   cf_t x[2], y[MAX_NSYMB];
 
@@ -147,13 +230,8 @@ int chest_ce_slot_port(chest_t *q, cf_t *input, cf_t *ce, uint32_t nslot, uint32
     if (q->refsignal[port_id][nslot].nsymbols <= 2) {
       refsignal_t *r = &q->refsignal[port_id][nslot];
 
-      DEBUG("Estimating channel slot=%d port=%d using %d reference signals\n",
-          nslot, port_id, r->nof_refs);
-
-      for (i=0;i<r->nof_refs;i++) {
-        chest_ce_ref(q, input, nslot, port_id, i);
-      }
-
+      chest_measure_slot_port(q, input, nslot, port_id);
+      
       /* interpolate the symbols with references
       * in the freq domain */
       for (i=0;i<r->nsymbols;i++) {
diff --git a/lte/phy/lib/ch_estimation/src/refsignal.c b/lte/phy/lib/ch_estimation/src/refsignal.c
index 037b45edb..bec652743 100644
--- a/lte/phy/lib/ch_estimation/src/refsignal.c
+++ b/lte/phy/lib/ch_estimation/src/refsignal.c
@@ -83,7 +83,7 @@ int refsignal_put(refsignal_t * q, cf_t * slot_symbols)
     for (i = 0; i < q->nof_refs; i++) {
       fidx = q->refs[i].freq_idx;       // reference frequency index
       tidx = q->refs[i].time_idx;       // reference time index
-      slot_symbols[SAMPLE_IDX(q->nof_prb, tidx, fidx)] = q->refs[i].simbol;
+      slot_symbols[SAMPLE_IDX(q->nof_prb, tidx, fidx)] = q->refs[i].symbol;
     }
     return LIBLTE_SUCCESS;
   } else {
@@ -153,6 +153,11 @@ int refsignal_init_LTEDL(refsignal_t * q, uint32_t port_id, uint32_t nslot,
       goto free_and_exit;
     }
 
+    q->recv_symbol = vec_malloc(q->nof_refs * sizeof(cf_t));
+    if (!q->recv_symbol) {
+      goto free_and_exit;
+    }
+
     ns = nslot;
     for (l = 0; l < nof_ref_symbols; l++) {
 
@@ -169,9 +174,9 @@ int refsignal_init_LTEDL(refsignal_t * q, uint32_t port_id, uint32_t nslot,
         mp = i + MAX_PRB - cell.nof_prb;
 
         /* generate signal */
-        __real__ q->refs[idx(l, i)].simbol =
+        __real__ q->refs[idx(l, i)].symbol =
           (1 - 2 * (float) seq.c[2 * mp]) / sqrt(2);
-        __imag__ q->refs[idx(l, i)].simbol =
+        __imag__ q->refs[idx(l, i)].symbol =
           (1 - 2 * (float) seq.c[2 * mp + 1]) / sqrt(2);
 
         /* mapping to resource elements */
@@ -259,17 +264,17 @@ int rs_sequence(ref_t * refs, uint32_t len, float alpha, uint32_t ns, uint32_t c
         cexpf(-I * M_PI * (float) q * (float) i * ((float) i + 1) / n_sz);
     }
     for (i = 0; i < len; i++) {
-      refs[i].simbol = cfg->beta * cexpf(I * alpha * i) * x_q[i % n_sz];
+      refs[i].symbol = cfg->beta * cexpf(I * alpha * i) * x_q[i % n_sz];
     }
     free(x_q);
   } else {
     if (len == RE_X_RB) {
       for (i = 0; i < len; i++) {
-        refs[i].simbol = cfg->beta * cexpf(I * (phi_M_sc_12[u][i] * M_PI / 4 + alpha * i));
+        refs[i].symbol = cfg->beta * cexpf(I * (phi_M_sc_12[u][i] * M_PI / 4 + alpha * i));
       }
     } else {
       for (i = 0; i < len; i++) {
-        refs[i].simbol = cfg->beta * cexpf(I * (phi_M_sc_24[u][i] * M_PI / 4 + alpha * i));
+        refs[i].symbol = cfg->beta * cexpf(I * (phi_M_sc_24[u][i] * M_PI / 4 + alpha * i));
       }
     }
   }
diff --git a/lte/phy/lib/modem/test/soft_demod_test.c b/lte/phy/lib/modem/test/soft_demod_test.c
index 96b76c5b0..5849f4693 100644
--- a/lte/phy/lib/modem/test/soft_demod_test.c
+++ b/lte/phy/lib/modem/test/soft_demod_test.c
@@ -189,7 +189,7 @@ int main(int argc, char **argv) {
     
     /* compute exponentially averaged execution time */
     if (n > 0) {
-      mean_texec = EXPAVERAGE((float) t[0].tv_usec, mean_texec, n-1);      
+      mean_texec = VEC_CMA((float) t[0].tv_usec, mean_texec, n-1);      
     }
     
     /* check MSE */
diff --git a/lte/phy/lib/phch/src/pdsch.c b/lte/phy/lib/phch/src/pdsch.c
index 2c21b7dd6..87d84f7c7 100644
--- a/lte/phy/lib/phch/src/pdsch.c
+++ b/lte/phy/lib/phch/src/pdsch.c
@@ -559,7 +559,7 @@ int pdsch_decode_tb(pdsch_t *q, char *data, uint32_t tbs, uint32_t nb_e,
         
       } while (q->nof_iterations < TDEC_MAX_ITERATIONS && !early_stop);
             
-      q->average_nof_iterations = EXPAVERAGE((float) q->nof_iterations, 
+      q->average_nof_iterations = VEC_CMA((float) q->nof_iterations, 
                                              q->average_nof_iterations, 
                                              q->average_nof_iterations_n);
       q->average_nof_iterations_n++;
diff --git a/lte/phy/lib/sync/src/pss.c b/lte/phy/lib/sync/src/pss.c
index 9cb7b53fa..3f42d7872 100644
--- a/lte/phy/lib/sync/src/pss.c
+++ b/lte/phy/lib/sync/src/pss.c
@@ -254,7 +254,7 @@ int pss_synch_find_pss(pss_synch_t *q, cf_t *input, float *corr_peak_value)
   #endif
 
     /* Find maximum of the absolute value of the correlation */
-    corr_peak_pos = vec_max_abs_ci(q->conv_output, conv_output_len);
+    corr_peak_pos = vec_max_abs_ci(q->conv_output, conv_output_len-1);
     if (corr_peak_value) {
       *corr_peak_value = cabsf(q->conv_output[corr_peak_pos]);
     }
diff --git a/lte/phy/lib/sync/src/sync.c b/lte/phy/lib/sync/src/sync.c
index 34e4bd28e..cd99cf496 100644
--- a/lte/phy/lib/sync/src/sync.c
+++ b/lte/phy/lib/sync/src/sync.c
@@ -55,6 +55,7 @@ int sync_init(sync_t *q, uint32_t frame_size, uint32_t fft_size) {
     bzero(q, sizeof(sync_t));
     q->detect_cp = true;
     q->normalize_en = true; 
+    q->mean_energy = 1.0;
     q->sss_en = true;
     q->N_id_2 = 1000; 
     q->N_id_1 = 1000;
@@ -144,6 +145,9 @@ void sync_cp_en(sync_t *q, bool enabled) {
 lte_cp_t sync_get_cp(sync_t *q) {
   return q->cp;
 }
+void sync_set_cp(sync_t *q, lte_cp_t cp) {
+  q->cp = cp;
+}
 
 /* CP detection algorithm taken from: 
  * "SSS Detection Method for Initial Cell Search in 3GPP LTE FDD/TDD Dual Mode Receiver"
@@ -183,9 +187,7 @@ static lte_cp_t detect_cp(sync_t *q, cf_t *input, uint32_t peak_pos)
 }
 
 int sync_sss(sync_t *q, cf_t *input, uint32_t peak_pos) {
-  uint32_t m0, m1;
   int sss_idx, ret;
-  float m0_value, m1_value;
 
   sss_synch_set_N_id_2(&q->sss, q->N_id_2);
 
@@ -195,17 +197,15 @@ int sync_sss(sync_t *q, cf_t *input, uint32_t peak_pos) {
   
   /* Make sure we have enough room to find SSS sequence */
   sss_idx = (int) peak_pos - 2*(q->fft_size + CP(q->fft_size, q->cp));
-
   if (sss_idx < 0) {
     INFO("Not enough room to decode CP SSS (sss_idx=%d, peak_pos=%d)\n", sss_idx, peak_pos);
     return LIBLTE_SUCCESS;
   }
       
-  /* try Normal CP length */
-  sss_synch_m0m1(&q->sss, &input[sss_idx], &m0, &m0_value, &m1, &m1_value);
+  sss_synch_m0m1(&q->sss, &input[sss_idx], &q->m0, &q->m0_value, &q->m1, &q->m1_value);
 
-  q->sf_idx = sss_synch_subframe(m0, m1);
-  ret = sss_synch_N_id_1(&q->sss, m0, m1);
+  q->sf_idx = sss_synch_subframe(q->m0, q->m1);
+  ret = sss_synch_N_id_1(&q->sss, q->m0, q->m1);
   if (ret >= 0) {
     q->N_id_1 = (uint32_t) ret;
   } else {
@@ -223,14 +223,14 @@ int sync_find(sync_t *q, cf_t *input, uint32_t find_offset, uint32_t *peak_posit
   
   int ret = LIBLTE_ERROR_INVALID_INPUTS; 
   
-  float peak_unnormalized, energy;
+  float peak_unnormalized=0, energy=1;
   
   if (q                 != NULL     &&
       input             != NULL     &&
       lte_N_id_2_isvalid(q->N_id_2) && 
       fft_size_isvalid(q->fft_size))
   {
-    uint32_t peak_pos;
+    int peak_pos;
     
     if (peak_position) {
       *peak_position = 0; 
@@ -239,14 +239,17 @@ int sync_find(sync_t *q, cf_t *input, uint32_t find_offset, uint32_t *peak_posit
     pss_synch_set_N_id_2(&q->pss, q->N_id_2);
   
     peak_pos = pss_synch_find_pss(&q->pss, &input[find_offset], &peak_unnormalized);
-    
+    if (peak_pos < 0) {
+      fprintf(stderr, "Error calling finding PSS sequence\n");
+      return LIBLTE_ERROR; 
+    }
     if (q->normalize_en        && 
         peak_pos + find_offset >= q->fft_size) 
     {
       /* Compute the energy of the received PSS sequence to normalize */
       cf_t *pss_ptr = &input[find_offset+peak_pos-q->fft_size];
       energy = sqrtf(crealf(vec_dot_prod_conj_ccc(pss_ptr, pss_ptr, q->fft_size)) / (q->fft_size));
-      q->mean_energy = EXPAVERAGE(energy, q->mean_energy, q->frame_cnt);
+      q->mean_energy = VEC_CMA(energy, q->mean_energy, q->frame_cnt);
     } else {     
       if (q->mean_energy == 0.0) {
         q->mean_energy = 1.0;
@@ -256,7 +259,7 @@ int sync_find(sync_t *q, cf_t *input, uint32_t find_offset, uint32_t *peak_posit
 
     /* Normalize and compute mean peak value */
     q->peak_value = peak_unnormalized/energy;
-    q->mean_peak_value = EXPAVERAGE(q->peak_value, q->mean_peak_value, q->frame_cnt);
+    q->mean_peak_value = VEC_CMA(q->peak_value, q->mean_peak_value, q->frame_cnt);
     q->frame_cnt++;
     
     /* If peak is over threshold, compute CFO and SSS */
@@ -264,25 +267,27 @@ int sync_find(sync_t *q, cf_t *input, uint32_t find_offset, uint32_t *peak_posit
       if (find_offset + peak_pos       >= q->fft_size) {
         q->cfo = pss_synch_cfo_compute(&q->pss, &input[find_offset+peak_pos-q->fft_size]);
         if (q->sss_en) {
-          if (sync_sss(q, input, find_offset + peak_pos) < 0) {
+          ret = sync_sss(q, input, find_offset + peak_pos); 
+          if (ret < 0) {
             fprintf(stderr, "Error synchronizing with SSS\n");
             return LIBLTE_ERROR;
-          }
-        } 
+          } 
+        } else {
+          ret = 1;
+        }
       } else {
         INFO("Warning: no space for CFO computation\n",0);
       }
       
       if (peak_position) {
-        *peak_position = peak_pos;
+        *peak_position = (uint32_t) peak_pos;
       }
-      ret = 1;
     } else {
       ret = LIBLTE_SUCCESS;
     }
-
-    INFO("SYNC ret=%d N_id_2=%d pos=%d peak=%.2f energy=%.3f threshold=%.2f sf_idx=%d\n",
-          ret, q->N_id_2, peak_pos, q->peak_value, energy, q->threshold, q->sf_idx);
+    
+    INFO("SYNC ret=%d N_id_2=%d pos=%d peak=%.2f/%.2f=%.2f threshold=%.2f sf_idx=%d offset=%d\n",
+          ret, q->N_id_2, peak_pos, peak_unnormalized,energy,q->peak_value, q->threshold, q->sf_idx, find_offset);
 
   } else if (lte_N_id_2_isvalid(q->N_id_2)) {
     fprintf(stderr, "Must call sync_set_N_id_2() first!\n");
diff --git a/lte/phy/lib/sync/test/pss_usrp.c b/lte/phy/lib/sync/test/pss_usrp.c
index 176780edb..b91833d27 100644
--- a/lte/phy/lib/sync/test/pss_usrp.c
+++ b/lte/phy/lib/sync/test/pss_usrp.c
@@ -161,7 +161,7 @@ int main(int argc, char **argv) {
                                                  fft_size);
     float x = peak_value/y;
     
-    mean_peak = EXPAVERAGE(x, mean_peak, frame_cnt);
+    mean_peak = VEC_CMA(x, mean_peak, frame_cnt);
     
     if (x >= threshold) {
       nof_det++;
diff --git a/lte/phy/lib/ue/src/ue_celldetect.c b/lte/phy/lib/ue/src/ue_celldetect.c
index fa4c75e82..ca20b38f1 100644
--- a/lte/phy/lib/ue/src/ue_celldetect.c
+++ b/lte/phy/lib/ue/src/ue_celldetect.c
@@ -109,7 +109,6 @@ void ue_celldetect_reset(ue_celldetect_t * q)
 {
   q->current_nof_detected = 0; 
   q->current_nof_total = 0; 
-  q->current_N_id_2 = 0; 
 }
 
 void ue_celldetect_set_threshold(ue_celldetect_t * q, float threshold)
@@ -189,7 +188,8 @@ void decide_cell(ue_celldetect_t * q, ue_celldetect_result_t *found_cell)
 int ue_celldetect_scan(ue_celldetect_t * q, 
                        cf_t *signal, 
                        uint32_t nsamples,
-                       ue_celldetect_result_t *found_cell)
+                       ue_celldetect_result_t *found_cell, 
+                       uint32_t N_id_2)
 {
   int ret = LIBLTE_ERROR_INVALID_INPUTS;
   uint32_t peak_idx;
@@ -198,7 +198,8 @@ int ue_celldetect_scan(ue_celldetect_t * q,
 
   if (q                 != NULL &&
       signal            != NULL && 
-      nsamples          >= 4800) 
+      nsamples          >= 4800 && 
+      lte_N_id_2_isvalid(N_id_2)) 
   {
     ret = LIBLTE_SUCCESS; 
     
@@ -209,16 +210,18 @@ int ue_celldetect_scan(ue_celldetect_t * q,
     nof_input_frames = nsamples/4800; 
     
     for (uint32_t nf=0;nf<nof_input_frames;nf++) {
-      sync_set_N_id_2(&q->sfind, q->current_N_id_2);
+      if (sync_set_N_id_2(&q->sfind, N_id_2)) {
+        return LIBLTE_ERROR;
+      }
 
       INFO("[%3d/%3d]: Searching cells with N_id_2=%d. %d frames\n", 
-           q->current_nof_detected, q->current_nof_total, q->current_N_id_2, nof_input_frames);
+           q->current_nof_detected, q->current_nof_total, N_id_2, nof_input_frames);
 
       /* Find peak and cell id */
       ret = sync_find(&q->sfind, &signal[nf*4800], 0, &peak_idx);
       if (ret < 0) {
         fprintf(stderr, "Error finding correlation peak (%d)\n", ret);
-        return -1;
+        return LIBLTE_ERROR;
       }
 
       /* If peak position does not allow to read SSS, return error -3 */
@@ -249,17 +252,13 @@ int ue_celldetect_scan(ue_celldetect_t * q,
       /* Decide cell ID and CP if we detected up to nof_frames_detected */
       if (q->current_nof_detected == q->nof_frames_detected) {
         decide_cell(q, found_cell);
-        q->current_N_id_2++;
         q->current_nof_detected = q->current_nof_total = 0; 
         ret = CS_CELL_DETECTED;
-      /* Or go to the next N_id_2 if we didn't detect the cell */
       } else if (q->current_nof_total == q->nof_frames_total) {
-        q->current_N_id_2++; 
         q->current_nof_detected = q->current_nof_total = 0; 
         ret = CS_CELL_NOT_DETECTED; 
-      }    
-      if (q->current_N_id_2 == 3) {
-        q->current_N_id_2 = 0; 
+      } else {
+        ret = 0;
       }
     } 
   }
diff --git a/lte/phy/lib/ue/src/ue_dl.c b/lte/phy/lib/ue/src/ue_dl.c
index 0b98459ee..c6957cf55 100644
--- a/lte/phy/lib/ue/src/ue_dl.c
+++ b/lte/phy/lib/ue/src/ue_dl.c
@@ -169,7 +169,7 @@ int ue_dl_decode(ue_dl_t *q, cf_t *input, char *data, uint32_t sf_idx, uint16_t
  
   gettimeofday(&t[2], NULL);
   get_time_interval(t);
-  mean_exec_time = (float) EXPAVERAGE((float) t[0].tv_usec, mean_exec_time, frame_cnt);
+  mean_exec_time = (float) VEC_CMA((float) t[0].tv_usec, mean_exec_time, frame_cnt);
   frame_cnt++;
   
   for (int i=0;i<MAX_PORTS;i++) {
diff --git a/lte/phy/lib/ue/src/ue_mib.c b/lte/phy/lib/ue/src/ue_mib.c
index 9652e75c8..855f855c9 100644
--- a/lte/phy/lib/ue/src/ue_mib.c
+++ b/lte/phy/lib/ue/src/ue_mib.c
@@ -37,11 +37,13 @@
 #include "liblte/phy/utils/vector.h"
 
 #define FIND_FFTSIZE   128
-#define FIND_SFLEN     10*SF_LEN(FIND_FFTSIZE)
+#define FIND_SFLEN     5*SF_LEN(FIND_FFTSIZE)
+
+#define MIB_FIND_THRESHOLD          0.0
 
 int ue_mib_init(ue_mib_t * q, 
-                       uint32_t cell_id, 
-                       lte_cp_t cp) 
+                uint32_t cell_id, 
+                lte_cp_t cp) 
 {
   int ret = LIBLTE_ERROR_INVALID_INPUTS;
 
@@ -80,6 +82,8 @@ int ue_mib_init(ue_mib_t * q,
     sync_set_threshold(&q->sfind, MIB_FIND_THRESHOLD);
     sync_sss_en(&q->sfind, true);
     sync_set_N_id_2(&q->sfind, cell_id % 3);
+    sync_cp_en(&q->sfind, false);
+    sync_set_cp(&q->sfind, cp);
 
     if (lte_fft_init(&q->fft, cp, cell.nof_prb)) {
       fprintf(stderr, "Error initializing FFT\n");
@@ -137,39 +141,42 @@ void ue_mib_set_threshold(ue_mib_t * q, float threshold)
 
 static int mib_decoder_run(ue_mib_t * q, cf_t *input, pbch_mib_t *mib)
 {
-  int ret;
+  int ret = LIBLTE_SUCCESS;
 
   /* Run FFT for the slot symbols */
   lte_fft_run_slot(&q->fft, input, q->slot1_symbols);
 
   /* Get channel estimates of slot #1 for each port */
   ret = chest_ce_slot(&q->chest, q->slot1_symbols, q->ce, 1);
-  if (ret == LIBLTE_SUCCESS) {
-    
-    /* Reset decoder if we missed a frame */
-    if ((q->last_frame_trial && (q->frame_cnt - q->last_frame_trial > 2)) || 
-        q->frame_cnt > 10) 
-    {
-      ue_mib_reset(q);
-      INFO("Resetting PBCH decoder: last trial %u, now is %u\n",
-          q->last_frame_trial, q->frame_cnt);
-    }
-    
-    /* Decode PBCH */
-    ret = pbch_decode(&q->pbch, q->slot1_symbols, q->ce, mib);
-    if (ret < 0) {
-      fprintf(stderr, "Error decoding PBCH\n");      
-    } else if (ret == 1) {
-      INFO("MIB decoded: %u\n", q->frame_cnt/2);
-      ue_mib_reset(q);
-      ret = 1; 
-    } else {
-      INFO("MIB not decoded: %u\n", q->frame_cnt / 2);
-      q->last_frame_trial = q->frame_cnt;
-    }    
+  if (ret < 0) {
+    return LIBLTE_ERROR;
   }
+
+  /* Reset decoder if we missed a frame */
+  if ((q->last_frame_trial && (abs(q->frame_cnt - q->last_frame_trial) > 2)) || 
+      q->frame_cnt > 16) 
+  {
+    INFO("Resetting PBCH decoder: last trial %u, now is %u\n",
+        q->last_frame_trial, q->frame_cnt);
+    ue_mib_reset(q);
+  }
+  
+  /* Decode PBCH */
+  ret = pbch_decode(&q->pbch, q->slot1_symbols, q->ce, mib);
+  if (ret < 0) {
+    fprintf(stderr, "Error decoding PBCH\n");      
+  } else if (ret == 1) {
+    INFO("MIB decoded: %u\n", q->frame_cnt/2);
+    ue_mib_reset(q);
+    ret = MIB_FOUND; 
+  } else {
+    INFO("MIB not decoded: %u\n", q->frame_cnt / 2);
+    q->last_frame_trial = q->frame_cnt;
+    ret = LIBLTE_SUCCESS;
+  }    
   return ret;
 }
+int counter1=0,counter2=0,counter3=0,counter4=0;
 
 int ue_mib_decode(ue_mib_t * q, 
                   cf_t *signal, 
@@ -177,13 +184,18 @@ int ue_mib_decode(ue_mib_t * q,
                   pbch_mib_t *mib)
 {
   int ret = LIBLTE_ERROR_INVALID_INPUTS;
-  uint32_t peak_idx;
+  uint32_t peak_idx=0;
   uint32_t nof_input_frames; 
 
 
   if (q                 != NULL &&
       signal            != NULL) 
   {
+    if (nsamples < MIB_FRAME_SIZE) {
+      fprintf(stderr, "Error: nsamples must be greater than %d\n", MIB_FRAME_SIZE);
+      return LIBLTE_ERROR;
+    }
+    
     ret = LIBLTE_SUCCESS; 
     
     if (nsamples % MIB_FRAME_SIZE) {
@@ -200,26 +212,41 @@ int ue_mib_decode(ue_mib_t * q,
         fprintf(stderr, "Error finding correlation peak (%d)\n", ret);
         return -1;
       }
-
-      /* If peak position does not allow to read SSS, return error -3 */
+      
+      if (ret == 0) {
+        counter2++;
+      } else if (ret == 1) {
+        counter4++;
+      }
+      
+      /* Check if we have space for reading the MIB and we are in Subframe #0 */
       if (ret                                    == 1        && 
           nf*MIB_FRAME_SIZE + peak_idx + 960     <= nsamples &&
           sync_sss_detected(&q->sfind)                       && 
           sync_get_sf_idx(&q->sfind)             == 0) 
       {
-        
+        INFO("Trying to decode MIB\n",0);
         ret = mib_decoder_run(q, &signal[nf*MIB_FRAME_SIZE+peak_idx], mib);
-        
+        counter3++;
       } else if ((ret == LIBLTE_SUCCESS && peak_idx != 0)   || 
                  (ret == 1              && nf*MIB_FRAME_SIZE + peak_idx + 960 > nsamples)) 
       {
+        printf("Not enough space for PBCH\n",0);
         ret = MIB_FRAME_UNALIGNED; 
       } else {
+        INFO("SSS not detected\n",0);
         ret = 0; 
       }
       
+      counter1++;
+      INFO("Total: %3d - Sync0: %3d - Sync1: %3d - Tried: %3d - Peak: %4d - Ret: %d\n",counter1,counter2,counter4, counter3, peak_idx, ret);
+      
       q->frame_cnt++;
     } 
   }
   return ret;
 }
+
+
+
+
diff --git a/lte/phy/lib/ue/src/ue_sync.c b/lte/phy/lib/ue/src/ue_sync.c
index 714a6212f..9122794f5 100644
--- a/lte/phy/lib/ue/src/ue_sync.c
+++ b/lte/phy/lib/ue/src/ue_sync.c
@@ -46,7 +46,7 @@ cf_t dummy[MAX_TIME_OFFSET];
 #define CURRENT_SLOTLEN_RE SLOT_LEN_RE(q->cell.nof_prb, q->cell.cp)
 #define CURRENT_SFLEN_RE SF_LEN_RE(q->cell.nof_prb, q->cell.cp)
 
-#define FIND_THRESHOLD          1.0
+#define FIND_THRESHOLD          1.2
 #define TRACK_THRESHOLD         0.2
 
 
@@ -138,7 +138,7 @@ float ue_sync_get_cfo(ue_sync_t *q) {
 }
 
 float ue_sync_get_sfo(ue_sync_t *q) {
-  return 1000*q->mean_time_offset/5;
+  return 1000*q->mean_time_offset;
 }
 
 void ue_sync_decode_sss_on_track(ue_sync_t *q, bool enabled) {
@@ -179,8 +179,9 @@ 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_sf_idx(&q->strack)) && q->decode_sss_on_track) {
-    INFO("Warning: Expected SF idx %d but got %d!\n", 
-          q->sf_idx, sync_get_sf_idx(&q->strack));
+    INFO("Warning: Expected SF idx %d but got %d (%d,%g - %d,%g)!\n", 
+          q->sf_idx, sync_get_sf_idx(&q->strack), q->strack.m0, q->strack.m1, q->strack.m0_value, q->strack.m1_value);
+    /* FIXME: What should we do in this case? */
     q->sf_idx = sync_get_sf_idx(&q->strack);
     q->state = SF_TRACK; 
   } else {
@@ -196,10 +197,9 @@ int track_peak_ok(ue_sync_t *q, uint32_t track_idx) {
     } 
     
     /* compute cumulative moving average CFO */
-    q->cur_cfo = EXPAVERAGE(sync_get_cfo(&q->strack), q->cur_cfo, q->frame_ok_cnt);
-    
+    q->cur_cfo = VEC_CMA(sync_get_cfo(&q->strack), q->cur_cfo, q->frame_ok_cnt);
     /* compute cumulative moving average time offset */
-    q->mean_time_offset = (float) EXPAVERAGE((float) q->time_offset, q->mean_time_offset, q->frame_ok_cnt);
+    q->mean_time_offset = (float) VEC_CMA((float) q->time_offset, q->mean_time_offset, q->frame_ok_cnt);
 
     q->peak_idx = CURRENT_SFLEN/2 + q->time_offset;  
     q->frame_ok_cnt++;
@@ -310,7 +310,7 @@ int ue_sync_get_buffer(ue_sync_t *q, cf_t **sf_symbols) {
           #ifdef MEASURE_EXEC_TIME
           gettimeofday(&t[2], NULL);
           get_time_interval(t);
-          q->mean_exec_time = (float) EXPAVERAGE((float) t[0].tv_usec, q->mean_exec_time, q->frame_total_cnt);
+          q->mean_exec_time = (float) VEC_CMA((float) t[0].tv_usec, q->mean_exec_time, q->frame_total_cnt);
           #endif
 
           if (ret == 1) {
diff --git a/lte/phy/lib/ue/test/ue_celldetect_mib_test.c b/lte/phy/lib/ue/test/ue_celldetect_mib_test.c
index c3921aba4..027f9b33e 100644
--- a/lte/phy/lib/ue/test/ue_celldetect_mib_test.c
+++ b/lte/phy/lib/ue/test/ue_celldetect_mib_test.c
@@ -169,7 +169,7 @@ int find_cell(void *uhd, ue_celldetect_t *s, cf_t *buffer, ue_celldetect_result_
       return LIBLTE_ERROR;
     }
     
-    n = ue_celldetect_scan(s, buffer, flen, found_cell);
+    n = ue_celldetect_scan(s, buffer, flen, found_cell, nof_scanned_cells);
     switch(n) {
       case CS_FRAME_UNALIGNED:
         printf("Realigning frame\n");