diff --git a/README.md b/README.md
index 245ccaac8..539310895 100644
--- a/README.md
+++ b/README.md
@@ -3,6 +3,62 @@ libLTE
 
 libLTE is a free and open-source LTE library for SDR UE and eNodeB. The library does not rely on any external dependencies or frameworks. 
 
+The license is LGPLv3.
 
 The project contains a set of Python tools for the automatic code generation of modules for popular SDR frameworks, including GNURadio, ALOE++, IRIS, and OSSIE. These tools are easy to use and adapt for generating targets for specific platforms or frameworks. 
 
+Support
+========
+
+Mailing list: https://lists.sourceforge.net/lists/listinfo/liblte-users
+
+
+Download & Install Instructions
+=================================
+
+```
+git clone https://github.com/ismagom/libLTE.git
+cd libLTE
+mkdir build
+cd build
+cmake ../
+make 
+sudo make install
+```
+
+Cell Search Example
+====================
+
+This program uses any hardware supported by the UHD driver to scan an LTE band for active cells. See http://niviuk.free.fr/lte_band.php for a list of available bands. The program first obtains a power spectral density of the entire band. For all frequencies with an RSSI higher than a threshold, it tries to find the LTE Primary Synchronization Signal (PSS) and then identifies the CELL ID using the Secondary Synchronization Signal (SSS). Finally, it estimates the Carrier Frequency Offset (CFO) and Sampling Frequency Offset (SFO). 
+
+For instance, the command:
+
+``` pss_scan_usrp -b 3 ```
+
+
+Scans the LTE band 3 (1805 to 1880 MHz). Note that you need a hardware supporting these frequencies (e.g. SBX daughterboard for USRP). The program outputs the following:
+
+```
+Opening UHD device...
+-- Opening a USRP2/N-Series device...
+-- Current recv frame size: 1472 bytes
+-- Current send frame size: 1472 bytes
+RSSI scan: 749 freqs in band 3, RSSI threshold -30.00 dBm
+Freq 1879.0 Mhz - RSSI: -43.96 dBm
+Done. Starting PSS search on 75 channels
+
+UHD Warning:
+    The hardware does not support the requested RX sample rate:
+    Target sample rate: 1.920000 MSps
+    Actual sample rate: 1.923077 MSps
+[199/749]: EARFCN 1399 Freq. 1824.90 MHz No PSS found
+[200/749]: FOUND EARFCN 1400 Freq. 1825.00 MHz, RSSI -22.43 dBm, PAR 15.86 dB, CFO=-0.25 KHz, SFO=+3.099 KHz, CELL_ID=150
+[433/749]: EARFCN 1633 Freq. 1848.30 MHz No PSS found
+
+Done
+```
+
+indicating that a Cell with ID 150 has been found at 1825.0 MHz. PAR indicates the peak-to-average ratio (in dB) at the output of the PSS correlator. 
+
+For more command arguments, type ``` pss_scan_usrp --help ```
+
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
index 70b0d7c29..d09f0101c 100644
--- a/examples/CMakeLists.txt
+++ b/examples/CMakeLists.txt
@@ -28,9 +28,6 @@ target_link_libraries(equalizer_test ${LIBRARIES})
 add_executable(viterbi_test viterbi_test.c)
 target_link_libraries(viterbi_test ${LIBRARIES})
 
-add_executable(bch_test bch_test.c)
-target_link_libraries(bch_test ${LIBRARIES})
-
 add_executable(cell_search cell_search.c)
 target_link_libraries(cell_search ${LIBRARIES})
 
diff --git a/examples/bch_test.c b/examples/bch_test.c
deleted file mode 100644
index 84bb6fdcb..000000000
--- a/examples/bch_test.c
+++ /dev/null
@@ -1,130 +0,0 @@
-#include <stdio.h>
-#include <stdlib.h>
-#include <strings.h>
-#include <unistd.h>
-
-#include "lte.h"
-
-char *input_file_name;
-int frame_length=1920, symbol_sz=128, nof_slots=1;
-
-void usage(char *prog) {
-	printf("Usage: %s [ls] -i input_file\n", prog);
-	printf("\t-l frame_length [Default %d]\n", frame_length);
-	printf("\t-s symbol_sz [Default %d]\n", symbol_sz);
-	printf("\t-n nof_frames [Default %d]\n", nof_slots);
-	printf("\t-v [set verbose to debug, default none]\n");
-}
-
-void parse_args(int argc, char **argv) {
-	int opt;
-	while ((opt = getopt(argc, argv, "ilsnv")) != -1) {
-		switch(opt) {
-		case 'i':
-			input_file_name = argv[optind];
-			break;
-		case 'l':
-			frame_length = atoi(argv[optind]);
-			break;
-		case 's':
-			symbol_sz = atoi(argv[optind]);
-			break;
-		case 'n':
-			nof_slots = atoi(argv[optind]);
-			break;
-		case 'v':
-			verbose++;
-			break;
-		default:
-			usage(argv[0]);
-			exit(-1);
-		}
-	}
-	if (!input_file_name) {
-		usage(argv[0]);
-		exit(-1);
-	}
-}
-
-void fft_run_slot(dft_plan_t *fft_plan, cf_t *input, cf_t *output) {
-	int i;
-	for (i=0;i<7;i++) {
-		input += CP_NORM(i, symbol_sz);
-		dft_run_c2c(fft_plan, input, output);
-		input += symbol_sz;
-		output += symbol_sz;
-	}
-}
-
-int main(int argc, char **argv) {
-	filesource_t fsrc;
-	pbch_mib_t pbch_data;
-	pbch_t pbch;
-	dft_plan_t fft_plan;
-
-	int frame_cnt;
-	cf_t *input, *outfft;
-
-	if (argc < 3) {
-		usage(argv[0]);
-		exit(-1);
-	}
-
-	parse_args(argc,argv);
-
-	if (filesource_init(&fsrc, input_file_name, COMPLEX_FLOAT)) {
-		fprintf(stderr, "Error opening file %s\n", input_file_name);
-		exit(-1);
-	}
-
-	input = malloc(frame_length*sizeof(cf_t));
-	if (!input) {
-		perror("malloc");
-		exit(-1);
-	}
-	outfft = malloc(7*symbol_sz*sizeof(cf_t));
-	if (!outfft) {
-		perror("malloc");
-		exit(-1);
-	}
-
-	/* Init FFT plan */
-	if (dft_plan_c2c(symbol_sz, FORWARD, &fft_plan)) {
-		fprintf(stderr, "Error initiating FFT plan\n");
-		exit(-1);
-	}
-	fft_plan.options = DFT_DC_OFFSET | DFT_MIRROR_POS | DFT_NORMALIZE;
-
-	if (pbch_init(&pbch, 0, CPNORM)) {
-		fprintf(stderr, "Error initiating PBCH\n");
-		exit(-1);
-	}
-	int res = 0;
-	frame_cnt = 0;
-	while (frame_length == filesource_read(&fsrc, input, frame_length)
-			&& frame_cnt < nof_slots
-			&& res == 0) {
-
-		fft_run_slot(&fft_plan, &input[960], outfft);
-
-		res = pbch_decode(&pbch, outfft, &pbch_data, 6, 1);
-		if (res == -1) {
-			fprintf(stderr, "Error decoding PBCH\n");
-			break;
-		}
-		frame_cnt++;
-	}
-
-	if (res == 1) {
-		printf("MIB found\n");
-	} else {
-		printf("MIB not found after %d frames\n", frame_cnt);
-	}
-
-	pbch_free(&pbch);
-	free(input);
-	free(outfft);
-
-	printf("Done\n");
-	exit(0);
-}
diff --git a/examples/cell_search.c b/examples/cell_search.c
index 0df21c6e9..cff0a0369 100644
--- a/examples/cell_search.c
+++ b/examples/cell_search.c
@@ -8,16 +8,18 @@
 
 char *input_file_name = NULL;
 int nof_slots=100;
-float corr_peak_threshold=2.5;
-int ntime = 4;
-int nfreq = 10;
+float corr_peak_threshold=15;
 int file_binary = 0;
 int force_N_id_2=-1;
+int nof_ports = 1;
+
+
+#define FLEN	9600
 
 filesource_t fsrc;
-cf_t *input_buffer, *fft_buffer;
+cf_t *input_buffer, *fft_buffer, *ce[MAX_PORTS];
 pbch_t pbch;
-dft_plan_t fft_plan;
+lte_fft_t fft;
 chest_t chest;
 sync_t synch;
 
@@ -64,29 +66,43 @@ void parse_args(int argc, char **argv) {
 }
 
 int base_init() {
+	int i;
+
 	file_data_type_t type = file_binary?COMPLEX_FLOAT_BIN:COMPLEX_FLOAT;
 	if (filesource_init(&fsrc, input_file_name, type)) {
 		fprintf(stderr, "Error opening file %s\n", input_file_name);
 		exit(-1);
 	}
 
-	input_buffer = malloc(4 * 960 * sizeof(cf_t));
+	input_buffer = malloc(4 * FLEN * sizeof(cf_t));
 	if (!input_buffer) {
 		perror("malloc");
 		exit(-1);
 	}
-	fft_buffer = malloc(CPNORM_NSYMB * 128 * sizeof(cf_t));
+
+	fft_buffer = malloc(CPNORM_NSYMB * 72 * sizeof(cf_t));
 	if (!fft_buffer) {
 		perror("malloc");
 		return -1;
 	}
 
-	/* Init FFT plan */
-	if (dft_plan_c2c(128, FORWARD, &fft_plan)) {
-		fprintf(stderr, "Error initiating FFT plan\n");
+	for (i=0;i<nof_ports;i++) {
+		ce[i] = malloc(CPNORM_NSYMB * 72 * sizeof(cf_t));
+		if (!ce[i]) {
+			perror("malloc");
+			return -1;
+		}
+	}
+
+	if (chest_init(&chest, CPNORM, 6, 1)) {
+		fprintf(stderr, "Error initializing equalizer\n");
+		return -1;
+	}
+
+	if (lte_fft_init(&fft, CPNORM, 6)) {
+		fprintf(stderr, "Error initializing FFT\n");
 		return -1;
 	}
-	fft_plan.options = DFT_DC_OFFSET | DFT_MIRROR_POS | DFT_NORMALIZE;
 
 	DEBUG("Memory init OK\n",0);
 	return 0;
@@ -96,14 +112,10 @@ int base_init() {
 
 int mib_decoder_init(int cell_id) {
 
-	/*
-	if (chest_LTEDL_init(&chest, ntime, nfreq, CPNORM_NSYMB, cell_id, 6)) {
-		fprintf(stderr, "Error initiating LTE equalizer\n");
+	if (chest_ref_LTEDL(&chest, cell_id)) {
+		fprintf(stderr, "Error initializing reference signal\n");
 		return -1;
 	}
-	*/
-
-	DEBUG("Channel estimation initiated ntime=%d nfreq=%d\n", ntime, nfreq);
 
 	if (pbch_init(&pbch, cell_id, CPNORM)) {
 		fprintf(stderr, "Error initiating PBCH\n");
@@ -113,21 +125,17 @@ int mib_decoder_init(int cell_id) {
 	return 0;
 }
 
-void fft_run_slot(dft_plan_t *fft_plan, cf_t *input, cf_t *output) {
-	int i;
-	for (i=0;i<CPNORM_NSYMB;i++) {
-		DEBUG("Running FFT %d\n", i);
-		input += CP_NORM(i, 128);
-		dft_run_c2c(fft_plan, input, output);
-		input += 128;
-		output += 128;
-	}
-}
-
 int mib_decoder_run(cf_t *input, pbch_mib_t *mib) {
-	fft_run_slot(&fft_plan, input, fft_buffer);
+	int i;
+	lte_fft_run(&fft, input, fft_buffer);
+
+	/* Get channel estimates for each port */
+	for (i=0;i<nof_ports;i++) {
+		chest_ce_slot_port(&chest, fft_buffer, ce[i], 1, 0);
+	}
+
 	DEBUG("Decoding PBCH\n", 0);
-	return pbch_decode(&pbch, fft_buffer, mib, 6, 1);
+	return pbch_decode(&pbch, fft_buffer, ce, nof_ports, 6, 1, mib);
 }
 
 int get_samples(int length, int offset) {
@@ -155,7 +163,7 @@ enum radio_state { DONE, SYNC, MIB};
 int main(int argc, char **argv) {
 	enum radio_state state;
 	int sf_size, slot_start;
-	int read_length, slot_idx;
+	int read_length, frame_idx;
 	int mib_attempts;
 	pbch_mib_t mib;
 	int cell_id;
@@ -176,19 +184,20 @@ int main(int argc, char **argv) {
 		exit(-1);
 	}
 
-	if (sync_init(&synch)) {
+	if (sync_init(&synch, FLEN)) {
 		fprintf(stderr, "Error initiating PSS/SSS\n");
 		exit(-1);
 	}
 
 	sync_force_N_id_2(&synch, force_N_id_2);
 	sync_set_threshold(&synch, corr_peak_threshold);
+	sync_pss_det_peakmean(&synch);
 
 	state = SYNC;
-	sf_size = 960;
+	sf_size = FLEN;
 	read_length = sf_size;
 	slot_start = 0;
-	slot_idx = 0;
+	frame_idx = 0;
 	mib_attempts = 0;
 	frame_cnt = -1;
 	read_offset = 0;
@@ -203,39 +212,40 @@ int main(int argc, char **argv) {
 		}
 		if (read_length) {
 			frame_cnt++;
-			INFO("\n\tSlot idx=%d\n\n", slot_idx);
+			INFO("\nFrame idx=%d\n\n", frame_idx);
 			INFO("Correcting CFO=%.4f\n", cfo);
 			nco_cexp_f_direct(&input_buffer[read_offset], -cfo/128, read_length);
 		}
 		switch(state) {
 		case SYNC:
-			INFO("State Sync, Slot idx=%d\n", slot_idx);
+			INFO("State Sync, Slot idx=%d\n", frame_idx);
 			idx = sync_run(&synch, input_buffer, read_offset);
 			if (idx != -1) {
+				idx -= 960;
 				slot_start = read_offset + idx;
 				read_length = idx;
-				read_offset += 960;
+				read_offset += FLEN;
 				cell_id = sync_get_cell_id(&synch);
 				cfo = sync_get_cfo(&synch);
-				slot_idx = sync_get_slot_id(&synch);
+				frame_idx = sync_get_slot_id(&synch)?1:0;
 				state = MIB;
 				if (mib_decoder_init(cell_id)) {
 					fprintf(stderr, "Error initiating MIB decoder\n");
 					exit(-1);
 				}
-				INFO("SYNC done, cell_id=%d slot_start=%d\n", cell_id, slot_start);
+				INFO("SYNC done, cell_id=%d slot_start=%d frame_idx=%d\n", cell_id, slot_start, frame_idx);
 			} else {
-				read_offset = 960;
-				memcpy(input_buffer, &input_buffer[960], 960 * sizeof(cf_t));
+				read_offset = FLEN;
+				memcpy(input_buffer, &input_buffer[FLEN], FLEN * sizeof(cf_t));
 			}
 			break;
 		case MIB:
-			read_length = 960;
+			read_length = FLEN;
 			read_offset = slot_start;
-			INFO("State MIB, Slot idx=%d\n", slot_idx);
-			if (slot_idx == 1) {
+			INFO("State MIB, frame idx=%d\n", frame_idx);
+			if (frame_idx == 0) {
 				INFO("Trying to find MIB offset %d\n", slot_start);
-				if (mib_decoder_run(&input_buffer[slot_start], &mib)) {
+				if (mib_decoder_run(&input_buffer[slot_start+FLEN/10], &mib)) {
 					INFO("MIB detected attempt=%d\n", mib_attempts);
 					state = DONE;
 				} else {
@@ -248,16 +258,16 @@ int main(int argc, char **argv) {
 			}
 			break;
 		case DONE:
-			INFO("State Done, Slot idx=%d\n", slot_idx);
+			INFO("State Done, Slot idx=%d\n", frame_idx);
 			pbch_mib_fprint(stdout, &mib);
 			printf("Done\n");
 			break;
 		}
 
 		if (read_length) {
-			slot_idx++;
-			if (slot_idx == 20) {
-				slot_idx = 0;
+			frame_idx++;
+			if (frame_idx == 2) {
+				frame_idx = 0;
 			}
 		}
 	}
diff --git a/examples/equalizer_test.c b/examples/equalizer_test.c
index 2e626ef05..ff685fc03 100644
--- a/examples/equalizer_test.c
+++ b/examples/equalizer_test.c
@@ -85,6 +85,7 @@ int main(int argc, char **argv) {
 	cf_t *input = NULL;
 	cf_t *outfft = NULL;
 	cf_t *ce = NULL;
+	int i;
 
 	if (argc < 3) {
 		usage(argv[0]);
@@ -119,7 +120,7 @@ int main(int argc, char **argv) {
 		goto do_exit;
 	}
 
-	if (lte_fft_init(&fft, cp, lte_symbol_sz(nof_prb))) {
+	if (lte_fft_init(&fft, cp, nof_prb)) {
 		fprintf(stderr, "Error: initializing FFT\n");
 		goto do_exit;
 	}
@@ -135,20 +136,30 @@ int main(int argc, char **argv) {
 	bzero(input, sizeof(cf_t) * in_slot_length());
 	bzero(outfft, sizeof(cf_t) * slot_length());
 
+	fprintf(f, "ce=zeros(%d, %d);\n", nof_slots * CP_NSYMB(cp), nof_prb * RE_X_RB);
 	/* read all file or nof_slots */
 	slot_cnt = 0;
 	while (in_slot_length() == filesource_read(&fsrc, input, in_slot_length())
 			&& (slot_cnt < nof_slots || nof_slots == -1)) {
 
+		fprintf(f, "infft=");
+		vec_fprint_c(f, input, CP_NSYMB(cp) * 128);
+		fprintf(f, ";\n");
+
 		lte_fft_run(&fft, input, outfft);
 
-		chest_ce_slot_port(&eq, outfft, ce, slot_cnt%20, 0);
+		fprintf(f, "outfft=");
+		vec_fprint_c(f, outfft, CP_NSYMB(cp) * nof_prb * RE_X_RB);
+		fprintf(f, ";\n");
+
+		chest_ce_slot_port(&eq, outfft, ce, 0, 0);
 
 		chest_fprint(&eq, f, slot_cnt%20, 0);
 
-		fprintf(f, "ce=[");
-		vec_fprint_c(f, ce, nof_prb * RE_X_RB * CP_NSYMB(cp));
-		fprintf(f, "];\n");
+		for (i=0;i<CP_NSYMB(cp);i++) {
+			fprintf(f, "ce(%d,:)=", slot_cnt * CP_NSYMB(cp) + i + 1);
+			vec_fprint_c(f, &ce[i * nof_prb * RE_X_RB], nof_prb * RE_X_RB);
+		}
 
 		slot_cnt++;
 	}
@@ -170,6 +181,6 @@ do_exit:
 	}
 	filesource_close(&fsrc);
 
-	printf("Done\n");
+	printf("Done processed %d slots\n", slot_cnt);
 	exit(0);
 }
diff --git a/examples/pss_scan_usrp.c b/examples/pss_scan_usrp.c
index afd700831..c7f1fef8e 100644
--- a/examples/pss_scan_usrp.c
+++ b/examples/pss_scan_usrp.c
@@ -12,67 +12,64 @@
 #include "uhd.h"
 #include "uhd_utils.h"
 
-int nof_slots=1000;
+#define MHZ 			1000000
+#define SAMP_FREQ 		1920000
+#define RSSI_FS			1000000
+#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 pss_threshold=15.0;
-int earfcn_start, earfcn_end = -1;
-float rssi_threshold = -42.0;
+float find_threshold = 40.0, track_threshold = 8.0;
+int earfcn_start=-1, earfcn_end = -1;
+float rssi_threshold = -30.0;
+int max_track_lost=9;
+int nof_frames_find=8, nof_frames_track=100, nof_samples_rssi=50000;
+int track_len=500;
 
 cf_t *input_buffer;
 float *cfo_v;
-int *idx_v;
+int *idx_v, *idx_valid, *t;
 float *p2a_v;
 void *uhd;
 int nof_bands;
-int force_N_id_2;
-float gain = 30.0;
+float gain = 20.0;
 
 #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];
 
-#define MHZ 			1000000
-#define SAMP_FREQ 		1920000
-#define RSSI_FS			1000000
-#define RSSI_NSAMP		50000
-#define FLEN 			9600
-#define FLEN_PERIOD		0.005
-
-#define IS_SIGNAL(i) (10*log10f(rssi[i]) + 30 > rssi_threshold)
+enum sync_state {INIT, FIND, TRACK, DONE};
 
 void print_to_matlab();
 
 void usage(char *prog) {
-	printf("Usage: %s [senvtr] -b band\n", prog);
-	printf("\t-s earfcn_start [Default %d]\n", earfcn_start);
+	printf("Usage: %s [seRrFfTtgv] -b band\n", prog);
+	printf("\t-s earfcn_start [Default All]\n");
 	printf("\t-e earfcn_end [Default All]\n");
-	printf("\t-n number of frames [Default %d]\n", nof_slots);
-	printf("\t-v [set verbose to debug, default none]\n");
-	printf("\t-t pss_threshold [Default %.2f]\n", pss_threshold);
+	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 force_N_id_2 [Default no]\n");
-	printf("\t-g gain [Default no %.2f dB]\n", gain);
+	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-t pss_track_threshold [Default %.2f]\n", track_threshold);
+	printf("\t-l pss_track_len [Default %d]\n", track_len);
+	printf("\t-g gain [Default %.2f dB]\n", gain);
+	printf("\t-v [set verbose to debug, default none]\n");
 }
 
 void parse_args(int argc, char **argv) {
 	int opt;
-	while ((opt = getopt(argc, argv, "gfrtbsenv")) != -1) {
+	while ((opt = getopt(argc, argv, "bseRrFfTtgv")) != -1) {
 		switch(opt) {
-		case 'g':
-			gain = atof(argv[optind]);
-			break;
-		case 'f':
-			force_N_id_2 = atoi(argv[optind]);
-			break;
-		case 't':
-			pss_threshold = atof(argv[optind]);
-			break;
-		case 'r':
-			rssi_threshold = -atof(argv[optind]);
-			break;
 		case 'b':
 			band = atoi(argv[optind]);
 			break;
@@ -82,8 +79,26 @@ void parse_args(int argc, char **argv) {
 		case 'e':
 			earfcn_end = atoi(argv[optind]);
 			break;
-		case 'n':
-			nof_slots = atoi(argv[optind]);
+		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]);
+			break;
+		case 't':
+			track_threshold = atof(argv[optind]);
+			break;
+		case 'g':
+			gain = atof(argv[optind]);
 			break;
 		case 'v':
 			verbose++;
@@ -103,17 +118,27 @@ int base_init(int frame_length) {
 		exit(-1);
 	}
 
-	idx_v = malloc(nof_slots * sizeof(int));
+	idx_v = malloc(nof_frames_track * sizeof(int));
 	if (!idx_v) {
 		perror("malloc");
 		exit(-1);
 	}
-	cfo_v = malloc(nof_slots * sizeof(float));
+	idx_valid = malloc(nof_frames_track * sizeof(int));
+	if (!idx_valid) {
+		perror("malloc");
+		exit(-1);
+	}
+	t = malloc(nof_frames_track * sizeof(int));
+	if (!t) {
+		perror("malloc");
+		exit(-1);
+	}
+	cfo_v = malloc(nof_frames_track * sizeof(float));
 	if (!cfo_v) {
 		perror("malloc");
 		exit(-1);
 	}
-	p2a_v = malloc(nof_slots * sizeof(float));
+	p2a_v = malloc(nof_frames_track * sizeof(float));
 	if (!p2a_v) {
 		perror("malloc");
 		exit(-1);
@@ -137,6 +162,8 @@ void base_free() {
 	uhd_close(&uhd);
 	free(input_buffer);
 	free(idx_v);
+	free(idx_valid);
+	free(t);
 	free(cfo_v);
 	free(p2a_v);
 }
@@ -158,14 +185,61 @@ float mean_valid(int *idx_v, float *x, int nof_frames) {
 	}
 }
 
-int main(int argc, char **argv) {
-	int frame_cnt;
+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 rssi_scan() {
+	int n=0;
 	int i;
-	int nsamples;
+
+	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++;
+		}
+		if (uhd_rssi_scan(uhd, freqs, rssi_d, n, (double) RSSI_FS, nof_samples_rssi)) {
+			fprintf(stderr, "Error while doing RSSI scan\n");
+			return -1;
+		}
+		/* 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;
+		}
+		if (uhd_rssi_scan(uhd, freqs, rssi, nof_bands, (double) RSSI_FS, nof_samples_rssi)) {
+			fprintf(stderr, "Error while doing RSSI scan\n");
+			return -1;
+		}
+		n = nof_bands;
+	}
+
+	return n;
+}
+
+
+int main(int argc, char **argv) {
+	int frame_cnt, valid_frames;
+	int freq;
 	int cell_id;
-	sync_t synch;
+	sync_t sfind, strack;
 	float max_peak_to_avg;
 	float sfo;
+	int find_idx, track_idx, last_found;
+	enum sync_state state;
+	int n;
+	filesink_t fs;
 
 	if (argc < 3) {
 		usage(argv[0]);
@@ -179,113 +253,162 @@ int main(int argc, char **argv) {
 		exit(-1);
 	}
 
-	if (sync_init(&synch)) {
+	if (sync_init(&sfind, FLEN)) {
 		fprintf(stderr, "Error initiating PSS/SSS\n");
 		exit(-1);
 	}
-	sync_set_threshold(&synch, pss_threshold);
-	sync_pss_det_peakmean(&synch);
+	sync_pss_det_peakmean(&sfind);
 
-	if (force_N_id_2 != -1) {
-		sync_force_N_id_2(&synch, force_N_id_2);
+	if (sync_init(&strack, track_len)) {
+		fprintf(stderr, "Error initiating PSS/SSS\n");
+		exit(-1);
 	}
+	sync_pss_det_peakmean(&strack);
 
 	nof_bands = lte_band_get_fd_band(band, channels, earfcn_start, earfcn_end, MAX_EARFCN);
-	printf("RSSI scan: %d freqs in band %d\n", nof_bands, band);
-	for (i=0;i<nof_bands;i++) {
-		freqs[i] = channels[i].fd * MHZ;
-	}
+	printf("RSSI scan: %d freqs in band %d, RSSI threshold %.2f dBm\n", nof_bands, band, rssi_threshold);
 
-
-	if (uhd_rssi_scan(uhd, freqs, rssi, nof_bands, (double) RSSI_FS, RSSI_NSAMP)) {
-		fprintf(stderr, "Error while doing RSSI scan\n");
+	n = rssi_scan();
+	if (n == -1) {
 		exit(-1);
 	}
 
-	printf("\nDone. Starting PSS search\n");
+	printf("\nDone. Starting PSS search on %d channels\n", n);
 	usleep(500000);
-	printf("Setting sampling frequency %.2f MHz\n", (float) SAMP_FREQ/MHZ);
+	INFO("Setting sampling frequency %.2f MHz\n", (float) SAMP_FREQ/MHZ);
 	uhd_set_rx_srate(uhd, SAMP_FREQ);
 
 	uhd_set_rx_gain(uhd, gain);
 
 	print_to_matlab();
 
-	int first = 1;
-	for (i=0;i<nof_bands;i++) {
+	filesink_init(&fs, "test.dat", COMPLEX_FLOAT_BIN);
+
+	freq=0;
+	state = INIT;
+	while(freq<nof_bands) {
 		/* scan only bands above rssi_threshold */
-		if (IS_SIGNAL(i)) {
-			uhd_set_rx_freq(uhd, (double) channels[i].fd * MHZ);
-			uhd_rx_wait_lo_locked(uhd);
-
-			if (first) {
-				INFO("Starting receiver...\n",0);
-				uhd_start_rx_stream(uhd);
-				first = 0;
+		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 == TRACK || state == FIND) {
+				uhd_recv(uhd, &input_buffer[FLEN], FLEN, 1);
 			}
+			switch(state) {
+			case INIT:
+				DEBUG("Stopping receiver...\n",0);
+				uhd_stop_rx_stream(uhd);
 
-			frame_cnt = 0;
-			nsamples = 0;
-			max_peak_to_avg = -99;
-			nsamples += uhd_recv(uhd, input_buffer, FLEN, 1);
-			cell_id = -1;
-			while(frame_cnt < nof_slots) {
-				if (frame_cnt) {
-					nsamples += uhd_recv(uhd, &input_buffer[FLEN], FLEN, 1);
-				}
+				/* set freq */
+				uhd_set_rx_freq(uhd, (double) channels[freq].fd * MHZ);
+				uhd_rx_wait_lo_locked(uhd);
+				DEBUG("Set freq to %.3f MHz\n", (double) channels[freq].fd);
 
-				idx_v[frame_cnt] = sync_run(&synch, input_buffer, frame_cnt?FLEN:0);
-				p2a_v[frame_cnt] = sync_get_peak_to_avg(&synch);
-				if (idx_v[frame_cnt] != -1) {
-					/* 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(&synch);
-					}
-					cfo_v[frame_cnt] = sync_get_cfo(&synch);
+				DEBUG("Starting receiver...\n",0);
+				uhd_start_rx_stream(uhd);
+
+				/* init variables */
+				frame_cnt = 0;
+				max_peak_to_avg = -99;
+				cell_id = -1;
+
+				/* receive first frame */
+				uhd_recv(uhd, input_buffer, FLEN, 1);
+
+				/* set find_threshold and go to FIND state */
+				sync_set_threshold(&sfind, find_threshold);
+				sync_force_N_id_2(&sfind, -1);
+				state = FIND;
+				break;
+			case FIND:
+				/* find peak in all frame */
+				find_idx = sync_run(&sfind, input_buffer, FLEN);
+				DEBUG("[%3d/%d]: PAR=%.2f\n", freq, nof_bands, sync_get_peak_to_avg(&sfind));
+				if (find_idx != -1) {
+					/* if found peak, go to track and set lower threshold */
+					frame_cnt = -1;
+					last_found = 0;
+					sync_set_threshold(&strack, track_threshold);
+					sync_force_N_id_2(&strack, sync_get_N_id_2(&sfind));
+					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_to_avg(&sfind)));
 				} else {
+					if (frame_cnt >= nof_frames_find) {
+						state = INIT;
+						printf("[%3d/%d]: EARFCN %d Freq. %.2f MHz No PSS found\r", freq, nof_bands,
+													channels[freq].id, channels[freq].fd, frame_cnt - last_found);
+						if (VERBOSE_ISINFO()) {
+							printf("\n");
+						}
+						freq++;
+					}
+				}
+				break;
+			case TRACK:
+				INFO("Tracking PSS find_idx %d offset %d\n", find_idx, find_idx + track_len);
+
+				filesink_write(&fs, &input_buffer[FLEN+find_idx+track_len], track_len);
+
+				track_idx = sync_run(&strack, input_buffer, FLEN + find_idx - track_len);
+				p2a_v[frame_cnt] = sync_get_peak_to_avg(&strack);
+
+				/* 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(&strack);
+				}
+				if (track_idx != -1) {
+					cfo_v[frame_cnt] = sync_get_cfo(&strack);
+					last_found = frame_cnt;
+					find_idx += track_idx - track_len;
+					idx_v[frame_cnt] = find_idx;
+				} else {
+					idx_v[frame_cnt] = -1;
 					cfo_v[frame_cnt] = 0.0;
 				}
-				if (frame_cnt) {
-					memcpy(input_buffer, &input_buffer[FLEN], FLEN * sizeof(cf_t));
+				/* 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) {
+					state = DONE;
 				}
-				if (VERBOSE_ISINFO()) {
-					printf("[%4d] - idx: %5d\tpeak-to-avg: %3.2f\tcfo=%.3f\r", frame_cnt,
-						idx_v[frame_cnt], p2a_v[frame_cnt], cfo_v[frame_cnt]);
-				}
-				frame_cnt++;
+				break;
+			case DONE:
+
+				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);
+
+				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);
+				state = INIT;
+				freq++;
+				break;
 			}
-
-			cfo[i] = mean_valid(idx_v, cfo_v, nof_slots);
-			p2a[i] = sum_r(p2a_v, nof_slots) / nof_slots;
-			if (channels[i].id == 1900
-					|| channels[i].id == 1901) {
-				vec_fprint_i(stdout, idx_v, nof_slots);
+			if (state == TRACK || (state == FIND && frame_cnt)) {
+				memcpy(input_buffer, &input_buffer[FLEN], FLEN * sizeof(cf_t));
 			}
-
-			sfo = sfo_estimate(idx_v, nof_slots, FLEN_PERIOD);
-			if (VERBOSE_ISINFO()) {
-				printf("\n");
-			}
-
-			printf("[%3d/%d]: EARFCN %d Freq. %.2f MHz, "
-					"RSSI %3.2f dBm, PSS %2.2f dB, CFO=%+2.1f KHz, SFO=%+2.1f KHz, CELL_ID=%3d\n", i, nof_bands,
-							channels[i].id, channels[i].fd, 10*log10f(rssi[i]) + 30,
-							10*log10f(p2a[i]), cfo[i] * 15, sfo / 1000, cell_id);
-			print_to_matlab();
-
-		} else {
-			INFO("[%3d/%d]: EARFCN %d Freq. %.2f MHz. RSSI below threshold (%3.2f < %3.2f dBm)\n",
-					i, nof_bands, channels[i].id, channels[i].fd, 10*log10f(rssi[i]) + 30, rssi_threshold);
+			frame_cnt++;
 		}
 	}
 
 	print_to_matlab();
 
-	sync_free(&synch);
+	sync_free(&sfind);
 	base_free();
 
-	printf("Done\n");
+	printf("\n\nDone\n");
 	exit(0);
 }
 
@@ -309,6 +432,13 @@ void print_to_matlab() {
 	}
 	fprintf(f, "];\n");
 
+
+	fprintf(f, "rssi_d=[");
+	for (i=0;i<nof_bands/RSSI_DECIM;i++) {
+		fprintf(f, "%g, ", rssi_d[i]);
+	}
+	fprintf(f, "];\n");
+
 	/*
 	fprintf(f, "cfo=[");
 	for (i=0;i<nof_bands;i++) {
diff --git a/include/ch_estimation/chest.h b/include/ch_estimation/chest.h
index 642d4a5c1..ce74f0cef 100644
--- a/include/ch_estimation/chest.h
+++ b/include/ch_estimation/chest.h
@@ -39,7 +39,6 @@ typedef struct {
 	int nof_ports;
 	int nof_symbols;
 	int nof_prb;
-	int symbol_sz;
 	lte_cp_t cp;
 	refsignal_t refsignal[MAX_PORTS][NSLOTS_X_FRAME];
 }chest_t;
diff --git a/include/lte/base.h b/include/lte/base.h
index 78c46200a..b359833f6 100644
--- a/include/lte/base.h
+++ b/include/lte/base.h
@@ -65,7 +65,7 @@ typedef enum {CPNORM, CPEXT} lte_cp_t;
 
 #define GUARD_RE(nof_prb)	((lte_symbol_sz(nof_prb)-nof_prb*RE_X_RB)/2)
 
-#define SAMPLE_IDX(symbol_sz, symbol_idx, sample_idx) (symbol_idx*symbol_sz + sample_idx)
+#define SAMPLE_IDX(nof_prb, symbol_idx, sample_idx) (symbol_idx*nof_prb*RE_X_RB + sample_idx)
 
 const int lte_symbol_sz(int nof_prb);
 int lte_re_x_prb(int ns, int symbol, int nof_ports, int nof_symbols);
diff --git a/include/lte/fft.h b/include/lte/fft.h
index 172ea9f1d..9859c9df2 100644
--- a/include/lte/fft.h
+++ b/include/lte/fft.h
@@ -33,14 +33,17 @@ typedef struct {
 	dft_plan_t fft_plan;
 	int nof_symbols;
 	int symbol_sz;
+	int nof_guards;
+	int nof_re;
 	lte_cp_t cp_type;
+	cf_t *tmp; // for removing zero padding
 }lte_fft_t;
 
-int lte_fft_init(lte_fft_t *q, lte_cp_t cp_type, int symbol_sz);
+int lte_fft_init(lte_fft_t *q, lte_cp_t cp_type, int nof_prb);
 void lte_fft_free(lte_fft_t *q);
 void lte_fft_run(lte_fft_t *q, cf_t *input, cf_t *output);
 
-int lte_ifft_init(lte_fft_t *q, lte_cp_t cp_type, int symbol_sz);
+int lte_ifft_init(lte_fft_t *q, lte_cp_t cp_type, int nof_prb);
 void lte_ifft_free(lte_fft_t *q);
 void lte_ifft_run(lte_fft_t *q, cf_t *input, cf_t *output);
 
diff --git a/include/phch/pbch.h b/include/phch/pbch.h
index 86c156345..497fa26ac 100644
--- a/include/phch/pbch.h
+++ b/include/phch/pbch.h
@@ -67,7 +67,7 @@ typedef struct {
 
 int pbch_init(pbch_t *q, int cell_id, lte_cp_t cp);
 void pbch_free(pbch_t *q);
-int pbch_decode(pbch_t *q, cf_t *slot1_symbols, pbch_mib_t *data, int nof_prb, float ebno);
+int pbch_decode(pbch_t *q, cf_t *slot1_symbols, cf_t **ce, int nof_ports, int nof_prb, float ebno, pbch_mib_t *data);
 void pbch_mib_fprint(FILE *stream, pbch_mib_t *mib);
 
 
diff --git a/include/resampling/interp.h b/include/resampling/interp.h
index 9d42e3a1c..207a39573 100644
--- a/include/resampling/interp.h
+++ b/include/resampling/interp.h
@@ -21,3 +21,4 @@ typedef _Complex float cf_t;
 
 void interp_linear_offset(cf_t *input, cf_t *output, int M, int len, int off_st, int off_end);
 void interp_linear(cf_t *input, cf_t *output, int M, int len);
+void interp_linear_f(float *input, float *output, int M, int len);
diff --git a/include/sync/pss.h b/include/sync/pss.h
index 2a5c3409b..67b3a8554 100644
--- a/include/sync/pss.h
+++ b/include/sync/pss.h
@@ -29,7 +29,7 @@ typedef _Complex float cf_t; /* this is only a shortcut */
 #define DEFAULT_CORRELATION_TH 10000
 #define DEFAULT_NOSYNC_TIMEOUT	5
 
-#define PSS_LEN_FREQ 	129
+#define PSS_LEN_FREQ 	129		// FFT-based convolution removes 1 leaving it in 128
 #define PSS_LEN			62
 #define PSS_RE			6*12
 
diff --git a/include/sync/sfo.h b/include/sync/sfo.h
index a2a45748e..a759184f1 100644
--- a/include/sync/sfo.h
+++ b/include/sync/sfo.h
@@ -20,6 +20,6 @@
 #define SFO_
 
 float sfo_estimate(int *t0, int len, float period);
-
+float sfo_estimate_period(int *t0, int *t, int len, float period);
 
 #endif
diff --git a/include/sync/sync.h b/include/sync/sync.h
index 59a2c2ad3..c3fc547fb 100644
--- a/include/sync/sync.h
+++ b/include/sync/sync.h
@@ -49,7 +49,7 @@ int sync_get_N_id_2(sync_t *q);
 int sync_get_N_id_1(sync_t *q);
 int sync_get_cell_id(sync_t *q);
 void sync_set_threshold(sync_t *q, float threshold);
-int sync_init(sync_t *q);
+int sync_init(sync_t *q, int frame_size);
 void sync_free(sync_t *q);
 
 #endif
diff --git a/lib/ch_estimation/src/chest.c b/lib/ch_estimation/src/chest.c
index 6f4ac002c..0e71f2b24 100644
--- a/lib/ch_estimation/src/chest.c
+++ b/lib/ch_estimation/src/chest.c
@@ -77,11 +77,15 @@ void chest_ce_ref(chest_t *q, cf_t *input, int nslot, int port_id, int nref) {
 	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;
-	channel_ref = input[SAMPLE_IDX(q->symbol_sz, tidx, fidx)];
+	channel_ref = input[SAMPLE_IDX(q->nof_prb, tidx, fidx)];
 	q->refsignal[port_id][nslot].refs[nref].recv_simbol = channel_ref;
+
+	DEBUG("Reference %d pos (%d,%d)=%d %.2f/%.2f=%.2f %.2f/%.2f=%.2f\n", nref, tidx, fidx, SAMPLE_IDX(q->nof_prb, tidx, fidx),
+			cabsf(channel_ref),cabsf(known_ref),cabsf(channel_ref/known_ref),
+			cargf(channel_ref)/M_PI,cargf(known_ref)/M_PI,cargf(channel_ref/known_ref)/M_PI);
 	/* FIXME: compare with treshold */
 	if (channel_ref != 0) {
-		q->refsignal[port_id][nslot].ch_est[nref] = known_ref/channel_ref;
+		q->refsignal[port_id][nslot].ch_est[nref] = channel_ref/known_ref;
 	} else {
 		q->refsignal[port_id][nslot].ch_est[nref] = 0;
 	}
@@ -118,14 +122,10 @@ void chest_ce_slot_port(chest_t *q, cf_t *input, cf_t *ce, int nslot, int port_i
 	for (i=0;i<q->nof_prb * RE_X_RB; i++) {
 		for (j=0;j<r->nsymbols;j++) {
 			x[j] = ce[r->symbols_ref[j] * q->nof_prb * RE_X_RB + i];
-			printf("x[%d]=ce[%d]=%.3f\n", j,
-					r->symbols_ref[j] * q->nof_prb * RE_X_RB + i,
-					cabsf(x[j]));
 		}
 		interp_linear_offset(x, y, r->symbols_ref[1]-r->symbols_ref[0],
 				2, r->symbols_ref[0], 3);
 		for (j=0;j<q->nof_symbols;j++) {
-			printf("ce[%d] = y[%d] =%.3f\n", j * q->nof_prb * RE_X_RB + i, j, cabsf(x[j]));
 			ce[j * q->nof_prb * RE_X_RB + i] = y[j];
 		}
 	}
@@ -151,12 +151,11 @@ int chest_init(chest_t *q, lte_cp_t cp, int nof_prb, int nof_ports) {
 
 	q->nof_ports = nof_ports;
 	q->nof_symbols = CP_NSYMB(cp);
-	q->symbol_sz = lte_symbol_sz(nof_prb);
 	q->cp = cp;
 	q->nof_prb = nof_prb;
 
-	INFO("Initializing channel estimator size %dx%d nof_prb=%d, nof_ports=%d\n",
-			q->nof_symbols, q->symbol_sz, nof_prb, nof_ports);
+	INFO("Initializing channel estimator size %dx%d, nof_ports=%d\n",
+			q->nof_symbols, nof_prb, nof_ports);
 
 	return 0;
 }
@@ -169,8 +168,6 @@ int chest_ref_LTEDL_slot_port(chest_t *q, int port, int nslot, int cell_id) {
 		return -1;
 	}
 
-	INFO("Setting LTE DL reference signals port=%d, nslot=%d, cell_id=%d\n", port, nslot, cell_id);
-
 	if (refsignal_init_LTEDL(&q->refsignal[port][nslot], port, nslot, cell_id, q->cp, q->nof_prb)) {
 		fprintf(stderr, "Error initiating CRS port=%d, slot=%d\n", port, nslot);
 		return -1;
diff --git a/lib/ch_estimation/src/refsignal.c b/lib/ch_estimation/src/refsignal.c
index f79cfe59a..8436f627f 100644
--- a/lib/ch_estimation/src/refsignal.c
+++ b/lib/ch_estimation/src/refsignal.c
@@ -20,6 +20,7 @@
 #include <string.h>
 #include <strings.h>
 #include <stdlib.h>
+#include <complex.h>
 
 #include "lte/base.h"
 #include "ch_estimation/refsignal.h"
@@ -110,9 +111,6 @@ int refsignal_init_LTEDL(refsignal_t *q, int port_id, int nslot,
 
 	memcpy(q->symbols_ref, lp, sizeof(int) * nof_ref_symbols);
 
-	DEBUG("Initializing %d CRS for LTE DL slot=%d, %d RE in %d symbols\n",
-			q->nof_refs, nslot, nof_refs_x_symbol, nof_ref_symbols);
-
 	q->refs = vec_malloc(q->nof_refs * sizeof(ref_t));
 	if (!q->refs) {
 		goto free_and_exit;
@@ -127,7 +125,6 @@ int refsignal_init_LTEDL(refsignal_t *q, int port_id, int nslot,
 
 		c_init = 1024 * (7 * (ns + 1) + lp[l] + 1) * (2 * cell_id + 1)
 				+ 2 * cell_id + N_cp;
-
 		if (sequence_LTEPRS(&seq, 2 * 2 * MAX_PRB, c_init)) {
 			goto free_and_exit;
 		}
@@ -142,13 +139,13 @@ int refsignal_init_LTEDL(refsignal_t *q, int port_id, int nslot,
 			__imag__ q->refs[idx(l,i)].simbol = (1 - 2 * (float) seq.c[2 * mp + 1]) / sqrt(2);
 
 			/* mapping to resource elements */
-			q->refs[idx(l,i)].freq_idx = refsignal_k(i, v, cell_id)+GUARD_RE(nof_prb);
+			q->refs[idx(l,i)].freq_idx = refsignal_k(i, v, cell_id);
 			q->refs[idx(l,i)].time_idx = lp[l];
 
 			/* print only first slot */
 			if (ns == 0) {
 				DEBUG("(%-2d,%2d) is mapped to (%-2d,%2d) (mp=%d, v=%d)\n",
-						l,i,q->refs[idx(l,i)].time_idx, q->refs[idx(l,i)].freq_idx-GUARD_RE(nof_prb), mp, v);
+						l,i,q->refs[idx(l,i)].time_idx, q->refs[idx(l,i)].freq_idx, mp, v);
 			}
 		}
 	}
diff --git a/lib/lte/src/fft.c b/lib/lte/src/fft.c
index 5195140f6..9f6f2f6b3 100644
--- a/lib/lte/src/fft.c
+++ b/lib/lte/src/fft.c
@@ -15,38 +15,61 @@
  * You should have received a copy of the GNU Lesser General Public License
  * along with OSLD-lib.  If not, see <http://www.gnu.org/licenses/>.
  */
+#include <string.h>
 #include <strings.h>
 #include <stdlib.h>
 
 #include "lte/base.h"
 #include "utils/dft.h"
 #include "lte/fft.h"
+#include "utils/debug.h"
 
-int lte_fft_init_(lte_fft_t *q, lte_cp_t cp_type, int symbol_sz, dft_dir_t dir) {
+int lte_fft_init_(lte_fft_t *q, lte_cp_t cp_type, int nof_prb, dft_dir_t dir) {
+	int symbol_sz = lte_symbol_sz(nof_prb);
 
-	if (dft_plan_c2c(symbol_sz, dir, &q->fft_plan)) {
+	if (symbol_sz == -1) {
+		fprintf(stderr, "Error: Invalid nof_prb=%d\n", nof_prb);
 		return -1;
 	}
+	if (dft_plan_c2c(symbol_sz, dir, &q->fft_plan)) {
+		fprintf(stderr, "Error: Creating DFT plan\n");
+		return -1;
+	}
+	q->tmp = malloc(symbol_sz * sizeof(cf_t));
+	if (!q->tmp) {
+		perror("malloc");
+		return -1;
+	}
+
 	q->fft_plan.options = DFT_DC_OFFSET | DFT_MIRROR_POS | DFT_NORMALIZE;
-	q->nof_symbols = CP_NSYMB(cp_type);
 	q->symbol_sz = symbol_sz;
+	q->nof_symbols = CP_NSYMB(cp_type);
 	q->cp_type = cp_type;
+	q->nof_re = nof_prb * RE_X_RB;
+	q->nof_guards = ((symbol_sz - q->nof_re) / 2);
+	DEBUG("Init %s symbol_sz=%d, nof_symbols=%d, cp_type=%s, nof_re=%d, nof_guards=%d\n",
+			dir==FORWARD?"FFT":"iFFT", q->symbol_sz, q->nof_symbols,
+					q->cp_type==CPNORM?"Normal":"Extended", q->nof_re, q->nof_guards);
 	return 0;
 }
+
 void lte_fft_free_(lte_fft_t *q) {
 	dft_plan_free(&q->fft_plan);
 	bzero(q, sizeof(lte_fft_t));
 }
 
-int lte_fft_init(lte_fft_t *q, lte_cp_t cp_type, int symbol_sz) {
-	return lte_fft_init_(q, cp_type, symbol_sz, FORWARD);
+int lte_fft_init(lte_fft_t *q, lte_cp_t cp_type, int nof_prb) {
+	return lte_fft_init_(q, cp_type, nof_prb, FORWARD);
 }
+
 void lte_fft_free(lte_fft_t *q) {
 	lte_fft_free_(q);
 }
-int lte_ifft_init(lte_fft_t *q, lte_cp_t cp_type, int symbol_sz) {
-	return lte_fft_init_(q, cp_type, symbol_sz, BACKWARD);
+
+int lte_ifft_init(lte_fft_t *q, lte_cp_t cp_type, int nof_prb) {
+	return lte_fft_init_(q, cp_type, nof_prb, BACKWARD);
 }
+
 void lte_ifft_free(lte_fft_t *q) {
 	lte_fft_free_(q);
 }
@@ -58,9 +81,10 @@ void lte_fft_run(lte_fft_t *q, cf_t *input, cf_t *output) {
 	int i;
 	for (i=0;i<q->nof_symbols;i++) {
 		input += CP_ISNORM(q->cp_type)?CP_NORM(i, q->symbol_sz):CP_EXT(q->symbol_sz);
-		dft_run_c2c(&q->fft_plan, input, output);
+		dft_run_c2c(&q->fft_plan, input, q->tmp);
+		memcpy(output, &q->tmp[q->nof_guards], q->nof_re * sizeof(cf_t));
 		input += q->symbol_sz;
-		output += q->symbol_sz;
+		output += q->nof_re;
 	}
 }
 
diff --git a/lib/lte/src/sequence.c b/lib/lte/src/sequence.c
index 7f85006ba..ec1a04a97 100644
--- a/lib/lte/src/sequence.c
+++ b/lib/lte/src/sequence.c
@@ -19,13 +19,11 @@
 #include "lte/sequence.h"
 
 #include <stdlib.h>
+#include <stdio.h>
 #include <strings.h>
 #include <assert.h>
 
 #define Nc 1600
-#define GOLDMAXLEN (Nc*10)
-static int x1[GOLDMAXLEN];
-static int x2[GOLDMAXLEN];
 
 
 /*
@@ -35,24 +33,37 @@ static int x2[GOLDMAXLEN];
  */
 void generate_prs_c(sequence_t *q, unsigned int seed) {
 	int n;
+	unsigned int *x1;
+	unsigned int *x2;
 
-	assert(q->len + Nc + 31 < GOLDMAXLEN);
+	x1 = calloc(Nc + q->len + 31, sizeof(unsigned int));
+	if (!x1) {
+		perror("calloc");
+		return;
+	}
+	x2 = calloc(Nc + q->len + 31, sizeof(unsigned int));
+	if (!x2) {
+		free(x1);
+		perror("calloc");
+		return;
+	}
 
 	for (n = 0; n < 31; n++) {
-		x1[n] = 0;
 		x2[n] = (seed >> n) & 0x1;
 	}
 	x1[0] = 1;
 
 	for (n = 0; n < Nc + q->len; n++) {
 		x1[n + 31] = (x1[n + 3] + x1[n]) & 0x1;
-		x2[n + 31] = (x2[n + 3] + x2[n + 2] + x2[n]) & 0x1;
+		x2[n + 31] = (x2[n + 3] + x2[n + 2] + +x2[n+1] + x2[n]) & 0x1;
 	}
 
 	for (n = 0; n < q->len; n++) {
 		q->c[n] = (x1[n + Nc] + x2[n + Nc]) & 0x1;
 	}
 
+	free(x1);
+	free(x2);
 }
 
 int sequence_LTEPRS(sequence_t *q, int len, int seed) {
diff --git a/lib/phch/src/pbch.c b/lib/phch/src/pbch.c
index 2ad4e95c8..7c4813f1d 100644
--- a/lib/phch/src/pbch.c
+++ b/lib/phch/src/pbch.c
@@ -40,39 +40,22 @@ int pbch_cp(cf_t *input, cf_t *output, int nof_prb, lte_cp_t cp, int cell_id, bo
 	if (put) {
 		ptr = input;
 		output += nof_prb * RE_X_RB / 2 - 36;
-		output += GUARD_RE(nof_prb);
 	} else {
 		ptr = output;
 		input += nof_prb * RE_X_RB / 2 - 36;
-		input += GUARD_RE(nof_prb);
 	}
 
 	/* symbol 0 & 1 */
 	for (i=0;i<2;i++) {
 		phch_cp_prb_ref(&input, &output, cell_id%3, 4, 6, put);
-		if (put) {
-			output += 2*GUARD_RE(nof_prb);
-		} else {
-			input += 2*GUARD_RE(nof_prb);
-		}
 	}
 	/* symbols 2 & 3 */
 	if (CP_ISNORM(cp)) {
 		for (i=0;i<2;i++) {
 			phch_cp_prb(&input, &output, 6);
-			if (put) {
-				output += 2*GUARD_RE(nof_prb);
-			} else {
-				input += 2*GUARD_RE(nof_prb);
-			}
 		}
 	} else {
 		phch_cp_prb(&input, &output, 6);
-		if (put) {
-			output += 2*GUARD_RE(nof_prb);
-		} else {
-			input += 2*GUARD_RE(nof_prb);
-		}
 		phch_cp_prb_ref(&input, &output, cell_id%3, 4, 6, put);
 	}
 	if (put) {
@@ -324,8 +307,9 @@ int pbch_decode_frame(pbch_t *q, pbch_mib_t *mib, int src, int dst, int n, int n
  *
  * Returns 1 if successfully decoded MIB, 0 if not and -1 on error
  */
-int pbch_decode(pbch_t *q, cf_t *slot1_symbols, pbch_mib_t *mib, int nof_prb, float ebno) {
-	int src, dst, res, nb;
+int pbch_decode(pbch_t *q, cf_t *slot1_symbols, cf_t **ce, int nof_ports,
+		int nof_prb, float ebno, pbch_mib_t *mib) {
+	int src, dst, res, nb, nant;
 
 	int nof_symbols = (CP_ISNORM(q->cp)) ? PBCH_RE_CPNORM: PBCH_RE_CPEXT;
 	int nof_bits = 2 * nof_symbols;
@@ -337,39 +321,52 @@ int pbch_decode(pbch_t *q, cf_t *slot1_symbols, pbch_mib_t *mib, int nof_prb, fl
 		return -1;
 	}
 
-	/* demodulate symbols */
-	demod_soft_sigma_set(&q->demod, ebno);
-	demod_soft_demodulate(&q->demod, q->pbch_symbols,
-			&q->pbch_llr[nof_bits * q->frame_idx], nof_symbols);
+	/* Try decoding for 1 to nof_ports antennas */
+	for (nant=0;nant<nof_ports;nant++) {
 
-	q->frame_idx++;
+		/* pre-decoder & matched filter */
+		int i;
+		for (i=0;i<nof_symbols;i++) {
+			q->pbch_symbols[i] /= ce[0][i];
+		}
 
-	INFO("PBCH: %d frames in buffer\n", q->frame_idx);
+		/* layer demapper */
+		//x    = lte_pre_decoder_and_matched_filter(y_est, ce(1:n,:), "tx_diversity");
+		//d    = lte_layer_demapper(x, 1, "tx_diversity");
 
-	/* We don't know where the 40 ms begin, so we try all combinations. E.g. if we received
-	 * 4 frames, try 1,2,3,4 individually, 12, 23, 34 in pairs, 123, 234 and finally 1234.
-	 * We know they are ordered.
-	 */
-	res = 0;
-	for (nb=0;nb<q->frame_idx && !res;nb++) {
-		for (dst=0;(dst<4-nb) && !res;dst++) {
-			for (src=0;src<q->frame_idx && !res;src++) {
-				DEBUG("Trying %d blocks at offset %d as subframe mod4 number %d\n", nb+1, src, dst);
-				res = pbch_decode_frame(q, mib, src, dst, nb+1, nof_bits);
+		/* demodulate symbols */
+		demod_soft_sigma_set(&q->demod, ebno);
+		demod_soft_demodulate(&q->demod, q->pbch_symbols,
+				&q->pbch_llr[nof_bits * q->frame_idx], nof_symbols);
+
+		q->frame_idx++;
+
+		INFO("PBCH: %d frames in buffer\n", q->frame_idx);
+
+		/* We don't know where the 40 ms begin, so we try all combinations. E.g. if we received
+		 * 4 frames, try 1,2,3,4 individually, 12, 23, 34 in pairs, 123, 234 and finally 1234.
+		 * We know they are ordered.
+		 */
+		res = 0;
+		for (nb=0;nb<q->frame_idx && !res;nb++) {
+			for (dst=0;(dst<4-nb) && !res;dst++) {
+				for (src=0;src<q->frame_idx && !res;src++) {
+					DEBUG("Trying %d blocks at offset %d as subframe mod4 number %d\n", nb+1, src, dst);
+					res = pbch_decode_frame(q, mib, src, dst, nb+1, nof_bits);
+				}
 			}
 		}
-	}
 
-	if (res) {
-		q->frame_idx = 0;
-		return 1;
-	} else {
-		/* make room for the next packet of radio frame symbols */
-		if (q->frame_idx == 4) {
-			memcpy(&q->pbch_llr[nof_bits], q->pbch_llr, nof_bits * 3 * sizeof(float));
-			q->frame_idx = 3;
+		if (res) {
+			q->frame_idx = 0;
+			return 1;
 		}
-		return 0;
 	}
 
+	/* If not found, make room for the next packet of radio frame symbols */
+	if (q->frame_idx == 4) {
+		memcpy(&q->pbch_llr[nof_bits], q->pbch_llr, nof_bits * 3 * sizeof(float));
+		q->frame_idx = 3;
+	}
+	return 0;
 }
diff --git a/lib/resampling/src/interp.c b/lib/resampling/src/interp.c
index 657c29e4d..392b5e511 100644
--- a/lib/resampling/src/interp.c
+++ b/lib/resampling/src/interp.c
@@ -57,3 +57,14 @@ void interp_linear_offset(cf_t *input, cf_t *output, int M, int len, int off_st,
 void interp_linear(cf_t *input, cf_t *output, int M, int len) {
 	interp_linear_offset(input, output, M, len, 0, 0);
 }
+
+
+/* Performs 1st order integer linear interpolation */
+void interp_linear_f(float *input, float *output, int M, int len) {
+	int i, j;
+	for (i=0;i<len-1;i++) {
+		for (j=0;j<M;j++) {
+			output[i*M+j] = input[i] + j * (input[i+1]-input[i]) / M;
+		}
+	}
+}
diff --git a/lib/sync/src/pss.c b/lib/sync/src/pss.c
index 3bd21a382..d4295a3e4 100644
--- a/lib/sync/src/pss.c
+++ b/lib/sync/src/pss.c
@@ -132,14 +132,14 @@ int pss_generate(cf_t *signal, int direction, int N_id_2) {
 	for (i = 0; i < PSS_LEN / 2; i++) {
 		arg = (float) sign * M_PI * root_value[root_idx]
 				* ((float) i * ((float) i + 1.0)) / 63.0;
-		__real__ signal[i] = cos(arg);
-		__imag__ signal[i] = sin(arg);
+		__real__ signal[i] = cosf(arg);
+		__imag__ signal[i] = sinf(arg);
 	}
 	for (i = PSS_LEN / 2; i < PSS_LEN; i++) {
 		arg = (float) sign * M_PI * root_value[root_idx]
 				* (((float) i + 2.0) * ((float) i + 1.0)) / 63.0;
-		__real__ signal[i] = cos(arg);
-		__imag__ signal[i] = sin(arg);
+		__real__ signal[i] = cosf(arg);
+		__imag__ signal[i] = sinf(arg);
 	}
 	return 0;
 }
diff --git a/lib/sync/src/sfo.c b/lib/sync/src/sfo.c
index 70f576272..ab5553370 100644
--- a/lib/sync/src/sfo.c
+++ b/lib/sync/src/sfo.c
@@ -16,6 +16,8 @@
  * along with OSLD-lib.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <stdio.h>
+#include <stdlib.h>
 #include "sync/sfo.h"
 
 /* Estimate SFO based on the array of time estimates t0
@@ -29,3 +31,17 @@ float sfo_estimate(int *t0, int len, float period) {
 	}
 	return sfo;
 }
+
+/* Same as sfo_estimate but period is non-uniform.
+ * Vector t is the sampling time times period for each t0
+ */
+float sfo_estimate_period(int *t0, int *t, int len, float period) {
+	int i;
+	float sfo=0.0;
+	for (i=1;i<len;i++) {
+		if (abs(t0[i]-t0[i-1]) < 5000) {
+			sfo += (t0[i]-t0[i-1])/(t[i] - t[i-1])/period;
+		}
+	}
+	return sfo/(len-2);
+}
diff --git a/lib/sync/src/sync.c b/lib/sync/src/sync.c
index 0e0a5be28..bcb14ce9c 100644
--- a/lib/sync/src/sync.c
+++ b/lib/sync/src/sync.c
@@ -23,7 +23,7 @@
 #include "lte/base.h"
 #include "sync/sync.h"
 
-int sync_init(sync_t *q) {
+int sync_init(sync_t *q, int frame_size) {
 	int N_id_2;
 
 	bzero(q, sizeof(sync_t));
@@ -32,7 +32,7 @@ int sync_init(sync_t *q) {
 	q->pss_mode = PEAK_MEAN;
 
 	for (N_id_2=0;N_id_2<3;N_id_2++) {
-		if (pss_synch_init(&q->pss[N_id_2], 960)) {
+		if (pss_synch_init(&q->pss[N_id_2], frame_size)) {
 			fprintf(stderr, "Error initializing PSS object\n");
 			return -1;
 		}
@@ -133,14 +133,14 @@ int sync_run(sync_t *q, cf_t *input, int read_offset) {
 				&peak_value[N_id_2], &mean_value[N_id_2]);
 	}
 
-	DEBUG("PSS possible peak N_id_2=%d, pos=%d value=%.2f threshold=%.2f\n",
-			N_id_2, peak_pos[N_id_2], peak_value[N_id_2], q->threshold);
-
 	q->peak_to_avg = peak_value[N_id_2] / mean_value[N_id_2];
 
+	DEBUG("PSS possible peak N_id_2=%d, pos=%d peak=%.2f par=%.2f threshold=%.2f\n",
+			N_id_2, peak_pos[N_id_2], peak_value[N_id_2], q->peak_to_avg, q->threshold);
+
 	/* If peak detected */
 	peak_detected = 0;
-	if (peak_pos[N_id_2] > 128) {
+	if (peak_pos[N_id_2] + read_offset > 128) {
 		if (q->pss_mode == ABSOLUTE) {
 			if (peak_value[N_id_2] > q->threshold) {
 				peak_detected = 1;
@@ -153,10 +153,11 @@ int sync_run(sync_t *q, cf_t *input, int read_offset) {
 	}
 	if (peak_detected) {
 
-		INFO("PSS peak detected N_id_2=%d, pos=%d value=%.2f\n", N_id_2, peak_pos[N_id_2], peak_value[N_id_2]);
 
 		q->cfo = pss_synch_cfo_compute(&q->pss[N_id_2], &input[read_offset + peak_pos[N_id_2]-128]);
-		INFO("Estimated CFO=%.4f\n", q->cfo);
+
+		INFO("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_to_avg, q->threshold, q->cfo);
 
 		sss_idx = read_offset + peak_pos[N_id_2]-2*(128+CP(128,CPNORM_LEN));
 		if (sss_idx>= 0) {
diff --git a/lib/utils/src/vector.c b/lib/utils/src/vector.c
index 1882cac45..1336173f5 100644
--- a/lib/utils/src/vector.c
+++ b/lib/utils/src/vector.c
@@ -117,7 +117,7 @@ void vec_fprint_c(FILE *stream, _Complex float *x, int len) {
 		//if (!((i+1)%10))
 		//	fprintf(stream, "\n");
 	}
-	fprintf(stream, "]\n");
+	fprintf(stream, "];\n");
 }
 
 void vec_fprint_f(FILE *stream, float *x, int len) {
@@ -128,7 +128,7 @@ void vec_fprint_f(FILE *stream, float *x, int len) {
 		//if (!((i+1)%10))
 		//	fprintf(stream, "\n");
 	}
-	fprintf(stream, "]\n");
+	fprintf(stream, "];\n");
 }
 
 
@@ -138,7 +138,7 @@ void vec_fprint_i(FILE *stream, int *x, int len) {
 	for (i=0;i<len;i++) {
 		fprintf(stream, "%d, ", x[i]);
 	}
-	fprintf(stream, "]\n");
+	fprintf(stream, "];\n");
 }
 
 void vec_conj(_Complex float *x, _Complex float *y, int len) {
diff --git a/matlab/common/read_complex.m b/matlab/common/read_complex.m
new file mode 100644
index 000000000..d9185a7ea
--- /dev/null
+++ b/matlab/common/read_complex.m
@@ -0,0 +1,20 @@
+function [ out ] = read_complex( filename, count )
+%READ_COMPLEX Summary of this function goes here
+%   Detailed explanation goes here
+
+    [tidin msg]=fopen(filename,'r');
+    if (tidin==-1)
+        fprintf('error opening %s: %s\n',filename, msg);
+        out=[];
+        return
+    end
+
+    if (nargin==1) 
+        count=inf;
+    end
+
+    x=fread(tidin,2*count,'single');
+    i=1:2:length(x);
+    out=x(i)+x(i+1)*1i;
+end
+
diff --git a/matlab/common/write_complex.m b/matlab/common/write_complex.m
new file mode 100644
index 000000000..a152c8672
--- /dev/null
+++ b/matlab/common/write_complex.m
@@ -0,0 +1,22 @@
+function [ out ] = write_complex( filename, x)
+%READ_COMPLEX Summary of this function goes here
+%   Detailed explanation goes here
+
+    [tidin msg]=fopen(filename,'w');
+    if (tidin==-1)
+        fprintf('error opening %s: %s\n',filename, msg);
+        out=[];
+        return
+    end
+
+    if (isreal(x))
+        y=x;
+    else
+        i=1:2:2*length(x);
+        y(i)=real(x);
+        y(i+1)=imag(x);
+    end
+    
+    fwrite(tidin,y,'single');
+end
+
diff --git a/matlab/sync/check_pss.m b/matlab/sync/check_pss.m
index bcc595c33..133e6f127 100644
--- a/matlab/sync/check_pss.m
+++ b/matlab/sync/check_pss.m
@@ -1,4 +1,4 @@
-function [ fs ] = check_pss( x, N_id_2)
+function [ fs ] = check_pss( x, N_id_2, threshold)
 %CHECK_PSS Summary of this function goes here
 %   Detailed explanation goes here
 flen=9600;
@@ -11,28 +11,23 @@ fs=zeros(nf,1);
 cfo=zeros(nf,1);
 cfo2=zeros(nf,1);
 m_p=zeros(nf,1);
-for i=1:nf-1
-    [fs(i) cfo(i) m_p(i)]=find_pss(xf(:,i),N_id_2,false);
-    if (fs(i)<0)
-        j=0;
-    end
-%    cfo2(i) = cfo_estimate_cp(xf(fs(i)+960:fs(i)+2*960,i),7,128,10,9);
+for i=1:nf
+    [fs(i) cfo(i) m_p(i)]=find_pss(xf(:,i),N_id_2,false, threshold);
 end
 
-sfo=sfo_estimate(fs, 5/1000);
+fs=fs+960;
+[sfo sfo_v]=sfo_estimate(fs, 5/1000);
 
 subplot(1,3,1)
 plot(1:nf,fs)
-legend('PSS-based');
 subplot(1,3,2)
-plot(1:nf, cfo, 1:nf, cfo2)
-legend('PSS-based','CP-based');
+plot(1:nf, cfo)
 if (nf > 0)
     axis([0 nf -0.5 0.5])
 end
 subplot(1,3,3)
 plot(m_p)
-fprintf('pss_mean=%g, pss_var=%g, cp_mean=%g, cp_var=%g m_p=%g sfo=%g Hz\n',mean(cfo),var(cfo), mean(cfo2), var(cfo2), mean(m_p), sfo)
+fprintf('cfo_mean=%g Hz, cfo_std=%g Hz, m_p=%g sfo=%g Hz\n',15000*nanmean(cfo),15000*nanstd(cfo), nanmean(m_p), sfo)
 end
 
 
diff --git a/matlab/sync/find_pss.m b/matlab/sync/find_pss.m
index 8ed366e1b..bc16c9a91 100644
--- a/matlab/sync/find_pss.m
+++ b/matlab/sync/find_pss.m
@@ -1,6 +1,10 @@
-function [ fs eps p_m w2] = find_pss( x, N_id_2, doplot)
+function [ fs eps p_m w2] = find_pss( x, N_id_2, doplot, threshold)
     if nargin == 2
         doplot = false;
+        threshold = 0;
+    end
+    if nargin == 3
+        threshold = 0;
     end
     
     c=lte_pss_zc(N_id_2);
@@ -10,26 +14,30 @@ function [ fs eps p_m w2] = find_pss( x, N_id_2, doplot)
 
     w2=conv(x,ccf);
     if (doplot)
-        plot(abs(w2))
+        %plot(10*log10(abs(w2)));%./mean(abs(w2))));
+       plot(abs(w2)) 
+       %axis([0 length(w2) 0 20])
     end
     [m i]=max(abs(w2));
     fs=i-960;
     p_m = m/mean(abs(w2));
+    
     if doplot
         fprintf('Frame starts at %d, m=%g, p=%g, p/m=%g dB\n',fs, ...
             mean(abs(w2)), m, 10*log10(m/mean(abs(w2))));
     end
     
     % Estimate PSS-aided CFO
-%     if (i - 129)
-%         y=ccf.*x(i-128:i-1);
-%     
-%         y0=y(1:64);
-%         y1=y(65:length(y));    
-%     
-%         eps=angle(conj(sum(y0))*sum(y1))/pi;    
-%     else
-        eps = NaN;
-%     end
+    if (i > 200 && i<length(x)&& p_m > threshold)
+        y=ccf.*x(i-128:i-1);
+    
+        y0=y(1:64);
+        y1=y(65:length(y));    
+    
+        eps=angle(conj(sum(y0))*sum(y1))/pi;    
+    else
+         eps = NaN;
+         fs = NaN;
+    end
 end
 
diff --git a/matlab/sync/ifo_pss.m b/matlab/sync/ifo_pss.m
new file mode 100644
index 000000000..739cbe1a7
--- /dev/null
+++ b/matlab/sync/ifo_pss.m
@@ -0,0 +1,13 @@
+function [ ifo ] = ifo_pss( r_pss, x_pss)
+
+k=1;
+v=-31:31;
+c=zeros(length(v),1);
+for i=v
+    c(k) = ifo_pss_corr(i, r_pss, x_pss);
+    k=k+1;
+end
+[m i]=max(c);
+ifo=v(i);
+plot(v,c); 
+
diff --git a/matlab/sync/ifo_pss_corr.m b/matlab/sync/ifo_pss_corr.m
new file mode 100644
index 000000000..c921cba9c
--- /dev/null
+++ b/matlab/sync/ifo_pss_corr.m
@@ -0,0 +1,10 @@
+function [ corr ] = ifo_pss_corr( n, r_pss, x_pss)
+
+    x=0;
+    for i=1:length(x_pss)
+        x=x+r_pss(1+mod(i+n-1,length(r_pss)))*conj(x_pss(i));
+    end
+    corr=real(exp(1i*2*pi*9*n/128)*x);
+%    corr=abs(x);
+end
+
diff --git a/matlab/sync/sfo_estimate.m b/matlab/sync/sfo_estimate.m
index d9f46690f..f0160a868 100644
--- a/matlab/sync/sfo_estimate.m
+++ b/matlab/sync/sfo_estimate.m
@@ -1,7 +1,18 @@
-function [ sfo ] = sfo_estimate( fs, T )
+function [ sfo sfo_v ] = sfo_estimate( fs, T )
 
-sfo = 0;
-for i=2:length(fs)
-    sfo=sfo + (fs(i)-fs(i-1))/length(fs)/T;
+
+nanfs=fs(~isnan(fs));
+idx=find(~isnan(fs));
+sfo_v = zeros(length(nanfs)-1,1);
+for i=2:length(nanfs)
+    if (abs(nanfs(i)-nanfs(i-1))<9000)
+        sfo_v(i-1)=(nanfs(i)-nanfs(i-1))/T/(idx(i)-idx(i-1));
+    else
+        sfo_v(i-1)=sfo_v(i-2);
+    end
 end
 
+sfo = mean(sfo_v);
+
+
+
diff --git a/uhd/uhd_imp.cpp b/uhd/uhd_imp.cpp
index 69d285b7c..1fa66766d 100644
--- a/uhd/uhd_imp.cpp
+++ b/uhd/uhd_imp.cpp
@@ -70,7 +70,7 @@ int uhd_open(char *args, void **h) {
 	std::string _args=std::string(args);
 	handler->usrp = uhd::usrp::multi_usrp::make(_args);
 
-	uhd::msg::register_handler(&my_handler);
+	//uhd::msg::register_handler(&my_handler);
 
 	std::string otw, cpu;
 	otw="sc16";
diff --git a/uhd/uhd_utils.c b/uhd/uhd_utils.c
index 173164cb5..b33eb344b 100644
--- a/uhd/uhd_utils.c
+++ b/uhd/uhd_utils.c
@@ -5,9 +5,10 @@
 
 #include "uhd.h"
 #include "utils/vector.h"
+#include "utils/debug.h"
 
 int uhd_rssi_scan(void *uhd, float *freqs, float *rssi, int nof_bands, double fs, int nsamp) {
-	int i;
+	int i, j;
 	int ret = -1;
 	_Complex float *buffer;
 	double f;
@@ -21,22 +22,25 @@ int uhd_rssi_scan(void *uhd, float *freqs, float *rssi, int nof_bands, double fs
 	uhd_set_rx_srate(uhd, fs);
 
 	for (i=0;i<nof_bands;i++) {
+		uhd_stop_rx_stream(uhd);
+
 		f = (double) freqs[i];
 		uhd_set_rx_freq(uhd, f);
 		uhd_rx_wait_lo_locked(uhd);
-		if (!i) {
-			uhd_start_rx_stream(uhd);
-			usleep(500000);
-		}
-		if (uhd_recv(uhd, buffer, nsamp, 1) != nsamp) {
-			goto free_and_exit;
+
+		uhd_start_rx_stream(uhd);
+
+		/* discard first samples */
+		for (j=0;j<2;j++) {
+			if (uhd_recv(uhd, buffer, nsamp, 1) != nsamp) {
+				goto free_and_exit;
+			}
 		}
 		rssi[i] = vec_power(buffer, nsamp);
-		/* FIXME: First sample has a bias of 30 dB!! */
-		if (i == 0) {
-			rssi[i] /= 1000;
+		printf("[%3d]: Freq %4.1f Mhz - RSSI: %3.2f dBm\r", i, f/1000000, 10*log10f(rssi[i]) + 30); fflush(stdout);
+		if (VERBOSE_ISINFO()) {
+			printf("\n");
 		}
-		printf("Freq %4.1f Mhz - RSSI: %3.2f dBm\r", f/1000000, 10*log10f(rssi[i]) + 30); fflush(stdout);
 	}
 	uhd_stop_rx_stream(uhd);