diff --git a/matlab/common/read_int16.m b/matlab/common/read_int16.m
index 71e875d7c..3db6a3179 100644
--- a/matlab/common/read_int16.m
+++ b/matlab/common/read_int16.m
@@ -1,4 +1,4 @@
-function [ out ] = read_real( filename, count )
+function [ out ] = read_int16( filename, count )
 %READ_COMPLEX Summary of this function goes here
 %   Detailed explanation goes here
 
diff --git a/matlab/tests/pdsch_bler.m b/matlab/tests/pdsch_bler.m
index 364c7976e..0d5f4a162 100644
--- a/matlab/tests/pdsch_bler.m
+++ b/matlab/tests/pdsch_bler.m
@@ -6,28 +6,29 @@
 
 recordedSignal=[];
 
-Npackets = 3;
-SNR_values = linspace(12,16,4);
+Npackets = 5;
+SNR_values = linspace(18,25,5);
 
 %% Choose RMC 
 [waveform,rgrid,rmccFgOut] = lteRMCDLTool('R.9',[1;0;0;1]);
 waveform = sum(waveform,2);
 
 if ~isempty(recordedSignal)
-    rmccFgOut = struct('NCellID',1,'CellRefP',1,'CFI',1,'NDLRB',50,'SamplingRate',3.84e6,'Nfft',256,'DuplexMode','FDD','CyclicPrefix','Normal'); 
+    rmccFgOut = struct('CellRefP',1,'NDLRB',100,'DuplexMode','FDD','CyclicPrefix','Normal'); 
     rmccFgOut.PDSCH.RNTI = 1234;
     rmccFgOut.PDSCH.PRBSet = repmat(transpose(0:rmccFgOut.NDLRB-1),1,2);
     rmccFgOut.PDSCH.TxScheme = 'Port0';
     rmccFgOut.PDSCH.NLayers = 1;   
     rmccFgOut.PDSCH.NTurboDecIts = 5;
     rmccFgOut.PDSCH.Modulation = {'64QAM'};
-    rmccFgOut.PDSCH.TrBlkSizes = [0 5992*ones(1,4) 0 5992*ones(1,4)];
+    trblklen=75376;
+    rmccFgOut.PDSCH.TrBlkSizes = trblklen*ones(10,1);
     rmccFgOut.PDSCH.RV = 0;
 end
 
 flen=rmccFgOut.SamplingRate/1000;
     
-Nsf = 9; 
+Nsf = 10; 
 
 %% Setup Fading channel model 
 cfg.Seed = 8;                  % Random channel seed
@@ -78,8 +79,8 @@ for snr_idx=1:length(SNR_values)
         %% Demodulate 
         frame_rx = lteOFDMDemodulate(rmccFgOut, rxWaveform);
 
-        for sf_idx=0:Nsf
-        %sf_idx=9;
+        for sf_idx=0:Nsf-1
+       % sf_idx=9;
             subframe_rx=frame_rx(:,sf_idx*14+1:(sf_idx+1)*14);
             rmccFgOut.NSubframe=sf_idx;
             rmccFgOut.TotSubframes=1;
@@ -96,9 +97,9 @@ for snr_idx=1:length(SNR_values)
 
             %% Same with srsLTE
             if (rmccFgOut.PDSCH.TrBlkSizes(sf_idx+1) > 0)
-                [dec2, data, pdschRx, pdschSymbols2, cws2, cb9, temp] = srslte_pdsch(rmccFgOut, rmccFgOut.PDSCH, ... 
+                [dec2, data, pdschRx, pdschSymbols2, cws2] = srslte_pdsch(rmccFgOut, rmccFgOut.PDSCH, ... 
                                                         rmccFgOut.PDSCH.TrBlkSizes(sf_idx+1), ...
-                                                        subframe_rx);
+                                                        subframe_rx, hest, nest);
             else
                 dec2 = 1;
             end
diff --git a/matlab/tests/pdsch_check_rm_files.m b/matlab/tests/pdsch_check_rm_files.m
new file mode 100644
index 000000000..0af91deb2
--- /dev/null
+++ b/matlab/tests/pdsch_check_rm_files.m
@@ -0,0 +1,22 @@
+nof_cb=11;
+TBS=61664;
+K=5632; % Only supporting 1 K for now
+rv=0;
+chs.Modulation='64QAM';
+chs.NLayers=1;
+% cws must be a vector of size TBS in workspace containing the output of the
+% descrambler
+rmout_mat=lteRateRecoverTurbo(cws{1},TBS,rv,chs);
+scale=700;
+%path='../../build/srslte/lib/phch/test';
+path='.';
+error=zeros(nof_cb,3*K+12);
+rmout_lib=zeros(nof_cb,3*K+12);
+for i=0:nof_cb-1   
+    filename=sprintf('%s/rmout_%d.dat',path,i);
+    x=read_int16(filename);
+    rmout_lib(i+1,:) = reshape(reshape(x,3,[])',[],1); 
+    error(i+1,:)=abs(transpose(rmout_mat{i+1})-rmout_lib(i+1,:)/scale);
+end
+
+plot(reshape(error',1,[]))
\ No newline at end of file
diff --git a/matlab/tests/pdsch_decode_test.m b/matlab/tests/pdsch_decode_test.m
new file mode 100644
index 000000000..4d542bbd1
--- /dev/null
+++ b/matlab/tests/pdsch_decode_test.m
@@ -0,0 +1,63 @@
+enb=struct('NCellID',16,'NDLRB',6,'NSubframe',5,'CFI',3,'CyclicPrefix','Normal','CellRefP',1,'Ng','One','PHICHDuration','Normal','DuplexMode','FDD');
+
+RNTI=65535;
+
+addpath('../../build/srslte/lib/phch/test')
+
+cec.PilotAverage = 'UserDefined';     % Type of pilot averaging
+cec.FreqWindow = 9;                   % Frequency window size    
+cec.TimeWindow = 9;                   % Time window size    
+cec.InterpType = 'cubic';             % 2D interpolation type
+cec.InterpWindow = 'Centered';        % Interpolation window type
+cec.InterpWinSize = 1;                % Interpolation window size  
+
+subframe_rx=lteOFDMDemodulate(enb,inputSignal);
+[hest,nest] = lteDLChannelEstimate(enb, cec, subframe_rx);    
+    
+% Search PDCCH
+pdcchIndices = ltePDCCHIndices(enb); 
+[pdcchRx, pdcchHest] = lteExtractResources(pdcchIndices, subframe_rx, hest);
+[dciBits, pdcchSymbols] = ltePDCCHDecode(enb, pdcchRx, pdcchHest, nest);
+pdcch = struct('RNTI', RNTI);  
+dci = ltePDCCHSearch(enb, pdcch, dciBits); % Search PDCCH for DCI                
+
+if ~isempty(dci)
+        
+    dci = dci{1};
+    disp(dci);
+    
+    % Get the PDSCH configuration from the DCI
+    [pdsch, trblklen] = hPDSCHConfiguration(enb, dci, pdcch.RNTI);
+    pdsch.NTurboDecIts = 5;
+    fprintf('PDSCH settings after DCI decoding:\n');
+    disp(pdsch);
+
+    fprintf('Decoding PDSCH...\n\n');        
+    % Get PDSCH indices
+    [pdschIndices,pdschIndicesInfo] = ltePDSCHIndices(enb, pdsch, pdsch.PRBSet);
+    [pdschRx, pdschHest] = lteExtractResources(pdschIndices, subframe_rx, hest);
+    % Decode PDSCH 
+    [dlschBits,pdschSymbols] = ltePDSCHDecode(enb, pdsch, pdschRx, pdschHest, nest);
+    [sib1, crc] = lteDLSCHDecode(enb, pdsch, trblklen, dlschBits);
+
+    [dec2, data, pdschRx2, pdschSymbols2, e_bits, indices] = srslte_pdsch(enb, pdsch, ... 
+                                                        trblklen, ...
+                                                        subframe_rx);
+
+    
+    scatter(real(pdschSymbols{1}),imag(pdschSymbols{1}))
+
+    if crc == 0
+        fprintf('PDSCH OK.\n\n');
+    else
+        fprintf('PDSCH ERROR.\n\n');
+    end
+        
+    else
+        % indicate that DCI decoding failed 
+        fprintf('DCI decoding failed.\n\n');
+end
+
+indices=indices+1;
+plot(t,indices(t),t,pdschIndices(t))
+    
\ No newline at end of file
diff --git a/mex/include/srslte/mex/mexutils.h b/mex/include/srslte/mex/mexutils.h
index 485fa0d26..ea9115bb9 100644
--- a/mex/include/srslte/mex/mexutils.h
+++ b/mex/include/srslte/mex/mexutils.h
@@ -85,6 +85,9 @@ SRSLTE_API int mexutils_write_int(int *buffer,
 SRSLTE_API int mexutils_read_uint8(const mxArray *ptr, 
                                    uint8_t **buffer);
 
+SRSLTE_API int mexutils_read_uint64(const mxArray *ptr, 
+                                   uint64_t **buffer);
+
 SRSLTE_API int mexutils_read_f(const mxArray *ptr, 
                                float **buffer);
 
diff --git a/mex/lib/mexutils.c b/mex/lib/mexutils.c
index 6be84da2f..618994c15 100644
--- a/mex/lib/mexutils.c
+++ b/mex/lib/mexutils.c
@@ -137,6 +137,22 @@ int mexutils_read_uint8(const mxArray *ptr, uint8_t **buffer) {
   }
 }
 
+
+int mexutils_read_uint64(const mxArray *ptr, uint64_t **buffer) {
+  int numelems = mxGetNumberOfElements(ptr);
+  uint64_t *tmp = srslte_vec_malloc(numelems * sizeof(uint64_t));
+  if (tmp) {
+    uint64_t *inr=(uint64_t*) mxGetPr(ptr);
+    for (int i=0;i<numelems;i++) {
+      tmp[i] = (uint64_t) inr[i];       
+    }    
+    *buffer = tmp; 
+    return numelems;
+  } else {
+    return -1;
+  }
+}
+
 int mexutils_write_cf(cf_t *buffer, mxArray **ptr, uint32_t nr, uint32_t nc) {
   *ptr = mxCreateDoubleMatrix(nr, nc, mxCOMPLEX); 
   if (*ptr) {
diff --git a/srslte/examples/pdsch_ue.c b/srslte/examples/pdsch_ue.c
index 074bab883..34dc9f198 100644
--- a/srslte/examples/pdsch_ue.c
+++ b/srslte/examples/pdsch_ue.c
@@ -79,13 +79,13 @@ typedef struct {
   int force_N_id_2;
   uint16_t rnti;
   char *input_file_name;
-  int file_offset; 
+  int file_offset_time; 
+  float file_offset_freq;
   uint32_t file_nof_prb;
   uint32_t file_nof_ports;
   uint32_t file_cell_id;
   char *uhd_args; 
   float uhd_freq; 
-  float uhd_freq_offset; 
   float uhd_gain;
   int net_port; 
   char *net_address; 
@@ -105,10 +105,10 @@ void args_default(prog_args_t *args) {
   args->file_nof_prb = 25; 
   args->file_nof_ports = 1; 
   args->file_cell_id = 0; 
-  args->file_offset = 0; 
+  args->file_offset_time = 0; 
+  args->file_offset_freq = 0; 
   args->uhd_args = "";
   args->uhd_freq = -1.0;
-  args->uhd_freq_offset = 0.0;
   args->uhd_gain = -1.0; 
   args->net_port = -1; 
   args->net_address = "127.0.0.1";
@@ -117,16 +117,16 @@ void args_default(prog_args_t *args) {
 }
 
 void usage(prog_args_t *args, char *prog) {
-  printf("Usage: %s [agpPOcildDnruv] -f rx_frequency (in Hz) | -i input_file\n", prog);
+  printf("Usage: %s [agpPoOcildDnruv] -f rx_frequency (in Hz) | -i input_file\n", prog);
 #ifndef DISABLE_UHD
   printf("\t-a UHD args [Default %s]\n", args->uhd_args);
   printf("\t-g UHD fix RX gain [Default AGC]\n");
-  printf("\t-o UHD RX freq offset [Default %.1f MHz]\n", args->uhd_freq_offset/1000000);
 #else
   printf("\t   UHD is disabled. CUHD library not available\n");
 #endif
   printf("\t-i input_file [Default USRP]\n");
-  printf("\t-O offset samples for input file [Default %d]\n", args->file_offset);
+  printf("\t-o offset frequency correction (in Hz) for input file [Default %.1f Hz]\n", args->file_offset_freq);
+  printf("\t-O offset samples for input file [Default %d]\n", args->file_offset_time);
   printf("\t-p nof_prb for input file [Default %d]\n", args->file_nof_prb);
   printf("\t-P nof_ports for input file [Default %d]\n", args->file_nof_ports);
   printf("\t-c cell_id for input file [Default %d]\n", args->file_cell_id);
@@ -162,8 +162,11 @@ void parse_args(prog_args_t *args, int argc, char **argv) {
     case 'P':
       args->file_nof_ports = atoi(argv[optind]);
       break;
+    case 'o':
+      args->file_offset_freq = atof(argv[optind]);
+      break;
     case 'O':
-      args->file_offset = atoi(argv[optind]);
+      args->file_offset_time = atoi(argv[optind]);
       break;
     case 'c':
       args->file_cell_id = atoi(argv[optind]);
@@ -180,9 +183,6 @@ void parse_args(prog_args_t *args, int argc, char **argv) {
     case 't':
       args->time_offset = atoi(argv[optind]);
       break;
-    case 'o':
-      args->uhd_freq_offset = atof(argv[optind]);
-      break;
     case 'f':
       args->uhd_freq = atof(argv[optind]);
       break;
@@ -320,7 +320,7 @@ int main(int argc, char **argv) {
     cuhd_set_master_clock_rate(uhd, 30.72e6);        
 
     /* set receiver frequency */
-    cuhd_set_rx_freq_offset(uhd, (double) prog_args.uhd_freq, prog_args.uhd_freq_offset);
+    cuhd_set_rx_freq(uhd, (double) prog_args.uhd_freq);
     cuhd_rx_wait_lo_locked(uhd);
     printf("Tunning receiver to %.3f MHz\n", (double ) prog_args.uhd_freq/1000000);
 
@@ -373,7 +373,8 @@ int main(int argc, char **argv) {
     cell.nof_ports = prog_args.file_nof_ports; 
     cell.nof_prb = prog_args.file_nof_prb; 
     
-    if (srslte_ue_sync_init_file(&ue_sync, prog_args.file_nof_prb, prog_args.input_file_name, prog_args.file_offset)) {
+    if (srslte_ue_sync_init_file(&ue_sync, prog_args.file_nof_prb, 
+      prog_args.input_file_name, prog_args.file_offset_time, prog_args.file_offset_freq)) {
       fprintf(stderr, "Error initiating ue_sync\n");
       exit(-1); 
     }
@@ -485,7 +486,7 @@ int main(int argc, char **argv) {
               uint32_t rv = ((uint32_t) ceilf((float)1.5*k))%4;
               n = srslte_ue_dl_decode_rnti_rv(&ue_dl, &sf_buffer[prog_args.time_offset], data, 
                                               srslte_ue_sync_get_sfidx(&ue_sync), 
-                                              SRSLTE_SIRNTI, rv);             
+                                              SRSLTE_SIRNTI, rv);                         
             }
             if (n < 0) {
              // fprintf(stderr, "Error decoding UE DL\n");fflush(stdout);
diff --git a/srslte/include/srslte/common/phy_common.h b/srslte/include/srslte/common/phy_common.h
index c17675cfb..b6b89ae81 100644
--- a/srslte/include/srslte/common/phy_common.h
+++ b/srslte/include/srslte/common/phy_common.h
@@ -216,6 +216,8 @@ SRSLTE_API int srslte_nof_prb(uint32_t symbol_sz);
 
 SRSLTE_API int srslte_sampling_freq_hz(uint32_t nof_prb);
 
+SRSLTE_API void srslte_use_standard_symbol_size(bool enabled); 
+
 SRSLTE_API uint32_t srslte_re_x_prb(uint32_t ns, 
                                     uint32_t symbol, 
                                     uint32_t nof_ports, 
diff --git a/srslte/include/srslte/cuhd/cuhd.h b/srslte/include/srslte/cuhd/cuhd.h
index f36b2028c..dc0a008c7 100644
--- a/srslte/include/srslte/cuhd/cuhd.h
+++ b/srslte/include/srslte/cuhd/cuhd.h
@@ -63,6 +63,8 @@ SRSLTE_API bool cuhd_rx_wait_lo_locked(void *h);
 SRSLTE_API void cuhd_set_master_clock_rate(void *h, 
                                            double rate); 
 
+SRSLTE_API bool cuhd_is_master_clock_dynamic(void *h); 
+
 SRSLTE_API double cuhd_set_rx_srate(void *h, 
                                     double freq);
 
diff --git a/srslte/include/srslte/fec/viterbi.h b/srslte/include/srslte/fec/viterbi.h
index d31252e90..7493730f5 100644
--- a/srslte/include/srslte/fec/viterbi.h
+++ b/srslte/include/srslte/fec/viterbi.h
@@ -39,6 +39,8 @@
 #include <stdbool.h>
 #include "srslte/config.h"
 
+
+
 typedef enum {
   SRSLTE_VITERBI_27 = 0, 
   SRSLTE_VITERBI_29, 
@@ -82,4 +84,14 @@ SRSLTE_API int srslte_viterbi_decode_uc(srslte_viterbi_t *q,
                                         uint8_t *data, 
                                         uint32_t frame_length);
 
+
+
+SRSLTE_API int srslte_viterbi_init_sse(srslte_viterbi_t *q, 
+                                   srslte_viterbi_type_t type, 
+                                   uint32_t poly[3], 
+                                   uint32_t max_frame_length, 
+                                   bool tail_bitting);
+
+
+
 #endif
diff --git a/srslte/include/srslte/ue/ue_sync.h b/srslte/include/srslte/ue/ue_sync.h
index 4eea06d8e..e81c348d2 100644
--- a/srslte/include/srslte/ue/ue_sync.h
+++ b/srslte/include/srslte/ue/ue_sync.h
@@ -80,6 +80,8 @@ typedef struct SRSLTE_API {
   
   srslte_filesource_t file_source; 
   bool file_mode; 
+  float file_cfo; 
+  srslte_cfo_t file_cfo_correct; 
   
   srslte_ue_sync_state_t state;
   
@@ -124,7 +126,8 @@ SRSLTE_API int srslte_ue_sync_init(srslte_ue_sync_t *q,
 SRSLTE_API int srslte_ue_sync_init_file(srslte_ue_sync_t *q, 
                                         uint32_t nof_prb,
                                         char *file_name, 
-                                        int offset);
+                                        int offset_time, 
+                                        float offset_freq);
 
 SRSLTE_API void srslte_ue_sync_free(srslte_ue_sync_t *q);
 
diff --git a/srslte/lib/common/src/phy_common.c b/srslte/lib/common/src/phy_common.c
index de91a31a2..4987db530 100644
--- a/srslte/lib/common/src/phy_common.c
+++ b/srslte/lib/common/src/phy_common.c
@@ -34,8 +34,10 @@
 #include "srslte/common/phy_common.h"
 #include "srslte/common/sequence.h"
 
-#ifndef FORCE_STANDARD_RATE
-#define USE_REDUCED_SAMPLING_RATES
+#ifdef FORCE_STANDARD_RATE
+static bool use_standard_rates = true; 
+#else 
+static bool use_standard_rates = false; 
 #endif
 
 /* Returns true if the structure pointed by cell has valid parameters
@@ -186,6 +188,10 @@ uint32_t srslte_N_ta_new_rar(uint32_t ta) {
 }
 
 
+void srslte_use_standard_symbol_size(bool enabled) {
+  use_standard_rates = enabled;
+}
+
 int srslte_sampling_freq_hz(uint32_t nof_prb) {
     int n = srslte_symbol_sz(nof_prb); 
     if (n == -1) {
@@ -217,88 +223,87 @@ int srslte_symbol_sz(uint32_t nof_prb) {
   if (nof_prb<=0) {
     return SRSLTE_ERROR;
   }
-#ifdef USE_REDUCED_SAMPLING_RATES
-  if (nof_prb<=6) {
-    return 128;
-  } else if (nof_prb<=15) {
-    return 256;
-  } else if (nof_prb<=25) {
-    return 384;
-  } else if (nof_prb<=50) {
-    return 768;
-  } else if (nof_prb<=75) {
-    return 1024;
-  } else if (nof_prb<=100) {
-    return 1536;
+  if (!use_standard_rates) {
+    if (nof_prb<=6) {
+      return 128;
+    } else if (nof_prb<=15) {
+      return 256;
+    } else if (nof_prb<=25) {
+      return 384;
+    } else if (nof_prb<=50) {
+      return 768;
+    } else if (nof_prb<=75) {
+      return 1024;
+    } else if (nof_prb<=100) {
+      return 1536;
+    } else {
+      return SRSLTE_ERROR;
+    }
   } else {
-    return SRSLTE_ERROR;
+    return srslte_symbol_sz_power2(nof_prb);
   }
-#else
-  return srslte_symbol_sz_power2(nof_prb);
-#endif
 }
 
 int srslte_nof_prb(uint32_t symbol_sz)
 {
-#ifdef USE_REDUCED_SAMPLING_RATES
-  switch(symbol_sz) {
-    case 128:
-      return 6;
-    case 256:
-      return 15;
-    case 384:
-      return 25;
-    case 768:
-      return 50;
-    case 1024:
-      return 75;
-    case 1536:
-      return 100;
-  }
-#else 
-  switch(symbol_sz) {
-    case 128:
-      return 6;
-    case 256:
-      return 15;
-    case 512:
-      return 25;
-    case 1024:
-      return 50;
-    case 1536:
-      return 75;
-    case 2048:
-      return 100;
+  if (!use_standard_rates) {
+    switch(symbol_sz) {
+      case 128:
+        return 6;
+      case 256:
+        return 15;
+      case 384:
+        return 25;
+      case 768:
+        return 50;
+      case 1024:
+        return 75;
+      case 1536:
+        return 100;
+    }
+  } else {
+    switch(symbol_sz) {
+      case 128:
+        return 6;
+      case 256:
+        return 15;
+      case 512:
+        return 25;
+      case 1024:
+        return 50;
+      case 1536:
+        return 75;
+      case 2048:
+        return 100;
+    }
   }
-#endif
   return SRSLTE_ERROR;
 }
 
 bool srslte_symbol_sz_isvalid(uint32_t symbol_sz) {
-#ifdef USE_REDUCED_SAMPLING_RATES 
-  if (symbol_sz == 128  || 
-      symbol_sz == 256  ||
-      symbol_sz == 384  ||
-      symbol_sz == 768  ||
-      symbol_sz == 1024 ||
-      symbol_sz == 1536) {
-    return true;
-  } else {
-    return false; 
-  }
-#else
-  if (symbol_sz == 128  || 
-      symbol_sz == 256  ||
-      symbol_sz == 512  ||
-      symbol_sz == 1024 ||
-      symbol_sz == 1536 ||
-      symbol_sz == 2048) {
-    return true;
+  if (!use_standard_rates) {
+    if (symbol_sz == 128  || 
+        symbol_sz == 256  ||
+        symbol_sz == 384  ||
+        symbol_sz == 768  ||
+        symbol_sz == 1024 ||
+        symbol_sz == 1536) {
+      return true;
+    } else {
+      return false; 
+    }
   } else {
-    return false; 
-  }
-#endif
-  
+    if (symbol_sz == 128  || 
+        symbol_sz == 256  ||
+        symbol_sz == 512  ||
+        symbol_sz == 1024 ||
+        symbol_sz == 1536 ||
+        symbol_sz == 2048) {
+      return true;
+    } else {
+      return false; 
+    }
+  }  
 }
 
 uint32_t srslte_voffset(uint32_t symbol_id, uint32_t cell_id, uint32_t nof_ports) {
diff --git a/srslte/lib/cuhd/src/cuhd_handler.hpp b/srslte/lib/cuhd/src/cuhd_handler.hpp
index 9f0bc5a74..797488a1e 100644
--- a/srslte/lib/cuhd/src/cuhd_handler.hpp
+++ b/srslte/lib/cuhd/src/cuhd_handler.hpp
@@ -51,4 +51,5 @@ public:
   size_t rx_nof_samples;
   size_t tx_nof_samples;
   double tx_rate;
+  bool dynamic_rate; 
 };
diff --git a/srslte/lib/cuhd/src/cuhd_imp.cpp b/srslte/lib/cuhd/src/cuhd_imp.cpp
index 69af96069..e77c0e875 100644
--- a/srslte/lib/cuhd/src/cuhd_imp.cpp
+++ b/srslte/lib/cuhd/src/cuhd_imp.cpp
@@ -34,11 +34,10 @@
 
 #include "cuhd_handler.hpp"
 #include "srslte/cuhd/cuhd.h"
+#include "srslte/srslte.h"
 
 //#define METADATA_VERBOSE
 
-//#define HIDE_MESSAGES
-
 cuhd_msg_handler_t msg_handler;
 
 void suppress_handler(uhd::msg::type_t type, const std::string & msg)
@@ -218,20 +217,26 @@ void cuhd_register_msg_handler(cuhd_msg_handler_t h)
 
 int cuhd_open_(char *args, void **h, bool create_thread_gain, bool tx_gain_same_rx)
 {
+  
+  *h = NULL; 
+  
+  /* Set priority to UHD threads */
   uhd::set_thread_priority_safe();
+  
+  
+  /* Get multiusrp handler */
   cuhd_handler *handler = new cuhd_handler();
-  // Buffer sizes optimized for reduced clock rates (see common/phy_common.c)
   std::string _args = std::string(args);
-  handler->usrp = uhd::usrp::multi_usrp::make(_args + ", recv_frame_size=9232,num_recv_frames=64,send_frame_size=9232,num_send_frames=64");
-  handler->usrp->set_clock_source("internal");
+  handler->usrp = uhd::usrp::multi_usrp::make(_args);// + ", recv_frame_size=9232,num_recv_frames=64,send_frame_size=9232,num_send_frames=64");
     
+  /* Initialize rx and tx stremers */
   std::string otw, cpu;
   otw = "sc16";
   cpu = "fc32";
   uhd::stream_args_t stream_args(cpu, otw);
   handler->rx_stream = handler->usrp->get_rx_stream(stream_args);
   handler->tx_stream = handler->usrp->get_tx_stream(stream_args);
-
+  
   handler->rx_nof_samples = handler->rx_stream->get_max_num_samps();
   handler->tx_nof_samples = handler->tx_stream->get_max_num_samps();
   
@@ -240,9 +245,7 @@ int cuhd_open_(char *args, void **h, bool create_thread_gain, bool tx_gain_same_
   handler->rx_gain_range = handler->usrp->get_rx_gain_range();
   handler->tx_gain_range = handler->usrp->get_tx_gain_range();
 
-  
-  *h = handler;
-
+  /* Create auxiliary thread and mutexes for AGC */
   if (create_thread_gain) {
     if (pthread_mutex_init(&handler->mutex, NULL)) {
       return -1; 
@@ -251,12 +254,34 @@ int cuhd_open_(char *args, void **h, bool create_thread_gain, bool tx_gain_same_
       return -1; 
     }
 
-    if (pthread_create(&handler->thread_gain, NULL, thread_gain_fcn, *h)) {
+    if (pthread_create(&handler->thread_gain, NULL, thread_gain_fcn, handler)) {
       perror("pthread_create");
       return -1; 
     }
   }
   
+  /* Find out if the master clock rate is configurable */
+  double cur_clock = handler->usrp->get_master_clock_rate();
+  printf("Trying to dynamically change Master clock...\n");
+  handler->usrp->set_master_clock_rate(cur_clock/2);
+  if (handler->usrp->get_master_clock_rate() == cur_clock) {
+    handler->dynamic_rate = false; 
+    /* Master clock rate is not configurable. Check if it is compatible with LTE */
+    int cur_clock_i = (int) cur_clock;
+    if (cur_clock_i % 1920000) {
+      fprintf(stderr, "Error: LTE sampling rates are not supported. Master clock rate is %.1f MHz\n", cur_clock/1e6);
+      return -1; 
+    } else {
+      printf("Master clock is not configurable. Using standard symbol sizes and sampling rates.\n");
+      srslte_use_standard_symbol_size(true);
+    }
+  } else {
+    printf("Master clock is configurable. Using reduced symbol sizes and sampling rates.\n");
+    handler->dynamic_rate = true; 
+  }
+
+  *h = handler;
+
   return 0;
 }
 
@@ -278,7 +303,14 @@ int cuhd_close(void *h)
 
 void cuhd_set_master_clock_rate(void *h, double rate) {
   cuhd_handler *handler = static_cast < cuhd_handler * >(h);
-  handler->usrp->set_master_clock_rate(rate);    
+  if (handler->dynamic_rate) {
+    handler->usrp->set_master_clock_rate(rate);    
+  }
+}
+
+bool cuhd_is_master_clock_dynamic(void *h) {
+  cuhd_handler *handler = static_cast < cuhd_handler * >(h);
+  return handler->dynamic_rate;
 }
 
 double cuhd_set_rx_srate(void *h, double freq)
diff --git a/srslte/lib/fec/src/viterbi.c b/srslte/lib/fec/src/viterbi.c
index 4731e1f5b..fef653df2 100644
--- a/srslte/lib/fec/src/viterbi.c
+++ b/srslte/lib/fec/src/viterbi.c
@@ -38,6 +38,8 @@
 
 #define DEB 0
 
+//#undef LV_HAVE_SSE
+
 int decode37(void *o, uint8_t *symbols, uint8_t *data, uint32_t frame_length) {
   srslte_viterbi_t *q = o;
   uint32_t i;
@@ -54,16 +56,17 @@ int decode37(void *o, uint8_t *symbols, uint8_t *data, uint32_t frame_length) {
   init_viterbi37_port(q->ptr, q->tail_biting ? -1 : 0);
 
   /* Decode block */
+  uint8_t *tmp = q->tmp;
   if (q->tail_biting) {
-    memcpy(q->tmp, symbols, 3 * frame_length * sizeof(uint8_t));
+    memcpy(tmp, symbols, 3 * frame_length * sizeof(uint8_t));
     for (i = 0; i < 3 * (q->K - 1); i++) {
       q->tmp[i + 3 * frame_length] = q->tmp[i];
     }
   } else {
-    q->tmp = symbols;
+    tmp = symbols;
   }
 
-  update_viterbi37_blk_port(q->ptr, q->tmp, frame_length + q->K - 1,
+  update_viterbi37_blk_port(q->ptr, tmp, frame_length + q->K - 1,
       q->tail_biting ? &best_state : NULL);
 
   /* Do Viterbi chainback */
@@ -73,6 +76,57 @@ int decode37(void *o, uint8_t *symbols, uint8_t *data, uint32_t frame_length) {
   return q->framebits;
 }
 
+
+#ifdef LV_HAVE_SSE
+int decode37_sse(void *o, uint8_t *symbols, uint8_t *data, uint32_t frame_length) {
+  srslte_viterbi_t *q = o;
+  uint32_t i;
+
+  uint32_t best_state;
+
+  if (frame_length > q->framebits) {
+    fprintf(stderr, "Initialized decoder for max frame length %d bits\n",
+        q->framebits);
+    return -1;
+  }
+
+  /* Initialize Viterbi decoder */
+  init_viterbi37_sse(q->ptr, q->tail_biting ? -1 : 0);
+
+  /* Decode block */
+  uint8_t *tmp = q->tmp;
+  if (q->tail_biting) {
+    memcpy(tmp, symbols, 3 * frame_length * sizeof(uint8_t));
+    for (i = 0; i < 3 * (q->K - 1); i++) {
+      q->tmp[i + 3 * frame_length] = q->tmp[i];
+    }
+  } else {
+    tmp = symbols;
+  }
+
+  update_viterbi37_blk_sse(q->ptr, tmp, frame_length + q->K - 1,
+      q->tail_biting ? &best_state : NULL);
+
+  /* Do Viterbi chainback */
+  chainback_viterbi37_sse(q->ptr, data, frame_length,
+      q->tail_biting ? best_state : 0);
+
+  return q->framebits;
+}
+
+void free37_sse(void *o) {
+  srslte_viterbi_t *q = o;
+  if (q->symbols_uc) {
+    free(q->symbols_uc);
+  }
+  if (q->tmp) {
+    free(q->tmp);
+  }
+  delete_viterbi37_sse(q->ptr);
+}
+
+#endif
+
 void free37(void *o) {
   srslte_viterbi_t *q = o;
   if (q->symbols_uc) {
@@ -108,7 +162,7 @@ int init37(srslte_viterbi_t *q, uint32_t poly[3], uint32_t framebits, bool tail_
   } else {
     q->tmp = NULL;
   }
-
+  
   if ((q->ptr = create_viterbi37_port(poly, framebits)) == NULL) {
     fprintf(stderr, "create_viterbi37 failed\n");
     free37(q);
@@ -118,21 +172,68 @@ int init37(srslte_viterbi_t *q, uint32_t poly[3], uint32_t framebits, bool tail_
   }     
 }
 
+#ifdef LV_HAVE_SSE
+int init37_sse(srslte_viterbi_t *q, uint32_t poly[3], uint32_t framebits, bool tail_biting) {
+  q->K = 7;
+  q->R = 3;
+  q->framebits = framebits;
+  q->gain_quant = 20; 
+  q->tail_biting = tail_biting;
+  q->decode = decode37_sse;
+  q->free = free37_sse;
+  q->decode_f = NULL;
+  q->symbols_uc = srslte_vec_malloc(3 * (q->framebits + q->K - 1) * sizeof(uint8_t));
+  if (!q->symbols_uc) {
+    perror("malloc");
+    return -1;
+  }
+  if (q->tail_biting) {
+    q->tmp = srslte_vec_malloc(3 * (q->framebits + q->K - 1) * sizeof(uint8_t));
+    if (!q->tmp) {
+      perror("malloc");
+      free37(q);
+      return -1;
+    }
+  } else {
+    q->tmp = NULL;
+  }
+  
+  if ((q->ptr = create_viterbi37_sse(poly, framebits)) == NULL) {
+    fprintf(stderr, "create_viterbi37 failed\n");
+    free37(q);
+    return -1;
+  } else {
+    return 0;
+  }     
+}
+#endif
+
 void srslte_viterbi_set_gain_quant(srslte_viterbi_t *q, float gain_quant) {
   q->gain_quant = gain_quant;
 }
 
-int srslte_viterbi_init(srslte_viterbi_t *q, srslte_viterbi_type_t type, uint32_t poly[3],
-    uint32_t max_frame_length, bool tail_bitting) {
+int srslte_viterbi_init(srslte_viterbi_t *q, srslte_viterbi_type_t type, uint32_t poly[3], uint32_t max_frame_length, bool tail_bitting) 
+{
   switch (type) {
   case SRSLTE_VITERBI_37:
+#ifdef LV_HAVE_SSE
+    return init37_sse(q, poly, max_frame_length, tail_bitting);
+#else
     return init37(q, poly, max_frame_length, tail_bitting);
+#endif
   default:
     fprintf(stderr, "Decoder not implemented\n");
     return -1;
   }
 }
 
+#ifdef LV_HAVE_SSE
+int srslte_viterbi_init_sse(srslte_viterbi_t *q, srslte_viterbi_type_t type, uint32_t poly[3], uint32_t max_frame_length, bool tail_bitting) 
+{
+  return init37_sse(q, poly, max_frame_length, tail_bitting);      
+}
+#endif
+
 void srslte_viterbi_free(srslte_viterbi_t *q) {
   if (q->free) {
     q->free(q);    
@@ -141,7 +242,8 @@ void srslte_viterbi_free(srslte_viterbi_t *q) {
 }
 
 /* symbols are real-valued */
-int srslte_viterbi_decode_f(srslte_viterbi_t *q, float *symbols, uint8_t *data, uint32_t frame_length) {
+int srslte_viterbi_decode_f(srslte_viterbi_t *q, float *symbols, uint8_t *data, uint32_t frame_length) 
+{
   uint32_t len;
   if (frame_length > q->framebits) {
     fprintf(stderr, "Initialized decoder for max frame length %d bits\n",
@@ -154,16 +256,15 @@ int srslte_viterbi_decode_f(srslte_viterbi_t *q, float *symbols, uint8_t *data,
     len = 3 * (frame_length + q->K - 1);
   }
   if (!q->decode_f) {
-    srslte_vec_quant_fuc(symbols, q->symbols_uc, q->gain_quant, 127.5, 255, len);
-    return q->decode(q, q->symbols_uc, data, frame_length);    
+    srslte_vec_quant_fuc(symbols, q->symbols_uc, q->gain_quant, 127.5, 255, len);    
+    return srslte_viterbi_decode_uc(q, q->symbols_uc, data, frame_length);    
   } else {
     return q->decode_f(q, symbols, data, frame_length);
-  }
-  
-  
+  }  
 }
 
-int srslte_viterbi_decode_uc(srslte_viterbi_t *q, uint8_t *symbols, uint8_t *data,
-    uint32_t frame_length) {
+
+int srslte_viterbi_decode_uc(srslte_viterbi_t *q, uint8_t *symbols, uint8_t *data, uint32_t frame_length) 
+{
   return q->decode(q, symbols, data, frame_length);
 }
diff --git a/srslte/lib/fec/src/viterbi37.h b/srslte/lib/fec/src/viterbi37.h
index 28f9a60d0..dd325d7b4 100644
--- a/srslte/lib/fec/src/viterbi37.h
+++ b/srslte/lib/fec/src/viterbi37.h
@@ -30,7 +30,7 @@ void *create_viterbi37_port(uint32_t polys[3],
                             uint32_t len);
 
 int init_viterbi37_port(void *p, 
-                        uint32_t starting_state);
+                        int starting_state);
 
 int chainback_viterbi37_port(void *p, 
                              uint8_t *data, 
@@ -43,3 +43,22 @@ int update_viterbi37_blk_port(void *p,
                               uint8_t *syms, 
                               uint32_t nbits, 
                               uint32_t *best_state);
+
+
+void *create_viterbi37_sse(uint32_t polys[3], 
+                            uint32_t len);
+
+int init_viterbi37_sse(void *p, 
+                        int starting_state);
+
+int chainback_viterbi37_sse(void *p, 
+                             uint8_t *data, 
+                             uint32_t nbits, 
+                             uint32_t endstate);
+
+void delete_viterbi37_sse(void *p);
+
+int update_viterbi37_blk_sse(void *p, 
+                              uint8_t *syms, 
+                              uint32_t nbits, 
+                              uint32_t *best_state);
diff --git a/srslte/lib/fec/src/viterbi37_port.c b/srslte/lib/fec/src/viterbi37_port.c
index f7644f345..c55d95b4e 100644
--- a/srslte/lib/fec/src/viterbi37_port.c
+++ b/srslte/lib/fec/src/viterbi37_port.c
@@ -1,8 +1,8 @@
-/* Adapted Viterbi Phil Karn's r=1/3 k=9 viterbi decoder to r=1/3 k=7
+/* Adapted Phil Karn's r=1/3 k=9 viterbi decoder to r=1/3 k=7
  * 
  * K=9 r=1/3 Viterbi decoder in portable C
  * Copyright Aug 2006, Phil Karn, KA9Q
- * May be used under the terms of the GNU Affero General Public License (LGPL)
+ * May be used under the terms of the GNU Lesser General Public License (LGPL)
  */
 #include <stdio.h>
 #include <stdlib.h>
@@ -13,6 +13,8 @@
 #include "parity.h"
 #include <limits.h>
 
+//#define DEBUG
+
 typedef union {
   uint32_t w[64];
 } metric_t;
@@ -21,7 +23,7 @@ typedef union {
 } decision_t;
 
 static union {
-  uint8_t c[128];
+  uint8_t c[32];
 } Branchtab37[3];
 
 /* State info for instance of Viterbi decoder */
@@ -34,7 +36,7 @@ struct v37 {
 };
 
 /* Initialize Viterbi decoder for start of new frame */
-int init_viterbi37_port(void *p, uint32_t starting_state) {
+int init_viterbi37_port(void *p, int starting_state) {
   struct v37 *vp = p;
   uint32_t i;
 
@@ -112,6 +114,9 @@ int chainback_viterbi37_port(void *p, uint8_t *data, /* Decoded output data */
     k = (d[nbits].w[(endstate >> 2) / 32] >> ((endstate >> 2) % 32)) & 1;
     endstate = (endstate >> 1) | (k << 7);
     data[nbits] = k;
+#ifdef DEBUG
+ //   printf("endstate=%3d, k=%d, w[0]=%d, w[1]=%d\n", endstate, k, d[nbits].w[0]&1, d[nbits].w[1]&1);
+#endif
   }
   return 0;
 }
@@ -156,6 +161,11 @@ int update_viterbi37_blk_port(void *p, uint8_t *syms, uint32_t nbits, uint32_t *
     return -1;
   uint32_t k=0;
   d = (decision_t *) vp->dp;
+  
+#ifdef DEBUG
+  printf("[");
+#endif
+  
   while (nbits--) {
     void *tmp;
     uint8_t sym0, sym1, sym2;
@@ -170,7 +180,20 @@ int update_viterbi37_blk_port(void *p, uint8_t *syms, uint32_t nbits, uint32_t *
     k++;
     for (i = 0; i < 32; i++)
       BFLY(i);
+    
+#ifdef DEBUG
+    uint32_t wmin=UINT_MAX;
+    int minstate = 0; 
+    for (int j=0;j<64;j++) {
+      if (vp->new_metrics->w[j] <= wmin) {
+        wmin = vp->new_metrics->w[j];
+        minstate = j; 
+      }
+    }
 
+    printf("%3d, ", minstate);
+#endif
+    
     d++;
     tmp = vp->old_metrics;
     vp->old_metrics = vp->new_metrics;
@@ -188,5 +211,10 @@ int update_viterbi37_blk_port(void *p, uint8_t *syms, uint32_t nbits, uint32_t *
     *best_state = bst;
   }
   vp->dp = d;
+
+#ifdef DEBUG
+  printf("];\n");
+#endif
+
   return 0;
 }
diff --git a/srslte/lib/fec/src/viterbi37_sse.c b/srslte/lib/fec/src/viterbi37_sse.c
new file mode 100644
index 000000000..39c4bf5e2
--- /dev/null
+++ b/srslte/lib/fec/src/viterbi37_sse.c
@@ -0,0 +1,294 @@
+/* Adapted Phil Karn's r=1/3 k=9 viterbi decoder to r=1/3 k=7
+ * 
+ * K=15 r=1/6 Viterbi decoder for x86 SSE2
+ * Copyright Mar 2004, Phil Karn, KA9Q
+ * May be used under the terms of the GNU Lesser General Public License (LGPL)
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <memory.h>
+#include <limits.h>
+#include "parity.h"
+
+//#define DEBUG
+
+#ifdef LV_HAVE_SSE
+
+#include <emmintrin.h>
+
+typedef union { 
+  unsigned char c[64]; 
+  __m128i       v[4];   
+} metric_t;
+typedef union { 
+  unsigned long  w[2]; 
+  unsigned char  c[8]; 
+  unsigned short s[4]; 
+  __m64 v[1];  
+} decision_t;
+
+union branchtab27 { 
+  unsigned char c[32]; 
+  __m128i       v[2];  
+} Branchtab37_sse2[3];
+
+
+/* State info for instance of Viterbi decoder */
+struct v37 {
+  metric_t metrics1; /* path metric buffer 1 */
+  metric_t metrics2; /* path metric buffer 2 */
+  decision_t *dp; /* Pointer to current decision */
+  metric_t *old_metrics,*new_metrics; /* Pointers to path metrics, swapped on every bit */
+  decision_t *decisions; /* Beginning of decisions for block */
+};
+
+void set_viterbi37_polynomial_sse(uint32_t polys[3]) {
+  int state;
+
+  for(state=0;state < 32;state++){
+    Branchtab37_sse2[0].c[state] = (polys[0] < 0) ^ parity((2*state) & polys[0]) ? 255:0;
+    Branchtab37_sse2[1].c[state] = (polys[1] < 0) ^ parity((2*state) & polys[1]) ? 255:0;
+    Branchtab37_sse2[2].c[state] = (polys[2] < 0) ^ parity((2*state) & polys[2]) ? 255:0;
+  }
+}
+
+
+/* Initialize Viterbi decoder for start of new frame */
+int init_viterbi37_sse(void *p, int starting_state) {
+  struct v37 *vp = p;
+  uint32_t i;
+
+  for(i=0;i<64;i++)
+    vp->metrics1.c[i] = 63;
+
+  vp->old_metrics = &vp->metrics1;
+  vp->new_metrics = &vp->metrics2;
+  vp->dp = vp->decisions;
+  if (starting_state != -1) {
+    vp->old_metrics->c[starting_state & 63] = 0; /* Bias known start state */
+  }
+  return 0;
+}
+
+/* Create a new instance of a Viterbi decoder */
+void *create_viterbi37_sse(uint32_t polys[3], uint32_t len) {
+  void *p;
+  struct v37 *vp;
+
+  set_viterbi37_polynomial_sse(polys);
+  
+  /* Ordinary malloc() only returns 8-byte alignment, we need 16 */
+  if(posix_memalign(&p, sizeof(__m128i),sizeof(struct v37)))
+    return NULL;
+
+  vp = (struct v37 *)p;
+  if(posix_memalign(&p, sizeof(__m128i),(len+6)*sizeof(decision_t))) {
+    free(vp);
+    return NULL;
+  }
+  vp->decisions = (decision_t *)p;
+  return vp;
+}
+
+
+/* Viterbi chainback */
+int chainback_viterbi37_sse(
+      void *p,
+      uint8_t *data, /* Decoded output data */
+      uint32_t nbits, /* Number of data bits */
+      uint32_t endstate) { /* Terminal encoder state */
+  struct v37 *vp = p;
+  
+  if (p == NULL)
+    return -1;
+
+  decision_t *d = (decision_t *)vp->decisions;
+
+  /* Make room beyond the end of the encoder register so we can
+   * accumulate a full byte of decoded data
+   */
+  endstate %= 64;
+  endstate <<= 2;
+
+  /* The store into data[] only needs to be done every 8 bits.
+   * But this avoids a conditional branch, and the writes will
+   * combine in the cache anyway
+   */
+  d += 6; /* Look past tail */
+  while(nbits-- != 0){
+    int k;
+
+    k = (d[nbits].c[(endstate>>2)/8] >> ((endstate>>2)%8)) & 1;
+    endstate = (endstate >> 1) | (k << 7);
+    data[nbits] = k;
+#ifdef DEBUG
+//    printf("endstate=%3d, k=%d, w[0]=%d, w[1]=%d\n", endstate, k, d[nbits].s[1]&1, d[nbits].s[2]&1);
+#endif
+  }
+  return 0;
+}
+
+/* Delete instance of a Viterbi decoder */
+void delete_viterbi37_sse(void *p){
+  struct v37 *vp = p;
+
+  if(vp != NULL){
+    free(vp->decisions);
+    free(vp);
+  }
+}
+
+void print_128i(char *s, __m128i val) {
+  
+  printf("%s: ", s);
+  
+  uint8_t *x = (uint8_t*) &val; 
+  for (int i=0;i<16;i++) {
+    printf("%3d, ", x[i]);
+  }
+  printf("\n");
+}
+
+void update_viterbi37_blk_sse(void *p,unsigned char *syms,int nbits, uint32_t *best_state) {
+  struct v37 *vp = p;
+  decision_t *d;
+
+  if(p == NULL)
+    return;
+  
+#ifdef DEBUG
+  printf("[");
+#endif
+  
+  d = (decision_t *) vp->dp;
+  while(nbits--) {
+    __m128i sym0v,sym1v,sym2v;
+    void *tmp;
+    int i;
+    
+    /* Splat the 0th symbol across sym0v, the 1st symbol across sym1v, etc */
+    sym0v = _mm_set1_epi8(syms[0]);
+    sym1v = _mm_set1_epi8(syms[1]);
+    sym2v = _mm_set1_epi8(syms[2]);
+    syms += 3;
+
+    for(i=0;i<2;i++){
+      __m128i decision0,decision1,metric,m_metric,m0,m1,m2,m3,survivor0,survivor1;
+
+      /* Form branch metrics */
+      m0 = _mm_avg_epu8(_mm_xor_si128(Branchtab37_sse2[0].v[i],sym0v),_mm_xor_si128(Branchtab37_sse2[1].v[i],sym1v));
+      metric = _mm_avg_epu8(_mm_xor_si128(Branchtab37_sse2[2].v[i],sym2v),m0);
+
+#ifdef DEBUG
+      print_128i("metric_initial", metric);
+#endif
+      /* There's no packed bytes right shift in SSE2, so we use the word version and mask
+       * (I'm *really* starting to like Altivec...)
+       */
+      metric = _mm_srli_epi16(metric,3);
+      metric = _mm_and_si128(metric,_mm_set1_epi8(31));
+      m_metric = _mm_sub_epi8(_mm_set1_epi8(31),metric);
+    
+#ifdef DEBUG
+      print_128i("metric        ", metric);
+      print_128i("m_metric      ", m_metric);
+#endif
+      
+      /* Add branch metrics to path metrics */
+      m0 = _mm_add_epi8(vp->old_metrics->v[i],metric);
+      m3 = _mm_add_epi8(vp->old_metrics->v[2+i],metric);
+      m1 = _mm_add_epi8(vp->old_metrics->v[2+i],m_metric);
+      m2 = _mm_add_epi8(vp->old_metrics->v[i],m_metric);
+    
+      /* Compare and select, using modulo arithmetic */
+      decision0 = _mm_cmpgt_epi8(_mm_sub_epi8(m0,m1),_mm_setzero_si128());
+      decision1 = _mm_cmpgt_epi8(_mm_sub_epi8(m2,m3),_mm_setzero_si128());
+      survivor0 = _mm_or_si128(_mm_and_si128(decision0,m1),_mm_andnot_si128(decision0,m0));
+      survivor1 = _mm_or_si128(_mm_and_si128(decision1,m3),_mm_andnot_si128(decision1,m2));
+ 
+      /* Pack each set of decisions into 16 bits */
+      d->s[2*i] = _mm_movemask_epi8(_mm_unpacklo_epi8(decision0,decision1));
+      d->s[2*i+1] = _mm_movemask_epi8(_mm_unpackhi_epi8(decision0,decision1));
+
+      /* Store surviving metrics */
+      vp->new_metrics->v[2*i] = _mm_unpacklo_epi8(survivor0,survivor1);
+      vp->new_metrics->v[2*i+1] = _mm_unpackhi_epi8(survivor0,survivor1);
+        
+    }
+
+
+#ifdef DEBUG      
+      uint8_t wmin=UINT8_MAX;
+      int minstate = 0; 
+      printf("[%d]: ", nbits);
+      for (int j=0;j<64;j++) {
+        printf("%d, ", vp->new_metrics->c[j]);
+        if (vp->new_metrics->c[j] <= wmin) {
+          wmin = vp->new_metrics->c[j];
+          minstate = j; 
+        }
+      }
+      printf("\n");
+      printf("%3d, ",minstate);
+#endif
+
+    // See if we need to normalize
+    if (vp->new_metrics->c[0] > 100) {
+      int i;
+      uint8_t adjust; 
+      __m128i adjustv;
+      union { __m128i v; signed short w[8]; } t;
+      
+      adjustv = vp->new_metrics->v[0];
+      for(i=1;i<4;i++) {
+        adjustv = _mm_min_epu8(adjustv,vp->new_metrics->v[i]);
+      }
+      
+      adjustv = _mm_min_epu8(adjustv,_mm_srli_si128(adjustv,8));
+      adjustv = _mm_min_epu8(adjustv,_mm_srli_si128(adjustv,4));
+      adjustv = _mm_min_epu8(adjustv,_mm_srli_si128(adjustv,2));
+      
+      t.v = adjustv;
+      adjust = t.w[0];
+      adjustv = _mm_set1_epi8(adjust);
+      
+      /* We cannot use a saturated subtract, because we often have to adjust by more than SHRT_MAX
+       * This is okay since it can't overflow anyway
+       */
+      for(i=0;i<4;i++)
+        vp->new_metrics->v[i] = _mm_sub_epi8(vp->new_metrics->v[i],adjustv);
+    }
+
+
+    d++;
+    /* Swap pointers to old and new metrics */
+    tmp = vp->old_metrics;
+    vp->old_metrics = vp->new_metrics;
+    vp->new_metrics = tmp;
+  }
+  
+  if (best_state) {
+    uint32_t i, bst=0;
+    uint8_t minmetric=UINT8_MAX;
+    for (i=0;i<64;i++) {
+      if (vp->old_metrics->c[i] <= minmetric) {
+        bst = i;
+        minmetric = vp->old_metrics->c[i];
+      }
+    }
+    *best_state = bst;
+  }
+
+  #ifdef DEBUG
+  printf("];\n===========================================\n");
+#endif
+
+  vp->dp = d;
+}
+
+#endif
+
+
+
diff --git a/srslte/lib/fec/test/CMakeLists.txt b/srslte/lib/fec/test/CMakeLists.txt
index b76a67152..b87076ec9 100644
--- a/srslte/lib/fec/test/CMakeLists.txt
+++ b/srslte/lib/fec/test/CMakeLists.txt
@@ -61,15 +61,15 @@ ADD_TEST(turbocoder_test_all turbocoder_test)
 ADD_EXECUTABLE(viterbi_test viterbi_test.c)
 TARGET_LINK_LIBRARIES(viterbi_test srslte)
 
-ADD_TEST(viterbi_40_0 viterbi_test -n 1000 -s 1 -l 40 -k 7 -t -e 0.0) 
-ADD_TEST(viterbi_40_2 viterbi_test -n 1000 -s 1 -l 40 -k 7 -t -e 2.0) 
-ADD_TEST(viterbi_40_3 viterbi_test -n 1000 -s 1 -l 40 -k 7 -t -e 3.0)
-ADD_TEST(viterbi_40_4 viterbi_test -n 1000 -s 1 -l 40 -k 7 -t -e 4.5)
-
-ADD_TEST(viterbi_1000_0 viterbi_test -n 100 -s 1 -l 1000 -k 7 -t -e 0.0) 
-ADD_TEST(viterbi_1000_2 viterbi_test -n 100 -s 1 -l 1000 -k 7 -t -e 2.0) 
-ADD_TEST(viterbi_1000_3 viterbi_test -n 100 -s 1 -l 1000 -k 7 -t -e 3.0)
-ADD_TEST(viterbi_1000_4 viterbi_test -n 100 -s 1 -l 1000 -k 7 -t -e 4.5)
+ADD_TEST(viterbi_40_0 viterbi_test -n 1000 -s 1 -l 40 -t -e 0.0) 
+ADD_TEST(viterbi_40_2 viterbi_test -n 1000 -s 1 -l 40 -t -e 2.0) 
+ADD_TEST(viterbi_40_3 viterbi_test -n 1000 -s 1 -l 40 -t -e 3.0)
+ADD_TEST(viterbi_40_4 viterbi_test -n 1000 -s 1 -l 40 -t -e 4.5)
+
+ADD_TEST(viterbi_1000_0 viterbi_test -n 100 -s 1 -l 1000 -t -e 0.0) 
+ADD_TEST(viterbi_1000_2 viterbi_test -n 100 -s 1 -l 1000 -t -e 2.0) 
+ADD_TEST(viterbi_1000_3 viterbi_test -n 100 -s 1 -l 1000 -t -e 3.0)
+ADD_TEST(viterbi_1000_4 viterbi_test -n 100 -s 1 -l 1000 -t -e 4.5)
 
 BuildMex(MEXNAME viterbi SOURCES viterbi_test_mex.c LIBRARIES srslte srslte_mex)
 
diff --git a/srslte/lib/fec/test/viterbi_test.c b/srslte/lib/fec/test/viterbi_test.c
index 06c47436f..9354e226e 100644
--- a/srslte/lib/fec/test/viterbi_test.c
+++ b/srslte/lib/fec/test/viterbi_test.c
@@ -37,33 +37,27 @@
 #include "viterbi_test.h"
 
 
-
 int frame_length = 1000, nof_frames = 128;
 float ebno_db = 100.0;
 uint32_t seed = 0;
 bool tail_biting = false;
-int K = -1;
 
 #define SNR_POINTS  10
 #define SNR_MIN    0.0
 #define SNR_MAX    5.0
 
-#define NCODS    3
-#define NTYPES    1+NCODS
-
 void usage(char *prog) {
-  printf("Usage: %s [nlestk]\n", prog);
+  printf("Usage: %s [nlest]\n", prog);
   printf("\t-n nof_frames [Default %d]\n", nof_frames);
   printf("\t-l frame_length [Default %d]\n", frame_length);
   printf("\t-e ebno in dB [Default scan]\n");
   printf("\t-s seed [Default 0=time]\n");
   printf("\t-t tail_bitting [Default %s]\n", tail_biting ? "yes" : "no");
-  printf("\t-k constraint length [Default both]\n", K);
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "nlstek")) != -1) {
+  while ((opt = getopt(argc, argv, "nlste")) != -1) {
     switch (opt) {
     case 'n':
       nof_frames = atoi(argv[optind]);
@@ -80,9 +74,6 @@ void parse_args(int argc, char **argv) {
     case 't':
       tail_biting = true;
       break;
-    case 'k':
-      K = atoi(argv[optind]);
-      break;
     default:
       usage(argv[0]);
       exit(-1);
@@ -90,49 +81,22 @@ void parse_args(int argc, char **argv) {
   }
 }
 
-void output_matlab(float ber[NTYPES][SNR_POINTS], int snr_points,
-    srslte_convcoder_t cod[NCODS], int ncods) {
-  int i, j, n;
-  FILE *f = fopen("srslte_viterbi_snr.m", "w");
-  if (!f) {
-    perror("fopen");
-    exit(-1);
-  }
-  fprintf(f, "ber=[");
-  for (j = 0; j < NTYPES; j++) {
-    for (i = 0; i < snr_points; i++) {
-      fprintf(f, "%g ", ber[j][i]);
-    }
-    fprintf(f, "; ");
-  }
-  fprintf(f, "];\n");
-  fprintf(f, "snr=linspace(%g,%g-%g/%d,%d);\n", SNR_MIN, SNR_MAX, SNR_MAX,
-      snr_points, snr_points);
-  fprintf(f, "semilogy(snr,ber,snr,0.5*erfc(sqrt(10.^(snr/10))));\n");
-  fprintf(f, "legend('uncoded',");
-  for (n=0;n<ncods;n++) {
-    fprintf(f,"'1/3 K=%d%s',",cod[n].K,cod[n].tail_biting?" tb":"");
-  }
-  fprintf(f,"'theory-uncoded');");
-  fprintf(f, "grid on;\n");
-  fclose(f);
-}
-
 int main(int argc, char **argv) {
   int frame_cnt;
   float *llr;
   uint8_t *llr_c;
-  uint8_t *data_tx, *data_rx[NTYPES], *symbols;
+  uint8_t *data_tx, *data_rx, *data_rx2, *symbols;
   int i, j;
   float var[SNR_POINTS], varunc[SNR_POINTS];
   int snr_points;
-  float ber[NTYPES][SNR_POINTS];
-  uint32_t errors[NTYPES];
-  srslte_viterbi_type_t srslte_viterbi_type[NCODS];
-  srslte_viterbi_t dec[NCODS];
-  srslte_convcoder_t cod[NCODS];
-  int coded_length[NCODS];
-  int n, ncods, max_coded_length;
+  uint32_t errors;
+#ifdef TEST_SSE
+  uint32_t errors2;
+  srslte_viterbi_t dec_sse;
+#endif
+  srslte_viterbi_t dec; 
+  srslte_convcoder_t cod;
+  int coded_length;
 
 
   parse_args(argc, argv);
@@ -142,58 +106,21 @@ int main(int argc, char **argv) {
   }
   srand(seed);
 
-  switch (K) {
-  case 9:
-    cod[0].poly[0] = 0x1ed;
-    cod[0].poly[1] = 0x19b;
-    cod[0].poly[2] = 0x127;
-    cod[0].tail_biting = false;
-    cod[0].K = 9;
-    srslte_viterbi_type[0] = SRSLTE_VITERBI_39;
-    ncods=1;
-    break;
-  case 7:
-    cod[0].poly[0] = 0x6D;
-    cod[0].poly[1] = 0x4F;
-    cod[0].poly[2] = 0x57;
-    cod[0].K = 7;
-    cod[0].tail_biting = tail_biting;
-    srslte_viterbi_type[0] = SRSLTE_VITERBI_37;
-    ncods=1;
-    break;
-  default:
-    cod[0].poly[0] = 0x1ed;
-    cod[0].poly[1] = 0x19b;
-    cod[0].poly[2] = 0x127;
-    cod[0].tail_biting = false;
-    cod[0].K = 9;
-    srslte_viterbi_type[0] = SRSLTE_VITERBI_39;
-    cod[1].poly[0] = 0x6D;
-    cod[1].poly[1] = 0x4F;
-    cod[1].poly[2] = 0x57;
-    cod[1].tail_biting = false;
-    cod[1].K = 7;
-    srslte_viterbi_type[1] = SRSLTE_VITERBI_37;
-    cod[2].poly[0] = 0x6D;
-    cod[2].poly[1] = 0x4F;
-    cod[2].poly[2] = 0x57;
-    cod[2].tail_biting = true;
-    cod[2].K = 7;
-    srslte_viterbi_type[2] = SRSLTE_VITERBI_37;
-    ncods=3;
-  }
-
-  max_coded_length = 0;
-  for (i=0;i<ncods;i++) {
-    cod[i].R = 3;
-    coded_length[i] = cod[i].R * (frame_length + ((cod[i].tail_biting) ? 0 : cod[i].K - 1));
-    if (coded_length[i] > max_coded_length) {
-      max_coded_length = coded_length[i];
-    }
-    srslte_viterbi_init(&dec[i], srslte_viterbi_type[i], cod[i].poly, frame_length, cod[i].tail_biting);
-    printf("Convolutional Code 1/3 K=%d Tail bitting: %s\n", cod[i].K, cod[i].tail_biting ? "yes" : "no");
-  }
-
+  cod.poly[0] = 0x6D;
+  cod.poly[1] = 0x4F;
+  cod.poly[2] = 0x57;
+  cod.K = 7;
+  cod.tail_biting = tail_biting;
+
+  cod.R = 3;
+  coded_length = cod.R * (frame_length + ((cod.tail_biting) ? 0 : cod.K - 1));
+  srslte_viterbi_init(&dec, SRSLTE_VITERBI_37, cod.poly, frame_length, cod.tail_biting);
+  printf("Convolutional Code 1/3 K=%d Tail bitting: %s\n", cod.K, cod.tail_biting ? "yes" : "no");  
+
+#ifdef TEST_SSE
+  srslte_viterbi_init_sse(&dec_sse, SRSLTE_VITERBI_37, cod.poly, frame_length, cod.tail_biting);
+#endif  
+  
   printf("  Frame length: %d\n", frame_length);
   if (ebno_db < 100.0) {
     printf("  EbNo: %.2f\n", ebno_db);
@@ -205,25 +132,29 @@ int main(int argc, char **argv) {
     exit(-1);
   }
 
-  for (i = 0; i < NTYPES; i++) {
-    data_rx[i] = malloc(frame_length * sizeof(uint8_t));
-    if (!data_rx[i]) {
-      perror("malloc");
-      exit(-1);
-    }
+  data_rx = malloc(frame_length * sizeof(uint8_t));
+  if (!data_rx) {
+    perror("malloc");
+    exit(-1);
   }
 
-  symbols = malloc(max_coded_length * sizeof(uint8_t));
+  data_rx2 = malloc(frame_length * sizeof(uint8_t));
+  if (!data_rx2) {
+    perror("malloc");
+    exit(-1);
+  }
+
+  symbols = malloc(coded_length * sizeof(uint8_t));
   if (!symbols) {
     perror("malloc");
     exit(-1);
   }
-  llr = malloc(max_coded_length * sizeof(float));
+  llr = malloc(coded_length * sizeof(float));
   if (!llr) {
     perror("malloc");
     exit(-1);
   }
-  llr_c = malloc(2 * max_coded_length * sizeof(uint8_t));
+  llr_c = malloc(2 * coded_length * sizeof(uint8_t));
   if (!llr_c) {
     perror("malloc");
     exit(-1);
@@ -250,9 +181,10 @@ int main(int argc, char **argv) {
 
   for (i = 0; i < snr_points; i++) {
     frame_cnt = 0;
-    for (j = 0; j < NTYPES; j++) {
-      errors[j] = 0;
-    }
+    errors = 0;
+#ifdef TEST_SSE
+    errors2 = 0;
+#endif
     while (frame_cnt < nof_frames) {
 
       /* generate data_tx */
@@ -265,76 +197,84 @@ int main(int argc, char **argv) {
         llr[j] = data_tx[j] ? sqrt(2) : -sqrt(2);
       }
       srslte_ch_awgn_f(llr, llr, varunc[i], frame_length);
-      for (j = 0; j < frame_length; j++) {
-        data_rx[0][j] = llr[j] > 0 ? 1 : 0;
-      }
 
       /* coded BER */
-      for (n=0;n<ncods;n++) {
-        srslte_convcoder_encode(&cod[n], data_tx, symbols, frame_length);
+      srslte_convcoder_encode(&cod, data_tx, symbols, frame_length);
 
-        for (j = 0; j < coded_length[n]; j++) {
-          llr[j] = symbols[j] ? sqrt(2) : -sqrt(2);
-        }
+      for (j = 0; j < coded_length; j++) {
+        llr[j] = symbols[j] ? sqrt(2) : -sqrt(2);
+      }
 
-        srslte_ch_awgn_f(llr, llr, var[i], coded_length[n]);
-        srslte_vec_quant_fuc(llr, llr_c, Gain, 127.5, 255, coded_length[n]);
+      srslte_ch_awgn_f(llr, llr, var[i], coded_length);
+      srslte_vec_quant_fuc(llr, llr_c, Gain, 127.5, 255, coded_length);
 
-        /* decoder 1 */
-        srslte_viterbi_decode_uc(&dec[n], llr_c, data_rx[1+n], frame_length);
+
+      struct timeval t[3];
+      gettimeofday(&t[1], NULL);
+      int M = 1; 
+      for (int i=0;i<M;i++) {
+        srslte_viterbi_decode_uc(&dec, llr_c, data_rx, frame_length);      
+      }
+      
+#ifdef TEST_SSE
+      gettimeofday(&t[2], NULL);
+      get_time_interval(t);
+      //printf("Execution time:\t\t%.1f us\n", (float) t[0].tv_usec/M);
+      gettimeofday(&t[1], NULL);
+      for (int i=0;i<M;i++) {
+        srslte_viterbi_decode_uc(&dec_sse, llr_c, data_rx2, frame_length);      
       }
+      gettimeofday(&t[2], NULL);
+      get_time_interval(t);
+      //printf("Execution time SIMD:\t%.1f us\n", (float) t[0].tv_usec/M);
+#endif
 
       /* check errors */
-      for (j = 0; j < 1+ncods; j++) {
-        errors[j] += srslte_bit_diff(data_tx, data_rx[j], frame_length);
-      }
+      errors += srslte_bit_diff(data_tx, data_rx, frame_length);
+#ifdef TEST_SSE
+      errors2 += srslte_bit_diff(data_tx, data_rx2, frame_length);
+#endif      
       frame_cnt++;
-      printf("Eb/No: %3.2f %10d/%d   ",
-          SNR_MIN + i * ebno_inc,frame_cnt,nof_frames);
-      for (n=0;n<1+ncods;n++) {
-        printf("BER: %.2e  ",(float) errors[n] / (frame_cnt * frame_length));
-      }
+      printf("Eb/No: %3.2f %10d/%d   ", SNR_MIN + i * ebno_inc,frame_cnt,nof_frames);
+      printf("BER: %.2e  ", (float) errors / (frame_cnt * frame_length));
+#ifdef TEST_SSE
+      printf("BER2: %.2e  ", (float) errors2 / (frame_cnt * frame_length));
+#endif
       printf("\r");
     }
     printf("\n");
-    for (j = 0; j < 1+ncods; j++) {
-      ber[j][i] = (float) errors[j] / (frame_cnt * frame_length);
-    }
-
+    
     if (snr_points == 1) {
-          printf("BER uncoded: %g\t%u errors\n",
-          (float) errors[0] / (frame_cnt * frame_length), errors[0]);
-      for (n=0;n<ncods;n++) {
-        printf("BER K=%d:    %g\t%u errors\n",cod[n].K,
-            (float) errors[1+n] / (frame_cnt * frame_length), errors[1+n]);
-      }
+      printf("BER    :    %g\t%u errors\n", (float) errors / (frame_cnt * frame_length), errors);      
+#ifdef TEST_SSE
+      printf("BER SSE:    %g\t%u errors\n", (float) errors2 / (frame_cnt * frame_length), errors2);      
+#endif
+      
     }
   }
-  for (n=0;n<ncods;n++) {
-    srslte_viterbi_free(&dec[n]);
-  }
-
+  srslte_viterbi_free(&dec);
+#ifdef TEST_SSE  
+  srslte_viterbi_free(&dec_sse);
+#endif
+  
   free(data_tx);
   free(symbols);
   free(llr);
   free(llr_c);
-  for (i = 0; i < NTYPES; i++) {
-    free(data_rx[i]);
-  }
-
+  free(data_rx);
+  
   if (snr_points == 1) {
     int expected_errors = get_expected_errors(nof_frames,
-        seed, frame_length, K, tail_biting, ebno_db);
+        seed, frame_length, tail_biting, ebno_db);
     if (expected_errors == -1) {
       fprintf(stderr, "Test parameters not defined in test_results.h\n");
       exit(-1);
     } else {
-      printf("errors =%d, expected =%d\n", errors[1], expected_errors);
-      exit(errors[1] > expected_errors);
+      printf("errors =%d, expected =%d\n", errors, expected_errors);
+      exit(errors > expected_errors);
     }
   } else {
     printf("\n");
-    output_matlab(ber, snr_points, cod, ncods);
     printf("Done\n");
     exit(0);
   }
diff --git a/srslte/lib/fec/test/viterbi_test.h b/srslte/lib/fec/test/viterbi_test.h
index 9c9e5d6ab..7d94e7c7e 100644
--- a/srslte/lib/fec/test/viterbi_test.h
+++ b/srslte/lib/fec/test/viterbi_test.h
@@ -30,35 +30,52 @@ typedef struct {
   int n;
   uint32_t s;
   int len;
-  int k;
   bool tail;
   float ebno;
   int errors;
 }expected_errors_t;
 
+/* The SSE implementation uses 5-bit metrics and has 0.75 dB loss approximation */
+#ifdef LV_HAVE_SSE
 
 static expected_errors_t expected_errors[] = {
-    {1000, 1, 40, 7, true, 0.0, 5363},
-    {1000, 1, 40, 7, true, 2.0, 356},
-    {1000, 1, 40, 7, true, 3.0, 48},
-    {1000, 1, 40, 7, true, 4.5, 0},
+    {1000, 1, 40, true, 0.0, 7282},
+    {1000, 1, 40, true, 2.0, 725},
+    {1000, 1, 40, true, 3.0, 176},
+    {1000, 1, 40, true, 4.5, 24},
 
-    {100, 1, 1000, 7, true, 0.0, 8753},
-    {100, 1, 1000, 7, true, 2.0, 350},
-    {100, 1, 1000, 7, true, 3.0, 33},
-    {100, 1, 1000, 7, true, 4.5, 0},
+    {100, 1, 1000, true, 0.0, 13208},
+    {100, 1, 1000, true, 2.0, 939},
+    {100, 1, 1000, true, 3.0, 110},
+    {100, 1, 1000, true, 4.5, 5},
 
-    {-1, -1, -1, -1, true, -1.0, -1}
+    {-1, -1, -1, true, -1.0, -1}
 };
 
-int get_expected_errors(int n, uint32_t s, int len, int k, bool tail, float ebno) {
+#else
+
+static expected_errors_t expected_errors[] = {
+    {1000, 1, 40, true, 0.0, 5363},
+    {1000, 1, 40, true, 2.0, 356},
+    {1000, 1, 40, true, 3.0, 48},
+    {1000, 1, 40, true, 4.5, 0},
+
+    {100, 1, 1000, true, 0.0, 8753},
+    {100, 1, 1000, true, 2.0, 350},
+    {100, 1, 1000, true, 3.0, 33},
+    {100, 1, 1000, true, 4.5, 0},
+
+    {-1, -1, -1, true, -1.0, -1}
+};
+
+#endif
+int get_expected_errors(int n, uint32_t s, int len, bool tail, float ebno) {
   int i;
   i=0;
   while(expected_errors[i].n != -1) {
     if (expected_errors[i].n == n
       && expected_errors[i].s == s
       && expected_errors[i].len == len
-      && expected_errors[i].k == k
       && expected_errors[i].tail == tail
       && expected_errors[i].ebno == ebno) {
       break;
diff --git a/srslte/lib/phch/src/pdsch.c b/srslte/lib/phch/src/pdsch.c
index 4117d6736..2bfb5090f 100644
--- a/srslte/lib/phch/src/pdsch.c
+++ b/srslte/lib/phch/src/pdsch.c
@@ -121,8 +121,12 @@ int srslte_pdsch_cp(srslte_pdsch_t *q, cf_t *input, cf_t *output, srslte_ra_dl_g
           // This is a symbol in a normal PRB with or without references
           if (l >= lstart && l < lend) {
             if (SRSLTE_SYMBOL_HAS_REF(l, q->cell.cp, q->cell.nof_ports)) {
-              if (nof_refs == 2 && l != 0) {    
-                offset = q->cell.id % 3 + 3;
+              if (nof_refs == 2) {
+                if (l == 0) {
+                  offset = q->cell.id % 6;
+                } else {
+                  offset = (q->cell.id + 3) % 6;                  
+                }
               } else {
                 offset = q->cell.id % 3;
               }
@@ -411,6 +415,11 @@ int srslte_pdsch_decode_rnti(srslte_pdsch_t *q,
           cfg->nbits.nof_re / q->cell.nof_ports);
     }
     
+    if (SRSLTE_VERBOSE_ISDEBUG()) {
+      DEBUG("SAVED FILE pdsch_symbols.dat: symbols after equalization\n",0);
+      srslte_vec_save_file("pdsch_symbols.dat", q->d, cfg->nbits.nof_re*sizeof(cf_t));
+    }
+    
     /* demodulate symbols 
     * The MAX-log-MAP algorithm used in turbo decoding is unsensitive to SNR estimation, 
     * thus we don't need tot set it in the LLRs normalization
@@ -429,6 +438,11 @@ int srslte_pdsch_decode_rnti(srslte_pdsch_t *q,
       srslte_scrambling_s_offset(&q->seq[cfg->sf_idx], q->e, 0, cfg->nbits.nof_bits);      
     }
 
+    if (SRSLTE_VERBOSE_ISDEBUG()) {
+      DEBUG("SAVED FILE llr.dat: LLR estimates after demodulation and descrambling\n",0);
+      srslte_vec_save_file("llr.dat", q->e, cfg->nbits.nof_bits*sizeof(int16_t));
+    }
+
     return srslte_dlsch_decode(&q->dl_sch, cfg, softbuffer, q->e, data);      
     
   } else {
diff --git a/srslte/lib/phch/src/sch.c b/srslte/lib/phch/src/sch.c
index 961bd6551..846295d49 100644
--- a/srslte/lib/phch/src/sch.c
+++ b/srslte/lib/phch/src/sch.c
@@ -284,19 +284,9 @@ static int encode_tb_off(srslte_sch_t *q,
         if (cb_segm->C > 1) {
           srslte_crc_attach_byte(&q->crc_cb, q->cb_in, rlen);
         }
-        
-        if (SRSLTE_VERBOSE_ISDEBUG()) {
-          DEBUG("CB#%d: ", i);
-          srslte_vec_fprint_byte(stdout, q->cb_in, cb_len/8);
-        }
 
         /* Turbo Encoding */
-        srslte_tcod_encode_lut(&q->encoder, q->cb_in, q->parity_bits, cblen_idx);
-        
-        if (SRSLTE_VERBOSE_ISDEBUG()) {
-          DEBUG("CB#%d encoded: ", i);
-          srslte_vec_fprint_byte(stdout, q->parity_bits, 2*cb_len/8);
-        }
+        srslte_tcod_encode_lut(&q->encoder, q->cb_in, q->parity_bits, cblen_idx);        
       }
       DEBUG("RM cblen_idx=%d, n_e=%d, wp=%d, nof_e_bits=%d\n",cblen_idx, n_e, wp, nof_e_bits);
       
@@ -400,6 +390,7 @@ static int decode_tb(srslte_sch_t *q,
         n_e = Qm * ((uint32_t) ceilf((float) Gp/cb_segm->C));
       }
 
+      bzero(softbuffer->buffer_f[i], (3*cb_len+12)*sizeof(int16_t));
       /* Rate Unmatching */
       if (srslte_rm_turbo_rx_lut(&e_bits[rp], softbuffer->buffer_f[i], n_e, cblen_idx, rv)) {
         fprintf(stderr, "Error in rate matching\n");
@@ -407,8 +398,10 @@ static int decode_tb(srslte_sch_t *q,
       }
 
       if (SRSLTE_VERBOSE_ISDEBUG()) {
-        DEBUG("CB#%d RMOUT: ", i);
-        srslte_vec_fprint_s(stdout, softbuffer->buffer_f[i], 3*cb_len+12);
+        char tmpstr[64]; 
+        snprintf(tmpstr,64,"rmout_%d.dat",i);
+        DEBUG("SAVED FILE %s: Encoded turbo code block %d\n", tmpstr, i);
+        srslte_vec_save_file(tmpstr, softbuffer->buffer_f[i], (3*cb_len+12)*sizeof(int16_t));
       }
 
       /* Turbo Decoding with CRC-based early stopping */
@@ -444,11 +437,6 @@ static int decode_tb(srslte_sch_t *q,
       INFO("CB#%d: cb_len: %d, rlen: %d, wp: %d, rp: %d, E: %d, n_iters=%d\n", i,
           cb_len, rlen, wp, rp, n_e, q->nof_iterations);
       
-
-      if (SRSLTE_VERBOSE_ISDEBUG()) {
-        DEBUG("CB#%d IN: ", i);
-        srslte_vec_fprint_byte(stdout, q->cb_in, cb_len/8);
-      }
             
       // If CB CRC is not correct, early_stop will be false and wont continue with rest of CBs
 
diff --git a/srslte/lib/phch/test/CMakeLists.txt b/srslte/lib/phch/test/CMakeLists.txt
index 9d922e3fe..0da0469e6 100644
--- a/srslte/lib/phch/test/CMakeLists.txt
+++ b/srslte/lib/phch/test/CMakeLists.txt
@@ -115,14 +115,14 @@ TARGET_LINK_LIBRARIES(phich_file_test srslte)
 ADD_EXECUTABLE(pdcch_file_test pdcch_file_test.c)
 TARGET_LINK_LIBRARIES(pdcch_file_test srslte)
 
-ADD_EXECUTABLE(pdsch_file_test pdsch_file_test.c)
-TARGET_LINK_LIBRARIES(pdsch_file_test srslte)
+ADD_EXECUTABLE(pdsch_pdcch_file_test pdsch_pdcch_file_test.c)
+TARGET_LINK_LIBRARIES(pdsch_pdcch_file_test srslte)
 
 ADD_TEST(pbch_file_test pbch_file_test -i ${CMAKE_CURRENT_SOURCE_DIR}/signal.1.92M.dat) 
 ADD_TEST(pcfich_file_test pcfich_file_test -c 150 -n 50 -p 2 -i ${CMAKE_CURRENT_SOURCE_DIR}/signal.10M.dat) 
 ADD_TEST(phich_file_test phich_file_test -c 150 -n 50 -p 2 -i ${CMAKE_CURRENT_SOURCE_DIR}/signal.10M.dat) 
 ADD_TEST(pdcch_file_test pdcch_file_test -c 1 -f 3 -n 6 -p 1 -i ${CMAKE_CURRENT_SOURCE_DIR}/signal.1.92M.amar.dat) 
-ADD_TEST(pdsch_file_test pdsch_file_test -c 1 -f 3 -n 6 -p 1 -i ${CMAKE_CURRENT_SOURCE_DIR}/signal.1.92M.amar.dat) 
+ADD_TEST(pdsch_pdcch_file_test pdsch_pdcch_file_test -c 1 -f 3 -n 6 -p 1 -i ${CMAKE_CURRENT_SOURCE_DIR}/signal.1.92M.amar.dat) 
 
 ########################################################################
 # PUSCH TEST  
@@ -195,5 +195,5 @@ BuildMex(MEXNAME prach SOURCES prach_test_mex.c LIBRARIES srslte srslte_mex)
 
 IF(UHD_FOUND)
   ADD_EXECUTABLE(prach_test_usrp prach_test_usrp.c)
-  TARGET_LINK_LIBRARIES(prach_test_usrp srslte srslte_uhd)
+  TARGET_LINK_LIBRARIES(prach_test_usrp srslte_uhd srslte)
 ENDIF(UHD_FOUND)
diff --git a/srslte/lib/phch/test/pdsch_file_test.c b/srslte/lib/phch/test/pdsch_pdcch_file_test.c
similarity index 100%
rename from srslte/lib/phch/test/pdsch_file_test.c
rename to srslte/lib/phch/test/pdsch_pdcch_file_test.c
diff --git a/srslte/lib/phch/test/pdsch_test.c b/srslte/lib/phch/test/pdsch_test.c
index afd48ac7c..a8b8e87e5 100644
--- a/srslte/lib/phch/test/pdsch_test.c
+++ b/srslte/lib/phch/test/pdsch_test.c
@@ -33,6 +33,9 @@
 
 #include "srslte/srslte.h"
 
+// Enable to measure execution time
+//#define DO_OFDM
+
 srslte_cell_t cell = {
   6,            // nof_prb
   1,            // nof_ports
@@ -105,17 +108,29 @@ void parse_args(int argc, char **argv) {
   }
 }
 
+uint8_t *data = NULL;
+cf_t *ce[SRSLTE_MAX_PORTS];
+srslte_softbuffer_rx_t softbuffer_rx;
+srslte_ra_dl_grant_t grant; 
+srslte_pdsch_cfg_t pdsch_cfg; 
+cf_t *sf_symbols;
+cf_t *slot_symbols[SRSLTE_MAX_PORTS];
+srslte_pdsch_t pdsch;
+srslte_ofdm_t ofdm_tx, ofdm_rx; 
+
+int dummy_function() {
+#ifdef DO_OFDM
+  srslte_ofdm_rx_sf(&ofdm_rx, sf_symbols, slot_symbols[1]);
+#endif
+  srslte_softbuffer_rx_reset_tbs(&softbuffer_rx, grant.mcs.tbs);
+  return srslte_pdsch_decode(&pdsch, &pdsch_cfg, &softbuffer_rx, slot_symbols[0], ce, 0, data);
+}
+
 int main(int argc, char **argv) {
-  srslte_pdsch_t pdsch;
   uint32_t i, j;
-  uint8_t *data = NULL;
-  cf_t *ce[SRSLTE_MAX_PORTS];
-  cf_t *slot_symbols[SRSLTE_MAX_PORTS];
   int ret = -1;
   struct timeval t[3];
-  srslte_pdsch_cfg_t pdsch_cfg; 
   srslte_softbuffer_tx_t softbuffer_tx;
-  srslte_softbuffer_rx_t softbuffer_rx;
   uint32_t rv;
 
   parse_args(argc,argv);
@@ -132,20 +147,24 @@ int main(int argc, char **argv) {
   dci.mcs_idx = mcs;
   dci.rv_idx = rv_idx;
   dci.type0_alloc.rbg_bitmask = 0xffffffff;
-  srslte_ra_dl_grant_t grant; 
   if (srslte_ra_dl_dci_to_grant(&dci, cell.nof_prb, true, &grant)) {
     fprintf(stderr, "Error computing resource allocation\n");
     return ret;
   }
+  
+  srslte_ofdm_tx_init(&ofdm_tx, cell.cp, cell.nof_prb);
+  srslte_ofdm_rx_init(&ofdm_rx, cell.cp, cell.nof_prb);
 
+  sf_symbols=srslte_vec_malloc(sizeof(cf_t)*SRSLTE_SF_LEN_PRB(cell.nof_prb));
+  
   /* Configure PDSCH */
-  if (srslte_pdsch_cfg(&pdsch_cfg, cell, &grant, cfi, subframe, 0)) {
+  if (srslte_pdsch_cfg(&pdsch_cfg, cell, &grant, cfi, subframe, rv_idx)) {
     fprintf(stderr, "Error configuring PDSCH\n");
     exit(-1);
   }
 
   /* init memory */
-  for (i=0;i<cell.nof_ports;i++) {
+  for (i=0;i<SRSLTE_MAX_PORTS;i++) {
     ce[i] = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp));
     if (!ce[i]) {
       perror("srslte_vec_malloc");
@@ -174,66 +193,83 @@ int main(int argc, char **argv) {
   
   srslte_pdsch_set_rnti(&pdsch, rnti);
   
-  if (srslte_softbuffer_tx_init(&softbuffer_tx, cell.nof_prb)) {
-    fprintf(stderr, "Error initiating TX soft buffer\n");
-    goto quit;
-  }
-
   if (srslte_softbuffer_rx_init(&softbuffer_rx, cell.nof_prb)) {
     fprintf(stderr, "Error initiating RX soft buffer\n");
     goto quit;
   }
-  
+
   if (input_file) {
     srslte_filesource_t fsrc;
     if (srslte_filesource_init(&fsrc, input_file, SRSLTE_COMPLEX_FLOAT_BIN)) {
       fprintf(stderr, "Error opening file %s\n", input_file);
       exit(-1);
     }
+#ifdef DO_OFDM
+    srslte_filesource_read(&fsrc, sf_symbols, SRSLTE_SF_LEN_PRB(cell.nof_prb));
+#else
     srslte_filesource_read(&fsrc, slot_symbols[0], SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp));
+#endif
+    
+    srslte_chest_dl_t chest; 
+    if (srslte_chest_dl_init(&chest, cell)) {
+      printf("Error initializing equalizer\n");
+      exit(-1);
+    }
+    srslte_chest_dl_estimate(&chest, slot_symbols[0], ce, subframe);    
+    srslte_chest_dl_free(&chest);
     
     srslte_filesource_free(&fsrc);
-  }
-  
-  if (SRSLTE_VERBOSE_ISNONE()) {
-    printf("Decoding TBS: %d\r",grant.mcs.tbs);
-  }
-  for (i=0;i<grant.mcs.tbs/8;i++) {
-    data[i] = rand()%256;
-  }
+  } else { 
+    if (srslte_softbuffer_tx_init(&softbuffer_tx, cell.nof_prb)) {
+      fprintf(stderr, "Error initiating TX soft buffer\n");
+      goto quit;
+    }
 
-  for (rv=0;rv<=rv_idx;rv++) {
+    for (i=0;i<grant.mcs.tbs/8;i++) {
+      data[i] = rand()%256;
+    }
+    for (rv=0;rv<=rv_idx;rv++) {
 
-    pdsch_cfg.rv = rv; 
-    
-    if (!input_file) {
-      if (srslte_pdsch_encode(&pdsch, &pdsch_cfg, &softbuffer_tx, data, slot_symbols)) {
-        fprintf(stderr, "Error encoding PDSCH\n");
-        goto quit;
+      pdsch_cfg.rv = rv; 
+      
+      if (!input_file) {
+        if (srslte_pdsch_encode(&pdsch, &pdsch_cfg, &softbuffer_tx, data, slot_symbols)) {
+          fprintf(stderr, "Error encoding PDSCH\n");
+          goto quit;
+        }
       }
-    }
-    
-    /* combine outputs */
-    for (i=0;i<cell.nof_ports;i++) {
-      for (j=0;j<SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp);j++) {
-        if (i > 0) {
-          slot_symbols[0][j] += slot_symbols[i][j];
+      
+      /* combine outputs */
+      for (i=0;i<cell.nof_ports;i++) {
+        for (j=0;j<SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp);j++) {
+          if (i > 0) {
+            slot_symbols[0][j] += slot_symbols[i][j];
+          }
+          ce[i][j] = 1;
         }
-        ce[i][j] = 1;
       }
-    }
-
-    gettimeofday(&t[1], NULL);
-    int r = srslte_pdsch_decode(&pdsch, &pdsch_cfg, &softbuffer_rx, slot_symbols[0], ce, 0, data);
-    gettimeofday(&t[2], NULL);
-    get_time_interval(t);
-    printf("DECODED %s in %d:%d (%.2f Mbps)\n", r?"Error":"OK",
-             (int) t[0].tv_sec, (int) t[0].tv_usec, (float) grant.mcs.tbs/t[0].tv_usec);                
-    if (r) {
-      ret = -1;
-      goto quit;
-    } 
+      
+  #ifdef DO_OFDM
+      srslte_ofdm_tx_sf(&ofdm_tx, slot_symbols[0], sf_symbols);
+  #endif
+    }    
   } 
+  int M=1;
+  int r=0; 
+  srslte_sch_set_max_noi(&pdsch.dl_sch, 10);
+  gettimeofday(&t[1], NULL);
+  for (i=0;i<M;i++) {
+    r = dummy_function();
+  }
+  gettimeofday(&t[2], NULL);
+  get_time_interval(t);
+  printf("DECODED %s in %.2f (PHY bitrate=%.2f Mbps. Processing bitrate=%.2f Mbps)\n", r?"Error":"OK",
+            (float) t[0].tv_usec/M, (float) grant.mcs.tbs/1000, (float) grant.mcs.tbs*M/t[0].tv_usec);                
+  if (r) {
+    ret = -1;
+    goto quit;
+  } 
+
   ret = 0;
 quit:
   srslte_pdsch_free(&pdsch);
diff --git a/srslte/lib/phch/test/pdsch_test_mex.c b/srslte/lib/phch/test/pdsch_test_mex.c
index b943cf25c..5bcc099b7 100644
--- a/srslte/lib/phch/test/pdsch_test_mex.c
+++ b/srslte/lib/phch/test/pdsch_test_mex.c
@@ -120,6 +120,9 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     mexErrMsgTxt("Field RV not found in pdsch config\n");
     return;
   }
+
+  uint32_t max_iterations = 5;
+  mexutils_read_uint32_struct(PDSCHCFG, "NTurboDecIts", &max_iterations);
   
   char *mod_str = mexutils_get_char_struct(PDSCHCFG, "Modulation");
   
@@ -136,7 +139,6 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
 
   mxFree(mod_str);
   
-  float *prbset; 
   mxArray *p; 
   p = mxGetField(PDSCHCFG, 0, "PRBSet");
   if (!p) {
@@ -144,9 +146,18 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     return;
   } 
   
-  // Only localized PRB supported 
-  grant.nof_prb = mexutils_read_f(p, &prbset);
-
+  float *prbset_f;
+  uint64_t *prbset;
+  if (mxGetClassID(p) == mxDOUBLE_CLASS) {
+    grant.nof_prb = mexutils_read_f(p, &prbset_f);
+    prbset = malloc(sizeof(uint64_t)*grant.nof_prb);
+    for (i=0;i<grant.nof_prb;i++) {
+      prbset[i] = (uint64_t) prbset_f[i];
+    }
+  } else {
+    grant.nof_prb = mexutils_read_uint64(p, &prbset);
+  }
+  
   for (i=0;i<cell.nof_prb;i++) {
     grant.prb_idx[0][i] = false; 
     for (int j=0;j<grant.nof_prb && !grant.prb_idx[0][i];j++) {
@@ -156,7 +167,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     }
     grant.prb_idx[1][i] = grant.prb_idx[0][i];
   }
-
+  
   free(prbset);
   
   /* Configure rest of pdsch_cfg parameters */
@@ -203,6 +214,8 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   if (!data_bytes) {
     return;
   }
+  
+  srslte_sch_set_max_noi(&pdsch.dl_sch, max_iterations);
 
   int r = srslte_pdsch_decode(&pdsch, &cfg, &softbuffer, input_fft, ce, noise_power, data_bytes);
 
@@ -226,9 +239,6 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   if (nlhs >= 5) {
     mexutils_write_s(pdsch.e, &plhs[4], cfg.nbits.nof_bits, 1);  
   }
-  if (nlhs >= 6) {
-    mexutils_write_s(softbuffer.buffer_f[9], &plhs[5], 16908, 1);  
-  }
   
   srslte_chest_dl_free(&chest);
   srslte_pdsch_free(&pdsch);
diff --git a/srslte/lib/sync/test/pss_file.c b/srslte/lib/sync/test/pss_file.c
index 90524c619..afca92f5c 100644
--- a/srslte/lib/sync/test/pss_file.c
+++ b/srslte/lib/sync/test/pss_file.c
@@ -243,7 +243,7 @@ int main(int argc, char **argv) {
           if (srslte_sss_synch_N_id_1(&sss, m0, m1) != N_id_1) {
             sss_error2++;            
           }
-          printf("sf_idx = %d\n", srslte_sss_synch_subframe(m0, m1));
+          INFO("sf_idx = %d\n", srslte_sss_synch_subframe(m0, m1));
           INFO("Partial N_id_1: %d\n", srslte_sss_synch_N_id_1(&sss, m0, m1));
           srslte_sss_synch_m0m1_diff(&sss, &buffer[sss_idx], &m0, &m0_value, &m1, &m1_value);
           if (srslte_sss_synch_N_id_1(&sss, m0, m1) != N_id_1) {
@@ -295,7 +295,7 @@ int main(int argc, char **argv) {
     printf("[%5d]: Pos: %5d, PSR: %4.1f (~%4.1f) Pdet: %4.2f, "
            "FA: %4.2f, CFO: %+4.1f KHz SSSmiss: %4.2f/%4.2f/%4.2f CPNorm: %.0f%%\r", 
            frame_cnt, 
-           peak_idx, 
+           peak_idx - flen/10, 
            peak_value, mean_peak,
            (float) nof_det/frame_cnt, 
            (float) nof_nopeakdet/frame_cnt, mean_cfo*15, 
diff --git a/srslte/lib/ue/src/ue_sync.c b/srslte/lib/ue/src/ue_sync.c
index c3c13ec7a..9620cdc91 100644
--- a/srslte/lib/ue/src/ue_sync.c
+++ b/srslte/lib/ue/src/ue_sync.c
@@ -47,7 +47,7 @@ cf_t dummy[MAX_TIME_OFFSET];
 
 cf_t kk[1024*1024];
 
-int srslte_ue_sync_init_file(srslte_ue_sync_t *q, uint32_t nof_prb, char *file_name, int offset) {
+int srslte_ue_sync_init_file(srslte_ue_sync_t *q, uint32_t nof_prb, char *file_name, int offset_time, float offset_freq) {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   
   if (q                   != NULL && 
@@ -58,6 +58,14 @@ int srslte_ue_sync_init_file(srslte_ue_sync_t *q, uint32_t nof_prb, char *file_n
     bzero(q, sizeof(srslte_ue_sync_t));
     q->file_mode = true; 
     q->sf_len = SRSLTE_SF_LEN(srslte_symbol_sz(nof_prb));
+    q->file_cfo = -offset_freq; 
+    q->correct_cfo = true; 
+    q->fft_size = srslte_symbol_sz(nof_prb);
+    
+    if (srslte_cfo_init(&q->file_cfo_correct, 2*q->sf_len)) {
+      fprintf(stderr, "Error initiating CFO\n");
+      goto clean_exit; 
+    }
     
     if (srslte_filesource_init(&q->file_source, file_name, SRSLTE_COMPLEX_FLOAT_BIN)) {
       fprintf(stderr, "Error opening file %s\n", file_name);
@@ -70,9 +78,9 @@ int srslte_ue_sync_init_file(srslte_ue_sync_t *q, uint32_t nof_prb, char *file_n
       goto clean_exit;
     }
     
-    INFO("Offseting input file by %d samples\n", offset);
+    INFO("Offseting input file by %d samples and %.1f KHz\n", offset_time, offset_freq/1000);
     
-    srslte_filesource_read(&q->file_source, kk, offset);
+    srslte_filesource_read(&q->file_source, kk, offset_time);
     srslte_ue_sync_reset(q);
     
     ret = SRSLTE_SUCCESS;
@@ -421,6 +429,13 @@ int srslte_ue_sync_zerocopy(srslte_ue_sync_t *q, cf_t *input_buffer) {
           return SRSLTE_ERROR; 
         }
       }
+      if (q->correct_cfo) {
+        srslte_cfo_correct(&q->file_cfo_correct, 
+                    input_buffer, 
+                    input_buffer, 
+                    q->file_cfo / 15000 / q->fft_size);               
+                    
+      }
       q->sf_idx++;
       if (q->sf_idx == 10) {
         q->sf_idx = 0;